[ros-ros-controllers] 01/02: Imported Upstream version 0.10.0
Leopold Palomo-Avellaneda
leo at alaxarxa.net
Thu Jul 28 10:36:57 UTC 2016
This is an automated email from the git hooks/post-receive script.
lepalom-guest pushed a commit to branch master
in repository ros-ros-controllers.
commit ec94657a8569ff7cadac23cf7c61ccae5a138043
Author: Leopold Palomo-Avellaneda <leopold.palomo at upc.edu>
Date: Mon Jan 18 16:12:16 2016 +0100
Imported Upstream version 0.10.0
---
.gitignore | 20 +
.travis.yml | 48 +
README.md | 14 +
diff_drive_controller/CHANGELOG.rst | 71 ++
diff_drive_controller/CMakeLists.txt | 76 ++
diff_drive_controller/README.md | 5 +
.../diff_drive_controller_plugins.xml | 9 +
.../diff_drive_controller/diff_drive_controller.h | 202 ++++
.../include/diff_drive_controller/odometry.h | 210 ++++
.../include/diff_drive_controller/speed_limiter.h | 131 +++
diff_drive_controller/package.xml | 39 +
.../src/diff_drive_controller.cpp | 561 +++++++++
diff_drive_controller/src/odometry.cpp | 174 +++
diff_drive_controller/src/speed_limiter.cpp | 137 +++
.../test/diff_drive_bad_urdf.test | 17 +
.../test/diff_drive_common.launch | 31 +
.../test/diff_drive_controller.test | 13 +
.../test/diff_drive_controller_limits.test | 16 +
.../test/diff_drive_controller_nan.test | 13 +
.../test/diff_drive_fail_test.cpp | 57 +
.../test/diff_drive_limits_test.cpp | 231 ++++
.../test/diff_drive_multipliers.test | 16 +
diff_drive_controller/test/diff_drive_nan_test.cpp | 92 ++
diff_drive_controller/test/diff_drive_odom_tf.test | 19 +
.../test/diff_drive_odom_tf_test.cpp | 59 +
.../test/diff_drive_open_loop.test | 16 +
.../test/diff_drive_radius_param.test | 19 +
.../test/diff_drive_radius_param_fail.test | 19 +
.../test/diff_drive_separation_param.test | 21 +
diff_drive_controller/test/diff_drive_test.cpp | 153 +++
diff_drive_controller/test/diff_drive_timeout.test | 16 +
.../test/diff_drive_timeout_test.cpp | 72 ++
diff_drive_controller/test/diff_drive_wrong.test | 16 +
diff_drive_controller/test/diffbot.cpp | 60 +
diff_drive_controller/test/diffbot.h | 139 +++
diff_drive_controller/test/diffbot.xacro | 77 ++
diff_drive_controller/test/diffbot_bad.xacro | 142 +++
.../test/diffbot_controllers.yaml | 8 +
diff_drive_controller/test/diffbot_limits.yaml | 19 +
.../test/diffbot_multipliers.yaml | 3 +
diff_drive_controller/test/diffbot_open_loop.yaml | 2 +
.../test/diffbot_sphere_wheels.xacro | 75 ++
diff_drive_controller/test/diffbot_timeout.yaml | 2 +
diff_drive_controller/test/diffbot_wrong.yaml | 8 +
.../test/skid_steer_common.launch | 31 +
.../test/skid_steer_controller.test | 13 +
.../test/skid_steer_no_wheels.test | 16 +
diff_drive_controller/test/skidsteerbot.cpp | 60 +
diff_drive_controller/test/skidsteerbot.xacro | 90 ++
.../test/skidsteerbot_controllers.yaml | 8 +
.../test/skidsteerbot_no_wheels.yaml | 8 +
diff_drive_controller/test/sphere_wheel.xacro | 43 +
diff_drive_controller/test/test_common.h | 97 ++
diff_drive_controller/test/view_diffbot.launch | 26 +
.../test/view_skidsteerbot.launch | 26 +
diff_drive_controller/test/wheel.xacro | 43 +
effort_controllers/.gitignore | 2 +
effort_controllers/CHANGELOG.rst | 115 ++
effort_controllers/CMakeLists.txt | 49 +
effort_controllers/effort_controllers_plugins.xml | 27 +
.../effort_controllers/joint_effort_controller.h | 62 +
.../joint_group_effort_controller.h | 63 ++
.../effort_controllers/joint_position_controller.h | 195 ++++
.../effort_controllers/joint_velocity_controller.h | 166 +++
effort_controllers/mainpage.dox | 14 +
effort_controllers/package.xml | 38 +
effort_controllers/src/joint_effort_controller.cpp | 48 +
.../src/joint_group_effort_controller.cpp | 49 +
.../src/joint_position_controller.cpp | 262 +++++
.../src/joint_velocity_controller.cpp | 170 +++
force_torque_sensor_controller/.gitignore | 3 +
force_torque_sensor_controller/CHANGELOG.rst | 78 ++
force_torque_sensor_controller/CMakeLists.txt | 33 +
.../force_torque_sensor_controller.launch | 7 +
.../force_torque_sensor_controller.yaml | 3 +
.../force_torque_sensor_plugin.xml | 7 +
.../force_torque_sensor_controller.h | 65 ++
force_torque_sensor_controller/package.xml | 34 +
.../src/force_torque_sensor_controller.cpp | 106 ++
forward_command_controller/.gitignore | 1 +
forward_command_controller/CHANGELOG.rst | 75 ++
forward_command_controller/CMakeLists.txt | 15 +
.../forward_command_controller.h | 101 ++
.../forward_joint_group_command_controller.h | 141 +++
forward_command_controller/mainpage.dox | 14 +
forward_command_controller/package.xml | 29 +
gripper_action_controller/CHANGELOG.rst | 36 +
gripper_action_controller/CMakeLists.txt | 177 +++
.../gripper_action_controller.h | 161 +++
.../gripper_action_controller_impl.h | 351 ++++++
.../hardware_interface_adapter.h | 190 ++++
gripper_action_controller/package.xml | 75 ++
gripper_action_controller/ros_control_plugins.xml | 17 +
.../src/gripper_action_controller.cpp | 57 +
imu_sensor_controller/.gitignore | 3 +
imu_sensor_controller/CHANGELOG.rst | 79 ++
imu_sensor_controller/CMakeLists.txt | 33 +
imu_sensor_controller/imu_sensor_controller.launch | 7 +
imu_sensor_controller/imu_sensor_controller.yaml | 3 +
imu_sensor_controller/imu_sensor_plugin.xml | 7 +
.../imu_sensor_controller/imu_sensor_controller.h | 65 ++
imu_sensor_controller/package.xml | 34 +
.../src/imu_sensor_controller.cpp | 195 ++++
joint_state_controller/.gitignore | 2 +
joint_state_controller/CHANGELOG.rst | 105 ++
joint_state_controller/CMakeLists.txt | 40 +
.../joint_state_controller.h | 103 ++
.../joint_state_controller.launch | 7 +
joint_state_controller/joint_state_controller.yaml | 3 +
joint_state_controller/joint_state_plugin.xml | 7 +
joint_state_controller/package.xml | 29 +
joint_state_controller/rosdoc.yaml | 5 +
.../src/joint_state_controller.cpp | 180 +++
.../test/joint_state_controller.test | 12 +
.../joint_state_controller_extra_joints_ko.yaml | 19 +
.../joint_state_controller_extra_joints_ok.yaml | 13 +
.../test/joint_state_controller_ko.yaml | 3 +
.../test/joint_state_controller_ok.yaml | 3 +
.../test/joint_state_controller_test.cpp | 328 ++++++
joint_trajectory_controller/.gitignore | 5 +
joint_trajectory_controller/CHANGELOG.rst | 139 +++
joint_trajectory_controller/CMakeLists.txt | 109 ++
joint_trajectory_controller/README.md | 7 +
.../images/new_trajectory.png | Bin 0 -> 73149 bytes
.../images/new_trajectory.svg | 1192 ++++++++++++++++++++
.../images/trajectory_replacement_future.png | Bin 0 -> 71754 bytes
.../images/trajectory_replacement_future.svg | 392 +++++++
.../images/trajectory_replacement_now.png | Bin 0 -> 64005 bytes
.../images/trajectory_replacement_now.svg | 345 ++++++
.../images/trajectory_replacement_past.png | Bin 0 -> 74230 bytes
.../images/trajectory_replacement_past.svg | 381 +++++++
.../hardware_interface_adapter.h | 322 ++++++
.../init_joint_trajectory.h | 391 +++++++
.../joint_trajectory_controller.h | 277 +++++
.../joint_trajectory_controller_impl.h | 712 ++++++++++++
.../joint_trajectory_msg_utils.h | 170 +++
.../joint_trajectory_segment.h | 280 +++++
.../joint_trajectory_controller/tolerances.h | 257 +++++
.../trajectory_interface/pos_vel_acc_state.h | 77 ++
.../trajectory_interface/quintic_spline_segment.h | 414 +++++++
.../trajectory_interface/trajectory_interface.h | 156 +++
joint_trajectory_controller/mainpage.dox | 14 +
joint_trajectory_controller/package.xml | 58 +
.../ros_control_plugins.xml | 30 +
joint_trajectory_controller/rosdoc.yaml | 5 +
.../src/joint_trajectory_controller.cpp | 71 ++
.../test/init_joint_trajectory_test.cpp | 843 ++++++++++++++
.../test/joint_trajectory_controller.test | 35 +
.../test/joint_trajectory_controller_test.cpp | 940 +++++++++++++++
.../test/joint_trajectory_msg_utils_test.cpp | 239 ++++
.../test/joint_trajectory_segment_test.cpp | 439 +++++++
.../test/quintic_spline_segment_test.cpp | 559 +++++++++
joint_trajectory_controller/test/rrbot.cpp | 127 +++
joint_trajectory_controller/test/rrbot.xacro | 129 +++
.../test/rrbot_controllers.yaml | 14 +
joint_trajectory_controller/test/tolerances.test | 4 +
joint_trajectory_controller/test/tolerances.yaml | 14 +
.../test/tolerances_test.cpp | 320 ++++++
.../test/trajectory_interface_test.cpp | 364 ++++++
position_controllers/.gitignore | 2 +
position_controllers/CHANGELOG.rst | 87 ++
position_controllers/CMakeLists.txt | 33 +
.../joint_group_position_controller.h | 63 ++
.../joint_position_controller.h | 62 +
position_controllers/mainpage.dox | 14 +
position_controllers/package.xml | 26 +
.../position_controllers_plugins.xml | 15 +
.../src/joint_group_position_controller.cpp | 53 +
.../src/joint_position_controller.cpp | 47 +
ros_controllers/CHANGELOG.rst | 66 ++
ros_controllers/CMakeLists.txt | 4 +
ros_controllers/package.xml | 35 +
rqt_joint_trajectory_controller/CHANGELOG.rst | 87 ++
rqt_joint_trajectory_controller/CMakeLists.txt | 19 +
rqt_joint_trajectory_controller/package.xml | 24 +
rqt_joint_trajectory_controller/plugin.xml | 21 +
.../resource/double_editor.ui | 92 ++
.../resource/joint_trajectory_controller.ui | 202 ++++
rqt_joint_trajectory_controller/resource/off.svg | 267 +++++
rqt_joint_trajectory_controller/resource/on.svg | 231 ++++
rqt_joint_trajectory_controller/resource/scope.png | Bin 0 -> 3205 bytes
rqt_joint_trajectory_controller/resource/scope.svg | 269 +++++
rqt_joint_trajectory_controller/rosdoc.yaml | 2 +
.../scripts/rqt_joint_trajectory_controller | 8 +
rqt_joint_trajectory_controller/setup.py | 11 +
.../rqt_joint_trajectory_controller/__init__.py | 0
.../double_editor.py | 121 ++
.../joint_limits_urdf.py | 82 ++
.../joint_trajectory_controller.py | 482 ++++++++
.../update_combo.py | 66 ++
velocity_controllers/.gitignore | 2 +
velocity_controllers/CHANGELOG.rst | 86 ++
velocity_controllers/CMakeLists.txt | 51 +
.../joint_group_velocity_controller.h | 64 ++
.../joint_position_controller.h | 202 ++++
.../joint_velocity_controller.h | 63 ++
velocity_controllers/mainpage.dox | 14 +
velocity_controllers/package.xml | 37 +
.../src/joint_group_velocity_controller.cpp | 49 +
.../src/joint_position_controller.cpp | 263 +++++
.../src/joint_velocity_controller.cpp | 48 +
.../velocity_controllers_plugins.xml | 21 +
202 files changed, 21132 insertions(+)
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..2703837
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,20 @@
+# vi stuff
+.*.sw?
+
+# Python stuff
+*.pyc
+
+# Data files
+*.dat
+*.csv
+
+# Emacs temp files
+.#*
+
+# OSX Files
+.DS_Store
+
+# We had to disable manifest.xml in all packages because of a package naming conflict
+# See issue https://github.com/ros-controls/ros_control/issues/101#issuecomment-21781620
+# and issue http://answers.gazebosim.org/question/4134/installing-gazebo-19-on-ros-groovy/?answer=4138#post-id-4138
+manifest.xml
diff --git a/.travis.yml b/.travis.yml
new file mode 100644
index 0000000..30f043f
--- /dev/null
+++ b/.travis.yml
@@ -0,0 +1,48 @@
+# Travis Continuous Integration Configuration File For ROS Control Projects
+# Author: Dave Coleman
+language:
+ - cpp
+ - python
+python:
+ - "2.7"
+compiler:
+ - gcc
+notifications:
+ email:
+ recipients:
+ - davetcoleman at gmail.com
+ - adolfo.rodriguez at pal-robotics.com
+ on_success: change #[always|never|change] # default: change
+ on_failure: change #[always|never|change] # default: always
+before_install: # Use this to prepare the system to install prerequisites or dependencies
+ # Define some config vars
+ - export ROS_DISTRO=hydro
+ - export CI_SOURCE_PATH=$(pwd)
+ - export REPOSITORY_NAME=${PWD##*/}
+ - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
+ - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise 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
+ - sudo apt-get install -qq -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-ros
+ # Setup rosdep
+ - sudo rosdep init
+ - rosdep update
+install: # Use this to install any prerequisites or dependencies necessary to run your build
+ # Create workspace
+ - mkdir -p ~/ros/ws_ros_controls/src
+ - cd ~/ros/ws_ros_controls/src
+ - wstool init .
+ # Download non-debian stuff
+ - wstool merge https://raw.github.com/ros-controls/ros_control/$ROS_DISTRO-devel/ros_control.rosinstall
+ - wstool update
+ # Delete the ros_control.rosinstall version of this repo and use the one of the branch we are testing
+ - rm -rf $REPOSITORY_NAME
+ - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
+ - cd ../
+ # Install dependencies for source repos
+ - rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
+before_script: # Use this to prepare your build for testing e.g. copy database configurations, environment variables, etc.
+ - source /opt/ros/$ROS_DISTRO/setup.bash
+script: # All commands must exit with code 0 on success. Anything else is considered failure.
+ - catkin_make -j2
+
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..b43ba8d
--- /dev/null
+++ b/README.md
@@ -0,0 +1,14 @@
+ros_controllers
+===============
+
+See [ros_control documentation](http://ros.org/wiki/ros_control) on ros.org
+
+
+rosbuild support
+---------------
+
+By the hydro-devel branch of ros_controllers has been catkinized but remains backwards compatible with Fuerte and earlier via a bash script that enables manifest.xml files to be shown. To enable, run ``. enable_manifest_xml.sh`` in the root of this meta package and all of the manifest.xml files will be enabled.
+
+### Build Status
+
+[](https://travis-ci.org/ros-controls/ros_controllers)
\ No newline at end of file
diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst
new file mode 100644
index 0000000..03e0fb2
--- /dev/null
+++ b/diff_drive_controller/CHANGELOG.rst
@@ -0,0 +1,71 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package diff_drive_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+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.
diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt
new file mode 100644
index 0000000..b0389ca
--- /dev/null
+++ b/diff_drive_controller/CMakeLists.txt
@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(diff_drive_controller)
+
+find_package(catkin REQUIRED COMPONENTS
+ controller_interface
+ urdf
+ realtime_tools
+ tf
+ nav_msgs)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(
+ include ${catkin_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})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES diff_drive_controller_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+if (CATKIN_ENABLE_TESTING)
+ find_package(catkin COMPONENTS rostest std_srvs controller_manager tf)
+ include_directories(include ${catkin_INCLUDE_DIRS})
+
+ add_executable(diffbot test/diffbot.cpp)
+ target_link_libraries(diffbot ${catkin_LIBRARIES})
+
+ add_executable(skidsteerbot test/skidsteerbot.cpp)
+ target_link_libraries(skidsteerbot ${catkin_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})
+ 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})
+ 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})
+ 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})
+ add_rostest(test/diff_drive_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})
+ 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})
+ 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_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)
+endif()
diff --git a/diff_drive_controller/README.md b/diff_drive_controller/README.md
new file mode 100644
index 0000000..371baed
--- /dev/null
+++ b/diff_drive_controller/README.md
@@ -0,0 +1,5 @@
+## Diff Drive Controller ##
+
+Controller for a differential drive mobile base.
+
+Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/diff_drive_controller).
diff --git a/diff_drive_controller/diff_drive_controller_plugins.xml b/diff_drive_controller/diff_drive_controller_plugins.xml
new file mode 100644
index 0000000..3ec80bf
--- /dev/null
+++ b/diff_drive_controller/diff_drive_controller_plugins.xml
@@ -0,0 +1,9 @@
+<library path="lib/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>
diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
new file mode 100644
index 0000000..781c219
--- /dev/null
+++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
@@ -0,0 +1,202 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the PAL Robotics nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <nav_msgs/Odometry.h>
+#include <tf/tfMessage.h>
+
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <diff_drive_controller/odometry.h>
+#include <diff_drive_controller/speed_limiter.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 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_;
+
+ /// Velocity command related:
+ struct Commands
+ {
+ double lin;
+ double ang;
+ ros::Time stamp;
+
+ Commands() : lin(0.0), ang(0.0), stamp(0.0) {}
+ };
+ realtime_tools::RealtimeBuffer<Commands> command_;
+ Commands command_struct_;
+ ros::Subscriber sub_command_;
+
+ /// Odometry related:
+ boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
+ boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
+ Odometry odometry_;
+
+ /// Wheel separation, wrt the midpoint of the wheel width:
+ double wheel_separation_;
+
+ /// 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 wheel_radius_multiplier_;
+
+ /// Timeout to consider cmd_vel commands old:
+ double cmd_vel_timeout_;
+
+ /// Frame to use for the robot base:
+ std::string base_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_;
+
+ 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);
+
+ };
+
+ PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase);
+} // namespace diff_drive_controller
diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h
new file mode 100644
index 0000000..8239785
--- /dev/null
+++ b/diff_drive_controller/include/diff_drive_controller/odometry.h
@@ -0,0 +1,210 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the PAL Robotics nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+
+#ifndef ODOMETRY_H_
+#define ODOMETRY_H_
+
+#include <ros/time.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/rolling_mean.hpp>
+#include <boost/function.hpp>
+
+namespace 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 Seperation between left and right wheels [m]
+ * \param wheel_radius Wheel radius [m]
+ */
+ void setWheelParams(double wheel_separation, double wheel_radius);
+
+ /**
+ * \brief Velocity rolling window size setter
+ * \param velocity_rolling_window_size Velocity rolling window size
+ */
+ void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+ private:
+
+ /// Rolling mean accumulator and window:
+ typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
+ typedef bacc::tag::rolling_window RollingWindow;
+
+ /**
+ * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
+ * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders
+ * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+ */
+ void integrateRungeKutta2(double linear, double angular);
+
+ /**
+ * \brief Integrates the velocities (linear and angular) using exact method
+ * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders
+ * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+ */
+ void integrateExact(double linear, double angular);
+
+ /**
+ * \brief Reset linear and angular accumulators
+ */
+ void resetAccumulators();
+
+ /// Current timestamp:
+ ros::Time timestamp_;
+
+ /// Current pose:
+ double x_; // [m]
+ double y_; // [m]
+ double heading_; // [rad]
+
+ /// Current velocity:
+ double linear_; // [m/s]
+ double angular_; // [rad/s]
+
+ /// Wheel kinematic parameters [m]:
+ double wheel_separation_;
+ double 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_;
+ };
+}
+
+#endif /* ODOMETRY_H_ */
diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.h b/diff_drive_controller/include/diff_drive_controller/speed_limiter.h
new file mode 100644
index 0000000..c3ea8f9
--- /dev/null
+++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.h
@@ -0,0 +1,131 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the PAL Robotics nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#ifndef SPEED_LIMITER_H
+#define SPEED_LIMITER_H
+
+namespace 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
+
+#endif // SPEED_LIMITER_H
diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml
new file mode 100644
index 0000000..6b831cc
--- /dev/null
+++ b/diff_drive_controller/package.xml
@@ -0,0 +1,39 @@
+<package>
+ <name>diff_drive_controller</name>
+ <version>0.10.0</version>
+ <description>Controller for a differential drive mobile base.</description>
+ <maintainer email="bence.magyar at pal-robotics.com">Bence Magyar</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 at pal-robotics.com">Bence Magyar</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>controller_interface</build_depend>
+ <build_depend>nav_msgs</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>urdf</build_depend>
+
+ <run_depend>controller_interface</run_depend>
+ <run_depend>nav_msgs</run_depend>
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>urdf</run_depend>
+
+ <!--Tests-->
+ <build_depend>rostest</build_depend>
+
+ <test_depend>std_srvs</test_depend>
+ <test_depend>controller_manager</test_depend>
+ <test_depend>xacro</test_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/diff_drive_controller_plugins.xml"/>
+ </export>
+</package>
diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
new file mode 100644
index 0000000..b581b04
--- /dev/null
+++ b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -0,0 +1,561 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the PAL Robotics nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Bence Magyar
+ */
+
+#include <cmath>
+
+#include <tf/transform_datatypes.h>
+
+#include <urdf_parser/urdf_parser.h>
+
+#include <boost/assign.hpp>
+
+#include <diff_drive_controller/diff_drive_controller.h>
+
+static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
+{
+ return std::sqrt(std::pow(vec1.x-vec2.x,2) +
+ std::pow(vec1.y-vec2.y,2) +
+ std::pow(vec1.z-vec2.z,2));
+}
+
+/*
+ * \brief Check if the link is modeled as a cylinder
+ * \param link Link
+ * \return true if the link is modeled as a Cylinder; false otherwise
+ */
+static bool isCylinder(const boost::shared_ptr<const urdf::Link>& link)
+{
+ if (!link)
+ {
+ ROS_ERROR("Link == NULL.");
+ return false;
+ }
+
+ if (!link->collision)
+ {
+ ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf.");
+ return false;
+ }
+
+ if (!link->collision->geometry)
+ {
+ ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf.");
+ return false;
+ }
+
+ if (link->collision->geometry->type != urdf::Geometry::CYLINDER)
+ {
+ ROS_ERROR_STREAM("Link " << link->name << " does not have cylinder geometry");
+ return false;
+ }
+
+ return true;
+}
+
+/*
+ * \brief Get the wheel radius
+ * \param [in] wheel_link Wheel link
+ * \param [out] wheel_radius Wheel radius [m]
+ * \return true if the wheel radius was found; false otherwise
+ */
+static bool getWheelRadius(const boost::shared_ptr<const urdf::Link>& wheel_link, double& wheel_radius)
+{
+ if (!isCylinder(wheel_link))
+ {
+ ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder!");
+ return false;
+ }
+
+ wheel_radius = (static_cast<urdf::Cylinder*>(wheel_link->collision->geometry.get()))->radius;
+ return true;
+}
+
+namespace diff_drive_controller{
+
+ DiffDriveController::DiffDriveController()
+ : open_loop_(false)
+ , command_struct_()
+ , wheel_separation_(0.0)
+ , wheel_radius_(0.0)
+ , wheel_separation_multiplier_(1.0)
+ , wheel_radius_multiplier_(1.0)
+ , cmd_vel_timeout_(0.5)
+ , base_frame_id_("base_link")
+ , enable_odom_tf_(true)
+ , wheel_joints_size_(0)
+ {
+ }
+
+ 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) or
+ !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_ << ".");
+
+ controller_nh.param("wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_);
+ ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
+ << 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("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("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 );
+
+ // 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 wr = wheel_radius_multiplier_ * wheel_radius_;
+ odometry_.setWheelParams(ws, wr);
+ ROS_INFO_STREAM_NAMED(name_,
+ "Odometry params : wheel separation " << ws
+ << ", wheel radius " << wr);
+
+ setOdomPubFields(root_nh, controller_nh);
+
+ // Get the joint object to use in the realtime loop
+ for (int 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);
+
+ return true;
+ }
+
+ void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
+ {
+ // COMPUTE AND PUBLISH ODOMETRY
+ if (open_loop_)
+ {
+ odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
+ }
+ else
+ {
+ double 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;
+
+ // Apply multipliers:
+ const double ws = wheel_separation_multiplier_ * wheel_separation_;
+ const double wr = wheel_radius_multiplier_ * wheel_radius_;
+
+ // Compute wheels velocities:
+ const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr;
+ const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr;
+
+ // 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);
+ }
+ }
+
+ void DiffDriveController::starting(const ros::Time& time)
+ {
+ brake();
+
+ // Register starting time used to keep fixed rate
+ last_state_publish_time_ = 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())
+ {
+ 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 descripion couldn't be retrieved from param server.");
+ return false;
+ }
+
+ boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
+
+ boost::shared_ptr<const urdf::Joint> left_wheel_joint(model->getJoint(left_wheel_name));
+ boost::shared_ptr<const urdf::Joint> right_wheel_joint(model->getJoint(right_wheel_name));
+
+ if (lookup_wheel_separation)
+ {
+ // Get wheel separation
+ 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;
+ }
+
+ 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";
+ odom_pub_->msg_.child_frame_id = base_frame_id_;
+ odom_pub_->msg_.pose.pose.position.z = 0;
+ odom_pub_->msg_.pose.covariance = boost::assign::list_of
+ (static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
+ (0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
+ (0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
+ (0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
+ (0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
+ (0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
+ odom_pub_->msg_.twist.twist.linear.y = 0;
+ odom_pub_->msg_.twist.twist.linear.z = 0;
+ odom_pub_->msg_.twist.twist.angular.x = 0;
+ odom_pub_->msg_.twist.twist.angular.y = 0;
+ odom_pub_->msg_.twist.covariance = boost::assign::list_of
+ (static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
+ (0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
+ (0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
+ (0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
+ (0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
+ (0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
+ tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
+ tf_odom_pub_->msg_.transforms.resize(1);
+ tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
+ tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
+ tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom";
+ }
+
+} // namespace diff_drive_controller
diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp
new file mode 100644
index 0000000..003572d
--- /dev/null
+++ b/diff_drive_controller/src/odometry.cpp
@@ -0,0 +1,174 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the PAL Robotics nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+
+#include <diff_drive_controller/odometry.h>
+
+#include <boost/bind.hpp>
+
+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)
+ , 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_(boost::bind(&Odometry::integrateExact, this, _1, _2))
+ {
+ }
+
+ void Odometry::init(const ros::Time& time)
+ {
+ // Reset accumulators and timestamp:
+ resetAccumulators();
+ timestamp_ = time;
+ }
+
+ bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
+ {
+ /// Get current wheel joint positions:
+ const double left_wheel_cur_pos = left_pos * wheel_radius_;
+ const double right_wheel_cur_pos = right_pos * wheel_radius_;
+
+ /// Estimate velocity of wheels using old and current position:
+ const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
+ const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
+
+ /// 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 wheel_radius)
+ {
+ wheel_separation_ = wheel_separation;
+ wheel_radius_ = wheel_radius;
+ }
+
+ void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
+ {
+ velocity_rolling_window_size_ = velocity_rolling_window_size;
+
+ resetAccumulators();
+ }
+
+ void Odometry::integrateRungeKutta2(double linear, double angular)
+ {
+ const double direction = heading_ + angular * 0.5;
+
+ /// Runge-Kutta 2nd order integration:
+ x_ += linear * cos(direction);
+ y_ += linear * sin(direction);
+ heading_ += angular;
+ }
+
+ /**
+ * \brief Other possible integration method provided by the class
+ * \param linear
+ * \param angular
+ */
+ void Odometry::integrateExact(double linear, double angular)
+ {
+ if (fabs(angular) < 1e-6)
+ integrateRungeKutta2(linear, angular);
+ else
+ {
+ /// Exact integration (should solve problems when angular is zero):
+ const double heading_old = heading_;
+ const double r = linear/angular;
+ heading_ += angular;
+ x_ += r * (sin(heading_) - sin(heading_old));
+ y_ += -r * (cos(heading_) - cos(heading_old));
+ }
+ }
+
+ void Odometry::resetAccumulators()
+ {
+ linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+ angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+ }
+
+} // namespace diff_drive_controller
diff --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp
new file mode 100644
index 0000000..630ec82
--- /dev/null
+++ b/diff_drive_controller/src/speed_limiter.cpp
@@ -0,0 +1,137 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the PAL Robotics nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#include <algorithm>
+
+#include <diff_drive_controller/speed_limiter.h>
+
+template<typename T>
+T clamp(T x, T min, T max)
+{
+ return std::min(std::max(min, x), max);
+}
+
+namespace diff_drive_controller
+{
+
+ SpeedLimiter::SpeedLimiter(
+ bool has_velocity_limits,
+ bool has_acceleration_limits,
+ bool has_jerk_limits,
+ double min_velocity,
+ double max_velocity,
+ double min_acceleration,
+ double max_acceleration,
+ double min_jerk,
+ double max_jerk
+ )
+ : has_velocity_limits(has_velocity_limits)
+ , has_acceleration_limits(has_acceleration_limits)
+ , has_jerk_limits(has_jerk_limits)
+ , min_velocity(min_velocity)
+ , max_velocity(max_velocity)
+ , min_acceleration(min_acceleration)
+ , max_acceleration(max_acceleration)
+ , min_jerk(min_jerk)
+ , max_jerk(max_jerk)
+ {
+ }
+
+ double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
+ {
+ const double tmp = v;
+
+ limit_jerk(v, v0, v1, dt);
+ limit_acceleration(v, v0, dt);
+ limit_velocity(v);
+
+ return tmp != 0.0 ? v / tmp : 1.0;
+ }
+
+ double SpeedLimiter::limit_velocity(double& v)
+ {
+ const double tmp = v;
+
+ if (has_velocity_limits)
+ {
+ v = clamp(v, min_velocity, max_velocity);
+ }
+
+ return tmp != 0.0 ? v / tmp : 1.0;
+ }
+
+ double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
+ {
+ const double tmp = v;
+
+ if (has_acceleration_limits)
+ {
+ const double dv_min = min_acceleration * dt;
+ const double dv_max = max_acceleration * dt;
+
+ const double dv = clamp(v - v0, dv_min, dv_max);
+
+ v = v0 + dv;
+ }
+
+ return tmp != 0.0 ? v / tmp : 1.0;
+ }
+
+ double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
+ {
+ const double tmp = v;
+
+ if (has_jerk_limits)
+ {
+ const double dv = v - v0;
+ const double dv0 = v0 - v1;
+
+ const double dt2 = 2. * dt * dt;
+
+ const double da_min = min_jerk * dt2;
+ const double da_max = max_jerk * dt2;
+
+ const double da = clamp(dv - dv0, da_min, da_max);
+
+ v = v0 + dv0 + da;
+ }
+
+ return tmp != 0.0 ? v / tmp : 1.0;
+ }
+
+} // namespace diff_drive_controller
diff --git a/diff_drive_controller/test/diff_drive_bad_urdf.test b/diff_drive_controller/test/diff_drive_bad_urdf.test
new file mode 100644
index 0000000..2932c6f
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_bad_urdf.test
@@ -0,0 +1,17 @@
+<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.py '$(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>
diff --git a/diff_drive_controller/test/diff_drive_common.launch b/diff_drive_controller/test/diff_drive_common.launch
new file mode 100644
index 0000000..b63aaa8
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_common.launch
@@ -0,0 +1,31 @@
+<launch>
+ <!-- Load diffbot model -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro.py '$(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>
diff --git a/diff_drive_controller/test/diff_drive_controller.test b/diff_drive_controller/test/diff_drive_controller.test
new file mode 100644
index 0000000..ceaaa46
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_controller.test
@@ -0,0 +1,13 @@
+<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>
diff --git a/diff_drive_controller/test/diff_drive_controller_limits.test b/diff_drive_controller/test/diff_drive_controller_limits.test
new file mode 100644
index 0000000..42d6bcb
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_controller_limits.test
@@ -0,0 +1,16 @@
+<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>
diff --git a/diff_drive_controller/test/diff_drive_controller_nan.test b/diff_drive_controller/test/diff_drive_controller_nan.test
new file mode 100644
index 0000000..892e0bd
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_controller_nan.test
@@ -0,0 +1,13 @@
+<launch>
+ <!-- Load common test stuff -->
+ <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
+
+ <!-- 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" />
+ </test>
+</launch>
diff --git a/diff_drive_controller/test/diff_drive_fail_test.cpp b/diff_drive_controller/test/diff_drive_fail_test.cpp
new file mode 100644
index 0000000..bb2a12a
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_fail_test.cpp
@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Paul Mathieu
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testWrongJointName)
+{
+ // the controller should never be alive
+ int secs = 0;
+ while(!isControllerAlive() && secs < 5)
+ {
+ ros::Duration(1.0).sleep();
+ secs++;
+ }
+ // give up and assume controller load failure after 5 seconds
+ EXPECT_GE(secs, 5);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "diff_drive_fail_test");
+
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ int ret = RUN_ALL_TESTS();
+ spinner.stop();
+ ros::shutdown();
+ return ret;
+}
diff --git a/diff_drive_controller/test/diff_drive_limits_test.cpp b/diff_drive_controller/test/diff_drive_limits_test.cpp
new file mode 100644
index 0000000..6f69b8f
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_limits_test.cpp
@@ -0,0 +1,231 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Paul Mathieu
+
+#include "test_common.h"
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testLinearJerkLimits)
+{
+ // wait for ROS
+ while(!isControllerAlive())
+ {
+ ros::Duration(0.1).sleep();
+ }
+ // zero everything before test
+ geometry_msgs::Twist cmd_vel;
+ cmd_vel.linear.x = 0.0;
+ cmd_vel.angular.z = 0.0;
+ publish(cmd_vel);
+ ros::Duration(2.0).sleep();
+ // get initial odom
+ nav_msgs::Odometry old_odom = getLastOdom();
+ // send a big command
+ cmd_vel.linear.x = 10.0;
+ publish(cmd_vel);
+ // wait for a while
+ ros::Duration(0.5).sleep();
+
+ nav_msgs::Odometry new_odom = getLastOdom();
+
+ // check if the robot speed is now 0.37m.s-1
+ EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE);
+ EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
+
+ cmd_vel.linear.x = 0.0;
+ publish(cmd_vel);
+}
+
+TEST_F(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);
+}
+
+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;
+}
diff --git a/diff_drive_controller/test/diff_drive_multipliers.test b/diff_drive_controller/test/diff_drive_multipliers.test
new file mode 100644
index 0000000..560330e
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_multipliers.test
@@ -0,0 +1,16 @@
+<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>
diff --git a/diff_drive_controller/test/diff_drive_nan_test.cpp b/diff_drive_controller/test/diff_drive_nan_test.cpp
new file mode 100644
index 0000000..2f06e7e
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_nan_test.cpp
@@ -0,0 +1,92 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2014, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Enrique Fernandez
+
+#include "test_common.h"
+
+// NaN
+#include <limits>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testNaN)
+{
+ // wait for ROS
+ while(!isControllerAlive())
+ {
+ ros::Duration(0.1).sleep();
+ }
+ // zero everything before test
+ geometry_msgs::Twist cmd_vel;
+ cmd_vel.linear.x = 0.0;
+ cmd_vel.angular.z = 0.0;
+ publish(cmd_vel);
+ ros::Duration(2.0).sleep();
+
+ // send a command
+ cmd_vel.linear.x = 0.1;
+ ros::Duration(2.0).sleep();
+
+ // stop robot (will generate NaN)
+ stop();
+ ros::Duration(2.0).sleep();
+
+ nav_msgs::Odometry odom = getLastOdom();
+
+ EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
+ EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
+
+ // start robot
+ start();
+ ros::Duration(2.0).sleep();
+
+ odom = getLastOdom();
+
+ EXPECT_NE(std::isnan(odom.twist.twist.linear.x), true);
+ EXPECT_NE(std::isnan(odom.twist.twist.angular.z), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.position.x), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.position.y), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.orientation.z), true);
+ EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "diff_drive_nan_test");
+
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ int ret = RUN_ALL_TESTS();
+ spinner.stop();
+ ros::shutdown();
+ return ret;
+}
diff --git a/diff_drive_controller/test/diff_drive_odom_tf.test b/diff_drive_controller/test/diff_drive_odom_tf.test
new file mode 100644
index 0000000..e733666
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_odom_tf.test
@@ -0,0 +1,19 @@
+<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>
diff --git a/diff_drive_controller/test/diff_drive_odom_tf_test.cpp b/diff_drive_controller/test/diff_drive_odom_tf_test.cpp
new file mode 100644
index 0000000..41aae1b
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_odom_tf_test.cpp
@@ -0,0 +1,59 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+ while(!isControllerAlive())
+ {
+ ros::Duration(0.1).sleep();
+ }
+ // 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;
+}
diff --git a/diff_drive_controller/test/diff_drive_open_loop.test b/diff_drive_controller/test/diff_drive_open_loop.test
new file mode 100644
index 0000000..31bf115
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_open_loop.test
@@ -0,0 +1,16 @@
+<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>
diff --git a/diff_drive_controller/test/diff_drive_radius_param.test b/diff_drive_controller/test/diff_drive_radius_param.test
new file mode 100644
index 0000000..cfd6a6e
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_radius_param.test
@@ -0,0 +1,19 @@
+<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.py '$(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"/>
+
+ <!-- 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>
diff --git a/diff_drive_controller/test/diff_drive_radius_param_fail.test b/diff_drive_controller/test/diff_drive_radius_param_fail.test
new file mode 100644
index 0000000..8f99587
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_radius_param_fail.test
@@ -0,0 +1,19 @@
+<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.py '$(find diff_drive_controller)/test/diffbot_sphere_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>
diff --git a/diff_drive_controller/test/diff_drive_separation_param.test b/diff_drive_controller/test/diff_drive_separation_param.test
new file mode 100644
index 0000000..e39af27
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_separation_param.test
@@ -0,0 +1,21 @@
+<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.py '$(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>
diff --git a/diff_drive_controller/test/diff_drive_test.cpp b/diff_drive_controller/test/diff_drive_test.cpp
new file mode 100644
index 0000000..884df1e
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_test.cpp
@@ -0,0 +1,153 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+ 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();
+ // 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();
+
+ // 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), 1.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);
+ 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
+ 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();
+ // 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);
+
+ // 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), M_PI, 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), M_PI/10.0, EPS);
+}
+
+TEST_F(DiffDriveControllerTest, testOdomFrame)
+{
+ // wait for ROS
+ while(!isControllerAlive())
+ {
+ ros::Duration(0.1).sleep();
+ }
+ // set up tf listener
+ tf::TransformListener listener;
+ ros::Duration(2.0).sleep();
+ // check the 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;
+}
diff --git a/diff_drive_controller/test/diff_drive_timeout.test b/diff_drive_controller/test/diff_drive_timeout.test
new file mode 100644
index 0000000..4563132
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_timeout.test
@@ -0,0 +1,16 @@
+<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>
diff --git a/diff_drive_controller/test/diff_drive_timeout_test.cpp b/diff_drive_controller/test/diff_drive_timeout_test.cpp
new file mode 100644
index 0000000..41a12bd
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_timeout_test.cpp
@@ -0,0 +1,72 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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
+ 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);
+ // 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;
+}
diff --git a/diff_drive_controller/test/diff_drive_wrong.test b/diff_drive_controller/test/diff_drive_wrong.test
new file mode 100644
index 0000000..bb4164f
--- /dev/null
+++ b/diff_drive_controller/test/diff_drive_wrong.test
@@ -0,0 +1,16 @@
+<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>
diff --git a/diff_drive_controller/test/diffbot.cpp b/diff_drive_controller/test/diffbot.cpp
new file mode 100644
index 0000000..60ae825
--- /dev/null
+++ b/diff_drive_controller/test/diffbot.cpp
@@ -0,0 +1,60 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
+
+// ROS
+#include <ros/ros.h>
+
+// ros_control
+#include <controller_manager/controller_manager.h>
+
+#include "diffbot.h"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "diffbot");
+ ros::NodeHandle nh;
+
+ Diffbot<> robot;
+ ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
+ controller_manager::ControllerManager cm(&robot, nh);
+
+ ros::Rate rate(1.0 / robot.getPeriod().toSec());
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ while(ros::ok())
+ {
+ robot.read();
+ cm.update(robot.getTime(), robot.getPeriod());
+ robot.write();
+ rate.sleep();
+ }
+ spinner.stop();
+
+ return 0;
+}
diff --git a/diff_drive_controller/test/diffbot.h b/diff_drive_controller/test/diffbot.h
new file mode 100644
index 0000000..c64d11c
--- /dev/null
+++ b/diff_drive_controller/test/diffbot.h
@@ -0,0 +1,139 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
+
+// ROS
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_srvs/Empty.h>
+
+// ros_control
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// NaN
+#include <limits>
+
+// ostringstream
+#include <sstream>
+
+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()
+ {
+ 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());
+ }
+
+ void write()
+ {
+ 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());
+ }
+ }
+
+ 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_;
+};
diff --git a/diff_drive_controller/test/diffbot.xacro b/diff_drive_controller/test/diffbot.xacro
new file mode 100644
index 0000000..cb33890
--- /dev/null
+++ b/diff_drive_controller/test/diffbot.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<!--
+Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+-->
+ <xacro:include filename="$(find diff_drive_controller)/test/wheel.xacro"/>
+
+ <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+ <!-- Constants for robot dimensions -->
+ <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+ <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+ <xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
+ <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+ <!-- Links: inertial,visual,collision -->
+ <link name="base_link">
+ <inertial>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="5"/>
+ <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </collision>
+ </link>
+
+
+ <link name="base_footprint">
+ <visual>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.00000001"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="base_footprint_joint" type="fixed">
+ <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+ <child link="base_link"/>
+ <parent link="base_footprint"/>
+ </joint>
+
+
+ <!-- Wheels -->
+ <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"/>
+ </wheel>
+ <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"/>
+ </wheel>
+
+
+ <!-- Colour -->
+ <gazebo reference="base_link">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+ <gazebo reference="base_footprint">
+ <material>Gazebo/Purple</material>
+ </gazebo>
+
+</robot>
diff --git a/diff_drive_controller/test/diffbot_bad.xacro b/diff_drive_controller/test/diffbot_bad.xacro
new file mode 100644
index 0000000..ec88c38
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_bad.xacro
@@ -0,0 +1,142 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<!--
+Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+-->
+ <!-- Constants for robot dimensions -->
+ <xacro:property name="PI" value="3.1415926535897931"/>
+ <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+ <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+ <xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
+ <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+ <!-- Links: inertial,visual,collision -->
+ <link name="base_link">
+ <inertial>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="5"/>
+ <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </collision>
+ </link>
+
+
+ <link name="base_footprint">
+ <visual>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.00000001"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="base_footprint_joint" type="fixed">
+ <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+ <child link="base_link"/>
+ <parent link="base_footprint"/>
+ </joint>
+
+
+ <link name="wheel1">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="1"/>
+ <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
+ <geometry>
+ <sphere radius="${wheel_radius}"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
+ <geometry>
+ <sphere radius="${wheel_radius}"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="joint_w1" type="continuous">
+ <parent link="base_link"/>
+ <child link="wheel1"/>
+ <origin xyz="${width/2+axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
+ <axis xyz="0 1 0"/>
+ </joint>
+
+
+ <link name="wheel2">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="1"/>
+ <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
+ <geometry>
+ <sphere radius="${wheel_radius}"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
+ <geometry>
+ <sphere radius="${wheel_radius}"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="joint_w2" type="continuous">
+ <parent link="base_link"/>
+ <child link="wheel2"/>
+ <origin xyz="${-width/2-axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
+ <axis xyz="0 1 0"/>
+ </joint>
+
+
+ <!-- Transmission is important to link the joints and the controller -->
+ <transmission name="joint_w1_trans" type="SimpleTransmission">
+ <actuator name="joint_w1_motor" />
+ <joint name="joint_w1" />
+ <mechanicalReduction>1</mechanicalReduction>
+ <motorTorqueConstant>1</motorTorqueConstant>
+ </transmission>
+
+ <transmission name="joint_w2_trans" type="SimpleTransmission">
+ <actuator name="joint_w2_motor" />
+ <joint name="joint_w2" />
+ <mechanicalReduction>1</mechanicalReduction>
+ <motorTorqueConstant>1</motorTorqueConstant>
+ </transmission>
+
+ <!-- Colour -->
+ <gazebo reference="base_link">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+ <gazebo reference="wheel1">
+ <material>Gazebo/Red</material>
+ </gazebo>
+
+ <gazebo reference="wheel2">
+ <material>Gazebo/Blue</material>
+ </gazebo>
+
+ <gazebo reference="base_footprint">
+ <material>Gazebo/Purple</material>
+ </gazebo>
+
+</robot>
diff --git a/diff_drive_controller/test/diffbot_controllers.yaml b/diff_drive_controller/test/diffbot_controllers.yaml
new file mode 100644
index 0000000..f89c38c
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_controllers.yaml
@@ -0,0 +1,8 @@
+diffbot_controller:
+ type: "diff_drive_controller/DiffDriveController"
+ left_wheel: 'wheel_0_joint'
+ right_wheel: 'wheel_1_joint'
+ publish_rate: 50.0 # defaults to 50
+ pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
diff --git a/diff_drive_controller/test/diffbot_limits.yaml b/diff_drive_controller/test/diffbot_limits.yaml
new file mode 100644
index 0000000..78f0bf9
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_limits.yaml
@@ -0,0 +1,19 @@
+diffbot_controller:
+ linear:
+ x:
+ has_velocity_limits: true
+ min_velocity: -0.5
+ max_velocity: 1.0
+ has_acceleration_limits: true
+ min_acceleration: -0.5
+ max_acceleration: 1.0
+ has_jerk_limits: true
+ max_jerk: 5.0
+ angular:
+ z:
+ has_velocity_limits: true
+ max_velocity: 2.0
+ has_acceleration_limits: true
+ max_acceleration: 2.0
+ has_jerk_limits: true
+ max_jerk: 10.0
diff --git a/diff_drive_controller/test/diffbot_multipliers.yaml b/diff_drive_controller/test/diffbot_multipliers.yaml
new file mode 100644
index 0000000..d2ef5d5
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_multipliers.yaml
@@ -0,0 +1,3 @@
+diffbot_controller:
+ wheel_separation_multiplier: 2.3
+ wheel_radius_multiplier: 1.4
diff --git a/diff_drive_controller/test/diffbot_open_loop.yaml b/diff_drive_controller/test/diffbot_open_loop.yaml
new file mode 100644
index 0000000..19224f3
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_open_loop.yaml
@@ -0,0 +1,2 @@
+diffbot_controller:
+ open_loop: true
diff --git a/diff_drive_controller/test/diffbot_sphere_wheels.xacro b/diff_drive_controller/test/diffbot_sphere_wheels.xacro
new file mode 100644
index 0000000..57b719a
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_sphere_wheels.xacro
@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<!--
+Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+-->
+ <xacro:include filename="$(find diff_drive_controller)/test/sphere_wheel.xacro"/>
+ <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+ <!-- Constants for robot dimensions -->
+ <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+ <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+ <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+ <!-- Links: inertial,visual,collision -->
+ <link name="base_link">
+ <inertial>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="5"/>
+ <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </collision>
+ </link>
+
+
+ <link name="base_footprint">
+ <visual>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.00000001"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="base_footprint_joint" type="fixed">
+ <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+ <child link="base_link"/>
+ <parent link="base_footprint"/>
+ </joint>
+
+
+ <!-- Wheels -->
+ <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"/>
+ </sphere_wheel>
+ <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"/>
+ </sphere_wheel>
+
+
+ <!-- Colour -->
+ <gazebo reference="base_link">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+ <gazebo reference="base_footprint">
+ <material>Gazebo/Purple</material>
+ </gazebo>
+
+</robot>
diff --git a/diff_drive_controller/test/diffbot_timeout.yaml b/diff_drive_controller/test/diffbot_timeout.yaml
new file mode 100644
index 0000000..4ecfa18
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_timeout.yaml
@@ -0,0 +1,2 @@
+diffbot_controller:
+ cmd_vel_timeout: 0.5
diff --git a/diff_drive_controller/test/diffbot_wrong.yaml b/diff_drive_controller/test/diffbot_wrong.yaml
new file mode 100644
index 0000000..516fd75
--- /dev/null
+++ b/diff_drive_controller/test/diffbot_wrong.yaml
@@ -0,0 +1,8 @@
+diffbot_controller:
+ type: "diff_drive_controller/DiffDriveController"
+ left_wheel: 'this_joint_does_not_exist'
+ right_wheel: 'right_wheel_joint'
+ publish_rate: 50.0 # defaults to 50
+ pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
diff --git a/diff_drive_controller/test/skid_steer_common.launch b/diff_drive_controller/test/skid_steer_common.launch
new file mode 100644
index 0000000..fe7e948
--- /dev/null
+++ b/diff_drive_controller/test/skid_steer_common.launch
@@ -0,0 +1,31 @@
+<launch>
+ <!-- Load skidsteerbot model -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro.py '$(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>
diff --git a/diff_drive_controller/test/skid_steer_controller.test b/diff_drive_controller/test/skid_steer_controller.test
new file mode 100644
index 0000000..b89aab1
--- /dev/null
+++ b/diff_drive_controller/test/skid_steer_controller.test
@@ -0,0 +1,13 @@
+<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>
diff --git a/diff_drive_controller/test/skid_steer_no_wheels.test b/diff_drive_controller/test/skid_steer_no_wheels.test
new file mode 100644
index 0000000..7100960
--- /dev/null
+++ b/diff_drive_controller/test/skid_steer_no_wheels.test
@@ -0,0 +1,16 @@
+<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>
diff --git a/diff_drive_controller/test/skidsteerbot.cpp b/diff_drive_controller/test/skidsteerbot.cpp
new file mode 100644
index 0000000..37ca8c3
--- /dev/null
+++ b/diff_drive_controller/test/skidsteerbot.cpp
@@ -0,0 +1,60 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
+
+// ROS
+#include <ros/ros.h>
+
+// ros_control
+#include <controller_manager/controller_manager.h>
+
+#include "diffbot.h"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "skidsteerbot");
+ ros::NodeHandle nh;
+
+ Diffbot<6> robot;
+ ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
+ controller_manager::ControllerManager cm(&robot, nh);
+
+ ros::Rate rate(1.0 / robot.getPeriod().toSec());
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ while(ros::ok())
+ {
+ robot.read();
+ cm.update(robot.getTime(), robot.getPeriod());
+ robot.write();
+ rate.sleep();
+ }
+ spinner.stop();
+
+ return 0;
+}
diff --git a/diff_drive_controller/test/skidsteerbot.xacro b/diff_drive_controller/test/skidsteerbot.xacro
new file mode 100644
index 0000000..5365499
--- /dev/null
+++ b/diff_drive_controller/test/skidsteerbot.xacro
@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<!--
+Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
+-->
+ <xacro:include filename="$(find diff_drive_controller)/test/wheel.xacro"/>
+
+ <xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
+
+ <!-- Constants for robot dimensions -->
+ <xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
+ <xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
+ <xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
+ <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+ <xacro:property name="y_offset" value="0.35" /> <!-- Offset for the wheels on the same side -->
+
+ <!-- Links: inertial,visual,collision -->
+ <link name="base_link">
+ <inertial>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <mass value="5"/>
+ <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </visual>
+ <collision>
+ <!-- origin is relative -->
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <box size="${width} 0.1 0.1"/>
+ </geometry>
+ </collision>
+ </link>
+
+
+ <link name="base_footprint">
+ <visual>
+ <geometry>
+ <sphere radius="0.01"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.00000001"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="base_footprint_joint" type="fixed">
+ <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
+ <child link="base_link"/>
+ <parent link="base_footprint"/>
+ </joint>
+
+
+ <!-- Wheels -->
+ <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"/>
+ </wheel>
+ <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"/>
+ </wheel>
+ <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"/>
+ </wheel>
+ <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"/>
+ </wheel>
+ <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"/>
+ </wheel>
+ <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"/>
+ </wheel>
+
+
+ <!-- Colour -->
+ <gazebo reference="base_link">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+ <gazebo reference="base_footprint">
+ <material>Gazebo/Purple</material>
+ </gazebo>
+
+</robot>
diff --git a/diff_drive_controller/test/skidsteerbot_controllers.yaml b/diff_drive_controller/test/skidsteerbot_controllers.yaml
new file mode 100644
index 0000000..ab49424
--- /dev/null
+++ b/diff_drive_controller/test/skidsteerbot_controllers.yaml
@@ -0,0 +1,8 @@
+skidsteerbot_controller:
+ type: "diff_drive_controller/DiffDriveController"
+ left_wheel: ['wheel_0_joint', 'wheel_1_joint', 'wheel_2_joint']
+ right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint']
+ publish_rate: 50.0 # defaults to 50
+ pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
diff --git a/diff_drive_controller/test/skidsteerbot_no_wheels.yaml b/diff_drive_controller/test/skidsteerbot_no_wheels.yaml
new file mode 100644
index 0000000..37f0d99
--- /dev/null
+++ b/diff_drive_controller/test/skidsteerbot_no_wheels.yaml
@@ -0,0 +1,8 @@
+skidsteerbot_controller:
+ type: "diff_drive_controller/DiffDriveController"
+ left_wheel: []
+ right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint']
+ publish_rate: 50.0 # defaults to 50
+ pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
diff --git a/diff_drive_controller/test/sphere_wheel.xacro b/diff_drive_controller/test/sphere_wheel.xacro
new file mode 100644
index 0000000..0694a30
--- /dev/null
+++ b/diff_drive_controller/test/sphere_wheel.xacro
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/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"/>
+ <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>
+ </macro>
+</robot>
diff --git a/diff_drive_controller/test/test_common.h b/diff_drive_controller/test/test_common.h
new file mode 100644
index 0000000..32bc266
--- /dev/null
+++ b/diff_drive_controller/test/test_common.h
@@ -0,0 +1,97 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Bence Magyar
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/tf.h>
+
+#include <std_srvs/Empty.h>
+
+// Floating-point value comparison threshold
+const double EPS = 0.01;
+const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision
+const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
+const double 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()
+ : cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
+ , odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this))
+ , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
+ , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
+ {
+ }
+
+ ~DiffDriveControllerTest()
+ {
+ odom_sub.shutdown();
+ }
+
+ nav_msgs::Odometry getLastOdom(){ return last_odom; }
+ void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
+ bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); }
+
+ void start(){ std_srvs::Empty srv; start_srv.call(srv); }
+ void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
+
+private:
+ ros::NodeHandle nh;
+ ros::Publisher cmd_pub;
+ ros::Subscriber odom_sub;
+ nav_msgs::Odometry last_odom;
+
+ ros::ServiceClient start_srv;
+ ros::ServiceClient stop_srv;
+
+ void odomCallback(const nav_msgs::Odometry& odom)
+ {
+ ROS_INFO_STREAM("Callback reveived: pos.x: " << odom.pose.pose.position.x
+ << ", orient.z: " << odom.pose.pose.orientation.z
+ << ", lin_est: " << odom.twist.twist.linear.x
+ << ", ang_est: " << odom.twist.twist.angular.z);
+ last_odom = odom;
+ }
+};
+
+inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
+{
+ return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
+}
+
diff --git a/diff_drive_controller/test/view_diffbot.launch b/diff_drive_controller/test/view_diffbot.launch
new file mode 100644
index 0000000..cdc362e
--- /dev/null
+++ b/diff_drive_controller/test/view_diffbot.launch
@@ -0,0 +1,26 @@
+<launch>
+ <!-- start world -->
+ <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
+ <!-- load robot -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro.py '$(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>
+
diff --git a/diff_drive_controller/test/view_skidsteerbot.launch b/diff_drive_controller/test/view_skidsteerbot.launch
new file mode 100644
index 0000000..66d8d5c
--- /dev/null
+++ b/diff_drive_controller/test/view_skidsteerbot.launch
@@ -0,0 +1,26 @@
+<launch>
+ <!-- start world -->
+ <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
+ <!-- load robot -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro.py '$(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>
+
diff --git a/diff_drive_controller/test/wheel.xacro b/diff_drive_controller/test/wheel.xacro
new file mode 100644
index 0000000..6fa1bd6
--- /dev/null
+++ b/diff_drive_controller/test/wheel.xacro
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/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"/>
+ <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>
+ </macro>
+</robot>
diff --git a/effort_controllers/.gitignore b/effort_controllers/.gitignore
new file mode 100644
index 0000000..61fd441
--- /dev/null
+++ b/effort_controllers/.gitignore
@@ -0,0 +1,2 @@
+build
+lib
\ No newline at end of file
diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst
new file mode 100644
index 0000000..c5fb7d1
--- /dev/null
+++ b/effort_controllers/CHANGELOG.rst
@@ -0,0 +1,115 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package effort_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+
+0.9.2 (2015-05-04)
+------------------
+* Thread-safe and realtime-safe forward controllers.
+* Contributors: Mathias Lüdtke
+
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Propagate changes made in forward_command_controller
+* Contributors: ipa-fxm
+
+0.8.1 (2014-07-11)
+------------------
+* Add depend on angles
+* Contributors: Scott K Logan
+
+0.8.0 (2014-05-12)
+------------------
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+* Added new has_velocity flag that indiciates if a target velocity has been set
+* Contributors: Dave Coleman
+
+0.5.4 (2013-09-30)
+------------------
+
+0.5.3 (2013-09-04)
+------------------
+* Removed manifest.xml from all packages to prevent rosdep heirarchy issues in Groovy and Hydro
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+* Minor comment fix
+* Critical bug: velocity controller init() does not get hardware_interface handle for joint
+* Fixes for joint_position_controller
+* Consolidated position and velocity command into one realtime buffer
+* Tweaked header guard
+* Added ability to set target velocity, CMake cleanup
+* Removed debug output from realtime context
+* Removed blocking msgs from realtime loop
+* Added joint limit enforcement for controller set point command
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+* Merged
+* Removed controller_msgs
+* Fixed PID destructor bug, cleaned up code
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+* Restore "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors""
+ This reverts commit 0862ad93696b0d736b565cd65ea36690dde0eaa7.
+* Fixing reversed error computation...
+* Adding install targets for plugin xml files
+* Revert "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors"
+ This reverts commit 2314b8b434e35dc9c1c298140118a004e00febd8.
+
+0.4.0 (2013-06-26)
+------------------
+* Version 0.4.0
+* Fixing position effort controller pid command args
+* Fixed control_toolbox deprecated errors with updatePid()
+* Fixed PLUGINLIB_DECLARE_CLASS depreacated errors
+* Propagate API changes in hardware_interface.
+* adding install targets
+* adding switches for hybrid buildsystem
+* adding back more manifests and makefiles
+* Trivial log message fix.
+* Fixing library export
+* adding these packages which weren't seen by catkinize_stack
+* bumping version
+* adding package.xml files
+* Catkinizing. Building, but could still be cleaned up
+* Extend joint_effort_controller to other interfaces
+ - Factor-out implementation of simple command-forwarding controller.
+ - Provide specializations (typedefs really) for effort, velocity and position
+ interfaces.
+* Fix documentation typo.
+* Add .gitignore files on a per-package basis.
+* effort_controllers::joint_velocity_controller was not being built
+* Fixing typos in JointVelocityController
+* port to new api with time and duration
+* fix xml filename
+* register controllers
+* fixes
+* add position controller
+* port another controller
+* clean up dependencies
+* first simple controller for testing
diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt
new file mode 100644
index 0000000..ec8e694
--- /dev/null
+++ b/effort_controllers/CMakeLists.txt
@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(effort_controllers)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS
+ controller_interface
+ control_msgs
+ forward_command_controller
+ control_toolbox
+ realtime_tools
+ urdf
+)
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS
+ controller_interface
+ control_msgs
+ control_toolbox
+ realtime_tools
+ urdf
+ forward_command_controller
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ )
+
+add_library(${PROJECT_NAME}
+ src/joint_effort_controller.cpp
+ src/joint_velocity_controller.cpp
+ src/joint_position_controller.cpp
+ src/joint_group_effort_controller.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES effort_controllers_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/effort_controllers/effort_controllers_plugins.xml b/effort_controllers/effort_controllers_plugins.xml
new file mode 100644
index 0000000..fca7d1f
--- /dev/null
+++ b/effort_controllers/effort_controllers_plugins.xml
@@ -0,0 +1,27 @@
+<library path="lib/libeffort_controllers">
+
+ <class name="effort_controllers/JointPositionController" type="effort_controllers::JointPositionController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointPositionController tracks position commands. It expects a EffortJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="effort_controllers/JointVelocityController" type="effort_controllers::JointVelocityController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointVelocityController tracks velocity commands. It expects a EffortJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="effort_controllers/JointEffortController" type="effort_controllers::JointEffortController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointEffortController tracks effort commands. It expects a EffortJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="effort_controllers/JointGroupEffortController" type="effort_controllers::JointGroupEffortController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointGroupEffortController tracks effort commands for a set of joints. It expects a EffortJointInterface type of hardware interface.
+ </description>
+ </class>
+
+</library>
diff --git a/effort_controllers/include/effort_controllers/joint_effort_controller.h b/effort_controllers/include/effort_controllers/joint_effort_controller.h
new file mode 100644
index 0000000..097d7ea
--- /dev/null
+++ b/effort_controllers/include/effort_controllers/joint_effort_controller.h
@@ -0,0 +1,62 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef EFFORT_CONTROLLERS__JOINT_EFFORT_CONTROLLER_H
+#define EFFORT_CONTROLLERS__JOINT_EFFORT_CONTROLLER_H
+
+#include <forward_command_controller/forward_command_controller.h>
+
+namespace effort_controllers
+{
+
+/**
+ * \brief Joint Effort Controller (torque or force)
+ *
+ * This class passes the commanded effort down to the joint.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointEffortController".
+ * \param joint Name of the joint to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64) : The joint effort to apply.
+ */
+typedef forward_command_controller::ForwardCommandController<hardware_interface::EffortJointInterface>
+ JointEffortController;
+}
+
+#endif
diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.h b/effort_controllers/include/effort_controllers/joint_group_effort_controller.h
new file mode 100644
index 0000000..36b93ba
--- /dev/null
+++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.h
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef EFFORT_CONTROLLERS_JOINT_GROUP_EFFORT_CONTROLLER_H
+#define EFFORT_CONTROLLERS_JOINT_GROUP_EFFORT_CONTROLLER_H
+
+#include <forward_command_controller/forward_joint_group_command_controller.h>
+
+namespace effort_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of effort controlled joints (torque or force).
+ *
+ * This class forwards the commanded efforts down to a set of joints.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointGroupEffortController".
+ * \param joints List of names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint efforts to apply
+ */
+typedef forward_command_controller::ForwardJointGroupCommandController<hardware_interface::EffortJointInterface>
+ JointGroupEffortController;
+}
+
+#endif
diff --git a/effort_controllers/include/effort_controllers/joint_position_controller.h b/effort_controllers/include/effort_controllers/joint_position_controller.h
new file mode 100644
index 0000000..6246ccb
--- /dev/null
+++ b/effort_controllers/include/effort_controllers/joint_position_controller.h
@@ -0,0 +1,195 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
+#define EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
+
+/**
+ @class effort_controllers::JointPositionController
+ @brief Joint Position Controller
+
+ This class controls positon using a pid loop.
+
+ @section ROS ROS interface
+
+ @param type Must be "effort_controllers::JointPositionController"
+ @param joint Name of the joint to control.
+ @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid
+
+ Subscribes to:
+
+ - @b command (std_msgs::Float64) : The joint position to achieve.
+
+ Publishes:
+
+ - @b state (control_msgs::JointControllerState) :
+ Current state of the controller, including pid error and gains.
+
+*/
+
+#include <ros/node_handle.h>
+#include <urdf/model.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_msgs/JointControllerState.h>
+#include <realtime_tools/realtime_buffer.h>
+
+namespace effort_controllers
+{
+
+class JointPositionController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+public:
+
+ /**
+ * \brief Store position and velocity command in struct to allow easier realtime buffer usage
+ */
+ struct Commands
+ {
+ double position_; // Last commanded position
+ double velocity_; // Last commanded velocity
+ bool has_velocity_; // false if no velocity command has been specified
+ };
+
+ JointPositionController();
+ ~JointPositionController();
+
+ /** \brief The init function is called to initialize the controller from a
+ * non-realtime thread with a pointer to the hardware interface, itself,
+ * instead of a pointer to a RobotHW.
+ *
+ * \param robot The specific hardware interface used by this controller.
+ *
+ * \param n A NodeHandle in the namespace from which the controller
+ * should read its configuration, and where it should set up its ROS
+ * interface.
+ *
+ * \returns True if initialization was successful and the controller
+ * is ready to be started.
+ */
+ bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ *
+ * \param command
+ */
+ void setCommand(double pos_target);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ * Also supports a target velocity
+ *
+ * \param pos_target - position setpoint
+ * \param vel_target - velocity setpoint
+ */
+ void setCommand(double pos_target, double vel_target);
+
+ /** \brief This is called from within the realtime thread just before the
+ * first call to \ref update
+ *
+ * \param time The current time
+ */
+ void starting(const ros::Time& time);
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+ void update(const ros::Time& time, const ros::Duration& period);
+
+ /**
+ * \brief Get the PID parameters
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+ /**
+ * \brief Print debug info to console
+ */
+ void printDebug();
+
+ /**
+ * \brief Get the PID parameters
+ */
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+
+ /**
+ * \brief Get the name of the joint this controller uses
+ */
+ std::string getJointName();
+
+ /**
+ * \brief Get the current position of the joint
+ * \return current position
+ */
+ double getPosition();
+
+ hardware_interface::JointHandle joint_;
+ boost::shared_ptr<const urdf::Joint> joint_urdf_;
+ realtime_tools::RealtimeBuffer<Commands> command_;
+ Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
+
+private:
+ int loop_count_;
+ control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
+
+ boost::scoped_ptr<
+ realtime_tools::RealtimePublisher<
+ control_msgs::JointControllerState> > controller_state_publisher_ ;
+
+ ros::Subscriber sub_command_;
+
+ /**
+ * \brief Callback from /command subscriber for setpoint
+ */
+ void setCommandCB(const std_msgs::Float64ConstPtr& msg);
+
+ /**
+ * \brief Check that the command is within the hard limits of the joint. Checks for joint
+ * type first. Sets command to limit if out of bounds.
+ * \param command - the input to test
+ */
+ void enforceJointLimits(double &command);
+
+};
+
+} // namespace
+
+#endif
diff --git a/effort_controllers/include/effort_controllers/joint_velocity_controller.h b/effort_controllers/include/effort_controllers/joint_velocity_controller.h
new file mode 100644
index 0000000..58a8711
--- /dev/null
+++ b/effort_controllers/include/effort_controllers/joint_velocity_controller.h
@@ -0,0 +1,166 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef EFFORT_CONTROLLERS__JOINT_VELOCITY_CONTROLLER_H
+#define EFFORT_CONTROLLERS__JOINT_VELOCITY_CONTROLLER_H
+
+/**
+ @class effort_controllers::JointVelocityController
+ @brief Joint Velocity Controller
+
+ This class controls velocity using a pid loop.
+
+ @section ROS ROS interface
+
+ @param type Must be "effort_controllers::JointVelocityController"
+ @param joint Name of the joint to control.
+ @param pid Contains the gains for the PID loop around velocity. See: control_toolbox::Pid
+
+ Subscribes to:
+
+ - @b command (std_msgs::Float64) : The joint velocity to achieve.
+
+ Publishes:
+
+ - @b state (control_msgs::JointControllerState) :
+ Current state of the controller, including pid error and gains.
+
+*/
+
+#include <ros/node_handle.h>
+#include <boost/thread/condition.hpp>
+#include <boost/scoped_ptr.hpp>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_toolbox/pid.h>
+#include <realtime_tools/realtime_publisher.h>
+
+namespace effort_controllers
+{
+
+class JointVelocityController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+public:
+
+ JointVelocityController();
+ ~JointVelocityController();
+
+ bool init(hardware_interface::EffortJointInterface *robot, const std::string &joint_name, const control_toolbox::Pid &pid);
+
+ /** \brief The init function is called to initialize the controller from a
+ * non-realtime thread with a pointer to the hardware interface, itself,
+ * instead of a pointer to a RobotHW.
+ *
+ * \param robot The specific hardware interface used by this controller.
+ *
+ * \param n A NodeHandle in the namespace from which the controller
+ * should read its configuration, and where it should set up its ROS
+ * interface.
+ *
+ * \returns True if initialization was successful and the controller
+ * is ready to be started.
+ */
+ bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+
+ /*!
+ * \brief Give set velocity of the joint for next update: revolute (angle) and prismatic (velocity)
+ *
+ * \param double pos Velocity command to issue
+ */
+ void setCommand(double cmd);
+
+ /*!
+ * \brief Get latest velocity command to the joint: revolute (angle) and prismatic (velocity).
+ */
+ void getCommand(double & cmd);
+
+ /** \brief This is called from within the realtime thread just before the
+ * first call to \ref update
+ *
+ * \param time The current time
+ */
+ void starting(const ros::Time& time);
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+ void update(const ros::Time& time, const ros::Duration& period);
+
+ /**
+ * \brief Get the PID parameters
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+ /**
+ * \brief Print debug info to console
+ */
+ void printDebug();
+
+ /**
+ * \brief Get the PID parameters
+ */
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+
+ /**
+ * \brief Get the name of the joint this controller uses
+ */
+ std::string getJointName();
+
+ hardware_interface::JointHandle joint_;
+ double command_; /**< Last commanded velocity. */
+
+private:
+ int loop_count_;
+ control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
+
+ //friend class JointVelocityControllerNode; // what is this for??
+
+ boost::scoped_ptr<
+ realtime_tools::RealtimePublisher<
+ control_msgs::JointControllerState> > controller_state_publisher_ ;
+
+ ros::Subscriber sub_command_;
+
+ /**
+ * \brief Callback from /command subscriber for setpoint
+ */
+ void setCommandCB(const std_msgs::Float64ConstPtr& msg);
+};
+
+} // namespace
+
+#endif
diff --git a/effort_controllers/mainpage.dox b/effort_controllers/mainpage.dox
new file mode 100644
index 0000000..2b3a59f
--- /dev/null
+++ b/effort_controllers/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b effort_controllers
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml
new file mode 100644
index 0000000..5395207
--- /dev/null
+++ b/effort_controllers/package.xml
@@ -0,0 +1,38 @@
+<package>
+ <name>effort_controllers</name>
+ <version>0.10.0</version>
+ <description>effort_controllers</description>
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Vijay Pradeep</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>angles</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>control_msgs</build_depend>
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>urdf</build_depend>
+ <build_depend>forward_command_controller</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+
+ <run_depend>angles</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>control_msgs</run_depend>
+ <run_depend>control_toolbox</run_depend>
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>urdf</run_depend>
+ <run_depend>forward_command_controller</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/effort_controllers_plugins.xml"/>
+ </export>
+</package>
diff --git a/effort_controllers/src/joint_effort_controller.cpp b/effort_controllers/src/joint_effort_controller.cpp
new file mode 100644
index 0000000..b05d64a
--- /dev/null
+++ b/effort_controllers/src/joint_effort_controller.cpp
@@ -0,0 +1,48 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <effort_controllers/joint_effort_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardCommandController<T>::starting(const ros::Time& time)
+{
+ // Start controller with 0.0 effort
+ command_buffer_.writeFromNonRT(0.0);
+}
+
+
+PLUGINLIB_EXPORT_CLASS(effort_controllers::JointEffortController,controller_interface::ControllerBase)
diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp
new file mode 100644
index 0000000..bf41e3c
--- /dev/null
+++ b/effort_controllers/src/joint_group_effort_controller.cpp
@@ -0,0 +1,49 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <effort_controllers/joint_group_effort_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+{
+ // Start controller with 0.0 efforts
+ commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
+}
+
+
+PLUGINLIB_EXPORT_CLASS(effort_controllers::JointGroupEffortController,controller_interface::ControllerBase)
diff --git a/effort_controllers/src/joint_position_controller.cpp b/effort_controllers/src/joint_position_controller.cpp
new file mode 100644
index 0000000..9a2608c
--- /dev/null
+++ b/effort_controllers/src/joint_position_controller.cpp
@@ -0,0 +1,262 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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: Vijay Pradeep
+ Contributors: Jonathan Bohren, Wim Meeussen, Dave Coleman
+ Desc: Effort(force)-based position controller using basic PID loop
+*/
+
+#include <effort_controllers/joint_position_controller.h>
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace effort_controllers {
+
+JointPositionController::JointPositionController()
+ : loop_count_(0)
+{}
+
+JointPositionController::~JointPositionController()
+{
+ sub_command_.shutdown();
+}
+
+bool JointPositionController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
+{
+ // Get joint name from parameter server
+ std::string joint_name;
+ if (!n.getParam("joint", joint_name))
+ {
+ ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ // Load PID Controller using gains set on parameter server
+ if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
+ return false;
+
+ // Start realtime state publisher
+ controller_state_publisher_.reset(
+ new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1));
+
+ // Start command subscriber
+ sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);
+
+ // Get joint handle from hardware interface
+ joint_ = robot->getHandle(joint_name);
+
+ // Get URDF info about joint
+ urdf::Model urdf;
+ if (!urdf.initParam("robot_description"))
+ {
+ ROS_ERROR("Failed to parse urdf file");
+ return false;
+ }
+ joint_urdf_ = urdf.getJoint(joint_name);
+ if (!joint_urdf_)
+ {
+ ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
+ return false;
+ }
+
+ return true;
+}
+
+void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
+{
+ pid_controller_.setGains(p,i,d,i_max,i_min);
+}
+
+void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
+{
+ pid_controller_.getGains(p,i,d,i_max,i_min);
+}
+
+void JointPositionController::printDebug()
+{
+ pid_controller_.printValues();
+}
+
+std::string JointPositionController::getJointName()
+{
+ return joint_.getName();
+}
+
+double JointPositionController::getPosition()
+{
+ return joint_.getPosition();
+}
+
+// Set the joint position command
+void JointPositionController::setCommand(double pos_command)
+{
+ command_struct_.position_ = pos_command;
+ command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it
+
+ // the writeFromNonRT can be used in RT, if you have the guarantee that
+ // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
+ // * there is only one single rt thread
+ command_.writeFromNonRT(command_struct_);
+}
+
+// Set the joint position command with a velocity command as well
+void JointPositionController::setCommand(double pos_command, double vel_command)
+{
+ command_struct_.position_ = pos_command;
+ command_struct_.velocity_ = vel_command;
+ command_struct_.has_velocity_ = true;
+
+ command_.writeFromNonRT(command_struct_);
+}
+
+void JointPositionController::starting(const ros::Time& time)
+{
+ double pos_command = joint_.getPosition();
+
+ // Make sure joint is within limits if applicable
+ enforceJointLimits(pos_command);
+
+ command_struct_.position_ = pos_command;
+ command_struct_.has_velocity_ = false;
+
+ command_.initRT(command_struct_);
+
+ pid_controller_.reset();
+}
+
+void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
+{
+ command_struct_ = *(command_.readFromRT());
+ double command_position = command_struct_.position_;
+ double command_velocity = command_struct_.velocity_;
+ bool has_velocity_ = command_struct_.has_velocity_;
+
+ double error, vel_error;
+ double commanded_effort;
+
+ double current_position = joint_.getPosition();
+
+ // Make sure joint is within limits if applicable
+ enforceJointLimits(command_position);
+
+ // Compute position error
+ if (joint_urdf_->type == urdf::Joint::REVOLUTE)
+ {
+ angles::shortest_angular_distance_with_limits(
+ current_position,
+ command_position,
+ joint_urdf_->limits->lower,
+ joint_urdf_->limits->upper,
+ error);
+ }
+ else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
+ {
+ error = angles::shortest_angular_distance(current_position, command_position);
+ }
+ else //prismatic
+ {
+ error = command_position - current_position;
+ }
+
+ // Decide which of the two PID computeCommand() methods to call
+ if (has_velocity_)
+ {
+ // Compute velocity error if a non-zero velocity command was given
+ vel_error = command_velocity - joint_.getVelocity();
+
+ // Set the PID error and compute the PID command with nonuniform
+ // time step size. This also allows the user to pass in a precomputed derivative error.
+ commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
+ }
+ else
+ {
+ // Set the PID error and compute the PID command with nonuniform
+ // time step size.
+ commanded_effort = pid_controller_.computeCommand(error, period);
+ }
+
+ joint_.setCommand(commanded_effort);
+
+ // publish state
+ if (loop_count_ % 10 == 0)
+ {
+ if(controller_state_publisher_ && controller_state_publisher_->trylock())
+ {
+ controller_state_publisher_->msg_.header.stamp = time;
+ controller_state_publisher_->msg_.set_point = command_position;
+ controller_state_publisher_->msg_.process_value = current_position;
+ controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
+ controller_state_publisher_->msg_.error = error;
+ controller_state_publisher_->msg_.time_step = period.toSec();
+ controller_state_publisher_->msg_.command = commanded_effort;
+
+ double dummy;
+ getGains(controller_state_publisher_->msg_.p,
+ controller_state_publisher_->msg_.i,
+ controller_state_publisher_->msg_.d,
+ controller_state_publisher_->msg_.i_clamp,
+ dummy);
+ controller_state_publisher_->unlockAndPublish();
+ }
+ }
+ loop_count_++;
+}
+
+void JointPositionController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
+{
+ setCommand(msg->data);
+}
+
+// Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved
+void JointPositionController::enforceJointLimits(double &command)
+{
+ // Check that this joint has applicable limits
+ if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC)
+ {
+ if( command > joint_urdf_->limits->upper ) // above upper limnit
+ {
+ command = joint_urdf_->limits->upper;
+ }
+ else if( command < joint_urdf_->limits->lower ) // below lower limit
+ {
+ command = joint_urdf_->limits->lower;
+ }
+ }
+}
+
+} // namespace
+
+PLUGINLIB_EXPORT_CLASS( effort_controllers::JointPositionController, controller_interface::ControllerBase)
diff --git a/effort_controllers/src/joint_velocity_controller.cpp b/effort_controllers/src/joint_velocity_controller.cpp
new file mode 100644
index 0000000..6ba7a8f
--- /dev/null
+++ b/effort_controllers/src/joint_velocity_controller.cpp
@@ -0,0 +1,170 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <effort_controllers/joint_velocity_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+
+namespace effort_controllers {
+
+JointVelocityController::JointVelocityController()
+: command_(0), loop_count_(0)
+{}
+
+JointVelocityController::~JointVelocityController()
+{
+ sub_command_.shutdown();
+}
+
+bool JointVelocityController::init(hardware_interface::EffortJointInterface *robot,
+ const std::string &joint_name, const control_toolbox::Pid &pid)
+{
+ pid_controller_ = pid;
+
+ // Get joint handle from hardware interface
+ joint_ = robot->getHandle(joint_name);
+
+ return true;
+}
+
+bool JointVelocityController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
+{
+ // Get joint name from parameter server
+ std::string joint_name;
+ if (!n.getParam("joint", joint_name)) {
+ ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ // Get joint handle from hardware interface
+ joint_ = robot->getHandle(joint_name);
+
+ // Load PID Controller using gains set on parameter server
+ if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
+ return false;
+
+ // Start realtime state publisher
+ controller_state_publisher_.reset(
+ new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
+ (n, "state", 1));
+
+ // Start command subscriber
+ sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointVelocityController::setCommandCB, this);
+
+ return true;
+}
+
+
+void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
+{
+ pid_controller_.setGains(p,i,d,i_max,i_min);
+
+}
+
+void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
+{
+ pid_controller_.getGains(p,i,d,i_max,i_min);
+}
+
+void JointVelocityController::printDebug()
+{
+ pid_controller_.printValues();
+}
+
+std::string JointVelocityController::getJointName()
+{
+ return joint_.getName();
+}
+
+// Set the joint velocity command
+void JointVelocityController::setCommand(double cmd)
+{
+ command_ = cmd;
+}
+
+// Return the current velocity command
+void JointVelocityController::getCommand(double& cmd)
+{
+ cmd = command_;
+}
+
+void JointVelocityController::starting(const ros::Time& time)
+{
+ command_ = 0.0;
+ pid_controller_.reset();
+}
+
+void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
+{
+ double error = command_ - joint_.getVelocity();
+
+ // Set the PID error and compute the PID command with nonuniform time
+ // step size. The derivative error is computed from the change in the error
+ // and the timestep dt.
+ double commanded_effort = pid_controller_.computeCommand(error, period);
+
+ joint_.setCommand(commanded_effort);
+
+ if(loop_count_ % 10 == 0)
+ {
+ if(controller_state_publisher_ && controller_state_publisher_->trylock())
+ {
+ controller_state_publisher_->msg_.header.stamp = time;
+ controller_state_publisher_->msg_.set_point = command_;
+ controller_state_publisher_->msg_.process_value = joint_.getVelocity();
+ controller_state_publisher_->msg_.error = error;
+ controller_state_publisher_->msg_.time_step = period.toSec();
+ controller_state_publisher_->msg_.command = commanded_effort;
+
+ double dummy;
+ getGains(controller_state_publisher_->msg_.p,
+ controller_state_publisher_->msg_.i,
+ controller_state_publisher_->msg_.d,
+ controller_state_publisher_->msg_.i_clamp,
+ dummy);
+ controller_state_publisher_->unlockAndPublish();
+ }
+ }
+ loop_count_++;
+}
+
+void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
+{
+ command_ = msg->data;
+}
+
+} // namespace
+
+PLUGINLIB_EXPORT_CLASS( effort_controllers::JointVelocityController, controller_interface::ControllerBase)
diff --git a/force_torque_sensor_controller/.gitignore b/force_torque_sensor_controller/.gitignore
new file mode 100644
index 0000000..5ddaf48
--- /dev/null
+++ b/force_torque_sensor_controller/.gitignore
@@ -0,0 +1,3 @@
+bin
+build
+lib
\ No newline at end of file
diff --git a/force_torque_sensor_controller/CHANGELOG.rst b/force_torque_sensor_controller/CHANGELOG.rst
new file mode 100644
index 0000000..6c2652a
--- /dev/null
+++ b/force_torque_sensor_controller/CHANGELOG.rst
@@ -0,0 +1,78 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package force_torque_sensor_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+* Address -Wunused-parameter warnings
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.2 (2015-05-04)
+------------------
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* Buildsystem fixes
+* Contributors: Dave Coleman
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Add missing controller resources to install target
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+* Install default config files
+* Contributors: Paul Mathieu
+
+0.5.4 (2013-09-30)
+------------------
+* Added install rules for plugin.xml
+* Fix license header string for some files.
+
+0.5.3 (2013-09-04)
+------------------
+* Removed manifest.xml from all packages to prevent rosdep heirarchy issues in Groovy and Hydro
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+* Make hybrid rosbuild-catkin packages.
+ Affects force-torque and imu sensor controllers.
+* Add package.xml scripts.
+* Fix author name typo.
+* Fix PLUGINLIB_DECLARE_CLASS depreacated errors.
+* Propagate sensor interfaces API changes.
+* Fix package URLs.
+* Propagate changes in hardware_interface.
+ - force-torque and IMU sensors no longer depend on Eigen.
+ - The controllers that publish sensor state don't need the Eigen wrappers
+ and now use the raw data directly.
+* Controller publishing the state of a F/T sensor.
+
+0.4.0 (2013-06-26)
+------------------
diff --git a/force_torque_sensor_controller/CMakeLists.txt b/force_torque_sensor_controller/CMakeLists.txt
new file mode 100644
index 0000000..437f097
--- /dev/null
+++ b/force_torque_sensor_controller/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(force_torque_sensor_controller)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS controller_interface hardware_interface geometry_msgs realtime_tools)
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS controller_interface hardware_interface geometry_msgs realtime_tools
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ )
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+ src/force_torque_sensor_controller.cpp include/force_torque_sensor_controller/force_torque_sensor_controller.h)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES force_torque_sensor_plugin.xml
+ force_torque_sensor_controller.yaml
+ force_torque_sensor_controller.launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/force_torque_sensor_controller/force_torque_sensor_controller.launch b/force_torque_sensor_controller/force_torque_sensor_controller.launch
new file mode 100644
index 0000000..022c6e5
--- /dev/null
+++ b/force_torque_sensor_controller/force_torque_sensor_controller.launch
@@ -0,0 +1,7 @@
+<launch>
+ <!-- load configuration -->
+ <rosparam command="load" file="$(find force_torque_sensor_controller)/force_torque_sensor_controller.yaml" />
+
+ <!-- spawn controller -->
+ <node name="force_torque_sensor_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="force_torque_sensor_controller" />
+</launch>
\ No newline at end of file
diff --git a/force_torque_sensor_controller/force_torque_sensor_controller.yaml b/force_torque_sensor_controller/force_torque_sensor_controller.yaml
new file mode 100644
index 0000000..ed4c767
--- /dev/null
+++ b/force_torque_sensor_controller/force_torque_sensor_controller.yaml
@@ -0,0 +1,3 @@
+force_torque_sensor_controller:
+ type: force_torque_sensor_controller/ForceTorqueSensorController
+ publish_rate: 50
\ No newline at end of file
diff --git a/force_torque_sensor_controller/force_torque_sensor_plugin.xml b/force_torque_sensor_controller/force_torque_sensor_plugin.xml
new file mode 100644
index 0000000..fa550f6
--- /dev/null
+++ b/force_torque_sensor_controller/force_torque_sensor_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libforce_torque_sensor_controller">
+ <class name="force_torque_sensor_controller/ForceTorqueSensorController" type="force_torque_sensor_controller::ForceTorqueSensorController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ This controller publishes the readings of all available force-torque sensors as geometry_msgs/WrenchStamped message. The controller expects a ForceTorqueSensorInterface type of hardware interface.
+ </description>
+ </class>
+</library>
\ No newline at end of file
diff --git a/force_torque_sensor_controller/include/force_torque_sensor_controller/force_torque_sensor_controller.h b/force_torque_sensor_controller/include/force_torque_sensor_controller/force_torque_sensor_controller.h
new file mode 100644
index 0000000..f793c17
--- /dev/null
+++ b/force_torque_sensor_controller/include/force_torque_sensor_controller/force_torque_sensor_controller.h
@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+// 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 S.L. 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: Adolfo Rodriguez Tsouroukdissian
+
+#ifndef FORCE_TORQUE_SENSOR_CONTROLLER_FORCE_TORQUE_SENSOR_CONTROLLER_H
+#define FORCE_TORQUE_SENSOR_CONTROLLER_FORCE_TORQUE_SENSOR_CONTROLLER_H
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <boost/shared_ptr.hpp>
+
+namespace force_torque_sensor_controller
+{
+
+// this controller gets access to the ForceTorqueSensorInterface
+class ForceTorqueSensorController: public controller_interface::Controller<hardware_interface::ForceTorqueSensorInterface>
+{
+public:
+ ForceTorqueSensorController(){}
+
+ virtual bool init(hardware_interface::ForceTorqueSensorInterface* hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh);
+ virtual void starting(const ros::Time& time);
+ virtual void update(const ros::Time& time, const ros::Duration& /*period*/);
+ virtual void stopping(const ros::Time& /*time*/);
+
+private:
+ std::vector<hardware_interface::ForceTorqueSensorHandle> sensors_;
+ typedef boost::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> > RtPublisherPtr;
+ std::vector<RtPublisherPtr> realtime_pubs_;
+ std::vector<ros::Time> last_publish_times_;
+ double publish_rate_;
+};
+
+}
+
+#endif
diff --git a/force_torque_sensor_controller/package.xml b/force_torque_sensor_controller/package.xml
new file mode 100644
index 0000000..e3091ca
--- /dev/null
+++ b/force_torque_sensor_controller/package.xml
@@ -0,0 +1,34 @@
+<package>
+ <name>force_torque_sensor_controller</name>
+ <version>0.10.0</version>
+ <description>Controller to publish state of force-torque sensors</description>
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Adolfo Rodriguez Tsouroukdissian</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>pluginlib</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>pluginlib</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/force_torque_sensor_plugin.xml"/>
+ </export>
+</package>
diff --git a/force_torque_sensor_controller/src/force_torque_sensor_controller.cpp b/force_torque_sensor_controller/src/force_torque_sensor_controller.cpp
new file mode 100644
index 0000000..3b210db
--- /dev/null
+++ b/force_torque_sensor_controller/src/force_torque_sensor_controller.cpp
@@ -0,0 +1,106 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+// 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 S.L. 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: Adolfo Rodriguez Tsouroukdissian
+
+
+#include "force_torque_sensor_controller/force_torque_sensor_controller.h"
+
+
+
+namespace force_torque_sensor_controller
+{
+
+ bool ForceTorqueSensorController::init(hardware_interface::ForceTorqueSensorInterface* hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh)
+ {
+ // get all joint states from the hardware interface
+ const std::vector<std::string>& sensor_names = hw->getNames();
+ for (unsigned i=0; i<sensor_names.size(); i++)
+ ROS_DEBUG("Got sensor %s", sensor_names[i].c_str());
+
+ // get publishing period
+ if (!controller_nh.getParam("publish_rate", publish_rate_)){
+ ROS_ERROR("Parameter 'publish_rate' not set");
+ return false;
+ }
+
+ for (unsigned i=0; i<sensor_names.size(); i++){
+ // sensor handle
+ sensors_.push_back(hw->getHandle(sensor_names[i]));
+
+ // realtime publisher
+ RtPublisherPtr rt_pub(new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(root_nh, sensor_names[i], 4));
+ realtime_pubs_.push_back(rt_pub);
+ }
+
+ // Last published times
+ last_publish_times_.resize(sensor_names.size());
+ return true;
+ }
+
+ void ForceTorqueSensorController::starting(const ros::Time& time)
+ {
+ // initialize time
+ for (unsigned i=0; i<last_publish_times_.size(); i++){
+ last_publish_times_[i] = time;
+ }
+ }
+
+ void ForceTorqueSensorController::update(const ros::Time& time, const ros::Duration& /*period*/)
+ {
+ // limit rate of publishing
+ for (unsigned i=0; i<realtime_pubs_.size(); i++){
+ if (publish_rate_ > 0.0 && last_publish_times_[i] + ros::Duration(1.0/publish_rate_) < time){
+ // try to publish
+ if (realtime_pubs_[i]->trylock()){
+ // we're actually publishing, so increment time
+ last_publish_times_[i] = last_publish_times_[i] + ros::Duration(1.0/publish_rate_);
+
+ // populate message
+ realtime_pubs_[i]->msg_.header.stamp = time;
+ realtime_pubs_[i]->msg_.header.frame_id = sensors_[i].getFrameId();
+
+ realtime_pubs_[i]->msg_.wrench.force.x = sensors_[i].getForce()[0];
+ realtime_pubs_[i]->msg_.wrench.force.y = sensors_[i].getForce()[1];
+ realtime_pubs_[i]->msg_.wrench.force.z = sensors_[i].getForce()[2];
+ realtime_pubs_[i]->msg_.wrench.torque.x = sensors_[i].getTorque()[0];
+ realtime_pubs_[i]->msg_.wrench.torque.y = sensors_[i].getTorque()[1];
+ realtime_pubs_[i]->msg_.wrench.torque.z = sensors_[i].getTorque()[2];
+
+ realtime_pubs_[i]->unlockAndPublish();
+ }
+ }
+ }
+ }
+
+ void ForceTorqueSensorController::stopping(const ros::Time& /*time*/)
+ {}
+
+}
+
+PLUGINLIB_EXPORT_CLASS(force_torque_sensor_controller::ForceTorqueSensorController, controller_interface::ControllerBase)
diff --git a/forward_command_controller/.gitignore b/forward_command_controller/.gitignore
new file mode 100644
index 0000000..c795b05
--- /dev/null
+++ b/forward_command_controller/.gitignore
@@ -0,0 +1 @@
+build
\ No newline at end of file
diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst
new file mode 100644
index 0000000..e794cd0
--- /dev/null
+++ b/forward_command_controller/CHANGELOG.rst
@@ -0,0 +1,75 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package forward_command_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+* Address -Wunused-parameter warnings
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.2 (2015-05-04)
+------------------
+* Thread-safe and realtime-safe forward controllers.
+* Complain if list of joints is empty.
+* Contributors: Mathias Lüdtke
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* ForwardJointGroupCommandController: Class for implementing multi-joint
+ command-forwarding controllers
+* Contributors: ipa-fxm
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+
+0.5.4 (2013-09-30)
+------------------
+
+0.5.3 (2013-09-04)
+------------------
+* Removed manifest.xml from all packages to prevent rosdep heirarchy issues in Groovy and Hydro
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+
+0.4.0 (2013-06-26)
+------------------
+* Version 0.4.0
+* Propagate API changes in hardware_interface.
+* adding install targets
+* adding switches for hybrid buildsystem
+* Remove unused method (legacy from the past).
+* adding these packages which weren't seen by catkinize_stack
+* Extend joint_effort_controller to other interfaces
+ - Factor-out implementation of simple command-forwarding controller.
+ - Provide specializations (typedefs really) for effort, velocity and position
+ interfaces.
diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt
new file mode 100644
index 0000000..891dc83
--- /dev/null
+++ b/forward_command_controller/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(forward_command_controller)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS controller_interface hardware_interface std_msgs realtime_tools)
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS controller_interface hardware_interface std_msgs realtime_tools
+ INCLUDE_DIRS include
+ )
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_command_controller.h
new file mode 100644
index 0000000..6987b91
--- /dev/null
+++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.h
@@ -0,0 +1,101 @@
+
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_COMMAND_CONTROLLER_H
+#define FORWARD_COMMAND_CONTROLLER_FORWARD_COMMAND_CONTROLLER_H
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64.h>
+#include <realtime_tools/realtime_buffer.h>
+
+
+namespace forward_command_controller
+{
+
+/**
+ * \brief Single joint controller.
+ *
+ * This class passes the command signal down to the joint.
+ * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled
+ * joint.
+ *
+ * \tparam T Type implementing the JointCommandInterface.
+ *
+ * \section ROS interface
+ *
+ * \param type hardware interface type.
+ * \param joint Name of the joint to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64) : The joint command to apply.
+ */
+template <class T>
+class ForwardCommandController: public controller_interface::Controller<T>
+{
+public:
+ ForwardCommandController() {}
+ ~ForwardCommandController() {sub_command_.shutdown();}
+
+ bool init(T* hw, ros::NodeHandle &n)
+ {
+ std::string joint_name;
+ if (!n.getParam("joint", joint_name))
+ {
+ ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+ joint_ = hw->getHandle(joint_name);
+ sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &ForwardCommandController::commandCB, this);
+ return true;
+ }
+
+ void starting(const ros::Time& time);
+ void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {joint_.setCommand(*command_buffer_.readFromRT());}
+
+ hardware_interface::JointHandle joint_;
+ realtime_tools::RealtimeBuffer<double> command_buffer_;
+
+private:
+ ros::Subscriber sub_command_;
+ void commandCB(const std_msgs::Float64ConstPtr& msg) {command_buffer_.writeFromNonRT(msg->data);}
+};
+
+}
+
+#endif
diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
new file mode 100644
index 0000000..46adc27
--- /dev/null
+++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
@@ -0,0 +1,141 @@
+
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H
+#define FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H
+
+#include <vector>
+#include <string>
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <realtime_tools/realtime_buffer.h>
+
+
+namespace forward_command_controller
+{
+
+/**
+ * \brief Forward command controller for a set of joints.
+ *
+ * This class forwards the command signal down to a set of joints.
+ * Command signal and joint hardware interface are of the same type, e.g. effort commands for an effort-controlled
+ * joint.
+ *
+ * \tparam T Type implementing the JointCommandInterface.
+ *
+ * \section ROS interface
+ *
+ * \param type hardware interface type.
+ * \param joints Names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint commands to apply.
+ */
+template <class T>
+class ForwardJointGroupCommandController: public controller_interface::Controller<T>
+{
+public:
+ ForwardJointGroupCommandController() {}
+ ~ForwardJointGroupCommandController() {sub_command_.shutdown();}
+
+ bool init(T* hw, ros::NodeHandle &n)
+ {
+ // List of controlled joints
+ std::string param_name = "joints";
+ if(!n.getParam(param_name, joint_names_))
+ {
+ ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
+ return false;
+ }
+ n_joints_ = joint_names_.size();
+
+ if(n_joints_ == 0){
+ ROS_ERROR_STREAM("List of joint names is empty.");
+ return false;
+ }
+ for(unsigned int i=0; i<n_joints_; i++)
+ {
+ try
+ {
+ joints_.push_back(hw->getHandle(joint_names_[i]));
+ }
+ catch (const hardware_interface::HardwareInterfaceException& e)
+ {
+ ROS_ERROR_STREAM("Exception thrown: " << e.what());
+ return false;
+ }
+ }
+
+ commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
+
+ sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this);
+ return true;
+ }
+
+ void starting(const ros::Time& time);
+ void update(const ros::Time& /*time*/, const ros::Duration& /*period*/)
+ {
+ std::vector<double> & commands = *commands_buffer_.readFromRT();
+ for(unsigned int i=0; i<n_joints_; i++)
+ { joints_[i].setCommand(commands[i]); }
+ }
+
+ std::vector< std::string > joint_names_;
+ std::vector< hardware_interface::JointHandle > joints_;
+ realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
+ unsigned int n_joints_;
+
+private:
+ ros::Subscriber sub_command_;
+ void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
+ {
+ if(msg->data.size()!=n_joints_)
+ {
+ ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
+ return;
+ }
+ commands_buffer_.writeFromNonRT(msg->data);
+ }
+};
+
+}
+
+#endif
diff --git a/forward_command_controller/mainpage.dox b/forward_command_controller/mainpage.dox
new file mode 100644
index 0000000..cf0c339
--- /dev/null
+++ b/forward_command_controller/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b forward_command_controller
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml
new file mode 100644
index 0000000..1ac22a1
--- /dev/null
+++ b/forward_command_controller/package.xml
@@ -0,0 +1,29 @@
+<package>
+ <name>forward_command_controller</name>
+ <version>0.10.0</version>
+ <description>forward_command_controller</description>
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Vijay Pradeep</author>
+ <author>Adolfo Rodriguez Tsouroukdissian</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>realtime_tools</run_depend>
+
+ <export>
+ <cpp cflags="-I${prefix}/include"/>
+ </export>
+</package>
diff --git a/gripper_action_controller/CHANGELOG.rst b/gripper_action_controller/CHANGELOG.rst
new file mode 100644
index 0000000..58b2398
--- /dev/null
+++ b/gripper_action_controller/CHANGELOG.rst
@@ -0,0 +1,36 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package gripper_action_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+
+0.9.2 (2015-05-04)
+------------------
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* Buildsystem fixes
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Lukas Bulwahn
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+
+0.7.2 (2014-04-01)
+------------------
+* Added missing deps to package.xml
+* Contributors: Scott K Logan
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+* gripper_action_controller: New controller for single dof grippers.
+* Contributors: Sachin Chitta.
diff --git a/gripper_action_controller/CMakeLists.txt b/gripper_action_controller/CMakeLists.txt
new file mode 100644
index 0000000..2b925a9
--- /dev/null
+++ b/gripper_action_controller/CMakeLists.txt
@@ -0,0 +1,177 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gripper_action_controller)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin
+ REQUIRED COMPONENTS
+ actionlib
+ angles
+ cmake_modules
+ roscpp
+ urdf
+ control_toolbox
+ controller_interface
+ hardware_interface
+ realtime_tools
+ control_msgs
+ trajectory_msgs
+ controller_manager
+ xacro
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependencies might have been
+## pulled in transitively but can be declared for certainty nonetheless:
+## * add a build_depend tag for "message_generation"
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# control_msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+INCLUDE_DIRS include
+LIBRARIES gripper_action_controller
+CATKIN_DEPENDS actionlib cmake_modules control_msgs controller_interface hardware_interface realtime_tools
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+ include ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(gripper_action_controller
+ src/gripper_action_controller.cpp
+ include/gripper_action_controller/gripper_action_controller.h
+)
+
+target_link_libraries(gripper_action_controller
+ ${catkin_LIBRARIES}
+)
+
+## Declare a cpp executable
+# add_executable(gripper_action_controller_node src/gripper_action_controller_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(gripper_action_controller_node gripper_action_controller_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(gripper_action_controller_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS gripper_action_controller
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES ros_control_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_gripper_action_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/gripper_action_controller/include/gripper_action_controller/gripper_action_controller.h b/gripper_action_controller/include/gripper_action_controller/gripper_action_controller.h
new file mode 100644
index 0000000..3c17180
--- /dev/null
+++ b/gripper_action_controller/include/gripper_action_controller/gripper_action_controller.h
@@ -0,0 +1,161 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2014, SRI International
+//
+// 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 SRI International 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 Sachin Chitta, Adolfo Rodriguez Tsouroukdissian
+
+#ifndef GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_H
+#define GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_H
+
+// C++ standard
+#include <cassert>
+#include <iterator>
+#include <stdexcept>
+#include <string>
+
+// Boost
+#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
+
+// ROS
+#include <ros/node_handle.h>
+
+// URDF
+#include <urdf/model.h>
+
+// ROS messages
+#include <control_msgs/GripperCommandAction.h>
+
+// actionlib
+#include <actionlib/server/action_server.h>
+
+// ros_controls
+#include <realtime_tools/realtime_server_goal_handle.h>
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/internal/demangle_symbol.h>
+#include <realtime_tools/realtime_buffer.h>
+
+// Project
+#include <gripper_action_controller/hardware_interface_adapter.h>
+
+namespace gripper_action_controller
+{
+
+/**
+ * \brief Controller for executing a gripper command action for simple single-dof grippers.
+ *
+ * \tparam HardwareInterface Controller hardware interface. Currently \p hardware_interface::PositionJointInterface and
+ * \p hardware_interface::EffortJointInterface are supported out-of-the-box.
+ */
+template <class HardwareInterface>
+class GripperActionController : public controller_interface::Controller<HardwareInterface>
+{
+public:
+
+ /**
+ * \brief Store position and max effort in struct to allow easier realtime buffer usage
+ */
+ struct Commands
+ {
+ double position_; // Last commanded position
+ double max_effort_; // Max allowed effort
+ };
+
+ GripperActionController();
+
+ /** \name Non Real-Time Safe Functions
+ *\{*/
+ bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+ /*\}*/
+
+ /** \name Real-Time Safe Functions
+ *\{*/
+ /** \brief Holds the current position. */
+ void starting(const ros::Time& time);
+
+ /** \brief Cancels the active action goal, if any. */
+ void stopping(const ros::Time& time);
+
+ void update(const ros::Time& time, const ros::Duration& period);
+ /*\}*/
+
+ realtime_tools::RealtimeBuffer<Commands> command_;
+ Commands command_struct_, command_struct_rt_; // pre-allocated memory that is re-used to set the realtime buffer
+
+private:
+
+ typedef actionlib::ActionServer<control_msgs::GripperCommandAction> ActionServer;
+ typedef boost::shared_ptr<ActionServer> ActionServerPtr;
+ typedef ActionServer::GoalHandle GoalHandle;
+ typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::GripperCommandAction> RealtimeGoalHandle;
+ typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
+
+ typedef HardwareInterfaceAdapter<HardwareInterface> HwIfaceAdapter;
+
+ bool update_hold_position_;
+
+ bool verbose_; ///< Hard coded verbose flag to help in debugging
+ std::string name_; ///< Controller name.
+ hardware_interface::JointHandle joint_; ///< Handles to controlled joints.
+ std::string joint_name_; ///< Controlled joint names.
+
+ HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface.
+
+ RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any.
+ control_msgs::GripperCommandResultPtr pre_alloc_result_;
+
+ ros::Duration action_monitor_period_;
+
+ // ROS API
+ ros::NodeHandle controller_nh_;
+ ActionServerPtr action_server_;
+
+ ros::Timer goal_handle_timer_;
+
+ void goalCB(GoalHandle gh);
+ void cancelCB(GoalHandle gh);
+ void preemptActiveGoal();
+ void setHoldPosition(const ros::Time& time);
+
+ ros::Time last_movement_time_; ///< Store stall time
+ double computed_command_; ///< Computed command
+
+ double stall_timeout_, stall_velocity_threshold_; ///< Stall related parameters
+ double default_max_effort_; ///< Max allowed effort
+ double goal_tolerance_;
+ /**
+ * \brief Check for success and publish appropriate result and feedback.
+ **/
+ void checkForSuccess(const ros::Time& time, double error_position, double current_position, double current_velocity);
+
+};
+
+} // namespace
+
+#include <gripper_action_controller/gripper_action_controller_impl.h>
+
+#endif // header guard
diff --git a/gripper_action_controller/include/gripper_action_controller/gripper_action_controller_impl.h b/gripper_action_controller/include/gripper_action_controller/gripper_action_controller_impl.h
new file mode 100644
index 0000000..2900741
--- /dev/null
+++ b/gripper_action_controller/include/gripper_action_controller/gripper_action_controller_impl.h
@@ -0,0 +1,351 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2014, SRI International
+//
+// 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 SRI International 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 Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser
+
+#ifndef GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_IMPL_H
+#define GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_IMPL_H
+
+namespace gripper_action_controller
+{
+namespace internal
+{
+std::string getLeafNamespace(const ros::NodeHandle& nh)
+{
+ const std::string complete_ns = nh.getNamespace();
+ std::size_t id = complete_ns.find_last_of("/");
+ return complete_ns.substr(id + 1);
+}
+
+boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+{
+ boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+
+ std::string urdf_str;
+ // Check for robot_description in proper namespace
+ if (nh.getParam(param_name, urdf_str))
+ {
+ if (!urdf->initString(urdf_str))
+ {
+ ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
+ nh.getNamespace() << ").");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ }
+ // Check for robot_description in root
+ else if (!urdf->initParam("robot_description"))
+ {
+ ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ return urdf;
+}
+
+typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
+std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
+{
+ std::vector<UrdfJointConstPtr> out;
+ for (unsigned int i = 0; i < joint_names.size(); ++i)
+ {
+ UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+ if (urdf_joint)
+ {
+ out.push_back(urdf_joint);
+ }
+ else
+ {
+ ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
+ return std::vector<UrdfJointConstPtr>();
+ }
+ }
+ return out;
+}
+
+} // namespace
+
+template <class HardwareInterface>
+inline void GripperActionController<HardwareInterface>::
+starting(const ros::Time& time)
+{
+ command_struct_rt_.position_ = joint_.getPosition();
+ command_struct_rt_.max_effort_ = default_max_effort_;
+ command_.initRT(command_struct_rt_);
+
+ // Hardware interface adapter
+ hw_iface_adapter_.starting(ros::Time(0.0));
+ last_movement_time_ = time;
+}
+
+template <class HardwareInterface>
+inline void GripperActionController<HardwareInterface>::
+stopping(const ros::Time& time)
+{
+ preemptActiveGoal();
+}
+
+template <class HardwareInterface>
+inline void GripperActionController<HardwareInterface>::
+preemptActiveGoal()
+{
+ RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+ // Cancels the currently active goal
+ if (current_active_goal)
+ {
+ // Marks the current goal as canceled
+ rt_active_goal_.reset();
+ if(current_active_goal->gh_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
+ current_active_goal->gh_.setCanceled();
+ }
+}
+
+template <class HardwareInterface>
+GripperActionController<HardwareInterface>::
+GripperActionController()
+: verbose_(false) // Set to true during debugging
+{}
+
+template <class HardwareInterface>
+bool GripperActionController<HardwareInterface>::init(HardwareInterface* hw,
+ ros::NodeHandle& root_nh,
+ ros::NodeHandle& controller_nh)
+{
+ using namespace internal;
+
+ // Cache controller node handle
+ controller_nh_ = controller_nh;
+
+ // Controller name
+ name_ = getLeafNamespace(controller_nh_);
+
+ // Action status checking update rate
+ double action_monitor_rate = 20.0;
+ controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
+ action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
+ ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");
+
+ // Controlled joint
+ controller_nh_.getParam("joint", joint_name_);
+ if (joint_name_.empty())
+ {
+ ROS_ERROR_STREAM_NAMED(name_, "Could not find joint name on param server");
+ return false;
+ }
+
+ // URDF joints
+ boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
+ if (!urdf)
+ {
+ return false;
+ }
+
+ std::vector<std::string> joint_names;
+ joint_names.push_back(joint_name_);
+ std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names);
+ if (urdf_joints.empty())
+ {
+ return false;
+ }
+
+ // Initialize members
+ // Joint handle
+ try
+ {
+ joint_ = hw->getHandle(joint_name_);
+ }
+ catch (...)
+ {
+ ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_name_ << "' in '" <<
+ this->getHardwareInterfaceType() << "'.");
+ return false;
+ }
+
+ ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
+ "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" <<
+ "\n");
+
+ // Default tolerances
+ controller_nh_.param<double>("goal_tolerance", goal_tolerance_, 0.01);
+ goal_tolerance_ = fabs(goal_tolerance_);
+ // Max allowable effort
+ controller_nh_.param<double>("max_effort", default_max_effort_, 0.0);
+ default_max_effort_ = fabs(default_max_effort_);
+ // Stall - stall velocity threshold, stall timeout
+ controller_nh_.param<double>("stall_velocity_threshold", stall_velocity_threshold_, 0.001);
+ controller_nh_.param<double>("stall_timeout", stall_timeout_, 1.0);
+
+ // Hardware interface adapter
+ hw_iface_adapter_.init(joint_, controller_nh_);
+
+ // Command - non RT version
+ command_struct_.position_ = joint_.getPosition();
+ command_struct_.max_effort_ = default_max_effort_;
+
+ // Result
+ pre_alloc_result_.reset(new control_msgs::GripperCommandResult());
+ pre_alloc_result_->position = command_struct_.position_;
+ pre_alloc_result_->reached_goal = false;
+ pre_alloc_result_->stalled = false;
+
+ // ROS API: Action interface
+ action_server_.reset(new ActionServer(controller_nh_, "gripper_cmd",
+ boost::bind(&GripperActionController::goalCB, this, _1),
+ boost::bind(&GripperActionController::cancelCB, this, _1),
+ false));
+ action_server_->start();
+ return true;
+}
+
+template <class HardwareInterface>
+void GripperActionController<HardwareInterface>::
+update(const ros::Time& time, const ros::Duration& period)
+{
+ command_struct_rt_ = *(command_.readFromRT());
+
+ double current_position = joint_.getPosition();
+ double current_velocity = joint_.getVelocity();
+
+ double error_position = command_struct_rt_.position_ - current_position;
+ double error_velocity = - current_velocity;
+
+ checkForSuccess(time, error_position, current_position, current_velocity);
+
+ // Hardware interface adapter: Generate and send commands
+ computed_command_ = hw_iface_adapter_.updateCommand(time, period,
+ command_struct_rt_.position_, 0.0,
+ error_position, error_velocity, command_struct_rt_.max_effort_);
+}
+
+template <class HardwareInterface>
+void GripperActionController<HardwareInterface>::
+goalCB(GoalHandle gh)
+{
+ ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
+
+ // Precondition: Running controller
+ if (!this->isRunning())
+ {
+ ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running.");
+ control_msgs::GripperCommandResult result;
+ gh.setRejected(result);
+ return;
+ }
+
+ // Try to update goal
+ RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
+
+ // Accept new goal
+ preemptActiveGoal();
+ gh.setAccepted();
+
+ // This is the non-realtime command_struct
+ // We use command_ for sharing
+ command_struct_.position_ = gh.getGoal()->command.position;
+ command_struct_.max_effort_ = gh.getGoal()->command.max_effort;
+ command_.writeFromNonRT(command_struct_);
+
+ pre_alloc_result_->reached_goal = false;
+ pre_alloc_result_->stalled = false;
+
+ last_movement_time_ = ros::Time::now();
+
+ // Setup goal status checking timer
+ goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
+ &RealtimeGoalHandle::runNonRealtime,
+ rt_goal);
+ goal_handle_timer_.start();
+ rt_active_goal_ = rt_goal;
+}
+
+template <class HardwareInterface>
+void GripperActionController<HardwareInterface>::
+cancelCB(GoalHandle gh)
+{
+ RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+ // Check that cancel request refers to currently active goal (if any)
+ if (current_active_goal && current_active_goal->gh_ == gh)
+ {
+ // Reset current goal
+ rt_active_goal_.reset();
+
+ // Enter hold current position mode
+ setHoldPosition(ros::Time(0.0));
+ ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");
+
+ // Mark the current goal as canceled
+ current_active_goal->gh_.setCanceled();
+ }
+}
+
+template <class HardwareInterface>
+void GripperActionController<HardwareInterface>::
+setHoldPosition(const ros::Time& time)
+{
+ command_struct_.position_ = joint_.getPosition();
+ command_struct_.max_effort_ = default_max_effort_;
+ command_.writeFromNonRT(command_struct_);
+}
+
+template <class HardwareInterface>
+void GripperActionController<HardwareInterface>::
+checkForSuccess(const ros::Time& time, double error_position, double current_position, double current_velocity)
+{
+ if(!rt_active_goal_)
+ return;
+
+ if(rt_active_goal_->gh_.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
+ return;
+
+ if(fabs(error_position) < goal_tolerance_)
+ {
+ pre_alloc_result_->effort = computed_command_;
+ pre_alloc_result_->position = current_position;
+ pre_alloc_result_->reached_goal = true;
+ pre_alloc_result_->stalled = false;
+ rt_active_goal_->setSucceeded(pre_alloc_result_);
+ }
+ else
+ {
+ if(fabs(current_velocity) > stall_velocity_threshold_)
+ {
+ last_movement_time_ = time;
+ }
+ else if( (time - last_movement_time_).toSec() > stall_timeout_)
+ {
+ pre_alloc_result_->effort = computed_command_;
+ pre_alloc_result_->position = current_position;
+ pre_alloc_result_->reached_goal = false;
+ pre_alloc_result_->stalled = true;
+ rt_active_goal_->setAborted(pre_alloc_result_);
+ }
+ }
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/gripper_action_controller/include/gripper_action_controller/hardware_interface_adapter.h b/gripper_action_controller/include/gripper_action_controller/hardware_interface_adapter.h
new file mode 100644
index 0000000..cd896e1
--- /dev/null
+++ b/gripper_action_controller/include/gripper_action_controller/hardware_interface_adapter.h
@@ -0,0 +1,190 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2014, SRI International
+// 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 SRI International 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 Sachin Chitta, Adolfo Rodriguez Tsouroukdissian
+
+#ifndef GRIPPER_ACTION_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
+#define GRIPPER_ACTION_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
+
+#include <cassert>
+#include <string>
+#include <vector>
+
+#include <boost/shared_ptr.hpp>
+
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <control_toolbox/pid.h>
+#include <hardware_interface/joint_command_interface.h>
+
+/**
+ * \brief Helper class to simplify integrating the GripperActionController with different hardware interfaces.
+ *
+ * The GripperActionController outputs position while
+ * it is supposed to work with either position or effort commands.
+ *
+ */
+template <class HardwareInterface>
+class HardwareInterfaceAdapter
+{
+public:
+ bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
+ {
+ return false;
+ }
+
+ void starting(const ros::Time& time) {}
+ void stopping(const ros::Time& time) {}
+
+ double updateCommand(const ros::Time& time,
+ const ros::Duration& period,
+ double desired_position,
+ double desired_velocity,
+ double error_position,
+ double error_velocity,
+ double max_allowed_effort) { return 0.0;}
+
+};
+
+/**
+ * \brief Adapter for a position-controlled hardware interface. Forwards desired positions as commands.
+ */
+template<>
+class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface>
+{
+public:
+ HardwareInterfaceAdapter() : joint_handle_ptr_(0) {}
+
+ bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
+ {
+ // Store pointer to joint handles
+ joint_handle_ptr_ = &joint_handle;
+
+ return true;
+ }
+
+ void starting(const ros::Time& time) {}
+ void stopping(const ros::Time& time) {}
+
+ double updateCommand(const ros::Time& /*time*/,
+ const ros::Duration& period,
+ double desired_position,
+ double desired_velocity,
+ double error_position,
+ double error_velocity,
+ double max_allowed_effort)
+ {
+ // Forward desired position to command
+ (*joint_handle_ptr_).setCommand(desired_position);
+ return max_allowed_effort;
+ }
+
+private:
+ hardware_interface::JointHandle* joint_handle_ptr_;
+};
+
+/**
+ * \brief Adapter for an effort-controlled hardware interface. Maps position and velocity errors to effort commands
+ * through a position PID loop.
+ *
+ * The following is an example configuration of a controller that uses this adapter. Notice the \p gains entry:
+ * \code
+ * gripper_controller:
+ * type: "gripper_action_controller/GripperActionController"
+ * joints: gripper_joint
+ * goal_tolerance: 0.01
+ * stalled_velocity_threshold: 0.01
+ * stall_timeout: 0.2
+ * gains:
+ * gripper_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ * \endcode
+ */
+template<>
+class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface>
+{
+public:
+ HardwareInterfaceAdapter() : joint_handle_ptr_(0) {}
+
+ bool init(hardware_interface::JointHandle& joint_handle, ros::NodeHandle& controller_nh)
+ {
+ // Store pointer to joint handles
+ joint_handle_ptr_ = &joint_handle;
+
+ // Initialize PIDs
+ ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handle.getName());
+
+ // Init PID gains from ROS parameter server
+ pid_.reset(new control_toolbox::Pid());
+ if (!pid_->init(joint_nh))
+ {
+ ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
+ return false;
+ }
+
+ return true;
+ }
+
+ void starting(const ros::Time& time)
+ {
+ if (!joint_handle_ptr_)
+ {
+ return;
+ }
+ // Reset PIDs, zero effort commands
+ pid_->reset();
+ (*joint_handle_ptr_).setCommand(0.0);
+ }
+
+ void stopping(const ros::Time& time) {}
+
+ double updateCommand(const ros::Time& /*time*/,
+ const ros::Duration& period,
+ double desired_position,
+ double desired_velocity,
+ double error_position,
+ double error_velocity,
+ double max_allowed_effort)
+ {
+ // Preconditions
+ if (!joint_handle_ptr_) {return 0.0;}
+
+ // Update PIDs
+ double command = pid_->computeCommand(error_position, error_velocity, period);
+ command = std::min<double>(fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
+ (*joint_handle_ptr_).setCommand(command);
+ return command;
+ }
+
+private:
+ typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
+ PidPtr pid_;
+ hardware_interface::JointHandle* joint_handle_ptr_;
+};
+
+#endif // header guard
diff --git a/gripper_action_controller/package.xml b/gripper_action_controller/package.xml
new file mode 100644
index 0000000..c506f8d
--- /dev/null
+++ b/gripper_action_controller/package.xml
@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<package>
+ <name>gripper_action_controller</name>
+ <version>0.10.0</version>
+ <description>The gripper_action_controller package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe at example.com">Jane Doe</maintainer> -->
+ <maintainer email="robot.moveit at gmail.com">Sachin Chitta</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/gripper_action_controller</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe at example.com">Jane Doe</author> -->
+ <author email="robot.moveit at gmail.com">Sachin Chitta</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>actionlib</build_depend>
+ <build_depend>angles</build_depend>
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>control_msgs</build_depend>
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>controller_manager</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <build_depend>urdf</build_depend>
+ <build_depend>xacro</build_depend>
+ <run_depend>actionlib</run_depend>
+ <run_depend>angles</run_depend>
+ <run_depend>cmake_modules</run_depend>
+ <run_depend>control_msgs</run_depend>
+ <run_depend>control_toolbox</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>controller_manager</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+ <run_depend>urdf</run_depend>
+ <run_depend>xacro</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
+ </export>
+</package>
diff --git a/gripper_action_controller/ros_control_plugins.xml b/gripper_action_controller/ros_control_plugins.xml
new file mode 100644
index 0000000..6a49206
--- /dev/null
+++ b/gripper_action_controller/ros_control_plugins.xml
@@ -0,0 +1,17 @@
+<library path="lib/libgripper_action_controller">
+
+ <class name="position_controllers/GripperActionController"
+ type="position_controllers::GripperActionController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ </description>
+ </class>
+
+ <class name="effort_controllers/GripperActionController"
+ type="effort_controllers::GripperActionController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ </description>
+ </class>
+
+</library>
diff --git a/gripper_action_controller/src/gripper_action_controller.cpp b/gripper_action_controller/src/gripper_action_controller.cpp
new file mode 100644
index 0000000..28cb4c0
--- /dev/null
+++ b/gripper_action_controller/src/gripper_action_controller.cpp
@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2014, SRI International
+//
+// 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 SRI International 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 Sachin Chitta
+
+// Pluginlib
+#include <pluginlib/class_list_macros.h>
+
+// Project
+#include <gripper_action_controller/gripper_action_controller.h>
+
+namespace position_controllers
+{
+ /**
+ * \brief Gripper action controller that sends
+ * commands to a \b position interface.
+ */
+ typedef gripper_action_controller::GripperActionController<hardware_interface::PositionJointInterface>
+ GripperActionController;
+}
+
+namespace effort_controllers
+{
+ /**
+ * \brief Gripper action controller that sends
+ * commands to a \b effort interface.
+ */
+ typedef gripper_action_controller::GripperActionController<hardware_interface::EffortJointInterface>
+ GripperActionController;
+}
+
+PLUGINLIB_EXPORT_CLASS(position_controllers::GripperActionController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(effort_controllers::GripperActionController, controller_interface::ControllerBase)
diff --git a/imu_sensor_controller/.gitignore b/imu_sensor_controller/.gitignore
new file mode 100644
index 0000000..5ddaf48
--- /dev/null
+++ b/imu_sensor_controller/.gitignore
@@ -0,0 +1,3 @@
+bin
+build
+lib
\ No newline at end of file
diff --git a/imu_sensor_controller/CHANGELOG.rst b/imu_sensor_controller/CHANGELOG.rst
new file mode 100644
index 0000000..3bb398c
--- /dev/null
+++ b/imu_sensor_controller/CHANGELOG.rst
@@ -0,0 +1,79 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package imu_sensor_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+* Fixed covariances in ImuSensorController::update
+* Address -Wunused-parameter warnings
+* Contributors: Adolfo Rodriguez Tsouroukdissian, tappan-at-git
+
+0.9.2 (2015-05-04)
+------------------
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* Buildsystem fixes
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Add missing controller resources to install target
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+* Install default config files
+* Contributors: Paul Mathieu
+
+0.5.4 (2013-09-30)
+------------------
+* Added install rules for plugin.xml
+* Fix license header string for some files.
+
+0.5.3 (2013-09-04)
+------------------
+* Removed manifest.xml from all packages to prevent rosdep heirarchy issues in Groovy and Hydro
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+* Make hybrid rosbuild-catkin packages.
+ Affects force-torque and imu sensor controllers.
+* Add package.xml scripts.
+* Fix author name typo.
+* Fix PLUGINLIB_DECLARE_CLASS depreacated errors.
+* Propagate sensor interfaces API changes.
+* Fix package URLs.
+* Propagate changes in hardware_interface.
+ - force-torque and IMU sensors no longer depend on Eigen.
+ - The controllers that publish sensor state don't need the Eigen wrappers
+ and now use the raw data directly.
+* Controller publishing the state of a IMU sensor.
+
+0.4.0 (2013-06-26)
+------------------
diff --git a/imu_sensor_controller/CMakeLists.txt b/imu_sensor_controller/CMakeLists.txt
new file mode 100644
index 0000000..2f44b7d
--- /dev/null
+++ b/imu_sensor_controller/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(imu_sensor_controller)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS controller_interface hardware_interface sensor_msgs realtime_tools)
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS controller_interface hardware_interface sensor_msgs realtime_tools
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ )
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+ src/imu_sensor_controller.cpp include/imu_sensor_controller/imu_sensor_controller.h)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES imu_sensor_plugin.xml
+ imu_sensor_controller.yaml
+ imu_sensor_controller.launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/imu_sensor_controller/imu_sensor_controller.launch b/imu_sensor_controller/imu_sensor_controller.launch
new file mode 100644
index 0000000..c29e66b
--- /dev/null
+++ b/imu_sensor_controller/imu_sensor_controller.launch
@@ -0,0 +1,7 @@
+<launch>
+ <!-- load configuration -->
+ <rosparam command="load" file="$(find imu_sensor_controller)/imu_sensor_controller.yaml" />
+
+ <!-- spawn controller -->
+ <node name="imu_sensor_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="imu_sensor_controller" />
+</launch>
\ No newline at end of file
diff --git a/imu_sensor_controller/imu_sensor_controller.yaml b/imu_sensor_controller/imu_sensor_controller.yaml
new file mode 100644
index 0000000..ae6c732
--- /dev/null
+++ b/imu_sensor_controller/imu_sensor_controller.yaml
@@ -0,0 +1,3 @@
+imu_sensor_controller:
+ type: imu_sensor_controller/ImuSensorController
+ publish_rate: 50
\ No newline at end of file
diff --git a/imu_sensor_controller/imu_sensor_plugin.xml b/imu_sensor_controller/imu_sensor_plugin.xml
new file mode 100644
index 0000000..8444de4
--- /dev/null
+++ b/imu_sensor_controller/imu_sensor_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libimu_sensor_controller">
+ <class name="imu_sensor_controller/ImuSensorController" type="imu_sensor_controller::ImuSensorController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ This controller publishes the readings of all available IMU sensors as sensor_msgs/Imu message. The controller expects a ImuSensorInterface type of hardware interface.
+ </description>
+ </class>
+</library>
\ No newline at end of file
diff --git a/imu_sensor_controller/include/imu_sensor_controller/imu_sensor_controller.h b/imu_sensor_controller/include/imu_sensor_controller/imu_sensor_controller.h
new file mode 100644
index 0000000..6f8ad48
--- /dev/null
+++ b/imu_sensor_controller/include/imu_sensor_controller/imu_sensor_controller.h
@@ -0,0 +1,65 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+// 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 S.L. 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: Adolfo Rodriguez Tsouroukdissian
+
+#ifndef IMU_SENSOR_CONTROLLER_IMU_SENSOR_CONTROLLER_H
+#define IMU_SENSOR_CONTROLLER_IMU_SENSOR_CONTROLLER_H
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <sensor_msgs/Imu.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <boost/shared_ptr.hpp>
+
+namespace imu_sensor_controller
+{
+
+// this controller gets access to the ImuSensorInterface
+class ImuSensorController: public controller_interface::Controller<hardware_interface::ImuSensorInterface>
+{
+public:
+ ImuSensorController(){}
+
+ virtual bool init(hardware_interface::ImuSensorInterface* hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh);
+ virtual void starting(const ros::Time& time);
+ virtual void update(const ros::Time& time, const ros::Duration& /*period*/);
+ virtual void stopping(const ros::Time& /*time*/);
+
+private:
+ std::vector<hardware_interface::ImuSensorHandle> sensors_;
+ typedef boost::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::Imu> > RtPublisherPtr;
+ std::vector<RtPublisherPtr> realtime_pubs_;
+ std::vector<ros::Time> last_publish_times_;
+ double publish_rate_;
+};
+
+}
+
+#endif
diff --git a/imu_sensor_controller/package.xml b/imu_sensor_controller/package.xml
new file mode 100644
index 0000000..170864e
--- /dev/null
+++ b/imu_sensor_controller/package.xml
@@ -0,0 +1,34 @@
+<package>
+ <name>imu_sensor_controller</name>
+ <version>0.10.0</version>
+ <description>Controller to publish state of IMU sensors</description>
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Adolfo Rodriguez Tsouroukdissian</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>pluginlib</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>pluginlib</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/imu_sensor_plugin.xml"/>
+ </export>
+</package>
diff --git a/imu_sensor_controller/src/imu_sensor_controller.cpp b/imu_sensor_controller/src/imu_sensor_controller.cpp
new file mode 100644
index 0000000..9986749
--- /dev/null
+++ b/imu_sensor_controller/src/imu_sensor_controller.cpp
@@ -0,0 +1,195 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+// 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 S.L. 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: Adolfo Rodriguez Tsouroukdissian
+
+
+#include "imu_sensor_controller/imu_sensor_controller.h"
+
+
+
+namespace imu_sensor_controller
+{
+
+ bool ImuSensorController::init(hardware_interface::ImuSensorInterface* hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh)
+ {
+ // get all joint states from the hardware interface
+ const std::vector<std::string>& sensor_names = hw->getNames();
+ for (unsigned i=0; i<sensor_names.size(); i++)
+ ROS_DEBUG("Got sensor %s", sensor_names[i].c_str());
+
+ // get publishing period
+ if (!controller_nh.getParam("publish_rate", publish_rate_)){
+ ROS_ERROR("Parameter 'publish_rate' not set");
+ return false;
+ }
+
+ for (unsigned i=0; i<sensor_names.size(); i++){
+ // sensor handle
+ sensors_.push_back(hw->getHandle(sensor_names[i]));
+
+ // realtime publisher
+ RtPublisherPtr rt_pub(new realtime_tools::RealtimePublisher<sensor_msgs::Imu>(root_nh, sensor_names[i], 4));
+ realtime_pubs_.push_back(rt_pub);
+ }
+
+ // Last published times
+ last_publish_times_.resize(sensor_names.size());
+ return true;
+ }
+
+ void ImuSensorController::starting(const ros::Time& time)
+ {
+ // initialize time
+ for (unsigned i=0; i<last_publish_times_.size(); i++){
+ last_publish_times_[i] = time;
+ }
+ }
+
+ void ImuSensorController::update(const ros::Time& time, const ros::Duration& /*period*/)
+ {
+ using namespace hardware_interface;
+
+ // limit rate of publishing
+ for (unsigned i=0; i<realtime_pubs_.size(); i++){
+ if (publish_rate_ > 0.0 && last_publish_times_[i] + ros::Duration(1.0/publish_rate_) < time){
+ // try to publish
+ if (realtime_pubs_[i]->trylock()){
+ // we're actually publishing, so increment time
+ last_publish_times_[i] = last_publish_times_[i] + ros::Duration(1.0/publish_rate_);
+
+ // populate message
+ realtime_pubs_[i]->msg_.header.stamp = time;
+ realtime_pubs_[i]->msg_.header.frame_id = sensors_[i].getFrameId();
+
+ // Orientation
+ if (sensors_[i].getOrientation())
+ {
+ realtime_pubs_[i]->msg_.orientation.x = sensors_[i].getOrientation()[0];
+ realtime_pubs_[i]->msg_.orientation.y = sensors_[i].getOrientation()[1];
+ realtime_pubs_[i]->msg_.orientation.z = sensors_[i].getOrientation()[2];
+ realtime_pubs_[i]->msg_.orientation.w = sensors_[i].getOrientation()[3];
+ }
+
+ // Orientation covariance
+ if (sensors_[i].getOrientationCovariance())
+ {
+ for (unsigned j=0; j<realtime_pubs_[i]->msg_.orientation_covariance.size(); ++j){
+ realtime_pubs_[i]->msg_.orientation_covariance[j] = sensors_[i].getOrientationCovariance()[j];
+ }
+ }
+ else
+ {
+ if (sensors_[i].getOrientation())
+ {
+ // Orientation available
+ for (unsigned j=0; j<realtime_pubs_[i]->msg_.orientation_covariance.size(); ++j){
+ realtime_pubs_[i]->msg_.orientation_covariance[j] = 0.0;
+ }
+ }
+ else
+ {
+ // No orientation available
+ realtime_pubs_[i]->msg_.orientation_covariance[0] = -1.0;
+ }
+ }
+
+ // Angular velocity
+ if (sensors_[i].getAngularVelocity())
+ {
+ realtime_pubs_[i]->msg_.angular_velocity.x = sensors_[i].getAngularVelocity()[0];
+ realtime_pubs_[i]->msg_.angular_velocity.y = sensors_[i].getAngularVelocity()[1];
+ realtime_pubs_[i]->msg_.angular_velocity.z = sensors_[i].getAngularVelocity()[2];
+ }
+
+ // Angular velocity covariance
+ if (sensors_[i].getAngularVelocityCovariance())
+ {
+ for (unsigned j=0; j<realtime_pubs_[i]->msg_.angular_velocity_covariance.size(); ++j){
+ realtime_pubs_[i]->msg_.angular_velocity_covariance[j] = sensors_[i].getAngularVelocityCovariance()[j];
+ }
+ }
+ else
+ {
+ if (sensors_[i].getAngularVelocity())
+ {
+ // Angular velocity available
+ for (unsigned j=0; j<realtime_pubs_[i]->msg_.angular_velocity_covariance.size(); ++j){
+ realtime_pubs_[i]->msg_.angular_velocity_covariance[j] = 0.0;
+ }
+ }
+ else
+ {
+ // No angular velocity available
+ realtime_pubs_[i]->msg_.angular_velocity_covariance[0] = -1.0;
+ }
+ }
+
+ // Linear acceleration
+ if (sensors_[i].getLinearAcceleration())
+ {
+ realtime_pubs_[i]->msg_.linear_acceleration.x = sensors_[i].getLinearAcceleration()[0];
+ realtime_pubs_[i]->msg_.linear_acceleration.y = sensors_[i].getLinearAcceleration()[1];
+ realtime_pubs_[i]->msg_.linear_acceleration.z = sensors_[i].getLinearAcceleration()[2];
+ }
+
+ // Linear acceleration covariance
+ if (sensors_[i].getLinearAccelerationCovariance())
+ {
+ for (unsigned j=0; j<realtime_pubs_[i]->msg_.linear_acceleration_covariance.size(); ++j){
+ realtime_pubs_[i]->msg_.linear_acceleration_covariance[j] = sensors_[i].getLinearAccelerationCovariance()[j];
+ }
+ }
+ else
+ {
+ if (sensors_[i].getLinearAcceleration())
+ {
+ // Linear acceleration available
+ for (unsigned j=0; j<realtime_pubs_[i]->msg_.linear_acceleration_covariance.size(); ++j){
+ realtime_pubs_[i]->msg_.linear_acceleration_covariance[j] = 0.0;
+ }
+ }
+ else
+ {
+ // No linear acceleration available
+ realtime_pubs_[i]->msg_.linear_acceleration_covariance[0] = -1.0;
+ }
+ }
+
+ realtime_pubs_[i]->unlockAndPublish();
+ }
+ }
+ }
+ }
+
+ void ImuSensorController::stopping(const ros::Time& /*time*/)
+ {}
+
+}
+
+PLUGINLIB_EXPORT_CLASS(imu_sensor_controller::ImuSensorController, controller_interface::ControllerBase)
diff --git a/joint_state_controller/.gitignore b/joint_state_controller/.gitignore
new file mode 100644
index 0000000..61fd441
--- /dev/null
+++ b/joint_state_controller/.gitignore
@@ -0,0 +1,2 @@
+build
+lib
\ No newline at end of file
diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst
new file mode 100644
index 0000000..c860679
--- /dev/null
+++ b/joint_state_controller/CHANGELOG.rst
@@ -0,0 +1,105 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package joint_state_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+* Address -Wunused-parameter warnings
+* Add extra joints support
+ Allow to optionally specify a set of extra joints for state publishing that
+ are not contained in the JointStateInterface associated to the controller.
+ The state of these joints can be specified via ROS parameters, and remains
+ constant over time.
+* Add test suite
+* Migrate to package format2
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.2 (2015-05-04)
+------------------
+
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Buildsystem fixes
+* Contributors: Dave Coleman
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Add missing controller resources to install target
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+* Link shared libraries to catkin libraries
+ GCC is quite lenient with missing symbols on shared libraries and
+ doesn't event output any warning about it.
+ When building with other compilers, missing symbols result in build
+ errors.
+* Install default config files
+* Contributors: Paul Mathieu
+
+0.5.4 (2013-09-30)
+------------------
+* Silence cppcheck warning on unit'ed variables.
+
+0.5.3 (2013-09-04)
+------------------
+* Removed last manifest.xml
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+* Added joint limit enforcement for controller set point command
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+* Merged
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+* Restore "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors""
+ This reverts commit 0862ad93696b0d736b565cd65ea36690dde0eaa7.
+* Adding install targets for plugin xml files
+* Revert "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors"
+ This reverts commit 2314b8b434e35dc9c1c298140118a004e00febd8.
+* Fix package URL in package.xml
+
+0.4.0 (2013-06-26)
+------------------
+* Version 0.4.0
+* Removed PR2 references and renamed github repo in docs
+* Fix package URL in package.xml
+* Fixed PLUGINLIB_DECLARE_CLASS depreacated errors
+* Propagate API changes in hardware_interface.
+* adding install targets
+* adding switches for hybrid buildsystem
+* adding back more manifests and makefiles
+* Fix package URL.
+* bumping version
+* adding package.xml files
+* Catkinizing. Building, but could still be cleaned up
+* use new root nodehandle to publish joint states in the namespace of the controller manager. This fixes a but when pushing the controller manager in a namespace, and keeps the same behavior when the controller manager is not in a namespace
+* Add .gitignore files on a per-package basis.
+* Add missing include guard.
+* Change tab indentation for spaces.
+* port to new api with time and duration
+* moved package with joint state controller
diff --git a/joint_state_controller/CMakeLists.txt b/joint_state_controller/CMakeLists.txt
new file mode 100644
index 0000000..96d6171
--- /dev/null
+++ b/joint_state_controller/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(joint_state_controller)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS realtime_tools roscpp hardware_interface pluginlib controller_interface sensor_msgs)
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS realtime_tools roscpp hardware_interface pluginlib controller_interface sensor_msgs
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ )
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+add_library(${PROJECT_NAME} src/joint_state_controller.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(rostest REQUIRED)
+
+ add_rostest_gtest(joint_state_controller_test
+ test/joint_state_controller.test
+ test/joint_state_controller_test.cpp)
+ target_link_libraries(joint_state_controller_test ${PROJECT_NAME})
+endif()
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES joint_state_plugin.xml
+ joint_state_controller.yaml
+ joint_state_controller.launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/joint_state_controller/include/joint_state_controller/joint_state_controller.h b/joint_state_controller/include/joint_state_controller/joint_state_controller.h
new file mode 100644
index 0000000..9075791
--- /dev/null
+++ b/joint_state_controller/include/joint_state_controller/joint_state_controller.h
@@ -0,0 +1,103 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// 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 hiDOF, 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: Wim Meeussen
+ */
+
+#ifndef JOINT_STATE_CONTROLLER_JOINT_STATE_CONTROLLER_H
+#define JOINT_STATE_CONTROLLER_JOINT_STATE_CONTROLLER_H
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <sensor_msgs/JointState.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <boost/shared_ptr.hpp>
+
+namespace joint_state_controller
+{
+
+/**
+ * \brief Controller that publishes the state of all joints in a robot.
+ *
+ * This controller publishes the state of all resources registered to a \c hardware_interface::JointStateInterface to a
+ * topic of type \c sensor_msgs/JointState. The following is a basic configuration of the controller.
+ *
+ * \code
+ * joint_state_controller:
+ * type: joint_state_controller/JointStateController
+ * publish_rate: 50
+ * \endcode
+ *
+ * It's possible to optionally specify a set of extra joints not contained in a
+ * \c hardware_interface::JointStateInterface with custom (and static) default values. The following is an example
+ * configuration specifying extra joints.
+ *
+ * \code
+ * joint_state_controller:
+ * type: joint_state_controller/JointStateController
+ * publish_rate: 50
+ * extra_joints:
+ * - name: 'extra1'
+ * position: 10.0
+ * velocity: 20.0
+ * effort: 30.0
+ *
+ * - name: 'extra2'
+ * position: -10.0
+ *
+ * - name: 'extra3'
+ * \endcode
+ *
+ * An unspecified position, velocity or acceleration defaults to zero.
+ */
+class JointStateController: public controller_interface::Controller<hardware_interface::JointStateInterface>
+{
+public:
+ JointStateController() : publish_rate_(0.0) {}
+
+ virtual bool init(hardware_interface::JointStateInterface* hw,
+ ros::NodeHandle& root_nh,
+ ros::NodeHandle& controller_nh);
+ virtual void starting(const ros::Time& time);
+ virtual void update(const ros::Time& time, const ros::Duration& /*period*/);
+ virtual void stopping(const ros::Time& /*time*/);
+
+private:
+ std::vector<hardware_interface::JointStateHandle> joint_state_;
+ boost::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > realtime_pub_;
+ ros::Time last_publish_time_;
+ double publish_rate_;
+ unsigned int num_hw_joints_; ///< Number of joints present in the JointStateInterface, excluding extra joints
+
+ void addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg);
+};
+
+}
+
+#endif
diff --git a/joint_state_controller/joint_state_controller.launch b/joint_state_controller/joint_state_controller.launch
new file mode 100644
index 0000000..dc5cbb6
--- /dev/null
+++ b/joint_state_controller/joint_state_controller.launch
@@ -0,0 +1,7 @@
+<launch>
+ <!-- load configuration -->
+ <rosparam command="load" file="$(find joint_state_controller)/joint_state_controller.yaml" />
+
+ <!-- spawn controller -->
+ <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="joint_state_controller" />
+</launch>
\ No newline at end of file
diff --git a/joint_state_controller/joint_state_controller.yaml b/joint_state_controller/joint_state_controller.yaml
new file mode 100644
index 0000000..d9fe32c
--- /dev/null
+++ b/joint_state_controller/joint_state_controller.yaml
@@ -0,0 +1,3 @@
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
\ No newline at end of file
diff --git a/joint_state_controller/joint_state_plugin.xml b/joint_state_controller/joint_state_plugin.xml
new file mode 100644
index 0000000..8276a91
--- /dev/null
+++ b/joint_state_controller/joint_state_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libjoint_state_controller">
+ <class name="joint_state_controller/JointStateController" type="joint_state_controller::JointStateController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The joint state controller is publishes the current joint state as a sensor_msgs/JointState message. The joint state controller expects a JointStateInterface type of hardware interface.
+ </description>
+ </class>
+</library>
\ No newline at end of file
diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml
new file mode 100644
index 0000000..fb2094f
--- /dev/null
+++ b/joint_state_controller/package.xml
@@ -0,0 +1,29 @@
+<package format="2">
+ <name>joint_state_controller</name>
+ <version>0.10.0</version>
+ <description>Controller to publish joint state</description>
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Wim Meeussen</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <depend>controller_interface</depend>
+ <depend>hardware_interface</depend>
+ <depend>pluginlib</depend>
+ <depend>sensor_msgs</depend>
+ <depend>realtime_tools</depend>
+ <depend>roscpp</depend>
+
+ <test_depend>rostest</test_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/joint_state_plugin.xml"/>
+ </export>
+</package>
diff --git a/joint_state_controller/rosdoc.yaml b/joint_state_controller/rosdoc.yaml
new file mode 100644
index 0000000..78d926a
--- /dev/null
+++ b/joint_state_controller/rosdoc.yaml
@@ -0,0 +1,5 @@
+- builder: doxygen
+ name: C++ API
+ output_dir: c++
+ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
+ exclude_patterns: 'test'
\ No newline at end of file
diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp
new file mode 100644
index 0000000..8a4856b
--- /dev/null
+++ b/joint_state_controller/src/joint_state_controller.cpp
@@ -0,0 +1,180 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// 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 hiDOF 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: Wim Meeussen
+ */
+
+#include <algorithm>
+#include <cstddef>
+
+#include "joint_state_controller/joint_state_controller.h"
+
+namespace joint_state_controller
+{
+
+ bool JointStateController::init(hardware_interface::JointStateInterface* hw,
+ ros::NodeHandle& root_nh,
+ ros::NodeHandle& controller_nh)
+ {
+ // get all joint names from the hardware interface
+ const std::vector<std::string>& joint_names = hw->getNames();
+ num_hw_joints_ = joint_names.size();
+ for (unsigned i=0; i<num_hw_joints_; i++)
+ ROS_DEBUG("Got joint %s", joint_names[i].c_str());
+
+ // get publishing period
+ if (!controller_nh.getParam("publish_rate", publish_rate_)){
+ ROS_ERROR("Parameter 'publish_rate' not set");
+ return false;
+ }
+
+ // realtime publisher
+ realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", 4));
+
+ // get joints and allocate message
+ for (unsigned i=0; i<num_hw_joints_; i++){
+ joint_state_.push_back(hw->getHandle(joint_names[i]));
+ realtime_pub_->msg_.name.push_back(joint_names[i]);
+ realtime_pub_->msg_.position.push_back(0.0);
+ realtime_pub_->msg_.velocity.push_back(0.0);
+ realtime_pub_->msg_.effort.push_back(0.0);
+ }
+ addExtraJoints(controller_nh, realtime_pub_->msg_);
+
+ return true;
+ }
+
+ void JointStateController::starting(const ros::Time& time)
+ {
+ // initialize time
+ last_publish_time_ = time;
+ }
+
+ void JointStateController::update(const ros::Time& time, const ros::Duration& /*period*/)
+ {
+ // limit rate of publishing
+ if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){
+
+ // try to publish
+ if (realtime_pub_->trylock()){
+ // we're actually publishing, so increment time
+ last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_);
+
+ // populate joint state message:
+ // - fill only joints that are present in the JointStateInterface, i.e. indices [0, num_hw_joints_)
+ // - leave unchanged extra joints, which have static values, i.e. indices from num_hw_joints_ onwards
+ realtime_pub_->msg_.header.stamp = time;
+ for (unsigned i=0; i<num_hw_joints_; i++){
+ realtime_pub_->msg_.position[i] = joint_state_[i].getPosition();
+ realtime_pub_->msg_.velocity[i] = joint_state_[i].getVelocity();
+ realtime_pub_->msg_.effort[i] = joint_state_[i].getEffort();
+ }
+ realtime_pub_->unlockAndPublish();
+ }
+ }
+ }
+
+ void JointStateController::stopping(const ros::Time& /*time*/)
+ {}
+
+ void JointStateController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
+ {
+
+ // Preconditions
+ XmlRpc::XmlRpcValue list;
+ if (!nh.getParam("extra_joints", list))
+ {
+ ROS_DEBUG("No extra joints specification found.");
+ return;
+ }
+
+ if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR("Extra joints specification is not an array. Ignoring.");
+ return;
+ }
+
+ for(std::size_t i = 0; i < list.size(); ++i)
+ {
+ if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
+ {
+ ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
+ "'. Ignoring.");
+ continue;
+ }
+
+ if (!list[i].hasMember("name"))
+ {
+ ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
+ continue;
+ }
+
+ const std::string name = list[i]["name"];
+ if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
+ {
+ ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
+ continue;
+ }
+
+ const bool has_pos = list[i].hasMember("position");
+ const bool has_vel = list[i].hasMember("velocity");
+ const bool has_eff = list[i].hasMember("effort");
+
+ const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
+ if (has_pos && list[i]["position"].getType() != typeDouble)
+ {
+ ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
+ continue;
+ }
+ if (has_vel && list[i]["velocity"].getType() != typeDouble)
+ {
+ ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
+ continue;
+ }
+ if (has_eff && list[i]["effort"].getType() != typeDouble)
+ {
+ ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
+ continue;
+ }
+
+ // State of extra joint
+ const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
+ const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
+ const double eff = has_eff ? static_cast<double>(list[i]["effort"]) : 0.0;
+
+ // Add extra joints to message
+ msg.name.push_back(name);
+ msg.position.push_back(pos);
+ msg.velocity.push_back(vel);
+ msg.effort.push_back(eff);
+ }
+ }
+
+}
+
+PLUGINLIB_EXPORT_CLASS( joint_state_controller::JointStateController, controller_interface::ControllerBase)
diff --git a/joint_state_controller/test/joint_state_controller.test b/joint_state_controller/test/joint_state_controller.test
new file mode 100644
index 0000000..e9e8e78
--- /dev/null
+++ b/joint_state_controller/test/joint_state_controller.test
@@ -0,0 +1,12 @@
+<launch>
+ <rosparam command="load" ns="test_ok"
+ file="$(find joint_state_controller)/test/joint_state_controller_ok.yaml" />
+ <rosparam command="load" ns="test_ko"
+ file="$(find joint_state_controller)/test/joint_state_controller_ko.yaml" />
+ <rosparam command="load" ns="test_extra_joints_ok"
+ file="$(find joint_state_controller)/test/joint_state_controller_extra_joints_ok.yaml" />
+ <rosparam command="load" ns="test_extra_joints_ko"
+ file="$(find joint_state_controller)/test/joint_state_controller_extra_joints_ko.yaml" />
+
+ <test test-name="joint_state_controller_test" pkg="joint_state_controller" type="joint_state_controller_test"/>
+</launch>
diff --git a/joint_state_controller/test/joint_state_controller_extra_joints_ko.yaml b/joint_state_controller/test/joint_state_controller_extra_joints_ko.yaml
new file mode 100644
index 0000000..195797b
--- /dev/null
+++ b/joint_state_controller/test/joint_state_controller_extra_joints_ko.yaml
@@ -0,0 +1,19 @@
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 10
+ extra_joints:
+ - name: 'joint1' # should be ignored, as already exists
+ position: 10.0
+ velocity: 20.0
+ effort: 30.0
+
+ - name: 'bad_position'
+ position: 'zero'
+
+ - name: 'bad_velocity'
+ velocity: 'zero'
+
+ - name: 'bad_effort'
+ effort: 'zero'
+
+ - nickname: 'no_name'
diff --git a/joint_state_controller/test/joint_state_controller_extra_joints_ok.yaml b/joint_state_controller/test/joint_state_controller_extra_joints_ok.yaml
new file mode 100644
index 0000000..f1ca396
--- /dev/null
+++ b/joint_state_controller/test/joint_state_controller_extra_joints_ok.yaml
@@ -0,0 +1,13 @@
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 10
+ extra_joints:
+ - name: 'extra1'
+ position: 10.0
+ velocity: 20.0
+ effort: 30.0
+
+ - name: 'extra2'
+ position: -10.0
+
+ - name: 'extra3'
diff --git a/joint_state_controller/test/joint_state_controller_ko.yaml b/joint_state_controller/test/joint_state_controller_ko.yaml
new file mode 100644
index 0000000..1f8f43b
--- /dev/null
+++ b/joint_state_controller/test/joint_state_controller_ko.yaml
@@ -0,0 +1,3 @@
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 0
diff --git a/joint_state_controller/test/joint_state_controller_ok.yaml b/joint_state_controller/test/joint_state_controller_ok.yaml
new file mode 100644
index 0000000..696979e
--- /dev/null
+++ b/joint_state_controller/test/joint_state_controller_ok.yaml
@@ -0,0 +1,3 @@
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 10
\ No newline at end of file
diff --git a/joint_state_controller/test/joint_state_controller_test.cpp b/joint_state_controller/test/joint_state_controller_test.cpp
new file mode 100644
index 0000000..9e76e76
--- /dev/null
+++ b/joint_state_controller/test/joint_state_controller_test.cpp
@@ -0,0 +1,328 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2015, 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 S.L. 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.
+//////////////////////////////////////////////////////////////////////////////
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+
+#include <hardware_interface/joint_state_interface.h>
+#include <joint_state_controller/joint_state_controller.h>
+
+using namespace joint_state_controller;
+
+class JointStateControllerTest : public ::testing::Test
+{
+public:
+ JointStateControllerTest()
+ : root_nh_(ros::NodeHandle()),
+ controller_nh_("test_ok/joint_state_controller")
+ {
+ // Intialize raw joint state data
+ names_.push_back("joint1"); names_.push_back("joint2");
+ pos_[0] = 0.0; pos_[1] = 0.0;
+ vel_[0] = 0.0; vel_[1] = 0.0;
+ eff_[0] = 0.0; eff_[1] = 0.0;
+
+ // Setup the joint state interface
+ hardware_interface::JointStateHandle state_handle_1(names_[0], &pos_[0], &vel_[0], &eff_[0]);
+ js_iface_.registerHandle(state_handle_1);
+
+ hardware_interface::JointStateHandle state_handle_2(names_[1], &pos_[1], &vel_[1], &eff_[1]);
+ js_iface_.registerHandle(state_handle_2);
+
+ // Initialize ROS interfaces
+ sub_ = root_nh_.subscribe<sensor_msgs::JointState>("joint_states",
+ 1,
+ &JointStateControllerTest::jointStateCb,
+ this);
+ }
+
+protected:
+ ros::NodeHandle root_nh_;
+ ros::NodeHandle controller_nh_;
+ ros::Subscriber sub_;
+ hardware_interface::JointStateInterface js_iface_;
+
+ // Raw joint state data
+ std::vector<std::string> names_;
+ double pos_[2];
+ double vel_[2];
+ double eff_[2];
+
+ // Received joint state messages counter
+ int rec_msgs_;
+
+ // Last received joint state message
+ sensor_msgs::JointState last_msg_;
+
+ void jointStateCb(const sensor_msgs::JointStateConstPtr& msg)
+ {
+ last_msg_ = *msg;
+ ++rec_msgs_;
+ }
+};
+
+TEST_F(JointStateControllerTest, initOk)
+{
+ JointStateController jsc;
+ EXPECT_TRUE(jsc.init(&js_iface_, root_nh_, controller_nh_));
+}
+
+TEST_F(JointStateControllerTest, initKo)
+{
+ JointStateController jsc;
+ ros::NodeHandle bad_controller_nh("no_period_namespace");
+ EXPECT_FALSE(jsc.init(&js_iface_, root_nh_, bad_controller_nh));
+}
+
+TEST_F(JointStateControllerTest, publishOk)
+{
+ JointStateController jsc;
+ EXPECT_TRUE(jsc.init(&js_iface_, root_nh_, controller_nh_));
+
+ int pub_rate;
+ ASSERT_TRUE(controller_nh_.getParam("publish_rate", pub_rate));
+
+ rec_msgs_ = 0;
+ const int test_duration = 1; // seconds
+ const int loop_freq = 2 * pub_rate; // faster than controller publish rate
+ ros::Rate loop_rate(static_cast<double>(loop_freq));
+ const ros::Time start_time = ros::Time::now();
+ jsc.starting(start_time);
+
+ while (true)
+ {
+ ros::Time now = ros::Time::now();
+ jsc.update(now, ros::Duration()); // NOTE: Second parameter is unused by implementation
+ ros::spinOnce();
+ if (rec_msgs_ >= test_duration * pub_rate) break; // Objective reached
+ if (now - start_time > ros::Duration(2.0 * test_duration)) break; // Prevents unexpected infinite loops
+ loop_rate.sleep();
+ }
+ jsc.stopping(ros::Time::now());
+
+ // NOTE: Below we subtract the loop rate because the the published message gets picked up in the
+ // next iteration's spinOnce()
+ const ros::Duration real_test_duration = ros::Time::now() - start_time - loop_rate.expectedCycleTime();
+ const double real_pub_rate = static_cast<double>(rec_msgs_) / real_test_duration.toSec();
+
+ // The publish rate should be close to the nominal value
+ EXPECT_NEAR(real_pub_rate, pub_rate, 0.05 * pub_rate);
+}
+
+TEST_F(JointStateControllerTest, publishKo)
+{
+ JointStateController jsc;
+ ros::NodeHandle negative_rate_nh("test_ko/joint_state_controller");
+ EXPECT_TRUE(jsc.init(&js_iface_, root_nh_, negative_rate_nh));
+
+ // Check non-positive publish rate
+ int pub_rate;
+ ASSERT_TRUE(negative_rate_nh.getParam("publish_rate", pub_rate));
+ ASSERT_LE(pub_rate, 0);
+
+ rec_msgs_ = 0;
+ const ros::Duration test_duration(1.0);
+ ros::Rate loop_rate(10.0);
+ const ros::Time start_time = ros::Time::now();
+ while(ros::Time::now() - start_time < test_duration)
+ {
+ ros::Time now = ros::Time::now();
+ jsc.update(now, ros::Duration()); // NOTE: Second parameter is unused by implementation
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+ jsc.stopping(ros::Time::now());
+
+ // No messages should have been published
+ EXPECT_EQ(rec_msgs_, 0);
+}
+
+TEST_F(JointStateControllerTest, valuesOk)
+{
+ JointStateController jsc;
+ EXPECT_TRUE(jsc.init(&js_iface_, root_nh_, controller_nh_));
+
+ int pub_rate;
+ ASSERT_TRUE(controller_nh_.getParam("publish_rate", pub_rate));
+ ros::Duration period(1.0 / pub_rate);
+ jsc.starting(ros::Time::now());
+
+ pos_[0] = 1.0; pos_[1] = -1.0;
+ vel_[0] = 2.0; vel_[1] = -2.0;
+ eff_[0] = 3.0; eff_[1] = -3.0;
+
+ period.sleep();
+ jsc.update(ros::Time::now(), ros::Duration());
+ period.sleep(); ros::spinOnce(); // To trigger callback
+
+ // Check payload sizes
+ ASSERT_EQ(names_.size(), last_msg_.name.size());
+ ASSERT_EQ(last_msg_.name.size(), last_msg_.position.size());
+ ASSERT_EQ(last_msg_.name.size(), last_msg_.velocity.size());
+ ASSERT_EQ(last_msg_.name.size(), last_msg_.effort.size());
+
+ // Check payload values
+ typedef std::vector<std::string>::size_type SizeType;
+ typedef std::vector<std::string>::iterator Iterator;
+ for (SizeType raw_id = 0; raw_id < names_.size(); ++raw_id)
+ {
+ Iterator it = std::find(last_msg_.name.begin(), last_msg_.name.end(), names_[raw_id]);
+ ASSERT_NE(last_msg_.name.end(), it);
+ SizeType msg_id = std::distance(last_msg_.name.begin(), it);
+ EXPECT_EQ(pos_[raw_id], last_msg_.position[msg_id]);
+ EXPECT_EQ(vel_[raw_id], last_msg_.velocity[msg_id]);
+ EXPECT_EQ(eff_[raw_id], last_msg_.effort[msg_id]);
+ }
+ jsc.stopping(ros::Time::now());
+}
+
+TEST_F(JointStateControllerTest, extraJointsOk)
+{
+ JointStateController jsc;
+ ros::NodeHandle extra_joints_nh("test_extra_joints_ok/joint_state_controller");
+ EXPECT_TRUE(jsc.init(&js_iface_, root_nh_, extra_joints_nh));
+
+ XmlRpc::XmlRpcValue extra_joints;
+ ASSERT_TRUE(extra_joints_nh.getParam("extra_joints", extra_joints));
+ const int num_joints = names_.size() + extra_joints.size();
+
+ int pub_rate;
+ ASSERT_TRUE(controller_nh_.getParam("publish_rate", pub_rate));
+ ros::Duration period(1.0 / pub_rate);
+ jsc.starting(ros::Time::now());
+
+ pos_[0] = 1.0; pos_[1] = -1.0;
+ vel_[0] = 2.0; vel_[1] = -2.0;
+ eff_[0] = 3.0; eff_[1] = -3.0;
+
+ period.sleep();
+ jsc.update(ros::Time::now(), ros::Duration());
+ period.sleep(); ros::spinOnce(); // To trigger callback
+
+ // Check payload sizes
+ ASSERT_EQ(num_joints, last_msg_.name.size());
+ ASSERT_EQ(num_joints, last_msg_.position.size());
+ ASSERT_EQ(num_joints, last_msg_.velocity.size());
+ ASSERT_EQ(num_joints, last_msg_.effort.size());
+
+ // Check payload values: joint state interface resources
+ typedef std::vector<std::string>::size_type SizeType;
+ typedef std::vector<std::string>::iterator Iterator;
+ for (SizeType raw_id = 0; raw_id < names_.size(); ++raw_id)
+ {
+ Iterator it = std::find(last_msg_.name.begin(), last_msg_.name.end(), names_[raw_id]);
+ ASSERT_NE(last_msg_.name.end(), it);
+ SizeType msg_id = std::distance(last_msg_.name.begin(), it);
+ EXPECT_EQ(pos_[raw_id], last_msg_.position[msg_id]);
+ EXPECT_EQ(vel_[raw_id], last_msg_.velocity[msg_id]);
+ EXPECT_EQ(eff_[raw_id], last_msg_.effort[msg_id]);
+ }
+
+ // Check payload values: Extra joints
+ {
+ Iterator it = std::find(last_msg_.name.begin(), last_msg_.name.end(), "extra1");
+ ASSERT_NE(last_msg_.name.end(), it);
+ SizeType msg_id = std::distance(last_msg_.name.begin(), it);
+ EXPECT_EQ(10.0, last_msg_.position[msg_id]);
+ EXPECT_EQ(20.0, last_msg_.velocity[msg_id]);
+ EXPECT_EQ(30.0, last_msg_.effort[msg_id]);
+ }
+ {
+ Iterator it = std::find(last_msg_.name.begin(), last_msg_.name.end(), "extra2");
+ ASSERT_NE(last_msg_.name.end(), it);
+ SizeType msg_id = std::distance(last_msg_.name.begin(), it);
+ EXPECT_EQ(-10.0, last_msg_.position[msg_id]);
+ EXPECT_EQ( 0.0, last_msg_.velocity[msg_id]);
+ EXPECT_EQ( 0.0, last_msg_.effort[msg_id]);
+ }
+ {
+ Iterator it = std::find(last_msg_.name.begin(), last_msg_.name.end(), "extra3");
+ ASSERT_NE(last_msg_.name.end(), it);
+ SizeType msg_id = std::distance(last_msg_.name.begin(), it);
+ EXPECT_EQ(0.0, last_msg_.position[msg_id]);
+ EXPECT_EQ(0.0, last_msg_.velocity[msg_id]);
+ EXPECT_EQ(0.0, last_msg_.effort[msg_id]);
+ }
+
+ jsc.stopping(ros::Time::now());
+}
+
+TEST_F(JointStateControllerTest, extraJointsKo)
+{
+ JointStateController jsc;
+ ros::NodeHandle extra_joints_nh("test_extra_joints_ko/joint_state_controller");
+ EXPECT_TRUE(jsc.init(&js_iface_, root_nh_, extra_joints_nh));
+
+ int pub_rate;
+ ASSERT_TRUE(controller_nh_.getParam("publish_rate", pub_rate));
+ ros::Duration period(1.0 / pub_rate);
+ jsc.starting(ros::Time::now());
+
+ pos_[0] = 1.0; pos_[1] = -1.0;
+ vel_[0] = 2.0; vel_[1] = -2.0;
+ eff_[0] = 3.0; eff_[1] = -3.0;
+
+ period.sleep();
+ jsc.update(ros::Time::now(), ros::Duration());
+ period.sleep(); ros::spinOnce(); // To trigger callback
+
+ // Check payload sizes
+ ASSERT_EQ(names_.size(), last_msg_.name.size());
+ ASSERT_EQ(last_msg_.name.size(), last_msg_.position.size());
+ ASSERT_EQ(last_msg_.name.size(), last_msg_.velocity.size());
+ ASSERT_EQ(last_msg_.name.size(), last_msg_.effort.size());
+
+ // Check payload values: No extra joints should be present
+ typedef std::vector<std::string>::size_type SizeType;
+ typedef std::vector<std::string>::iterator Iterator;
+ for (SizeType raw_id = 0; raw_id < names_.size(); ++raw_id)
+ {
+ Iterator it = std::find(last_msg_.name.begin(), last_msg_.name.end(), names_[raw_id]);
+ ASSERT_NE(last_msg_.name.end(), it);
+ SizeType msg_id = std::distance(last_msg_.name.begin(), it);
+ EXPECT_EQ(pos_[raw_id], last_msg_.position[msg_id]);
+ EXPECT_EQ(vel_[raw_id], last_msg_.velocity[msg_id]);
+ EXPECT_EQ(eff_[raw_id], last_msg_.effort[msg_id]);
+ }
+ jsc.stopping(ros::Time::now());
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "joint_state_controller_test");
+
+ int ret = RUN_ALL_TESTS();
+ ros::shutdown();
+ return ret;
+}
diff --git a/joint_trajectory_controller/.gitignore b/joint_trajectory_controller/.gitignore
new file mode 100644
index 0000000..2d9f7b1
--- /dev/null
+++ b/joint_trajectory_controller/.gitignore
@@ -0,0 +1,5 @@
+bin
+build
+doc
+lib
+manifest.xml
diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst
new file mode 100644
index 0000000..0da45c8
--- /dev/null
+++ b/joint_trajectory_controller/CHANGELOG.rst
@@ -0,0 +1,139 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package joint_trajectory_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+* Add joint limits spec to rrbot test robot
+* Address -Wunused-parameter warnings
+* Reset to semantic zero in HardwareInterfaceAdapter for PositionJointInterface
+* Contributors: Adolfo Rodriguez Tsouroukdissian, ipa-fxm
+
+0.9.2 (2015-05-04)
+------------------
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* Check that waypoint times are strictly increasing before accepting a command
+* velocity_controllers::JointTrajectoryController: New plugin variant for
+ velocity-controlled joints
+* Buildsystem fixes
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Lukas Bulwahn, ipa-fxm, Dave Coleman
+
+0.8.1 (2014-07-11)
+------------------
+* joint_trajectory_controller: Critical targets declared before calling catkin_package
+* check for CATKIN_ENABLE_TESTING
+* Contributors: Jonathan Bohren, Lukas Bulwahn
+
+0.8.0 (2014-05-12)
+------------------
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+* Add support for an joint interfaces are not inherited from JointHandle.
+* Contributors: Igorec
+
+0.6.0 (2014-02-05)
+------------------
+* Merge pull request `#72 <https://github.com/ros-controls/ros_controllers/issues/72>`_ from pal-robotics/minor-maintenance
+ Minor maintenance
+* Default stop_trajectory_duration to zero. Refs `#73 <https://github.com/ros-controls/ros_controllers/issues/73>`_
+* Better logs when dropping traj points. Refs `#68 <https://github.com/ros-controls/ros_controllers/issues/68>`_.
+* Fix class member reorder warning in constructor.
+* Add missing headers to target files.
+* Action interface rejects empty goals. Fixes `#70 <https://github.com/ros-controls/ros_controllers/issues/70>`_.
+* Reorder how time and traj data are updated.
+ In the update method, fetching the currently executed trajectory should be done
+ before updating the time data to prevent a potential scenario in which there
+ is no trajectory defined for the current control cycle.
+* Work tolerance checking methods.
+ Until now we used the currently active goal handle for performing tolerance
+ checks. Using the goal handle stored in segments is more robust to unexpected
+ goal updates by the non-rt thread.
+* Refactor how the currrent trajectory is stored.
+ - Handle concurrency in the current trajectory between rt and non-rt threads
+ using the simpler RealtimeBox instead of the RealtimeBuffer, because our
+ usecase does not fit well the non-rt->writes / rt->reads semantics.
+ - As a consequence we no longer need to store the msg_trajectory member, but
+ only the hold_trajectory, which must still be preallocated.
+* Honor unspecified vel/acc in ROS message. Fix `#65 <https://github.com/ros-controls/ros_controllers/issues/65>`_.
+* Fixes per Adolfo
+* Added verbose flag
+* Fixing realtime issues
+* Merge branch 'hydro-devel' into joint_trajectory_tweaks
+* Tweaked error messages
+* Added more debug info
+* Fix for microsecond delay that caused header time=0 (now) to start too late
+* Reworded debug message
+* Image update.
+* Update README.md
+ Factor out user documentation to the ROS wiki.
+* Merge branch 'hydro-devel' of https://github.com/willowgarage/ros_controllers into hydro-devel
+* Rename hold_trajectory_duration
+ - hold_trajectory_duration -> stop_trajectory_duration for more clarity.
+ - During Hydro, hold_trajectory_duration will still work, giving a deprecation
+ warning.
+* Add basic description in package.xml.
+* Add images used in the ROS wiki doc.
+* Added better debug info
+* Throttled debug output
+* Added more debug and error information
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Dave Coleman
+
+0.5.4 (2013-09-30)
+------------------
+* Added install rules for plugin.xml
+* Remove PID sign flip.
+ This is now done in the state error computation.
+* Merge pull request `#45 <https://github.com/davetcoleman/ros_controllers/issues/45>`_ from ros-controls/effort_fixes
+ Added check for ~/robot_description and fixed hardware interface abstraction bug
+* Flip state error sign.
+* PID sign was wrong
+* Added check for ~/robot_description and fixed hardware interface abstraction bug
+* Update README.md
+* Create README.md
+* Fix license header string for some files.
+* Less verbose init logging.
+ Statement detailing controller joint count, as well as segment and hardware
+ interface types moved from INFO to DEBUG severity.
+
+0.5.3 (2013-09-04)
+------------------
+* joint_trajectory_controller: New package implementing a controller for executing joint-space trajectories on a
+ set of joints.
+
+ * ROS API
+
+ * Commands: FollowJointTrajectory action and trajectory_msgs::JointTrajectory topic.
+ * Current controller state is available in a control_msgs::JointTrajectoryControllerState topic.
+ * Controller state at any future time can be queried through a control_msgs::JointTrajectoryControllerState
+ service.
+
+ * Trajectory segment type
+
+ * Controller is templated on the segment type.
+ * Multi-dof quintic spline segment implementation provided by default.
+
+ * Hardware interface type
+
+ * Controller is templated on the hardware interface type.
+ * Position and effort control joint interfaces provided by default.
+
+ * Other
+
+ * Realtime-safe.
+ * Proper handling of wrapping (continuous) joints.
+ * Discontinuous system clock changes do not cause discontinuities in the execution of already queued
+ trajectory segments.
\ No newline at end of file
diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
new file mode 100644
index 0000000..985e042
--- /dev/null
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(joint_trajectory_controller)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin
+ REQUIRED COMPONENTS
+ actionlib
+ angles
+ cmake_modules
+ roscpp
+ urdf
+ control_toolbox
+ controller_interface
+ hardware_interface
+ realtime_tools
+ control_msgs
+ trajectory_msgs
+ controller_manager
+ xacro
+)
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS
+ actionlib
+ angles
+ roscpp
+ urdf
+ control_toolbox
+ controller_interface
+ hardware_interface
+ realtime_tools
+ control_msgs
+ trajectory_msgs
+ controller_manager
+ xacro
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} src/joint_trajectory_controller.cpp
+ include/joint_trajectory_controller/hardware_interface_adapter.h
+ include/joint_trajectory_controller/init_joint_trajectory.h
+ include/joint_trajectory_controller/joint_trajectory_controller.h
+ include/joint_trajectory_controller/joint_trajectory_controller_impl.h
+ include/joint_trajectory_controller/joint_trajectory_msg_utils.h
+ include/joint_trajectory_controller/joint_trajectory_segment.h
+ include/joint_trajectory_controller/tolerances.h
+ include/trajectory_interface/trajectory_interface.h
+ include/trajectory_interface/quintic_spline_segment.h
+ include/trajectory_interface/pos_vel_acc_state.h)
+
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(rostest REQUIRED)
+
+ catkin_add_gtest(quintic_spline_segment_test test/quintic_spline_segment_test.cpp)
+ target_link_libraries(quintic_spline_segment_test ${catkin_LIBRARIES})
+
+ catkin_add_gtest(trajectory_interface_test test/trajectory_interface_test.cpp)
+ target_link_libraries(trajectory_interface_test ${catkin_LIBRARIES})
+
+ catkin_add_gtest(joint_trajectory_segment_test test/joint_trajectory_segment_test.cpp)
+ target_link_libraries(joint_trajectory_segment_test ${catkin_LIBRARIES})
+
+ catkin_add_gtest(joint_trajectory_msg_utils_test test/joint_trajectory_msg_utils_test.cpp)
+ target_link_libraries(joint_trajectory_msg_utils_test ${catkin_LIBRARIES})
+
+ catkin_add_gtest(init_joint_trajectory_test test/init_joint_trajectory_test.cpp)
+ target_link_libraries(init_joint_trajectory_test ${catkin_LIBRARIES})
+
+ add_rostest_gtest(tolerances_test
+ test/tolerances.test
+ test/tolerances_test.cpp)
+ target_link_libraries(tolerances_test ${catkin_LIBRARIES})
+
+ add_executable(rrbot test/rrbot.cpp)
+ target_link_libraries(rrbot ${catkin_LIBRARIES})
+
+ add_dependencies(tests rrbot)
+ add_dependencies(tests ${PROJECT_NAME})
+
+ add_rostest_gtest(joint_trajectory_controller_test
+ test/joint_trajectory_controller.test
+ test/joint_trajectory_controller_test.cpp)
+ target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES})
+endif()
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(DIRECTORY include/trajectory_interface/
+ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/trajectory_interface/)
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES ros_control_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# TODO: Install test resource files as well?
diff --git a/joint_trajectory_controller/README.md b/joint_trajectory_controller/README.md
new file mode 100644
index 0000000..663a1c7
--- /dev/null
+++ b/joint_trajectory_controller/README.md
@@ -0,0 +1,7 @@
+## Joint Trajectory Controller ##
+
+Controller for executing joint-space trajectories on a group of joints.
+
+Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/joint_trajectory_controller).
+
+
diff --git a/joint_trajectory_controller/images/new_trajectory.png b/joint_trajectory_controller/images/new_trajectory.png
new file mode 100644
index 0000000..b7c07ee
Binary files /dev/null and b/joint_trajectory_controller/images/new_trajectory.png differ
diff --git a/joint_trajectory_controller/images/new_trajectory.svg b/joint_trajectory_controller/images/new_trajectory.svg
new file mode 100644
index 0000000..a4e10e7
--- /dev/null
+++ b/joint_trajectory_controller/images/new_trajectory.svg
@@ -0,0 +1,1192 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="416.03885"
+ height="214.70184"
+ id="svg2"
+ version="1.1"
+ inkscape:version="0.48.3.1 r9886"
+ sodipodi:docname="new_trajectory.svg"
+ inkscape:export-filename="/home/adolfo/git-test/ros_controllers/joint_trajectory_controller/images/new_trajectory.png"
+ inkscape:export-xdpi="300"
+ inkscape:export-ydpi="300">
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.979899"
+ inkscape:cx="129.06864"
+ inkscape:cy="112.53477"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="false"
+ inkscape:snap-grids="false"
+ inkscape:window-width="1609"
+ inkscape:window-height="1030"
+ inkscape:window-x="69"
+ inkscape:window-y="-3"
+ inkscape:window-maximized="1"
+ fit-margin-top="0"
+ fit-margin-left="0"
+ fit-margin-right="0"
+ fit-margin-bottom="0">
+ <inkscape:grid
+ originy="-785.242px"
+ originx="-25.092007px"
+ snapvisiblegridlinesonly="true"
+ enabled="true"
+ visible="true"
+ empspacing="5"
+ type="xygrid"
+ id="grid2985" />
+ </sodipodi:namedview>
+ <defs
+ id="defs4">
+ <marker
+ style="overflow:visible"
+ id="Arrow2Mstart"
+ refX="0.0"
+ refY="0.0"
+ orient="auto"
+ inkscape:stockid="Arrow2Mstart">
+ <path
+ transform="scale(0.6) translate(0,0)"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ style="fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round"
+ id="path3814" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2Mend"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path3817"
+ style="fill-rule:evenodd;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2Mend-4"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path3817-5"
+ style="fill-rule:evenodd;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2Mend-0"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path3817-1"
+ style="fill-rule:evenodd;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path4496"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-7"
+ style="overflow:visible">
+ <path
+ id="path4496-2"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1"
+ style="overflow:visible">
+ <path
+ id="path4496-4"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-1"
+ style="overflow:visible">
+ <path
+ id="path4496-4-4"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-9"
+ style="overflow:visible">
+ <path
+ id="path4496-4-0"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-97"
+ style="overflow:visible">
+ <path
+ id="path4496-4-8"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker5277"
+ style="overflow:visible">
+ <path
+ id="path5279"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker5281"
+ style="overflow:visible">
+ <path
+ id="path5283"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-3"
+ style="overflow:visible">
+ <path
+ id="path4496-4-46"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-4"
+ style="overflow:visible">
+ <path
+ id="path4496-4-2"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-0"
+ style="overflow:visible">
+ <path
+ id="path4496-4-87"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker5516"
+ style="overflow:visible">
+ <path
+ id="path5518"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker5520"
+ style="overflow:visible">
+ <path
+ id="path5522"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-01"
+ style="overflow:visible">
+ <path
+ id="path4496-4-7"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker5978"
+ style="overflow:visible">
+ <path
+ id="path5980"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker5982"
+ style="overflow:visible">
+ <path
+ id="path5984"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM-5"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854-7"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-37"
+ style="overflow:visible">
+ <path
+ id="path4496-4-5"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6192"
+ style="overflow:visible">
+ <path
+ id="path6194"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1k"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1k"
+ style="overflow:visible">
+ <path
+ id="path6288"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1N"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1N"
+ style="overflow:visible">
+ <path
+ id="path6291"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1N"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1N-4"
+ style="overflow:visible">
+ <path
+ id="path6291-5"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1k"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1k-4"
+ style="overflow:visible">
+ <path
+ id="path6288-8"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-6"
+ style="overflow:visible">
+ <path
+ id="path4496-4-04"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6527"
+ style="overflow:visible">
+ <path
+ id="path6529"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6531"
+ style="overflow:visible">
+ <path
+ id="path6533"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM-4"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854-8" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1N"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1N-8"
+ style="overflow:visible">
+ <path
+ id="path6291-1"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1k"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1k-1"
+ style="overflow:visible">
+ <path
+ id="path6288-82"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-7"
+ style="overflow:visible">
+ <path
+ id="path4496-4-48"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6693"
+ style="overflow:visible">
+ <path
+ id="path6695"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6697"
+ style="overflow:visible">
+ <path
+ id="path6699"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM-0"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854-80" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1N"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1N-2"
+ style="overflow:visible">
+ <path
+ id="path6291-3"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1k"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1k-48"
+ style="overflow:visible">
+ <path
+ id="path6288-7"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-2"
+ style="overflow:visible">
+ <path
+ id="path4496-4-3"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6890"
+ style="overflow:visible">
+ <path
+ id="path6892"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker6894"
+ style="overflow:visible">
+ <path
+ id="path6896"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM-43"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854-9" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM-2"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854-2" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotM-3"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path3854-23" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMC"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMC">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7304" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMu"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMu">
+ <path
+ inkscape:connector-curvature="0"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7307" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMu-2"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMu">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7307-1"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMC-0"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMC">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7304-9"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMu-9"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMu">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7307-2"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMu-5"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMu">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7307-5"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMC-9"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMC">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7304-4"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMu-53"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMu">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7307-6"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMu-3"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMu">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7307-0"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="DotMC-7"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="DotMC">
+ <path
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ id="path7304-7"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1N"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1N-44"
+ style="overflow:visible">
+ <path
+ id="path6291-50"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM-1k"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1k-46"
+ style="overflow:visible">
+ <path
+ id="path6288-4"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2MendM-1-79"
+ style="overflow:visible">
+ <path
+ id="path4496-4-06"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2MendM"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="marker7921"
+ style="overflow:visible">
+ <path
+ id="path7923"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)"
+ inkscape:connector-curvature="0" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2Mstartx"
+ refX="0.0"
+ refY="0.0"
+ orient="auto"
+ inkscape:stockid="Arrow2Mstartx">
+ <path
+ transform="scale(0.6) translate(0,0)"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ style="stroke-linejoin:round;stroke:#3f3f3f;stroke-width:0.62500000;fill:#3f3f3f;fill-rule:evenodd"
+ id="path10336" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend-0h"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow2Mend-0h"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path10339"
+ style="stroke-linejoin:round;stroke:#3f3f3f;stroke-width:0.625;fill:#3f3f3f;fill-rule:evenodd"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ transform="scale(-0.6,-0.6)" />
+ </marker>
+ </defs>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ transform="translate(-25.092007,-52.418331)"
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <rect
+ ry="0"
+ rx="0"
+ y="52.418331"
+ x="111.07904"
+ height="202.12242"
+ width="328.047"
+ id="rect5479"
+ style="fill:#ffebeb;fill-opacity:1;stroke:none" />
+ <path
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 39.285714,53.790753 0,199.999997 400.714286,0"
+ id="path2987"
+ inkscape:connector-curvature="0" />
+ <path
+ id="path5665"
+ d="m 39.901026,202.08444 70.521274,0"
+ style="fill:none;stroke:#808080;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:connector-curvature="0" />
+ <path
+ id="path3776"
+ d="m 110.4223,202.08444 329.12033,0"
+ style="fill:none;stroke:#808080;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.5, 4.5;stroke-dashoffset:0"
+ inkscape:connector-curvature="0" />
+ <path
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 110.66299,202.06628 c 39.92604,1.15726 29.12567,-66.50132 60.86169,-98.23734 34.00887,-34.008858 49.17066,76.05856 84.34774,122.22846 35.17708,46.16991 57.5911,-72.16031 118.69292,-73.4886 l 63.72094,0"
+ id="path4416"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cszcc" />
+ <path
+ sodipodi:nodetypes="ccc"
+ inkscape:connector-curvature="0"
+ id="path5667"
+ d="m 169.57936,105.59671 -12.81631,-12.816312 -38.44894,0"
+ style="fill:#000000;fill-opacity:0;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1" />
+ <path
+ sodipodi:nodetypes="cc"
+ inkscape:connector-curvature="0"
+ id="path5413-5-4"
+ d="m 170.02649,60.820256 0,44.446714"
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0" />
+ <g
+ id="g5186">
+ <path
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ sodipodi:ry="3.2142856"
+ sodipodi:rx="3.2142856"
+ sodipodi:cy="131.64789"
+ sodipodi:cx="80"
+ id="path3780"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ sodipodi:type="arc" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ <g
+ id="g5186-1"
+ transform="matrix(1,0,0,-1,87.512162,333.7351)">
+ <path
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ sodipodi:ry="3.2142856"
+ sodipodi:rx="3.2142856"
+ sodipodi:cy="131.64789"
+ sodipodi:cx="80"
+ id="path3780-7"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ sodipodi:type="arc" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784-0"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ <g
+ id="g5186-1-8"
+ transform="matrix(0.70710678,-0.70710678,-0.70710678,-0.70710678,329.38547,347.28358)">
+ <path
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ sodipodi:ry="3.2142856"
+ sodipodi:rx="3.2142856"
+ sodipodi:cy="131.64789"
+ sodipodi:cx="80"
+ id="path3780-7-6"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ sodipodi:type="arc" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784-0-0"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ <g
+ id="g7632">
+ <path
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0"
+ d="m 59.598996,53.275471 0,200.515279"
+ id="path5413"
+ inkscape:connector-curvature="0" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ x="50.507618"
+ y="264.44318"
+ id="text5415"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417"
+ x="50.507618"
+ y="264.44318"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial">now</tspan></text>
+ </g>
+ <g
+ id="g7627">
+ <path
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0"
+ d="m 111.07143,53.27547 0,200.51528"
+ id="path5413-5"
+ inkscape:connector-curvature="0" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ x="92.951309"
+ y="265.01569"
+ id="text5415-4"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-7"
+ x="92.951309"
+ y="265.01569"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial">traj start</tspan></text>
+ </g>
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8"
+ y="89.435745"
+ x="117.36658"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ y="89.435745"
+ x="117.36658"
+ id="tspan5417-7-1"
+ sodipodi:role="line">waypoint </tspan></text>
+ <g
+ transform="translate(50,0)"
+ id="g7764">
+ <path
+ style="fill:#000000;fill-opacity:0;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMu)"
+ d="m 138.51716,201.97158 12.81631,12.81631 33.27191,0"
+ id="path5667-3"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="ccc" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ x="155.26207"
+ y="223.40723"
+ id="text5415-4-8-5"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-7-1-4"
+ x="155.26207"
+ y="223.40723"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial">old traj</tspan></text>
+ </g>
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-3"
+ y="264.44318"
+ x="422.47363"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic"
+ y="264.44318"
+ x="422.47363"
+ id="tspan5417-2"
+ sodipodi:role="line">time</tspan></text>
+ <text
+ transform="matrix(0,-1,1,0,0,0)"
+ sodipodi:linespacing="125%"
+ id="text5415-3-0"
+ y="32.25021"
+ x="-88.479744"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic"
+ y="32.25021"
+ x="-88.479744"
+ id="tspan5417-2-0"
+ sodipodi:role="line">position</tspan></text>
+ <g
+ id="g7652">
+ <path
+ style="fill:none;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMC)"
+ d="m 220.4709,152.27959 12.8163,-12.81631 36.80745,0"
+ id="path5667-3-6"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="ccc" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Sans"
+ x="235.86801"
+ y="135.65022"
+ id="text5415-4-8-5-7"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-7-1-4-9"
+ x="235.86801"
+ y="135.65022"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Arial;-inkscape-font-specification:Arial">new traj</tspan></text>
+ </g>
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8-9"
+ y="66.651733"
+ x="172.21466"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ y="66.651733"
+ x="172.21466"
+ id="tspan5417-7-1-6"
+ sodipodi:role="line">time from start</tspan></text>
+ <path
+ sodipodi:nodetypes="cc"
+ inkscape:connector-curvature="0"
+ id="path5413-5-4-4"
+ d="m 113.73289,63.574436 54.04317,0"
+ style="stroke-linejoin:round;marker-end:url(#Arrow2Mend-0h);stroke-opacity:1;fill-opacity:0;marker-start:url(#Arrow2Mstartx);stroke-dashoffset:2.1;stroke:#3f3f3f;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:4.5,4.5;stroke-width:1.5;marker-mid:none;fill:#ffffff" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8-9-3"
+ y="213.12302"
+ x="65.806389"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ y="213.12302"
+ x="65.806389"
+ id="tspan5417-7-1-6-8"
+ sodipodi:role="line">pos hold</tspan></text>
+ </g>
+</svg>
diff --git a/joint_trajectory_controller/images/trajectory_replacement_future.png b/joint_trajectory_controller/images/trajectory_replacement_future.png
new file mode 100644
index 0000000..582a6ac
Binary files /dev/null and b/joint_trajectory_controller/images/trajectory_replacement_future.png differ
diff --git a/joint_trajectory_controller/images/trajectory_replacement_future.svg b/joint_trajectory_controller/images/trajectory_replacement_future.svg
new file mode 100644
index 0000000..c443817
--- /dev/null
+++ b/joint_trajectory_controller/images/trajectory_replacement_future.svg
@@ -0,0 +1,392 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="415.66559"
+ height="214.70187"
+ id="svg8640"
+ version="1.1"
+ inkscape:version="0.48.3.1 r9886"
+ sodipodi:docname="trajectory_replacement_future.svg"
+ inkscape:export-filename="/home/adolfo/git-test/ros_controllers/joint_trajectory_controller/images/trajectory_replacement_future.png"
+ inkscape:export-xdpi="300"
+ inkscape:export-ydpi="300">
+ <defs
+ id="defs8642">
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ id="path4496-4" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="marker8586"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ id="path8588" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1k"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM-1k">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ id="path6288" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1N"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM-1N">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ id="path6291" />
+ </marker>
+ <marker
+ inkscape:stockid="DotMC"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="DotMC"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path7304"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)" />
+ </marker>
+ <marker
+ inkscape:stockid="DotMu"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="DotMu"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path7307"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)" />
+ </marker>
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.4"
+ inkscape:cx="141.32215"
+ inkscape:cy="107.20189"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="false"
+ inkscape:window-width="1609"
+ inkscape:window-height="1030"
+ inkscape:window-x="69"
+ inkscape:window-y="-3"
+ inkscape:window-maximized="1"
+ fit-margin-top="0"
+ fit-margin-left="0"
+ fit-margin-right="0"
+ fit-margin-bottom="0" />
+ <metadata
+ id="metadata8645">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1"
+ transform="translate(-117.88528,-427.86839)">
+ <rect
+ style="fill:#ffebeb;fill-opacity:1;stroke:none"
+ id="rect5479-4-3-4"
+ width="171.16412"
+ height="202.12242"
+ x="203.1147"
+ y="427.86838"
+ rx="0"
+ ry="0" />
+ <rect
+ style="fill:#ebffeb;fill-opacity:1;stroke:none"
+ id="rect5479-4-3"
+ width="157.6769"
+ height="202.12242"
+ x="374.24243"
+ y="427.86838"
+ rx="0"
+ ry="0" />
+ <path
+ style="fill:none;stroke:#808080;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 132.07899,577.53452 70.52127,0"
+ id="path5665-3-5"
+ inkscape:connector-curvature="0" />
+ <path
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0"
+ d="m 203.86471,428.72555 0,200.51528"
+ id="path5413-5-5-0"
+ inkscape:connector-curvature="0" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path2987-6-6"
+ d="m 132.07899,429.24083 0,200 400.71429,0"
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <path
+ id="path7158"
+ d="m 203.45627,577.51636 c 39.92604,1.15726 29.12567,-66.50132 60.86169,-98.23734 34.00888,-34.00886 49.32092,76.25579 84.34774,122.22846 9.13676,11.99199 17.41248,12.88634 25.72873,7.61567"
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cssc" />
+ <g
+ transform="translate(92.79328,375.45008)"
+ id="g5186-14-3">
+ <path
+ sodipodi:type="arc"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ id="path3780-1-9"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)" />
+ <path
+ sodipodi:nodetypes="cc"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ id="path3784-7-1"
+ inkscape:connector-curvature="0" />
+ </g>
+ <g
+ transform="matrix(1,0,0,-1,180.30544,709.18518)"
+ id="g5186-1-2-7">
+ <path
+ sodipodi:type="arc"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ id="path3780-7-63-5"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)" />
+ <path
+ sodipodi:nodetypes="cc"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ id="path3784-0-1-9"
+ inkscape:connector-curvature="0" />
+ </g>
+ <path
+ id="path4416-0-9"
+ d="m 374.39443,609.12315 c 23.7018,-15.02174 47.73272,-80.12098 92.96419,-81.10427 l 63.72094,0"
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.5, 4.5;stroke-dashoffset:0"
+ inkscape:connector-curvature="0" />
+ <path
+ sodipodi:nodetypes="cscc"
+ inkscape:connector-curvature="0"
+ id="path6505-1"
+ d="m 374.01347,609.05227 c 24.45036,-16.7458 8.25159,-107.6354 43.81253,-98.10687 40.16682,10.76267 32.78923,78.91679 80.2946,78.91679 l 33.0822,0"
+ style="fill:none;stroke:#319331;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <g
+ id="g7039"
+ transform="translate(98.571427,-111.1791)">
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-42-6"
+ y="751.07233"
+ x="192.10419"
+ style="font-size:10px;font-style:normal;font-weight:normal;text-align:center;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:center;text-anchor:middle;font-family:Arial;-inkscape-font-specification:Arial"
+ y="751.07233"
+ x="192.10419"
+ sodipodi:role="line"
+ id="tspan6122-8">now</tspan></text>
+ <path
+ inkscape:connector-curvature="0"
+ id="path5413-51-0"
+ d="m 192.39227,539.90465 0,200.51528"
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0" />
+ </g>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ x="-463.92978"
+ y="125.04349"
+ id="text5415-3-0-1-1"
+ sodipodi:linespacing="125%"
+ transform="matrix(0,-1,1,0,0,0)"><tspan
+ sodipodi:role="line"
+ id="tspan5417-2-0-9-0"
+ x="-463.92978"
+ y="125.04349"
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic">position</tspan></text>
+ <g
+ transform="matrix(1,0,0,-1,177.41378,865.53352)"
+ id="g6500-7">
+ <path
+ sodipodi:type="arc"
+ style="fill:#005f00;fill-opacity:1;stroke:none"
+ id="path3780-7-0-3-5-4"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.8082904,0.46666668,-0.46666668,0.8082904,235.93375,211.14023)" />
+ <g
+ id="g6497-4">
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784-0-3-9-2-2"
+ d="m 239.66398,354.65079 27.61223,-7.39868"
+ style="fill:none;stroke:#005f00;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1k)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ </g>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ x="514.88605"
+ y="639.89325"
+ id="text5415-3-8-5"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-2-4-6"
+ x="514.88605"
+ y="639.89325"
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic">time</tspan></text>
+ <g
+ id="g6507-5"
+ transform="translate(155.19042,143.08036)">
+ <path
+ sodipodi:type="arc"
+ style="fill:#005f00;fill-opacity:1;stroke:none"
+ id="path3780-7-6-2-7-0-0"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.65996635,-0.65996635,-0.65996635,-0.65996635,377.95383,586.33224)" />
+ <path
+ sodipodi:nodetypes="cc"
+ style="fill:none;stroke:#005f00;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1N)"
+ d="m 344.41368,446.74624 28.58629,0"
+ id="path3784-0-0-5-7-5-8"
+ inkscape:connector-curvature="0" />
+ </g>
+ <g
+ id="g6117-6"
+ transform="translate(263.27165,375.45008)">
+ <path
+ inkscape:connector-curvature="0"
+ id="path5413-5-0"
+ d="m 111.07143,53.27547 0,200.51528"
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-2"
+ y="265.01569"
+ x="92.951309"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-family:Arial;-inkscape-font-specification:Arial"
+ y="265.01569"
+ x="92.951309"
+ id="tspan5417-7-9"
+ sodipodi:role="line">traj start</tspan></text>
+ </g>
+ <g
+ transform="translate(186.97525,358.65828)"
+ id="g7652-6-2">
+ <path
+ sodipodi:nodetypes="ccc"
+ inkscape:connector-curvature="0"
+ id="path5667-3-6-0-7"
+ d="m 220.4709,152.27959 12.8163,-12.81631 36.80745,0"
+ style="fill:none;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMC)" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8-5-7-5-3"
+ y="135.65022"
+ x="235.86801"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Arial;-inkscape-font-specification:Arial"
+ y="135.65022"
+ x="235.86801"
+ id="tspan5417-7-1-4-9-9-2"
+ sodipodi:role="line">new traj</tspan></text>
+ </g>
+ <g
+ id="g7764-6-8"
+ transform="translate(260.04201,379.29471)">
+ <path
+ sodipodi:nodetypes="ccc"
+ inkscape:connector-curvature="0"
+ id="path5667-3-9-5"
+ d="m 138.51716,201.97158 12.81631,12.81631 33.27191,0"
+ style="fill:#000000;fill-opacity:0;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMu)" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8-5-1-8"
+ y="223.40723"
+ x="155.26207"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ y="223.40723"
+ x="155.26207"
+ id="tspan5417-7-1-4-2-3"
+ sodipodi:role="line">old traj</tspan></text>
+ </g>
+ </g>
+</svg>
diff --git a/joint_trajectory_controller/images/trajectory_replacement_now.png b/joint_trajectory_controller/images/trajectory_replacement_now.png
new file mode 100644
index 0000000..a7cd2f6
Binary files /dev/null and b/joint_trajectory_controller/images/trajectory_replacement_now.png differ
diff --git a/joint_trajectory_controller/images/trajectory_replacement_now.svg b/joint_trajectory_controller/images/trajectory_replacement_now.svg
new file mode 100644
index 0000000..b677aa0
--- /dev/null
+++ b/joint_trajectory_controller/images/trajectory_replacement_now.svg
@@ -0,0 +1,345 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="415.66849"
+ height="226.62933"
+ id="svg8407"
+ version="1.1"
+ inkscape:version="0.48.3.1 r9886"
+ sodipodi:docname="trajectory_replacement_now.svg"
+ inkscape:export-filename="/home/adolfo/git-test/ros_controllers/joint_trajectory_controller/images/trajectory_replacement_now.png"
+ inkscape:export-xdpi="300"
+ inkscape:export-ydpi="300">
+ <defs
+ id="defs8409">
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1k"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM-1k">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ id="path6288" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1N"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM-1N">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ id="path6291" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ id="path4496-4" />
+ </marker>
+ <marker
+ inkscape:stockid="DotMC"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="DotMC"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path7304"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)" />
+ </marker>
+ <marker
+ inkscape:stockid="DotMu"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="DotMu"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path7307"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)" />
+ </marker>
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.4"
+ inkscape:cx="312.94836"
+ inkscape:cy="24.722717"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="false"
+ fit-margin-top="0"
+ fit-margin-left="0"
+ fit-margin-right="0"
+ fit-margin-bottom="0"
+ inkscape:window-width="1609"
+ inkscape:window-height="1030"
+ inkscape:window-x="69"
+ inkscape:window-y="-3"
+ inkscape:window-maximized="1" />
+ <metadata
+ id="metadata8412">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1"
+ transform="translate(-62.170999,-76.904651)">
+ <g
+ transform="translate(38.047567,-208.10661)"
+ id="g8314">
+ <rect
+ ry="0"
+ rx="0"
+ y="285.01126"
+ x="109.35285"
+ height="202.12242"
+ width="328.80463"
+ id="rect5479-4"
+ style="fill:#ffebeb;fill-opacity:1;stroke:none" />
+ <rect
+ ry="0"
+ rx="0"
+ y="285.01126"
+ x="197.26927"
+ height="202.12242"
+ width="240.88821"
+ id="rect5479-4-3-7"
+ style="fill:#ebffeb;fill-opacity:1;stroke:none" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path5665-3"
+ d="m 38.31714,434.67737 70.52127,0"
+ style="fill:none;stroke:#808080;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path5413-5-5"
+ d="m 110.10286,285.8684 0,200.51528"
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0" />
+ <path
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 38.31714,286.38368 0,200 400.71429,0"
+ id="path2987-6"
+ inkscape:connector-curvature="0" />
+ <path
+ sodipodi:nodetypes="cscc"
+ inkscape:connector-curvature="0"
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.5, 4.5;stroke-dashoffset:0"
+ d="m 197.15509,339.13841 c 18.32474,23.63466 33.13843,87.21087 57.74876,119.51192 34.5304,45.32114 59.35269,-72.19861 118.69292,-73.4886 l 63.72094,0"
+ id="path4416-0" />
+ <path
+ style="fill:none;stroke:#319331;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 197.60461,339.94512 c 16.64436,24.12929 21.9428,22.20563 44.10188,28.14313 40.16682,10.76266 32.78923,78.91679 80.2946,78.91679 l 115.15709,0"
+ id="path6505"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cscc" />
+ <g
+ id="g7641">
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;text-align:center;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ x="197.14813"
+ y="497.0361"
+ id="text5415-42"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-6"
+ x="197.14813"
+ y="497.0361"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:center;text-anchor:middle;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial">now,</tspan><tspan
+ id="tspan6122"
+ sodipodi:role="line"
+ x="197.14813"
+ y="509.5361"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:center;text-anchor:middle;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial">traj start</tspan></text>
+ <path
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0"
+ d="m 197.20185,285.8684 0,200.51528"
+ id="path5413-51"
+ inkscape:connector-curvature="0" />
+ </g>
+ <text
+ transform="matrix(0,-1,1,0,0,0)"
+ sodipodi:linespacing="125%"
+ id="text5415-3-0-1"
+ y="31.281635"
+ x="-321.07266"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic"
+ y="31.281635"
+ x="-321.07266"
+ id="tspan5417-2-0-9"
+ sodipodi:role="line">position</tspan></text>
+ <g
+ id="g6500"
+ transform="matrix(1,0,0,-1,1.0101525,722.67637)">
+ <path
+ transform="matrix(0.8082904,0.46666668,-0.46666668,0.8082904,235.93375,211.14023)"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ sodipodi:ry="3.2142856"
+ sodipodi:rx="3.2142856"
+ sodipodi:cy="131.64789"
+ sodipodi:cx="80"
+ id="path3780-7-0-3-5"
+ style="fill:#005f00;fill-opacity:1;stroke:none"
+ sodipodi:type="arc" />
+ <g
+ id="g6497">
+ <path
+ sodipodi:nodetypes="cc"
+ style="fill:none;stroke:#005f00;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1k)"
+ d="m 239.66398,354.65079 27.61223,-7.39868"
+ id="path3784-0-3-9-2"
+ inkscape:connector-curvature="0" />
+ </g>
+ </g>
+ <g
+ transform="translate(-21.213203,0)"
+ id="g6507">
+ <path
+ transform="matrix(0.65996635,-0.65996635,-0.65996635,-0.65996635,377.95383,586.33224)"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ sodipodi:ry="3.2142856"
+ sodipodi:rx="3.2142856"
+ sodipodi:cy="131.64789"
+ sodipodi:cx="80"
+ id="path3780-7-6-2-7-0"
+ style="fill:#005f00;fill-opacity:1;stroke:none"
+ sodipodi:type="arc" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784-0-0-5-7-5"
+ d="m 344.41368,446.74624 28.58629,0"
+ style="fill:none;stroke:#005f00;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1N)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-3-8"
+ y="498.89157"
+ x="421.12421"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic"
+ y="498.89157"
+ x="421.12421"
+ id="tspan5417-2-4"
+ sodipodi:role="line">time</tspan></text>
+ <path
+ inkscape:connector-curvature="0"
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 109.69442,434.65921 c 39.92603,1.15726 29.12565,-66.50133 60.86168,-98.23734 10.21583,-10.21583 18.73105,-7.43129 26.59899,2.71654"
+ id="path7136" />
+ <g
+ id="g5186-14"
+ transform="translate(-0.96857275,232.59293)">
+ <path
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ sodipodi:ry="3.2142856"
+ sodipodi:rx="3.2142856"
+ sodipodi:cy="131.64789"
+ sodipodi:cx="80"
+ id="path3780-1"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ sodipodi:type="arc" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784-7"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ <g
+ id="g7652-6"
+ transform="translate(49.259579,240.93963)">
+ <path
+ style="fill:none;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMC)"
+ d="m 220.4709,152.27959 12.8163,-12.81631 36.80745,0"
+ id="path5667-3-6-0"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="ccc" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Sans"
+ x="235.86801"
+ y="135.65022"
+ id="text5415-4-8-5-7-5"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-7-1-4-9-9"
+ x="235.86801"
+ y="135.65022"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Arial;-inkscape-font-specification:Arial">new traj</tspan></text>
+ </g>
+ <g
+ transform="translate(151.60843,256.42181)"
+ id="g7764-6">
+ <path
+ style="fill:#000000;fill-opacity:0;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMu)"
+ d="m 138.51716,201.97158 12.81631,12.81631 33.27191,0"
+ id="path5667-3-9"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="ccc" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ x="155.26207"
+ y="223.40723"
+ id="text5415-4-8-5-1"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-7-1-4-2"
+ x="155.26207"
+ y="223.40723"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial">old traj</tspan></text>
+ </g>
+ </g>
+ </g>
+</svg>
diff --git a/joint_trajectory_controller/images/trajectory_replacement_past.png b/joint_trajectory_controller/images/trajectory_replacement_past.png
new file mode 100644
index 0000000..a719f6f
Binary files /dev/null and b/joint_trajectory_controller/images/trajectory_replacement_past.png differ
diff --git a/joint_trajectory_controller/images/trajectory_replacement_past.svg b/joint_trajectory_controller/images/trajectory_replacement_past.svg
new file mode 100644
index 0000000..eaedf5d
--- /dev/null
+++ b/joint_trajectory_controller/images/trajectory_replacement_past.svg
@@ -0,0 +1,381 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="415.65799"
+ height="214.70183"
+ id="svg8938"
+ version="1.1"
+ inkscape:version="0.48.3.1 r9886"
+ sodipodi:docname="trajectory_replacement_past.svg"
+ inkscape:export-filename="/home/adolfo/git-test/ros_controllers/joint_trajectory_controller/images/trajectory_replacement_past.png"
+ inkscape:export-xdpi="300"
+ inkscape:export-ydpi="300">
+ <defs
+ id="defs8940">
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#5f0000;fill-rule:evenodd;stroke:#5f0000;stroke-width:0.625;stroke-linejoin:round"
+ id="path4496-4" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1N"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM-1N">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ id="path6291" />
+ </marker>
+ <marker
+ inkscape:stockid="DotMC"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="DotMC"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path7304"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)" />
+ </marker>
+ <marker
+ inkscape:stockid="DotMu"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="DotMu"
+ style="overflow:visible">
+ <path
+ inkscape:connector-curvature="0"
+ id="path7307"
+ d="m -2.5,-1 c 0,2.76 -2.24,5 -5,5 -2.76,0 -5,-2.24 -5,-5 0,-2.76 2.24,-5 5,-5 2.76,0 5,2.24 5,5 z"
+ style="fill:#3f3f3f;fill-rule:evenodd;stroke:#3f3f3f;stroke-width:1pt"
+ transform="matrix(0.4,0,0,0.4,2.96,0.4)" />
+ </marker>
+ <marker
+ style="overflow:visible"
+ id="Arrow2MendM-1k"
+ refX="0"
+ refY="0"
+ orient="auto"
+ inkscape:stockid="Arrow2MendM-1k">
+ <path
+ inkscape:connector-curvature="0"
+ transform="scale(-0.6,-0.6)"
+ d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
+ style="fill:#005f00;fill-rule:evenodd;stroke:#005f00;stroke-width:0.625;stroke-linejoin:round"
+ id="path6288" />
+ </marker>
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.979899"
+ inkscape:cx="171.43232"
+ inkscape:cy="40.077622"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="false"
+ inkscape:window-width="1609"
+ inkscape:window-height="1030"
+ inkscape:window-x="69"
+ inkscape:window-y="-3"
+ inkscape:window-maximized="1"
+ fit-margin-top="0"
+ fit-margin-left="0"
+ fit-margin-right="0"
+ fit-margin-bottom="0" />
+ <metadata
+ id="metadata8943">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1"
+ transform="translate(-149.31385,-359.29698)">
+ <rect
+ style="fill:#ffebeb;fill-opacity:1;stroke:none"
+ id="rect5479-4-3-4-5"
+ width="88.664124"
+ height="202.12242"
+ x="234.54327"
+ y="359.29697"
+ rx="0"
+ ry="0" />
+ <rect
+ style="fill:#ebffeb;fill-opacity:1;stroke:none"
+ id="rect5479-4-3-9"
+ width="240.98047"
+ height="202.12242"
+ x="322.36743"
+ y="359.29697"
+ rx="0"
+ ry="0" />
+ <path
+ style="fill:none;stroke:#808080;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 163.50757,508.96307 70.52126,0"
+ id="path5665-3-5-3"
+ inkscape:connector-curvature="0" />
+ <path
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0"
+ d="m 235.29328,360.1541 0,200.51528"
+ id="path5413-5-5-0-8"
+ inkscape:connector-curvature="0" />
+ <path
+ inkscape:connector-curvature="0"
+ id="path2987-6-6-9"
+ d="m 163.50757,360.66938 0,200 400.71428,0"
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <path
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 234.7107,508.84478 c 39.92604,1.1572 29.12567,-66.50131 60.86169,-98.23733 10.44087,-10.44087 19.10538,-7.3023 27.11802,3.39784"
+ id="path8174"
+ inkscape:connector-curvature="0" />
+ <g
+ transform="translate(124.22186,306.87863)"
+ id="g5186-14-3-1">
+ <path
+ sodipodi:type="arc"
+ style="fill:#5f0000;fill-opacity:1;stroke:none"
+ id="path3780-1-9-3"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.93333336,0,0,0.93333336,95.359823,-17.352965)" />
+ <path
+ sodipodi:nodetypes="cc"
+ style="fill:none;stroke:#5f0000;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1)"
+ d="M 170.34557,105.06571 190.55913,84.852151"
+ id="path3784-7-1-5"
+ inkscape:connector-curvature="0" />
+ </g>
+ <path
+ style="fill:none;stroke:#933131;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1.5, 4.5;stroke-dashoffset:0"
+ d="m 322.69041,414.00529 c 18.08679,24.15323 32.85215,86.83508 57.22971,118.83059 35.28144,46.30681 57.5911,-72.16028 118.69292,-73.48857 l 63.72094,0"
+ id="path4416-9-2"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cscc" />
+ <path
+ sodipodi:nodetypes="ccc"
+ inkscape:connector-curvature="0"
+ id="path6505-1-5"
+ d="m 322.57347,414.48748 c 24.28572,23.03572 31.9852,106.80326 56.63343,106.80326 l 183.26077,0"
+ style="fill:none;stroke:#319331;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <g
+ transform="translate(130,-179.75055)"
+ id="g7039-9">
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-42-6-4"
+ y="751.07233"
+ x="192.10419"
+ style="font-size:10px;font-style:normal;font-weight:normal;text-align:center;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:center;text-anchor:middle;font-family:Arial;-inkscape-font-specification:Arial"
+ y="751.07233"
+ x="192.10419"
+ sodipodi:role="line"
+ id="tspan6122-8-0">now</tspan></text>
+ <path
+ inkscape:connector-curvature="0"
+ id="path5413-51-0-1"
+ d="m 192.39227,539.90465 0,200.51528"
+ style="fill:none;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0" />
+ </g>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ x="-395.35837"
+ y="156.47206"
+ id="text5415-3-0-1-1-4"
+ sodipodi:linespacing="125%"
+ transform="matrix(0,-1,1,0,0,0)"><tspan
+ sodipodi:role="line"
+ id="tspan5417-2-0-9-0-0"
+ x="-395.35837"
+ y="156.47206"
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic">position</tspan></text>
+ <g
+ id="g6507-5-2"
+ transform="translate(36.27672,74.50891)">
+ <path
+ sodipodi:type="arc"
+ style="fill:#005f00;fill-opacity:1;stroke:none"
+ id="path3780-7-6-2-7-0-0-3"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.65996635,-0.65996635,-0.65996635,-0.65996635,377.95383,586.33224)" />
+ <path
+ sodipodi:nodetypes="cc"
+ style="fill:none;stroke:#005f00;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1N)"
+ d="m 344.41368,446.74624 28.58629,0"
+ id="path3784-0-0-5-7-5-8-1"
+ inkscape:connector-curvature="0" />
+ </g>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:italic;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans;-inkscape-font-specification:Arial Italic"
+ x="546.31464"
+ y="571.32178"
+ id="text5415-3-8-5-1"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan5417-2-4-6-3"
+ x="546.31464"
+ y="571.32178"
+ style="font-size:10px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial Italic">time</tspan></text>
+ <g
+ id="g6117-6-1"
+ transform="translate(144.72742,306.87863)">
+ <path
+ inkscape:connector-curvature="0"
+ id="path5413-5-0-8"
+ d="m 111.07143,53.27547 0,200.51528"
+ style="fill:#ffffff;fill-opacity:0;stroke:#3f3f3f;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:4.5, 4.5;stroke-dashoffset:0" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-2-0"
+ y="265.01569"
+ x="92.951309"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-family:Arial;-inkscape-font-specification:Arial"
+ y="265.01569"
+ x="92.951309"
+ id="tspan5417-7-9-3"
+ sodipodi:role="line">traj start</tspan></text>
+ </g>
+ <g
+ transform="translate(223.76097,369.31127)"
+ id="g7652-6-2-1">
+ <path
+ sodipodi:nodetypes="ccc"
+ inkscape:connector-curvature="0"
+ id="path5667-3-6-0-7-8"
+ d="m 220.4709,152.27959 12.8163,-12.81631 36.80745,0"
+ style="fill:none;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMC)" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8-5-7-5-3-2"
+ y="135.65022"
+ x="235.86801"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;stroke:#000000;stroke-opacity:0;font-family:Arial;-inkscape-font-specification:Arial"
+ y="135.65022"
+ x="235.86801"
+ id="tspan5417-7-1-4-9-9-2-9"
+ sodipodi:role="line">new traj</tspan></text>
+ </g>
+ <g
+ id="g7764-6-8-0"
+ transform="translate(273.97059,333.5804)">
+ <path
+ sodipodi:nodetypes="ccc"
+ inkscape:connector-curvature="0"
+ id="path5667-3-9-5-5"
+ d="m 138.51716,201.97158 12.81631,12.81631 33.27191,0"
+ style="fill:#000000;fill-opacity:0;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1;marker-start:url(#DotMu)" />
+ <text
+ sodipodi:linespacing="125%"
+ id="text5415-4-8-5-1-8-5"
+ y="223.40723"
+ x="155.26207"
+ style="font-size:10px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ xml:space="preserve"><tspan
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ y="223.40723"
+ x="155.26207"
+ id="tspan5417-7-1-4-2-3-8"
+ sodipodi:role="line">old traj</tspan></text>
+ </g>
+ <path
+ style="fill:#000000;fill-opacity:0;stroke:#3f3f3f;stroke-width:1px;stroke-linecap:round;stroke-linejoin:round;stroke-opacity:1"
+ d="m 298.00956,441.75982 12.81631,-12.81631 111.68501,0"
+ id="path5667-4"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="ccc" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:normal;text-align:center;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#3f3f3f;fill-opacity:1;stroke:none;font-family:Sans"
+ x="384.91821"
+ y="438.49411"
+ id="text5415-4-8-0"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ x="384.91821"
+ y="438.49411"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:center;text-anchor:middle;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ id="tspan8200">discarded waypoint</tspan><tspan
+ sodipodi:role="line"
+ x="384.91821"
+ y="450.99411"
+ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:center;text-anchor:middle;fill:#3f3f3f;fill-opacity:1;font-family:Arial;-inkscape-font-specification:Arial"
+ id="tspan8204">(in the past)</tspan></text>
+ <g
+ transform="matrix(1,0,0,-1,58.500081,796.96209)"
+ id="g6500-7-5">
+ <path
+ sodipodi:type="arc"
+ style="fill:#005f00;fill-opacity:1;stroke:none"
+ id="path3780-7-0-3-5-4-8"
+ sodipodi:cx="80"
+ sodipodi:cy="131.64789"
+ sodipodi:rx="3.2142856"
+ sodipodi:ry="3.2142856"
+ d="m 83.214286,131.64789 c 0,1.7752 -1.439085,3.21428 -3.214286,3.21428 -1.775201,0 -3.214286,-1.43908 -3.214286,-3.21428 0,-1.7752 1.439085,-3.21429 3.214286,-3.21429 1.775201,0 3.214286,1.43909 3.214286,3.21429 z"
+ transform="matrix(0.8082904,0.46666668,-0.46666668,0.8082904,235.93375,211.14023)" />
+ <g
+ id="g6497-4-5">
+ <path
+ inkscape:connector-curvature="0"
+ id="path3784-0-3-9-2-2-7"
+ d="m 239.66398,354.65079 27.61223,-7.39868"
+ style="fill:none;stroke:#005f00;stroke-width:1.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow2MendM-1k)"
+ sodipodi:nodetypes="cc" />
+ </g>
+ </g>
+ </g>
+</svg>
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h b/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h
new file mode 100644
index 0000000..df32af1
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h
@@ -0,0 +1,322 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
+#define JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
+
+#include <cassert>
+#include <string>
+#include <vector>
+
+#include <boost/shared_ptr.hpp>
+
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <control_toolbox/pid.h>
+#include <hardware_interface/joint_command_interface.h>
+
+/**
+ * \brief Helper class to simplify integrating the JointTrajectoryController with different hardware interfaces.
+ *
+ * The JointTrajectoryController outputs position, velocity and acceleration command triplets, while the more common hardware
+ * interfaces accept position, velocity or effort commands.
+ *
+ * Use one of the available template specializations of this class (or create your own) to adapt the
+ * JointTrajectoryController to a specidfic hardware interface.
+ */
+template <class HardwareInterface, class State>
+class HardwareInterfaceAdapter
+{
+public:
+ bool init(std::vector<typename HardwareInterface::ResourceHandleType>& /*joint_handles*/, ros::NodeHandle& /*controller_nh*/)
+ {
+ return false;
+ }
+
+ void starting(const ros::Time& /*time*/) {}
+ void stopping(const ros::Time& /*time*/) {}
+
+ void updateCommand(const ros::Time& /*time*/,
+ const ros::Duration& /*period*/,
+ const State& /*desired_state*/,
+ const State& /*state_error*/) {}
+};
+
+/**
+ * \brief Adapter for a position-controlled hardware interface. Forwards desired positions as commands.
+ *
+ * The following is an example configuration of a controller that uses this adapter.
+ * \code
+ * head_controller:
+ * type: "position_controllers/JointTrajectoryController"
+ * joints:
+ * - head_1_joint
+ * - head_2_joint
+ *
+ * constraints:
+ * goal_time: 0.6
+ * stopped_velocity_tolerance: 0.02
+ * head_1_joint: {trajectory: 0.05, goal: 0.02}
+ * head_2_joint: {trajectory: 0.05, goal: 0.02}
+ * stop_trajectory_duration: 0.5
+ * state_publish_rate: 25
+ * \endcode
+ */
+template <class State>
+class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface, State>
+{
+public:
+ HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
+
+ bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& /*controller_nh*/)
+ {
+ // Store pointer to joint handles
+ joint_handles_ptr_ = &joint_handles;
+
+ return true;
+ }
+
+ void starting(const ros::Time& /*time*/)
+ {
+ if (!joint_handles_ptr_) {return;}
+
+ // Semantic zero for commands
+ for (unsigned int i = 0; i < joint_handles_ptr_->size(); ++i)
+ {
+ (*joint_handles_ptr_)[i].setCommand((*joint_handles_ptr_)[i].getPosition());
+ }
+ }
+
+ void stopping(const ros::Time& /*time*/) {}
+
+ void updateCommand(const ros::Time& /*time*/,
+ const ros::Duration& /*period*/,
+ const State& desired_state,
+ const State& /*state_error*/)
+ {
+ // Forward desired position to command
+ const unsigned int n_joints = joint_handles_ptr_->size();
+ for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
+ }
+
+private:
+ std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
+};
+
+/**
+ * \brief Adapter for an velocity-controlled hardware interface. Maps position and velocity errors to velocity commands
+ * through a velocity PID loop.
+ *
+ * The following is an example configuration of a controller that uses this adapter. Notice the \p gains entry:
+ * \code
+ * head_controller:
+ * type: "velocity_controllers/JointTrajectoryController"
+ * joints:
+ * - head_1_joint
+ * - head_2_joint
+ * gains:
+ * head_1_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ * head_2_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ * constraints:
+ * goal_time: 0.6
+ * stopped_velocity_tolerance: 0.02
+ * head_1_joint: {trajectory: 0.05, goal: 0.02}
+ * head_2_joint: {trajectory: 0.05, goal: 0.02}
+ * stop_trajectory_duration: 0.5
+ * state_publish_rate: 25
+ * \endcode
+ */
+template <class State>
+class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State>
+{
+public:
+ HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
+
+ bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
+ {
+ // Store pointer to joint handles
+ joint_handles_ptr_ = &joint_handles;
+
+ // Initialize PIDs
+ pids_.resize(joint_handles.size());
+ for (unsigned int i = 0; i < pids_.size(); ++i)
+ {
+ // Node handle to PID gains
+ ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());
+
+ // Init PID gains from ROS parameter server
+ pids_[i].reset(new control_toolbox::Pid());
+ if (!pids_[i]->init(joint_nh))
+ {
+ ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ void starting(const ros::Time& /*time*/)
+ {
+ if (!joint_handles_ptr_) {return;}
+
+ // Reset PIDs, zero velocity commands
+ for (unsigned int i = 0; i < pids_.size(); ++i)
+ {
+ pids_[i]->reset();
+ (*joint_handles_ptr_)[i].setCommand(0.0);
+ }
+ }
+
+ void stopping(const ros::Time& time) {}
+
+ void updateCommand(const ros::Time& /*time*/,
+ const ros::Duration& period,
+ const State& /*desired_state*/,
+ const State& state_error)
+ {
+ const unsigned int n_joints = joint_handles_ptr_->size();
+
+ // Preconditions
+ if (!joint_handles_ptr_)
+ return;
+ assert(n_joints == state_error.position.size());
+ assert(n_joints == state_error.velocity.size());
+
+ // Update PIDs
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
+ (*joint_handles_ptr_)[i].setCommand(command);
+ }
+ }
+
+private:
+ typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
+ std::vector<PidPtr> pids_;
+
+ std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
+};
+
+/**
+ * \brief Adapter for an effort-controlled hardware interface. Maps position and velocity errors to effort commands
+ * through a position PID loop.
+ *
+ * The following is an example configuration of a controller that uses this adapter. Notice the \p gains entry:
+ * \code
+ * head_controller:
+ * type: "effort_controllers/JointTrajectoryController"
+ * joints:
+ * - head_1_joint
+ * - head_2_joint
+ * gains:
+ * head_1_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ * head_2_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ * constraints:
+ * goal_time: 0.6
+ * stopped_velocity_tolerance: 0.02
+ * head_1_joint: {trajectory: 0.05, goal: 0.02}
+ * head_2_joint: {trajectory: 0.05, goal: 0.02}
+ * stop_trajectory_duration: 0.5
+ * state_publish_rate: 25
+ * \endcode
+ */
+template <class State>
+class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State>
+{
+public:
+ HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
+
+ bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
+ {
+ // Store pointer to joint handles
+ joint_handles_ptr_ = &joint_handles;
+
+ // Initialize PIDs
+ pids_.resize(joint_handles.size());
+ for (unsigned int i = 0; i < pids_.size(); ++i)
+ {
+ // Node handle to PID gains
+ ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());
+
+ // Init PID gains from ROS parameter server
+ pids_[i].reset(new control_toolbox::Pid());
+ if (!pids_[i]->init(joint_nh))
+ {
+ ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ void starting(const ros::Time& /*time*/)
+ {
+ if (!joint_handles_ptr_) {return;}
+
+ // Reset PIDs, zero effort commands
+ for (unsigned int i = 0; i < pids_.size(); ++i)
+ {
+ pids_[i]->reset();
+ (*joint_handles_ptr_)[i].setCommand(0.0);
+ }
+ }
+
+ void stopping(const ros::Time& /*time*/) {}
+
+ void updateCommand(const ros::Time& /*time*/,
+ const ros::Duration& period,
+ const State& /*desired_state*/,
+ const State& state_error)
+ {
+ const unsigned int n_joints = joint_handles_ptr_->size();
+
+ // Preconditions
+ if (!joint_handles_ptr_) {return;}
+ assert(n_joints == state_error.position.size());
+ assert(n_joints == state_error.velocity.size());
+
+ // Update PIDs
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
+ (*joint_handles_ptr_)[i].setCommand(command);
+ }
+ }
+
+private:
+ typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
+ std::vector<PidPtr> pids_;
+
+ std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
+};
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
new file mode 100644
index 0000000..75cf188
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
@@ -0,0 +1,391 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_INIT_JOINT_TRAJECTORY_H
+#define JOINT_TRAJECTORY_CONTROLLER_INIT_JOINT_TRAJECTORY_H
+
+// C++ standard
+#include <algorithm>
+#include <iomanip>
+#include <sstream>
+#include <stdexcept>
+#include <vector>
+
+// Boost
+#include <boost/shared_ptr.hpp>
+
+// ROS messages
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+// ros_controls
+#include <realtime_tools/realtime_server_goal_handle.h>
+
+// Project
+#include <joint_trajectory_controller/joint_trajectory_msg_utils.h>
+#include <joint_trajectory_controller/joint_trajectory_segment.h>
+
+namespace joint_trajectory_controller
+{
+
+namespace internal
+{
+/**
+ * \return The permutation vector between two containers.
+ * If \p t1 is <tt>"{A, B, C, D}"</tt> and \p t2 is <tt>"{B, D, A, C}"</tt>, the associated permutation vector is
+ * <tt>"{2, 0, 3, 1}"</tt>.
+ */
+template <class T>
+inline std::vector<unsigned int> permutation(const T& t1, const T& t2)
+{
+ typedef unsigned int SizeType;
+
+ // Arguments must have the same size
+ if (t1.size() != t2.size()) {return std::vector<SizeType>();}
+
+ std::vector<SizeType> permutation_vector(t1.size()); // Return value
+ for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
+ {
+ typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
+ if (t2.end() == t2_it) {return std::vector<SizeType>();}
+ else
+ {
+ const SizeType t1_dist = std::distance(t1.begin(), t1_it);
+ const SizeType t2_dist = std::distance(t2.begin(), t2_it);
+ permutation_vector[t1_dist] = t2_dist;
+ }
+ }
+ return permutation_vector;
+}
+
+} // namespace
+
+/**
+ * \brief Options used when initializing a joint trajectory from ROS message data.
+ * \sa initJointTrajectory
+ */
+template <class Trajectory>
+struct InitJointTrajectoryOptions
+{
+ typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
+ typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
+ typedef typename Trajectory::value_type::Scalar Scalar;
+
+ InitJointTrajectoryOptions()
+ : current_trajectory(0),
+ joint_names(0),
+ angle_wraparound(0),
+ rt_goal_handle(),
+ default_tolerances(0),
+ other_time_base(0)
+ {}
+
+ Trajectory* current_trajectory;
+ std::vector<std::string>* joint_names;
+ std::vector<bool>* angle_wraparound;
+ RealtimeGoalHandlePtr rt_goal_handle;
+ SegmentTolerances<Scalar>* default_tolerances;
+ ros::Time* other_time_base;
+};
+
+/**
+ * \brief Initialize a joint trajectory from ROS message data.
+ *
+ * \param msg Trajectory message.
+ *
+ * \param time Time from which data is to be extracted. All trajectory points in \p msg occurring \b after
+ * \p time will be extracted; or put otherwise, all points occurring at a time <b>less or equal</b> than \p time
+ * will be discarded. Set this value to zero to process all points in \p msg.
+ *
+ * \param options Options that change how the trajectory gets initialized.
+ *
+ * The \ref InitJointTrajectoryOptions "options" parameter is optional. The meaning of its different members follows:
+ * - \b current_trajectory Currently executed trajectory. Use this parameter if you want to update an existing
+ * trajectory with the data in \p msg; that is, keep the useful parts of \p current_trajectory and \p msg.
+ * If specified, the output trajectory will not only contain data in \p msg occurring \b after \p time, but will also
+ * contain data from \p current_trajectory \b between \p time and the start time of \p msg
+ * (which might not be equal to \p time).
+ *
+ * - \b joint_names Joints expected to be found in \p msg. If specified, this function will return an empty trajectory
+ * when \p msg contains joints that differ in number or names to \p joint_names. If \p msg contains the same joints as
+ * \p joint_names, but in a different order, the resulting trajectory will be ordered according to \p joint_names
+ * (and not \p msg). If unspecified (empty), no checks will be performed against expected joints, and the resulting
+ * trajectory will preserve the joint ordering of \p msg.
+ *
+ * - \b angle_wraparound Vector of booleans where true values correspond to joints that wrap around (ie. are continuous).
+ * If specified, combining \p current_trajectory with \p msg will not result in joints performing multiple turns at the
+ * transition. This parameter \b requires \p current_trajectory to also be specified, otherwise it is ignored.
+ *
+ * - \b rt_goal_handle Action goal handle associated to the new trajectory. If specified, newly added segments will have
+ * a pointer to it, and to the trajectory tolerances it contains (if any).
+ *
+ * - \b default_tolerances Default trajectory tolerances. This option is only used when \p rt_goal_handle is also
+ * specified. It contains the default tolernaces to check when executing an action goal. If the action goal specifies
+ * tolerances (totally or partially), these values will take precedence over the defaults.
+ *
+ * - \b other_time_base When initializing a new trajectory, it might be the case that we desire the result expressed in
+ * a \b different time base than that contained in \p msg. If specified, the value of this variable should be the
+ * equivalent of the \p time parameter, but expressed in the desired time base.
+ * If the \p current_trajectory option is also specified, it must be expressed in \p other_time_base.
+ * The typical usecase for this variable is when the \p current_trajectory option is specified, and contains data in
+ * a different time base (eg. monotonically increasing) than \p msg (eg. system-clock synchronized).
+ *
+ * \return Trajectory container.
+ *
+ * \tparam Trajectory Trajectory type. Should be a \e sequence container \e sorted by segment start time.
+ * Additionally, the contained segment type must implement a constructor with the following signature:
+ * \code
+ * Segment(const ros::Time& traj_start_time,
+ * const trajectory_msgs::JointTrajectoryPoint& start_point,
+ * const trajectory_msgs::JointTrajectoryPoint& end_point,
+ * const std::vector<unsigned int>& permutation,
+ * const std::vector<Scalar>& position_offset)
+ * \endcode
+ * The following function must also be defined to properly handle continuous joints:
+ * \code
+ * std::vector<Scalar> wraparoundOffset(const typename Segment::State& prev_state,
+ * const typename Segment::State& next_state,
+ * const std::vector<bool>& angle_wraparound)
+ * \endcode
+ *
+ * \note This function does not throw any exceptions by itself, but the segment constructor might.
+ * In such a case, this method should be wrapped inside a \p try block.
+ */
+// TODO: Return useful bits of current trajectory if input msg is useless?
+template <class Trajectory>
+Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg,
+ const ros::Time& time,
+ const InitJointTrajectoryOptions<Trajectory>& options =
+ InitJointTrajectoryOptions<Trajectory>())
+{
+ typedef typename Trajectory::value_type Segment;
+ typedef typename Segment::Scalar Scalar;
+
+ const unsigned int n_joints = msg.joint_names.size();
+
+ const ros::Time msg_start_time = internal::startTime(msg, time); // Message start time
+
+ ROS_DEBUG_STREAM("Figuring out new trajectory starting at time "
+ << std::fixed << std::setprecision(3) << msg_start_time.toSec());
+
+ // Empty trajectory
+ if (msg.points.empty())
+ {
+ ROS_DEBUG("Trajectory message contains empty trajectory. Nothing to convert.");
+ return Trajectory();
+ }
+
+ // Non strictly-monotonic waypoints
+ if (!isTimeStrictlyIncreasing(msg))
+ {
+ ROS_ERROR("Trajectory message contains waypoints that are not strictly increasing in time.");
+ return Trajectory();
+ }
+
+ // Validate options
+ const bool has_current_trajectory = options.current_trajectory && !options.current_trajectory->empty();
+ const bool has_joint_names = options.joint_names && !options.joint_names->empty();
+ const bool has_angle_wraparound = options.angle_wraparound && !options.angle_wraparound->empty();
+ const bool has_rt_goal_handle = options.rt_goal_handle;
+ const bool has_other_time_base = options.other_time_base;
+ const bool has_default_tolerances = options.default_tolerances;
+
+ if (!has_current_trajectory && has_angle_wraparound)
+ {
+ ROS_WARN("Vector specifying whether joints wrap around will not be used because no current trajectory was given.");
+ }
+
+ // Compute trajectory start time and data extraction time associated to the 'other' time base, if it applies
+ // The o_ prefix indicates that time values are represented in this 'other' time base.
+ ros::Time o_time;
+ ros::Time o_msg_start_time;
+ if (has_other_time_base)
+ {
+ ros::Duration msg_start_duration = msg_start_time - time;
+ o_time = *options.other_time_base;
+ o_msg_start_time = o_time + msg_start_duration;
+ ROS_DEBUG_STREAM("Using alternate time base. In it, the new trajectory starts at time "
+ << std::fixed << std::setprecision(3) << o_msg_start_time.toSec());
+ }
+ else
+ {
+ o_time = time;
+ o_msg_start_time = msg_start_time;
+ }
+
+ // Permutation vector mapping the expected joint order to the message joint order
+ // If unspecified, a trivial map (no permutation) is computed
+ const std::vector<std::string> joint_names = has_joint_names ? *(options.joint_names) : msg.joint_names;
+
+ std::vector<unsigned int> permutation_vector = internal::permutation(joint_names, msg.joint_names);
+
+ if (permutation_vector.empty())
+ {
+ ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints.");
+ return Trajectory();
+ }
+
+ // Tolerances to be used in all new segments
+ SegmentTolerances<Scalar> tolerances = has_default_tolerances ?
+ *(options.default_tolerances) : SegmentTolerances<Scalar>(n_joints);
+
+ if (has_rt_goal_handle && options.rt_goal_handle->gh_.getGoal())
+ {
+ updateSegmentTolerances<Scalar>(*(options.rt_goal_handle->gh_.getGoal()), joint_names, tolerances);
+ }
+
+ // Find first point of new trajectory occurring after current time
+ // This point is used later on in this function, but is computed here, in advance because if the trajectory message
+ // contains a trajectory in the past, we can quickly return without spending additional computational resources
+ std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator
+ it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time)
+ if (it == msg.points.end())
+ {
+ it = msg.points.begin(); // Entire trajectory is after current time
+ }
+ else
+ {
+ ++it; // Points to first point after current time OR sequence end
+ if (it == msg.points.end())
+ {
+ ros::Duration last_point_dur = time - (msg_start_time + (--it)->time_from_start);
+ ROS_WARN_STREAM("Dropping all " << msg.points.size() <<
+ " trajectory point(s), as they occur before the current time.\n" <<
+ "Last point is " << std::fixed << std::setprecision(3) << last_point_dur.toSec() <<
+ "s in the past.");
+ return Trajectory();
+ }
+ else
+ {
+ ros::Duration next_point_dur = msg_start_time + it->time_from_start - time;
+ ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), it) <<
+ " trajectory point(s) out of " << msg.points.size() <<
+ ", as they occur before the current time.\n" <<
+ "First valid point will be reached in " << std::fixed << std::setprecision(3) <<
+ next_point_dur.toSec() << "s.");
+ }
+ }
+
+ // Initialize result trajectory: combination of:
+ // - Useful segments of currently followed trajectory
+ // - Useful segments of new trajectory (contained in ROS message)
+ Trajectory result_traj; // Currently empty
+
+ // Initialize offsets due to wrapping joints to zero
+ std::vector<Scalar> position_offset(n_joints, 0.0);
+
+ // Bridge current trajectory to new one
+ if (has_current_trajectory)
+ {
+ const Trajectory& curr_traj = *(options.current_trajectory);
+
+ // Get the last time and state that will be executed from the current trajectory
+ const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important!
+ typename Segment::State last_curr_state;
+ sample(curr_traj, last_curr_time, last_curr_state);
+
+ // Get the first time and state that will be executed from the new trajectory
+ const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec();
+ typename Segment::State first_new_state(*it, permutation_vector); // Here offsets are not yet applied
+
+ // Compute offsets due to wrapping joints
+ if (has_angle_wraparound)
+ {
+ position_offset = wraparoundOffset(last_curr_state.position,
+ first_new_state.position,
+ *(options.angle_wraparound));
+ if (position_offset.empty())
+ {
+ ROS_ERROR("Cannot create trajectory from message. "
+ "Vector specifying whether joints wrap around has an invalid size.");
+ return Trajectory();
+ }
+ }
+
+ // Apply offset to first state that will be executed from the new trajectory
+ first_new_state = typename Segment::State(*it, permutation_vector, position_offset); // Now offsets are applied
+
+ // Add useful segments of current trajectory to result
+ {
+ typedef typename Trajectory::const_iterator TrajIter;
+ TrajIter first = findSegment(curr_traj, o_time.toSec()); // Currently active segment
+ TrajIter last = findSegment(curr_traj, last_curr_time); // Segment active when new trajectory starts
+ if (first == curr_traj.end() || last == curr_traj.end())
+ {
+ ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer.");
+ return Trajectory();
+ }
+ result_traj.insert(result_traj.begin(), first, ++last); // Range [first,last) will still be executed
+ }
+
+ // Add segment bridging current and new trajectories to result
+ Segment bridge_seg(last_curr_time, last_curr_state,
+ first_new_time, first_new_state);
+ bridge_seg.setGoalHandle(options.rt_goal_handle);
+ if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances);}
+ result_traj.push_back(bridge_seg);
+ }
+
+ // Constants used in log statement at the end
+ const unsigned int num_old_segments = result_traj.size() -1;
+ const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1;
+
+ // Add useful segments of new trajectory to result
+ // - Construct all trajectory segments occurring after current time
+ // - As long as there remain two trajectory points we can construct the next trajectory segment
+ while (std::distance(it, msg.points.end()) >= 2)
+ {
+ std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator next_it = it; ++next_it;
+ Segment segment(o_msg_start_time, *it, *next_it, permutation_vector, position_offset);
+ segment.setGoalHandle(options.rt_goal_handle);
+ if (has_rt_goal_handle) {segment.setTolerances(tolerances);}
+ result_traj.push_back(segment);
+ ++it;
+ }
+
+ // Useful debug info
+ std::stringstream log_str;
+ log_str << "Trajectory has " << result_traj.size() << " segments";
+ if (has_current_trajectory)
+ {
+ log_str << ":";
+ log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory.";
+ log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message.";
+ if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) <<
+ " points) taken from the input trajectory.";}
+ }
+ else {log_str << ".";}
+ ROS_DEBUG_STREAM(log_str.str());
+
+ return result_traj;
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
new file mode 100644
index 0000000..4aaf291
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
@@ -0,0 +1,277 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian, Stuart Glaser
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H
+#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H
+
+// C++ standard
+#include <cassert>
+#include <iterator>
+#include <stdexcept>
+#include <string>
+
+// Boost
+#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
+
+// ROS
+#include <ros/node_handle.h>
+
+// URDF
+#include <urdf/model.h>
+
+// ROS messages
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <control_msgs/QueryTrajectoryState.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+// actionlib
+#include <actionlib/server/action_server.h>
+
+// realtime_tools
+#include <realtime_tools/realtime_box.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+// ros_controls
+#include <realtime_tools/realtime_server_goal_handle.h>
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/internal/demangle_symbol.h>
+
+// Project
+#include <trajectory_interface/trajectory_interface.h>
+
+#include <joint_trajectory_controller/joint_trajectory_segment.h>
+#include <joint_trajectory_controller/init_joint_trajectory.h>
+#include <joint_trajectory_controller/hardware_interface_adapter.h>
+
+namespace joint_trajectory_controller
+{
+
+/**
+ * \brief Controller for executing joint-space trajectories on a group of joints.
+ *
+ * \note Non-developer documentation and usage instructions can be found in the package's ROS wiki page.
+ *
+ * \tparam SegmentImpl Trajectory segment representation to use. The type must comply with the following structure:
+ * \code
+ * class FooSegment
+ * {
+ * public:
+ * // Required types
+ * typedef double Scalar; // Scalar can be anything convertible to double
+ * typedef Scalar Time;
+ * typedef PosVelAccState<Scalar> State;
+ *
+ * // Default constructor
+ * FooSegment();
+ *
+ * // Constructor from start and end states (boundary conditions)
+ * FooSegment(const Time& start_time,
+ * const State& start_state,
+ * const Time& end_time,
+ * const State& end_state);
+ *
+ * // Start and end states initializer (the guts of the above constructor)
+ * // May throw std::invalid_argument if parameters are invalid
+ * void init(const Time& start_time,
+ * const State& start_state,
+ * const Time& end_time,
+ * const State& end_state);
+ *
+ * // Sampler (realtime-safe)
+ * void sample(const Time& time, State& state) const;
+ *
+ * // Accesors (realtime-safe)
+ * Time startTime() const;
+ * Time endTime() const;
+ * unsigned int size() const;
+ * };
+ * \endcode
+ *
+ * \tparam HardwareInterface Controller hardware interface. Currently \p hardware_interface::PositionJointInterface,
+ * \p hardware_interface::VelocityJointInterface, and \p hardware_interface::EffortJointInterface are supported
+ * out-of-the-box.
+ */
+template <class SegmentImpl, class HardwareInterface>
+class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>
+{
+public:
+
+ JointTrajectoryController();
+
+ /** \name Non Real-Time Safe Functions
+ *\{*/
+ bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+ /*\}*/
+
+ /** \name Real-Time Safe Functions
+ *\{*/
+ /** \brief Holds the current position. */
+ void starting(const ros::Time& time);
+
+ /** \brief Cancels the active action goal, if any. */
+ void stopping(const ros::Time& /*time*/);
+
+ void update(const ros::Time& time, const ros::Duration& period);
+ /*\}*/
+
+private:
+
+ struct TimeData
+ {
+ TimeData() : time(0.0), period(0.0), uptime(0.0) {}
+
+ ros::Time time; ///< Time of last update cycle
+ ros::Duration period; ///< Period of last update cycle
+ ros::Time uptime; ///< Controller uptime. Set to zero at every restart.
+ };
+
+ typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> ActionServer;
+ typedef boost::shared_ptr<ActionServer> ActionServerPtr;
+ typedef ActionServer::GoalHandle GoalHandle;
+ typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
+ typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
+ typedef trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr;
+ typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> StatePublisher;
+ typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;
+
+ typedef JointTrajectorySegment<SegmentImpl> Segment;
+ typedef std::vector<Segment> Trajectory;
+ typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
+ typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
+ typedef typename Segment::Scalar Scalar;
+
+ typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> HwIfaceAdapter;
+ typedef typename HardwareInterface::ResourceHandleType JointHandle;
+
+ bool verbose_; ///< Hard coded verbose flag to help in debugging
+ std::string name_; ///< Controller name.
+ std::vector<JointHandle> joints_; ///< Handles to controlled joints.
+ std::vector<bool> angle_wraparound_; ///< Whether controlled joints wrap around or not.
+ std::vector<std::string> joint_names_; ///< Controlled joint names.
+ SegmentTolerances<Scalar> default_tolerances_; ///< Default trajectory segment tolerances.
+ HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface.
+
+ RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any.
+
+ /**
+ * Thread-safe container with a smart pointer to trajectory currently being followed.
+ * Can be either a hold trajectory or a trajectory received from a ROS message.
+ *
+ * We store the hold trajectory in a separate class member because the \p starting(time) method must be realtime-safe.
+ * The (single segment) hold trajectory is preallocated at initialization time and its size is kept unchanged.
+ */
+ TrajectoryBox curr_trajectory_box_;
+ TrajectoryPtr hold_trajectory_ptr_; ///< Last hold trajectory values.
+
+ typename Segment::State current_state_; ///< Preallocated workspace variable.
+ typename Segment::State desired_state_; ///< Preallocated workspace variable.
+ typename Segment::State state_error_; ///< Preallocated workspace variable.
+ typename Segment::State hold_start_state_; ///< Preallocated workspace variable.
+ typename Segment::State hold_end_state_; ///< Preallocated workspace variable.
+
+ realtime_tools::RealtimeBuffer<TimeData> time_data_;
+
+ ros::Duration state_publisher_period_;
+ ros::Duration action_monitor_period_;
+
+ typename Segment::Time stop_trajectory_duration_;
+
+ // ROS API
+ ros::NodeHandle controller_nh_;
+ ros::Subscriber trajectory_command_sub_;
+ ActionServerPtr action_server_;
+ ros::ServiceServer query_state_service_;
+ StatePublisherPtr state_publisher_;
+
+ ros::Timer goal_handle_timer_;
+ ros::Time last_state_publish_time_;
+
+ bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh);
+ void trajectoryCommandCB(const JointTrajectoryConstPtr& msg);
+ void goalCB(GoalHandle gh);
+ void cancelCB(GoalHandle gh);
+ void preemptActiveGoal();
+ bool queryStateService(control_msgs::QueryTrajectoryState::Request& req,
+ control_msgs::QueryTrajectoryState::Response& resp);
+
+ /**
+ * \brief Publish current controller state at a throttled frequency.
+ * \note This method is realtime-safe and is meant to be called from \ref update, as it shares data with it without
+ * any locking.
+ */
+ void publishState(const ros::Time& time);
+
+ /**
+ * \brief Hold the current position.
+ *
+ * Substitutes the current trajectory with a single-segment one going from the current position and velocity to the
+ * current position and zero velocity.
+ * \note This method is realtime-safe.
+ */
+ void setHoldPosition(const ros::Time& time);
+
+ /**
+ * \brief Check path tolerances.
+ *
+ * If path tolerances are violated, the currently active action goal will be aborted.
+ *
+ * \param state_error Error between the current and desired trajectory states.
+ * \param segment Currently active trajectory segment.
+ *
+ * \pre \p segment is associated to the active goal handle.
+ **/
+ void checkPathTolerances(const typename Segment::State& state_error,
+ const Segment& segment);
+
+ /**
+ * \brief Check goal tolerances.
+ *
+ * If goal tolerances are fulfilled, the currently active action goal will be considered successful.
+ * If they are violated, the action goal will be aborted.
+ *
+ * \param state_error Error between the current and desired trajectory states.
+ * \param segment Currently active trajectory segment.
+ *
+ * \pre \p segment is associated to the active goal handle.
+ **/
+ void checkGoalTolerances(const typename Segment::State& state_error,
+ const Segment& segment);
+
+};
+
+} // namespace
+
+#include <joint_trajectory_controller/joint_trajectory_controller_impl.h>
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
new file mode 100644
index 0000000..2455348
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
@@ -0,0 +1,712 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian, Stuart Glaser
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H
+#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_IMP_H
+
+
+namespace joint_trajectory_controller
+{
+
+namespace internal
+{
+
+template <class Enclosure, class Member>
+inline boost::shared_ptr<Member> share_member(boost::shared_ptr<Enclosure> enclosure, Member &member)
+{
+ actionlib::EnclosureDeleter<Enclosure> d(enclosure);
+ boost::shared_ptr<Member> p(&member, d);
+ return p;
+}
+
+std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
+{
+ using namespace XmlRpc;
+ XmlRpcValue xml_array;
+ if (!nh.getParam(param_name, xml_array))
+ {
+ ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ if (xml_array.getType() != XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR_STREAM("The '" << param_name << "' parameter is not an array (namespace: " <<
+ nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+
+ std::vector<std::string> out;
+ for (int i = 0; i < xml_array.size(); ++i)
+ {
+ if (xml_array[i].getType() != XmlRpcValue::TypeString)
+ {
+ ROS_ERROR_STREAM("The '" << param_name << "' parameter contains a non-string element (namespace: " <<
+ nh.getNamespace() << ").");
+ return std::vector<std::string>();
+ }
+ out.push_back(static_cast<std::string>(xml_array[i]));
+ }
+ return out;
+}
+
+boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+{
+ boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+
+ std::string urdf_str;
+ // Check for robot_description in proper namespace
+ if (nh.getParam(param_name, urdf_str))
+ {
+ if (!urdf->initString(urdf_str))
+ {
+ ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
+ nh.getNamespace() << ").");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ }
+ // Check for robot_description in root
+ else if (!urdf->initParam("robot_description"))
+ {
+ ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
+ return boost::shared_ptr<urdf::Model>();
+ }
+ return urdf;
+}
+
+typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
+std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
+{
+ std::vector<UrdfJointConstPtr> out;
+ for (unsigned int i = 0; i < joint_names.size(); ++i)
+ {
+ UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+ if (urdf_joint)
+ {
+ out.push_back(urdf_joint);
+ }
+ else
+ {
+ ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
+ return std::vector<UrdfJointConstPtr>();
+ }
+ }
+ return out;
+}
+
+std::string getLeafNamespace(const ros::NodeHandle& nh)
+{
+ const std::string complete_ns = nh.getNamespace();
+ std::size_t id = complete_ns.find_last_of("/");
+ return complete_ns.substr(id + 1);
+}
+
+} // namespace
+
+template <class SegmentImpl, class HardwareInterface>
+inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+starting(const ros::Time& time)
+{
+ // Update time data
+ TimeData time_data;
+ time_data.time = time;
+ time_data.uptime = ros::Time(0.0);
+ time_data_.initRT(time_data);
+
+ // Hold current position
+ setHoldPosition(time_data.uptime);
+
+ // Initialize last state update time
+ last_state_publish_time_ = time_data.uptime;
+
+ // Hardware interface adapter
+ hw_iface_adapter_.starting(time_data.uptime);
+}
+
+template <class SegmentImpl, class HardwareInterface>
+inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+stopping(const ros::Time& /*time*/)
+{
+ preemptActiveGoal();
+}
+
+template <class SegmentImpl, class HardwareInterface>
+inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+trajectoryCommandCB(const JointTrajectoryConstPtr& msg)
+{
+ const bool update_ok = updateTrajectoryCommand(msg, RealtimeGoalHandlePtr());
+ if (update_ok) {preemptActiveGoal();}
+}
+
+template <class SegmentImpl, class HardwareInterface>
+inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+preemptActiveGoal()
+{
+ RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+ // Cancels the currently active goal
+ if (current_active_goal)
+ {
+ // Marks the current goal as canceled
+ rt_active_goal_.reset();
+ current_active_goal->gh_.setCanceled();
+ }
+}
+
+template <class SegmentImpl, class HardwareInterface>
+inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+checkPathTolerances(const typename Segment::State& state_error,
+ const Segment& segment)
+{
+ const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
+ const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
+ if (!checkStateTolerance(state_error, tolerances.state_tolerance))
+ {
+ rt_segment_goal->preallocated_result_->error_code =
+ control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
+ rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ }
+}
+
+template <class SegmentImpl, class HardwareInterface>
+inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+checkGoalTolerances(const typename Segment::State& state_error,
+ const Segment& segment)
+{
+ // Controller uptime
+ const ros::Time uptime = time_data_.readFromRT()->uptime;
+
+ // Checks that we have ended inside the goal tolerances
+ const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
+ const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
+ const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance);
+
+ if (inside_goal_tolerances)
+ {
+ rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
+ rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ }
+ else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance)
+ {
+ // Still have some time left to meet the goal state tolerances
+ }
+ else
+ {
+ if (verbose_)
+ {
+ ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed");
+ // Check the tolerances one more time to output the errors that occures
+ checkStateTolerance(state_error, tolerances.goal_state_tolerance, true);
+ }
+
+ rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
+ rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ }
+}
+
+template <class SegmentImpl, class HardwareInterface>
+JointTrajectoryController<SegmentImpl, HardwareInterface>::
+JointTrajectoryController()
+ : verbose_(false), // Set to true during debugging
+ hold_trajectory_ptr_(new Trajectory)
+{}
+
+template <class SegmentImpl, class HardwareInterface>
+bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInterface* hw,
+ ros::NodeHandle& root_nh,
+ ros::NodeHandle& controller_nh)
+{
+ using namespace internal;
+
+ // Cache controller node handle
+ controller_nh_ = controller_nh;
+
+ // Controller name
+ name_ = getLeafNamespace(controller_nh_);
+
+ // State publish rate
+ double state_publish_rate = 50.0;
+ controller_nh_.getParam("state_publish_rate", state_publish_rate);
+ ROS_DEBUG_STREAM_NAMED(name_, "Controller state will be published at " << state_publish_rate << "Hz.");
+ state_publisher_period_ = ros::Duration(1.0 / state_publish_rate);
+
+ // Action status checking update rate
+ double action_monitor_rate = 20.0;
+ controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
+ action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
+ ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");
+
+ // Stop trajectory duration
+ stop_trajectory_duration_ = 0.0;
+ if (!controller_nh_.getParam("stop_trajectory_duration", stop_trajectory_duration_))
+ {
+ // TODO: Remove this check/warning in Indigo
+ if (controller_nh_.getParam("hold_trajectory_duration", stop_trajectory_duration_))
+ {
+ ROS_WARN("The 'hold_trajectory_duration' has been deprecated in favor of the 'stop_trajectory_duration' parameter. Please update your controller configuration.");
+ }
+ }
+ ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s.");
+
+ // List of controlled joints
+ joint_names_ = getStrings(controller_nh_, "joints");
+ if (joint_names_.empty()) {return false;}
+ const unsigned int n_joints = joint_names_.size();
+
+ // URDF joints
+ boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
+ if (!urdf) {return false;}
+
+ std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
+ if (urdf_joints.empty()) {return false;}
+ assert(n_joints == urdf_joints.size());
+
+ // Initialize members
+ joints_.resize(n_joints);
+ angle_wraparound_.resize(n_joints);
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ // Joint handle
+ try {joints_[i] = hw->getHandle(joint_names_[i]);}
+ catch (...)
+ {
+ ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_names_[i] << "' in '" <<
+ this->getHardwareInterfaceType() << "'.");
+ return false;
+ }
+
+ // Whether a joint is continuous (ie. has angle wraparound)
+ angle_wraparound_[i] = urdf_joints[i]->type == urdf::Joint::CONTINUOUS;
+ const std::string not_if = angle_wraparound_[i] ? "" : "non-";
+
+ ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" <<
+ this->getHardwareInterfaceType() << "'.");
+ }
+
+ assert(joints_.size() == angle_wraparound_.size());
+ ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
+ "\n- Number of joints: " << joints_.size() <<
+ "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" <<
+ "\n- Trajectory segment type: '" << hardware_interface::internal::demangledTypeName<SegmentImpl>() << "'");
+
+ // Default tolerances
+ ros::NodeHandle tol_nh(controller_nh_, "constraints");
+ default_tolerances_ = getSegmentTolerances<Scalar>(tol_nh, joint_names_);
+
+ // Hardware interface adapter
+ hw_iface_adapter_.init(joints_, controller_nh_);
+
+ // ROS API: Subscribed topics
+ trajectory_command_sub_ = controller_nh_.subscribe("command", 1, &JointTrajectoryController::trajectoryCommandCB, this);
+
+ // ROS API: Published topics
+ state_publisher_.reset(new StatePublisher(controller_nh_, "state", 1));
+
+ // ROS API: Action interface
+ action_server_.reset(new ActionServer(controller_nh_, "follow_joint_trajectory",
+ boost::bind(&JointTrajectoryController::goalCB, this, _1),
+ boost::bind(&JointTrajectoryController::cancelCB, this, _1),
+ false));
+ action_server_->start();
+
+ // ROS API: Provided services
+ query_state_service_ = controller_nh_.advertiseService("query_state",
+ &JointTrajectoryController::queryStateService,
+ this);
+
+ // Preeallocate resources
+ current_state_ = typename Segment::State(n_joints);
+ desired_state_ = typename Segment::State(n_joints);
+ state_error_ = typename Segment::State(n_joints);
+ hold_start_state_ = typename Segment::State(n_joints);
+ hold_end_state_ = typename Segment::State(n_joints);
+
+ Segment hold_segment(0.0, current_state_, 0.0, current_state_);
+ hold_trajectory_ptr_->resize(1, hold_segment);
+
+ {
+ state_publisher_->lock();
+ state_publisher_->msg_.joint_names = joint_names_;
+ state_publisher_->msg_.desired.positions.resize(n_joints);
+ state_publisher_->msg_.desired.velocities.resize(n_joints);
+ state_publisher_->msg_.desired.accelerations.resize(n_joints);
+ state_publisher_->msg_.actual.positions.resize(n_joints);
+ state_publisher_->msg_.actual.velocities.resize(n_joints);
+ state_publisher_->msg_.error.positions.resize(n_joints);
+ state_publisher_->msg_.error.velocities.resize(n_joints);
+ state_publisher_->unlock();
+ }
+
+ return true;
+}
+
+template <class SegmentImpl, class HardwareInterface>
+void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+update(const ros::Time& time, const ros::Duration& period)
+{
+ // Get currently followed trajectory
+ TrajectoryPtr curr_traj_ptr;
+ curr_trajectory_box_.get(curr_traj_ptr);
+ Trajectory& curr_traj = *curr_traj_ptr;
+
+ // Update time data
+ TimeData time_data;
+ time_data.time = time; // Cache current time
+ time_data.period = period; // Cache current control period
+ time_data.uptime = time_data_.readFromRT()->uptime + period; // Update controller uptime
+ time_data_.writeFromNonRT(time_data); // TODO: Grrr, we need a lock-free data structure here!
+
+ // NOTE: It is very important to execute the two above code blocks in the specified sequence: first get current
+ // trajectory, then update time data. Hopefully the following paragraph sheds a bit of light on the rationale.
+ // The non-rt thread responsible for processing new commands enqueues trajectories that can start at the _next_
+ // control cycle (eg. zero start time) or later (eg. when we explicitly request a start time in the future).
+ // If we reverse the order of the two blocks above, and update the time data first; it's possible that by the time we
+ // fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts in the
+ // next control cycle, leaving the current cycle without a valid trajectory.
+
+ // Update desired state: sample trajectory at current time
+ typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_);
+ if (curr_traj.end() == segment_it)
+ {
+ // Non-realtime safe, but should never happen under normal operation
+ ROS_ERROR_NAMED(name_,
+ "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
+ return;
+ }
+
+ // Update current state and state error
+ for (unsigned int i = 0; i < joints_.size(); ++i)
+ {
+ current_state_.position[i] = joints_[i].getPosition();
+ current_state_.velocity[i] = joints_[i].getVelocity();
+ // There's no acceleration data available in a joint handle
+
+ state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
+ state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
+ state_error_.acceleration[i] = 0.0;
+ }
+
+ // Check tolerances if segment corresponds to currently active action goal
+ const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
+ if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
+ {
+ // Check tolerances
+ if (time_data.uptime.toSec() < segment_it->endTime())
+ {
+ // Currently executing a segment: check path tolerances
+ checkPathTolerances(state_error_,
+ *segment_it);
+ }
+ else if (segment_it == --curr_traj.end())
+ {
+ if (verbose_)
+ ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances");
+
+ // Finished executing the LAST segment: check goal tolerances
+ checkGoalTolerances(state_error_,
+ *segment_it);
+ }
+ }
+
+ // Hardware interface adapter: Generate and send commands
+ hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period,
+ desired_state_, state_error_);
+
+ // Publish state
+ publishState(time_data.uptime);
+}
+
+template <class SegmentImpl, class HardwareInterface>
+bool JointTrajectoryController<SegmentImpl, HardwareInterface>::
+updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh)
+{
+ typedef InitJointTrajectoryOptions<Trajectory> Options;
+
+ // Preconditions
+ if (!this->isRunning())
+ {
+ ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
+ return false;
+ }
+
+ if (!msg)
+ {
+ ROS_WARN_NAMED(name_, "Received null-pointer trajectory message, skipping.");
+ return false;
+ }
+
+ // Time data
+ TimeData* time_data = time_data_.readFromRT(); // TODO: Grrr, we need a lock-free data structure here!
+
+ // Time of the next update
+ const ros::Time next_update_time = time_data->time + time_data->period;
+
+ // Uptime of the next update
+ ros::Time next_update_uptime = time_data->uptime + time_data->period;
+
+ // Hold current position if trajectory is empty
+ if (msg->points.empty())
+ {
+ setHoldPosition(time_data->uptime);
+ ROS_DEBUG_NAMED(name_, "Empty trajectory command, stopping.");
+ return true;
+ }
+
+ // Trajectory initialization options
+ TrajectoryPtr curr_traj_ptr;
+ curr_trajectory_box_.get(curr_traj_ptr);
+
+ Options options;
+ options.other_time_base = &next_update_uptime;
+ options.current_trajectory = curr_traj_ptr.get();
+ options.joint_names = &joint_names_;
+ options.angle_wraparound = &angle_wraparound_;
+ options.rt_goal_handle = gh;
+ options.default_tolerances = &default_tolerances_;
+
+ // Update currently executing trajectory
+ try
+ {
+ TrajectoryPtr traj_ptr(new Trajectory);
+ *traj_ptr = initJointTrajectory<Trajectory>(*msg, next_update_time, options);
+ if (!traj_ptr->empty())
+ {
+ curr_trajectory_box_.set(traj_ptr);
+ }
+ else
+ {
+ // All trajectory points are in the past, nothing new to execute. Keep on executing current trajectory
+ return false;
+ }
+ }
+ catch(const std::invalid_argument& ex)
+ {
+ ROS_ERROR_STREAM_NAMED(name_, ex.what());
+ return false;
+ }
+ catch(...)
+ {
+ ROS_ERROR_NAMED(name_, "Unexpected exception caught when initializing trajectory from ROS message data.");
+ return false;
+ }
+
+ return true;
+}
+
+template <class SegmentImpl, class HardwareInterface>
+void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+goalCB(GoalHandle gh)
+{
+ ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
+
+ // Precondition: Running controller
+ if (!this->isRunning())
+ {
+ ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running.");
+ control_msgs::FollowJointTrajectoryResult result;
+ result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; // TODO: Add better error status to msg?
+ gh.setRejected(result);
+ return;
+ }
+
+ // Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case
+ using internal::permutation;
+ std::vector<unsigned int> permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names);
+
+ if (permutation_vector.empty())
+ {
+ ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
+ control_msgs::FollowJointTrajectoryResult result;
+ result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
+ gh.setRejected(result);
+ return;
+ }
+
+ // Try to update new trajectory
+ RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
+ const bool update_ok = updateTrajectoryCommand(internal::share_member(gh.getGoal(), gh.getGoal()->trajectory),
+ rt_goal);
+
+ if (update_ok)
+ {
+ // Accept new goal
+ preemptActiveGoal();
+ gh.setAccepted();
+ rt_active_goal_ = rt_goal;
+
+ // Setup goal status checking timer
+ goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
+ &RealtimeGoalHandle::runNonRealtime,
+ rt_goal);
+ goal_handle_timer_.start();
+ }
+ else
+ {
+ // Reject invalid goal
+ control_msgs::FollowJointTrajectoryResult result;
+ result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
+ gh.setRejected(result);
+ }
+}
+
+template <class SegmentImpl, class HardwareInterface>
+void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+cancelCB(GoalHandle gh)
+{
+ RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+
+ // Check that cancel request refers to currently active goal (if any)
+ if (current_active_goal && current_active_goal->gh_ == gh)
+ {
+ // Reset current goal
+ rt_active_goal_.reset();
+
+ // Controller uptime
+ const ros::Time uptime = time_data_.readFromRT()->uptime;
+
+ // Enter hold current position mode
+ setHoldPosition(uptime);
+ ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");
+
+ // Mark the current goal as canceled
+ current_active_goal->gh_.setCanceled();
+ }
+}
+
+template <class SegmentImpl, class HardwareInterface>
+bool JointTrajectoryController<SegmentImpl, HardwareInterface>::
+queryStateService(control_msgs::QueryTrajectoryState::Request& req,
+ control_msgs::QueryTrajectoryState::Response& resp)
+{
+ // Preconditions
+ if (!this->isRunning())
+ {
+ ROS_ERROR_NAMED(name_, "Can't sample trajectory. Controller is not running.");
+ return false;
+ }
+
+ // Convert request time to internal monotonic representation
+ TimeData* time_data = time_data_.readFromRT();
+ const ros::Duration time_offset = req.time - time_data->time;
+ const ros::Time sample_time = time_data->uptime + time_offset;
+
+ // Sample trajectory at requested time
+ TrajectoryPtr curr_traj_ptr;
+ curr_trajectory_box_.get(curr_traj_ptr);
+ Trajectory& curr_traj = *curr_traj_ptr;
+
+ typename Segment::State state;
+ typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state);
+ if (curr_traj.end() == segment_it)
+ {
+ ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time.");
+ return false;
+ }
+
+ // Populate response
+ resp.name = joint_names_;
+ resp.position = state.position;
+ resp.velocity = state.velocity;
+ resp.acceleration = state.acceleration;
+
+ return true;
+}
+
+template <class SegmentImpl, class HardwareInterface>
+void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+publishState(const ros::Time& time)
+{
+ // Check if it's time to publish
+ if (!state_publisher_period_.isZero() && last_state_publish_time_ + state_publisher_period_ < time)
+ {
+ if (state_publisher_ && state_publisher_->trylock())
+ {
+ last_state_publish_time_ += state_publisher_period_;
+
+ state_publisher_->msg_.header.stamp = time_data_.readFromRT()->time;
+ state_publisher_->msg_.desired.positions = desired_state_.position;
+ state_publisher_->msg_.desired.velocities = desired_state_.velocity;
+ state_publisher_->msg_.desired.accelerations = desired_state_.acceleration;
+ state_publisher_->msg_.actual.positions = current_state_.position;
+ state_publisher_->msg_.actual.velocities = current_state_.velocity;
+ state_publisher_->msg_.error.positions = state_error_.position;
+ state_publisher_->msg_.error.velocities = state_error_.velocity;
+
+ state_publisher_->unlockAndPublish();
+ }
+ }
+}
+
+template <class SegmentImpl, class HardwareInterface>
+void JointTrajectoryController<SegmentImpl, HardwareInterface>::
+setHoldPosition(const ros::Time& time)
+{
+ // Settle position in a fixed time. We do the following:
+ // - Create segment that goes from current (pos,vel) to (pos,-vel) in 2x the desired stop time
+ // - Assuming segment symmetry, sample segment at its midpoint (desired stop time). It should have zero velocity
+ // - Create segment that goes from current state to above zero velocity state, in the desired time
+ // NOTE: The symmetry assumption from the second point above might not hold for all possible segment types
+
+ assert(1 == hold_trajectory_ptr_->size());
+
+ const typename Segment::Time start_time = time.toSec();
+ const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_;
+ const typename Segment::Time end_time_2x = time.toSec() + 2.0 * stop_trajectory_duration_;
+
+ // Create segment that goes from current (pos,vel) to (pos,-vel)
+ const unsigned int n_joints = joints_.size();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ hold_start_state_.position[i] = joints_[i].getPosition();
+ hold_start_state_.velocity[i] = joints_[i].getVelocity();
+ hold_start_state_.acceleration[i] = 0.0;
+
+ hold_end_state_.position[i] = joints_[i].getPosition();
+ hold_end_state_.velocity[i] = -joints_[i].getVelocity();
+ hold_end_state_.acceleration[i] = 0.0;
+ }
+ hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
+ end_time_2x, hold_end_state_);
+
+ // Sample segment at its midpoint, that should have zero velocity
+ hold_trajectory_ptr_->front().sample(end_time, hold_end_state_);
+
+ // Now create segment that goes from current state to one with zero end velocity
+ hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
+ end_time, hold_end_state_);
+
+ curr_trajectory_box_.set(hold_trajectory_ptr_);
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_msg_utils.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_msg_utils.h
new file mode 100644
index 0000000..780587d
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_msg_utils.h
@@ -0,0 +1,170 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_MSG_UTILS_H
+#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_MSG_UTILS_H
+
+#include <algorithm>
+#include <iterator>
+#include <string>
+#include <vector>
+
+#include <ros/console.h>
+#include <ros/time.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include <trajectory_interface/trajectory_interface.h>
+
+namespace joint_trajectory_controller
+{
+namespace internal
+{
+
+class IsBeforePoint
+{
+public:
+ IsBeforePoint(const ros::Time& msg_start_time) : msg_start_time_(msg_start_time) {}
+
+ bool operator()(const ros::Time& time, const trajectory_msgs::JointTrajectoryPoint& point)
+ {
+ const ros::Time point_start_time = msg_start_time_ + point.time_from_start;
+ return time < point_start_time;
+ }
+
+private:
+ ros::Time msg_start_time_;
+};
+
+/**
+ * \param msg Trajectory message.
+ * \param time Trajectory start time, if unspecified in message.
+ * \return Start time specified in message. If unspecified (set to zero) return \p time.
+ */
+inline ros::Time startTime(const trajectory_msgs::JointTrajectory& msg,
+ const ros::Time& time)
+{
+ return msg.header.stamp.isZero() ? time : msg.header.stamp;
+}
+
+} // namespace
+
+/**
+ * \param point Trajectory point message.
+ * \param joint_dim Expected dimension of the position, velocity and acceleration fields.
+ * \return True if sizes of the position, velocity and acceleration fields are consistent. An empty field means that
+ * it's unspecified, so in this particular case its dimension must not coincide with \p joint_dim.
+ */
+inline bool isValid(const trajectory_msgs::JointTrajectoryPoint& point, const unsigned int joint_dim)
+{
+ if (!point.positions.empty() && point.positions.size() != joint_dim) {return false;}
+ if (!point.velocities.empty() && point.velocities.size() != joint_dim) {return false;}
+ if (!point.accelerations.empty() && point.accelerations.size() != joint_dim) {return false;}
+ return true;
+}
+
+/**
+ * \param msg Trajectory message.
+ * \return True if sizes of input message are consistent (joint names, position, velocity, acceleration).
+ */
+inline bool isValid(const trajectory_msgs::JointTrajectory& msg)
+{
+ const std::vector<std::string>::size_type joint_dim = msg.joint_names.size();
+
+ typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator PointConstIterator;
+
+ for (PointConstIterator it = msg.points.begin(); it != msg.points.end(); ++it)
+ {
+ const std::iterator_traits<PointConstIterator>::difference_type index = std::distance(msg.points.begin(), it);
+
+ if(!isValid(*it, joint_dim))
+ {
+ ROS_ERROR_STREAM("Invalid trajectory point at index: " << index <<
+ ". Size mismatch in joint names, position, velocity or acceleration data.");
+ return false;
+ }
+ }
+ return true;
+}
+
+/**
+ * \param msg Trajectory message.
+ * \return True if each trajectory waypoint is reached at a later time than its predecessor.
+ */
+inline bool isTimeStrictlyIncreasing(const trajectory_msgs::JointTrajectory& msg)
+{
+ if (msg.points.size() < 2) {return true;}
+
+ typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator PointConstIterator;
+
+ PointConstIterator it = msg.points.begin();
+ PointConstIterator end_it = --msg.points.end();
+ while (it != end_it)
+ {
+ const ros::Duration& t1 = it->time_from_start;
+ const ros::Duration& t2 = (++it)->time_from_start;
+ if (t1 >= t2) {return false;}
+ }
+ return true;
+}
+
+/**
+ * \brief Find an iterator to the trajectory point with the greatest start time < \p time.
+ *
+ * \param msg Trajectory message.
+ * \param time Time to search for in the range.
+ *
+ * \return Iterator to the trajectory point with the greatest start time < \p time.
+ * If all points occur at a time greater than \p time, then \p the points sequence end is returned.
+ *
+ * \pre The points in \p msg have monotonically increasing times.
+ *
+ * \note On average, this method has logarithmic time complexity when used on containers with \b random-access iterators.
+ * On \b non-random-access iterators, iterator advances incur an additional linear time cost.
+ */
+inline std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator
+findPoint(const trajectory_msgs::JointTrajectory& msg,
+ const ros::Time& time)
+{
+ // Message trajectory start time
+ // If message time is == 0.0, the trajectory should start at the current time
+ const ros::Time msg_start_time = internal::startTime(msg, time);
+ internal::IsBeforePoint isBeforePoint(msg_start_time);
+
+ typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator ConstIterator;
+ const ConstIterator first = msg.points.begin();
+ const ConstIterator last = msg.points.end();
+
+ return (first == last || isBeforePoint(time, *first))
+ ? last // Optimization when time preceeds all segments, or when an empty range is passed
+ : --std::upper_bound(first, last, time, isBeforePoint); // Notice decrement operator
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
new file mode 100644
index 0000000..3c024c7
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
@@ -0,0 +1,280 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_SEGMENT_H
+#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_SEGMENT_H
+
+// C++ standard
+#include <cmath>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+// angles
+#include <angles/angles.h>
+
+// ROS messages
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+// ros_controls
+#include <realtime_tools/realtime_server_goal_handle.h>
+
+// Project
+#include <joint_trajectory_controller/joint_trajectory_msg_utils.h>
+#include <joint_trajectory_controller/tolerances.h>
+
+namespace joint_trajectory_controller
+{
+
+/**
+ * \brief Class representing a multi-dimensional quintic spline segment with a start and end time.
+ *
+ * It additionally allows to construct the segment and its state type from ROS message data.
+ *
+ * \tparam Segment Segment type. The state type (\p Segment::State) must define a \p Scalar type
+ * (\p Segment::State::Scalar), which can be anything convertible to a \p double; and have the following public members:
+ * \code
+ * std::vector<Scalar> position;
+ * std::vector<Scalar> velocity;
+ * std::vector<Scalar> acceleration;
+ * \endcode
+ */
+template <class Segment>
+class JointTrajectorySegment : public Segment
+{
+public:
+ typedef typename Segment::Scalar Scalar;
+ typedef typename Segment::Time Time;
+
+ typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
+ typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
+
+ struct State : public Segment::State
+ {
+ typedef typename Segment::State::Scalar Scalar;
+ State() : Segment::State() {}
+ State(unsigned int size) : Segment::State(size) {}
+
+ /**
+ * \param point Trajectory point.
+ *
+ * \param permutation Permutation vector for mapping the joint order of a \p point to a desired order.
+ * For instance, if \p point contains data associated to joints <tt>"{B, D, A, C}"</tt>, and we are interested in
+ * constructing a segment with joints ordered as <tt>"{A, B, C, D}"</tt>, the permutation vector should
+ * be set to <tt>"{2, 0, 3, 1}"</tt>.
+ * If unspecified (empty), the joint order of \p point is preserved; if specified, its size must coincide with that
+ * of \p point.
+ * This vector can be computed using the \ref trajectory_interface::internal::permutation()
+ * "permutation" utility function.
+ *
+ * \param position_offset Position offset to applpy to the data in \p point. This parameter is useful for handling
+ * joints that wrap around (ie. continuous), to compensate for multi-turn offsets.
+ * If unspecified (empty), zero offsets are applied; if specified, its size must coincide with that of \p point.
+ *
+ * \note The offsets in \p position_offsets correspond to joints not ordered according to \p point, but to joints
+ * in the expected order, that is \p point with \p permutation applied to it.
+ */
+ State(const trajectory_msgs::JointTrajectoryPoint& point,
+ const std::vector<unsigned int>& permutation = std::vector<unsigned int>(),
+ const std::vector<Scalar>& position_offset = std::vector<Scalar>())
+ {
+ init(point, permutation, position_offset);
+ }
+
+ void init(const trajectory_msgs::JointTrajectoryPoint& point,
+ const std::vector<unsigned int>& permutation = std::vector<unsigned int>(),
+ const std::vector<Scalar>& position_offset = std::vector<Scalar>())
+ {
+ using std::invalid_argument;
+
+ const unsigned int joint_dim = point.positions.size();
+
+ // Preconditions
+ if (!isValid(point, joint_dim))
+ {
+ throw(invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
+ }
+ if (!permutation.empty() && joint_dim != permutation.size())
+ {
+ throw(invalid_argument("Size mismatch between trajectory point and permutation vector."));
+ }
+ for (unsigned int i = 0; i < permutation.size(); ++i)
+ {
+ if (permutation[i] >= joint_dim)
+ {
+ throw(invalid_argument("Permutation vector contains out-of-range indices."));
+ }
+ }
+ if (!position_offset.empty() && joint_dim != position_offset.size())
+ {
+ throw(invalid_argument("Size mismatch between trajectory point "
+ "and vector specifying whether joints wrap around."));
+ }
+
+ // Initialize state
+ if (!point.positions.empty()) {this->position.resize(joint_dim);}
+ if (!point.velocities.empty()) {this->velocity.resize(joint_dim);}
+ if (!point.accelerations.empty()) {this->acceleration.resize(joint_dim);}
+
+ for (unsigned int i = 0; i < joint_dim; ++i)
+ {
+ // Apply permutation only if it was specified, otherwise preserve original message order
+ const unsigned int id = permutation.empty() ? i : permutation[i];
+
+ // Apply position offset only if it was specified
+ const Scalar offset = position_offset.empty() ? 0.0 : position_offset[i];
+
+ if (!point.positions.empty()) {this->position[i] = point.positions[id] + offset;}
+ if (!point.velocities.empty()) {this->velocity[i] = point.velocities[id];}
+ if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[id];}
+ }
+ }
+ };
+
+ /**
+ * \brief Construct segment from start and end states (boundary conditions).
+ *
+ * \param start_time Time at which the segment state equals \p start_state.
+ * \param start_state State at \p start_time.
+ * \param end_time Time at which the segment state equals \p end_state.
+ * \param end_state State at time \p end_time.
+ */
+ JointTrajectorySegment(const Time& start_time,
+ const State& start_state,
+ const Time& end_time,
+ const State& end_state)
+ : rt_goal_handle_(),
+ tolerances_(start_state.position.size())
+ {
+ Segment::init(start_time, start_state, end_time, end_state);
+ }
+
+ /**
+ * \brief Construct a segment from start and end points (boundary conditions) specified in ROS message format.
+ *
+ * \param traj_start_time Time at which the trajectory containing the segment starts. Note that this is \e not the
+ * segment start time.
+ * \param start_point Start state in ROS message format.
+ * \param end_point End state in ROS message format.
+ * \param permutation See \ref JointTrajectorySegment::State.
+ * \param position_offset See \ref JointTrajectorySegment::State.
+ *
+ * \throw std::invalid_argument If input parameters are inconsistent and a valid segment can't be constructed.
+ */
+ JointTrajectorySegment(const ros::Time& traj_start_time,
+ const trajectory_msgs::JointTrajectoryPoint& start_point,
+ const trajectory_msgs::JointTrajectoryPoint& end_point,
+ const std::vector<unsigned int>& permutation = std::vector<unsigned int>(),
+ const std::vector<Scalar>& position_offset = std::vector<Scalar>())
+ : rt_goal_handle_(),
+ tolerances_(start_point.positions.size())
+ {
+ if (start_point.positions.size() != end_point.positions.size())
+ {
+ throw(std::invalid_argument("Can't construct segment from ROS message: "
+ "Start/end points data size mismatch."));
+ }
+
+ const Time start_time = (traj_start_time + start_point.time_from_start).toSec();
+ const Time end_time = (traj_start_time + end_point.time_from_start).toSec();
+
+ try
+ {
+ const State start_state(start_point, permutation, position_offset);
+ const State end_state(end_point, permutation, position_offset);
+
+ this->init(start_time, start_state,
+ end_time, end_state);
+ }
+ catch(const std::invalid_argument& ex)
+ {
+ std::string msg = "Can't construct segment from ROS message: " + std::string(ex.what());
+ throw(std::invalid_argument(msg));
+ }
+ }
+
+ /** \return Pointer to (realtime) goal handle associated to this segment. */
+ RealtimeGoalHandlePtr getGoalHandle() const {return rt_goal_handle_;}
+
+ /** \brief Set the (realtime) goal handle associated to this segment. */
+ void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle) {rt_goal_handle_ = rt_goal_handle;}
+
+ /** \return Tolerances this segment is associated to. */
+ const SegmentTolerances<Scalar>& getTolerances() const {return tolerances_;}
+
+ /** \brief Set the tolerances this segment is associated to. */
+ void setTolerances(const SegmentTolerances<Scalar>& tolerances) {tolerances_ = tolerances;}
+
+private:
+ RealtimeGoalHandlePtr rt_goal_handle_;
+ SegmentTolerances<Scalar> tolerances_;
+};
+
+/**
+ * \param prev_position Previous position from which to compute the wraparound offset.
+ * \param next_position Next position from which to compute the wraparound offset.
+ * \param angle_wraparound Vector of booleans where true values correspond to joints that wrap around
+ * (ie. are continuous). Offsets will be computed only for these joints, otherwise they are set to zero.
+ * \return Wraparound offsets that should be applied to \p next_position such that no multi-turns are performed when
+ * transitioning from \p prev_position.
+ * \tparam Scalar Scalar type.
+ */
+template <class Scalar>
+std::vector<Scalar> wraparoundOffset(const std::vector<Scalar>& prev_position,
+ const std::vector<Scalar>& next_position,
+ const std::vector<bool>& angle_wraparound)
+{
+ // Preconditions
+ const unsigned int n_joints = angle_wraparound.size();
+ if (n_joints != prev_position.size() || n_joints != next_position.size()) {return std::vector<Scalar>();}
+
+ // Return value
+ std::vector<Scalar> pos_offset(n_joints, 0.0);
+
+ for (unsigned int i = 0; i < angle_wraparound.size(); ++i)
+ {
+ if (angle_wraparound[i])
+ {
+ Scalar dist = angles::shortest_angular_distance(prev_position[i], next_position[i]);
+
+ // Deal with singularity at M_PI shortest distance
+ if (std::abs(dist) - M_PI < 1e-9)
+ {
+ dist = next_position[i] > prev_position[i] ? std::abs(dist) : -std::abs(dist);
+ }
+ pos_offset[i] = (prev_position[i] + dist) - next_position[i];
+ }
+ }
+ return pos_offset;
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
new file mode 100644
index 0000000..efc0658
--- /dev/null
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
@@ -0,0 +1,257 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_TOLERANCES_H
+#define JOINT_TRAJECTORY_CONTROLLER_TOLERANCES_H
+
+// C++ standard
+#include <cassert>
+#include <cmath>
+#include <string>
+#include <vector>
+
+// ROS
+#include <ros/node_handle.h>
+
+// ROS messages
+#include <control_msgs/FollowJointTrajectoryAction.h>
+
+namespace joint_trajectory_controller
+{
+
+/**
+ * \brief Trajectory state tolerances for position, velocity and acceleration variables.
+ *
+ * A tolerance value of zero means that no tolerance will be applied for that variable.
+ */
+template<class Scalar>
+struct StateTolerances
+{
+ StateTolerances(Scalar position_tolerance = static_cast<Scalar>(0.0),
+ Scalar velocity_tolerance = static_cast<Scalar>(0.0),
+ Scalar acceleration_tolerance = static_cast<Scalar>(0.0))
+ : position(position_tolerance),
+ velocity(velocity_tolerance),
+ acceleration(acceleration_tolerance)
+ {}
+
+ Scalar position;
+ Scalar velocity;
+ Scalar acceleration;
+};
+
+/**
+ * \brief Trajectory segment tolerances.
+ */
+template<class Scalar>
+struct SegmentTolerances
+{
+ SegmentTolerances(const typename std::vector<StateTolerances<Scalar> >::size_type& size = 0)
+ : state_tolerance(size, static_cast<Scalar>(0.0)),
+ goal_state_tolerance(size, static_cast<Scalar>(0.0)),
+ goal_time_tolerance(static_cast<Scalar>(0.0))
+ {}
+
+ /** State tolerances that appply during segment execution. */
+ std::vector<StateTolerances<Scalar> > state_tolerance;
+
+ /** State tolerances that apply for the goal state only.*/
+ std::vector<StateTolerances<Scalar> > goal_state_tolerance;
+
+ /** Extra time after the segment end time allowed to reach the goal state tolerances. */
+ Scalar goal_time_tolerance;
+};
+
+/**
+ * \param state_error State error to check.
+ * \param state_tolerance State tolerances to check \p state_error against.
+ * \param show_errors If the joints that violate their tolerances should be output to console. NOT REALTIME if true
+ * \return True if \p state_error fulfills \p state_tolerance.
+ */
+template <class State>
+inline bool checkStateTolerance(const State& state_error,
+ const std::vector<StateTolerances<typename State::Scalar> >& state_tolerance,
+ bool show_errors = false)
+{
+ const unsigned int n_joints = state_tolerance.size();
+
+ // Preconditions
+ assert(n_joints == state_error.position.size());
+ assert(n_joints == state_error.velocity.size());
+ assert(n_joints == state_error.acceleration.size());
+
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ using std::abs;
+ const StateTolerances<typename State::Scalar>& tol = state_tolerance[i]; // Alias for brevity
+ const bool is_valid = !(tol.position > 0.0 && abs(state_error.position[i]) > tol.position) &&
+ !(tol.velocity > 0.0 && abs(state_error.velocity[i]) > tol.velocity) &&
+ !(tol.acceleration > 0.0 && abs(state_error.acceleration[i]) > tol.acceleration);
+
+ if (!is_valid)
+ {
+ if( show_errors )
+ {
+ ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed on joint " << i);
+
+ if (tol.position > 0.0 && abs(state_error.position[i]) > tol.position)
+ ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[i] <<
+ " Position Tolerance: " << tol.position);
+ if (tol.velocity > 0.0 && abs(state_error.velocity[i]) > tol.velocity)
+ ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[i] <<
+ " Velocity Tolerance: " << tol.velocity);
+ if (tol.acceleration > 0.0 && abs(state_error.acceleration[i]) > tol.acceleration)
+ ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[i] <<
+ " Acceleration Tolerance: " << tol.acceleration);
+ }
+ return false;
+ }
+ }
+ return true;
+}
+
+/**
+ * \brief Update data in \p tols from data in \p msg_tol.
+ *
+ * - If a value in \p tol_msg is positive, the corresponding values in \p tols will be overritten.
+ * - If a value in \p tol_msg is negative, the corresponding values in \p tols will be reset.
+ * - If a value in \p tol_msg is zero, the corresponding values in \p tols is unchanged.
+ *
+ * \param[in] tol_msg Message containing tolerance values \p tols will be updated with.
+ * \param[out] tols Tolerances values to update.
+ *
+ **/
+template<class Scalar>
+void updateStateTolerances(const control_msgs::JointTolerance& tol_msg, StateTolerances<Scalar>& tols)
+{
+ if (tol_msg.position > 0.0) {tols.position = static_cast<Scalar>(tol_msg.position);}
+ else if (tol_msg.position < 0.0) {tols.position = static_cast<Scalar>(0.0);}
+
+ if (tol_msg.velocity > 0.0) {tols.velocity = static_cast<Scalar>(tol_msg.velocity);}
+ else if (tol_msg.velocity < 0.0) {tols.velocity = static_cast<Scalar>(0.0);}
+
+ if (tol_msg.acceleration > 0.0) {tols.acceleration = static_cast<Scalar>(tol_msg.acceleration);}
+ else if (tol_msg.acceleration < 0.0) {tols.acceleration = static_cast<Scalar>(0.0);}
+}
+
+/**
+ * \brief Update data in \p tols from data in \p goal.
+ *
+ * \param[in] goal Action goal data containing tolerance values \p tols will be updated with.
+ * \param[in] joint_names Names of joints in \p tols, with the same ordering.
+ * \param[out] tols Tolerances values to update.
+ */
+template<class Scalar>
+void updateSegmentTolerances(const control_msgs::FollowJointTrajectoryGoal& goal,
+ const std::vector<std::string>& joint_names,
+ SegmentTolerances<Scalar>& tols
+)
+{
+ // Preconditions
+ assert(joint_names.size() == tols.state_tolerance.size());
+ assert(joint_names.size() == tols.goal_state_tolerance.size());
+
+ typedef typename std::vector<std::string>::const_iterator StringConstIterator;
+ typedef typename std::vector<control_msgs::JointTolerance>::const_iterator TolMsgConstIterator;
+
+ for (StringConstIterator names_it = joint_names.begin(); names_it != joint_names.end(); ++names_it)
+ {
+ const typename std::vector<std::string>::size_type id = std::distance(joint_names.begin(), names_it);
+
+ // Update path tolerances
+ const std::vector<control_msgs::JointTolerance>& state_tol = goal.path_tolerance;
+ for(TolMsgConstIterator state_tol_it = state_tol.begin(); state_tol_it != state_tol.end(); ++state_tol_it)
+ {
+ if (*names_it == state_tol_it->name) {updateStateTolerances(*state_tol_it, tols.state_tolerance[id]);}
+ }
+
+ // Update goal state tolerances
+ const std::vector<control_msgs::JointTolerance>& g_state_tol = goal.goal_tolerance;
+ for(TolMsgConstIterator g_state_tol_it = g_state_tol.begin(); g_state_tol_it != g_state_tol.end(); ++g_state_tol_it)
+ {
+ if (*names_it == g_state_tol_it->name) {updateStateTolerances(*g_state_tol_it, tols.goal_state_tolerance[id]);}
+ }
+ }
+
+ // Update goal time tolerance
+ const ros::Duration& goal_time_tolerance = goal.goal_time_tolerance;
+ if (goal_time_tolerance < ros::Duration(0.0)) {tols.goal_time_tolerance = 0.0;}
+ else if (goal_time_tolerance > ros::Duration(0.0)) {tols.goal_time_tolerance = goal_time_tolerance.toSec();}
+}
+
+/**
+ * \brief Populate trajectory segment tolerances from data in the ROS parameter server.
+ *
+ * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters
+ * will take the defaults shown in the comments:
+ *
+ * \code
+ * constraints:
+ * goal_time: 1.0 # Defaults to zero
+ * stopped_velocity_tolerance: 0.02 # Defaults to 0.01
+ * foo_joint:
+ * trajectory: 0.05 # Defaults to zero (ie. the tolerance is not enforced)
+ * goal: 0.03 # Defaults to zero (ie. the tolerance is not enforced)
+ * bar_joint:
+ * goal: 0.01
+ * \endcode
+ *
+ * \param nh NodeHandle where the tolerances are specified.
+ * \param joint_names Names of joints to look for in the parameter server for a tolerance specification.
+ * \return Trajectory segment tolerances.
+ */
+template<class Scalar>
+SegmentTolerances<Scalar> getSegmentTolerances(const ros::NodeHandle& nh,
+ const std::vector<std::string>& joint_names)
+{
+ const unsigned int n_joints = joint_names.size();
+ joint_trajectory_controller::SegmentTolerances<Scalar> tolerances;
+
+ // State and goal state tolerances
+ double stopped_velocity_tolerance;
+ nh.param("stopped_velocity_tolerance", stopped_velocity_tolerance, 0.01);
+
+ tolerances.state_tolerance.resize(n_joints);
+ tolerances.goal_state_tolerance.resize(n_joints);
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ nh.param(joint_names[i] + "/trajectory", tolerances.state_tolerance[i].position, 0.0);
+ nh.param(joint_names[i] + "/goal", tolerances.goal_state_tolerance[i].position, 0.0);
+ tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance;
+ }
+
+ // Goal time tolerance
+ nh.param("goal_time", tolerances.goal_time_tolerance, 0.0);
+
+ return tolerances;
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/trajectory_interface/pos_vel_acc_state.h b/joint_trajectory_controller/include/trajectory_interface/pos_vel_acc_state.h
new file mode 100644
index 0000000..0049974
--- /dev/null
+++ b/joint_trajectory_controller/include/trajectory_interface/pos_vel_acc_state.h
@@ -0,0 +1,77 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef TRAJECTORY_INTERFACE_POS_VEL_ACC_STATE_H
+#define TRAJECTORY_INTERFACE_POS_VEL_ACC_STATE_H
+
+#include <vector>
+
+namespace trajectory_interface
+{
+
+/**
+ * \brief Multi-dof trajectory state containing position, velocity and acceleration data.
+ */
+template <class ScalarType>
+struct PosVelAccState
+{
+ typedef ScalarType Scalar;
+
+ PosVelAccState() {}
+
+ /**
+ * \brief Resource-preallocating constructor.
+ *
+ * Position, velocity and acceleration vectors are resized to \p size, and their values are set to zero.
+ * Note that these two situations are different:
+ * \code
+ * // 2-dimensional state specifying zero position, velocity and acceleration
+ * State zero_pos_vel_acc(2);
+ *
+ * // 2-dimensional state specifying zero position
+ * State zero_pos;
+ * zero_pos.position.resize(2);
+ * \endcode
+ */
+ PosVelAccState(const typename std::vector<Scalar>::size_type size)
+ : position( std::vector<Scalar>(size, static_cast<Scalar>(0))),
+ velocity( std::vector<Scalar>(size, static_cast<Scalar>(0))),
+ acceleration(std::vector<Scalar>(size, static_cast<Scalar>(0)))
+ {}
+
+ std::vector<Scalar> position;
+ std::vector<Scalar> velocity;
+ std::vector<Scalar> acceleration;
+};
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h b/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h
new file mode 100644
index 0000000..2468d1a
--- /dev/null
+++ b/joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h
@@ -0,0 +1,414 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian, Stuart Glaser, Mrinal Kalakrishnan
+
+#ifndef TRAJECTORY_INTERFACE_QUINTIC_SPLINE_SEGMENT_H
+#define TRAJECTORY_INTERFACE_QUINTIC_SPLINE_SEGMENT_H
+
+#include <iterator>
+#include <stdexcept>
+
+#include <boost/array.hpp>
+#include <boost/foreach.hpp>
+
+#include <trajectory_interface/pos_vel_acc_state.h>
+
+namespace trajectory_interface
+{
+
+/**
+ * \brief Class representing a multi-dimensional quintic spline segment with a start and end time.
+ *
+ * \tparam ScalarType Scalar type
+ */
+template<class ScalarType>
+class QuinticSplineSegment
+{
+public:
+ typedef ScalarType Scalar;
+ typedef Scalar Time;
+ typedef PosVelAccState<Scalar> State;
+
+ /**
+ * \brief Creates an empty segment.
+ *
+ * \note Calling <tt> size() </tt> on an empty segment will yield zero, and sampling it will yield a state with empty
+ * data.
+ */
+ QuinticSplineSegment()
+ : coefs_(),
+ duration_(static_cast<Scalar>(0)),
+ start_time_(static_cast<Scalar>(0))
+ {}
+
+ /**
+ * \brief Construct segment from start and end states (boundary conditions).
+ *
+ * Please refer to the \ref init method documentation for the description of each parameter and the exceptions that
+ * can be thrown.
+ */
+ QuinticSplineSegment(const Time& start_time,
+ const State& start_state,
+ const Time& end_time,
+ const State& end_state)
+ {
+ init(start_time, start_state, end_time, end_state);
+ }
+
+ /**
+ * \brief Initialize segment from start and end states (boundary conditions).
+ *
+ * The start and end states need not necessarily be specified all the way to the acceleration level:
+ * - If only \b positions are specified, linear interpolation will be used.
+ * - If \b positions and \b velocities are specified, a cubic spline will be used.
+ * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used.
+ *
+ * \note If start and end states have different specifications
+ * (eg. start is positon-only, end is position-velocity), the lowest common specification will be used
+ * (position-only in the example).
+ *
+ * \param start_time Time at which the segment state equals \p start_state.
+ * \param start_state State at \p start_time.
+ * \param end_time Time at which the segment state equals \p end_state.
+ * \param end_state State at time \p end_time.
+ *
+ * \throw std::invalid_argument If the \p end_time is earlier than \p start_time or if one of the states is
+ * uninitialized.
+ */
+ void init(const Time& start_time,
+ const State& start_state,
+ const Time& end_time,
+ const State& end_state);
+
+ /**
+ * \brief Sample the segment at a specified time.
+ *
+ * \note Within the <tt>[start_time, end_time]</tt> interval, spline interpolation takes place, outside it this method
+ * will output the start/end states with zero velocity and acceleration.
+ *
+ * \param[in] time Where the segment is to be sampled.
+ * \param[out] state Segment state at \p time.
+ */
+ void sample(const Time& time, State& state) const
+ {
+ // Resize state data. Should be a no-op if appropriately sized
+ state.position.resize(coefs_.size());
+ state.velocity.resize(coefs_.size());
+ state.acceleration.resize(coefs_.size());
+
+ // Sample each dimension
+ typedef typename std::vector<SplineCoefficients>::const_iterator ConstIterator;
+ for(ConstIterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
+ {
+ const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
+ sampleWithTimeBounds(*coefs_it,
+ duration_, (time - start_time_),
+ state.position[id], state.velocity[id], state.acceleration[id]);
+ }
+ }
+
+ /** \return Segment start time. */
+ Time startTime() const {return start_time_;}
+
+ /** \return Segment end time. */
+ Time endTime() const {return start_time_ + duration_;}
+
+ /** \return Segment size (dimension). */
+ unsigned int size() const {return coefs_.size();}
+
+private:
+ typedef boost::array<Scalar, 6> SplineCoefficients;
+
+ /** Coefficients represent a quintic polynomial like so:
+ *
+ * <tt> coefs_[0] + coefs_[1]*x + coefs_[2]*x^2 + coefs_[3]*x^3 + coefs_[4]*x^4 + coefs_[5]*x^5 </tt>
+ */
+ std::vector<SplineCoefficients> coefs_;
+ Time duration_;
+ Time start_time_;
+
+ // These methods are borrowed from the previous controller's implementation
+ // TODO: Clean their implementation, use the Horner algorithm for more numerically stable polynomial evaluation
+ static void generatePowers(int n, const Scalar& x, Scalar* powers);
+
+ static void computeCoefficients(const Scalar& start_pos,
+ const Scalar& end_pos,
+ const Scalar& time,
+ SplineCoefficients& coefficients);
+
+ static void computeCoefficients(const Scalar& start_pos, const Scalar& start_vel,
+ const Scalar& end_pos, const Scalar& end_vel,
+ const Scalar& time,
+ SplineCoefficients& coefficients);
+
+ static void computeCoefficients(const Scalar& start_pos, const Scalar& start_vel, const Scalar& start_acc,
+ const Scalar& end_pos, const Scalar& end_vel, const Scalar& end_acc,
+ const Scalar& time,
+ SplineCoefficients& coefficients);
+
+ static void sample(const SplineCoefficients& coefficients, const Scalar& time,
+ Scalar& position, Scalar& velocity, Scalar& acceleration);
+
+ static void sampleWithTimeBounds(const SplineCoefficients& coefficients, const Scalar& duration, const Scalar& time,
+ Scalar& position, Scalar& velocity, Scalar& acceleration);
+};
+
+template<class ScalarType>
+void QuinticSplineSegment<ScalarType>::init(const Time& start_time,
+ const State& start_state,
+ const Time& end_time,
+ const State& end_state)
+{
+ // Preconditions
+ if (end_time < start_time)
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: end_time < start_time."));
+ }
+ if (start_state.position.empty() || end_state.position.empty())
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: Endpoint positions can't be empty."));
+ }
+ if (start_state.position.size() != end_state.position.size())
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: Endpoint positions size mismatch."));
+ }
+
+ const unsigned int dim = start_state.position.size();
+ const bool has_velocity = !start_state.velocity.empty() && !end_state.velocity.empty();
+ const bool has_acceleration = !start_state.acceleration.empty() && !end_state.acceleration.empty();
+
+ if (has_velocity && dim != start_state.velocity.size())
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: Start state velocity size mismatch."));
+ }
+ if (has_velocity && dim != end_state.velocity.size())
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: End state velocity size mismatch."));
+ }
+ if (has_acceleration && dim!= start_state.acceleration.size())
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: Start state acceleration size mismatch."));
+ }
+ if (has_acceleration && dim != end_state.acceleration.size())
+ {
+ throw(std::invalid_argument("Quintic spline segment can't be constructed: End state acceleratios size mismatch."));
+ }
+
+ // Time data
+ start_time_ = start_time;
+ duration_ = end_time - start_time;
+
+ // Spline coefficients
+ coefs_.resize(dim);
+
+ typedef typename std::vector<SplineCoefficients>::iterator Iterator;
+ if (!has_velocity)
+ {
+ // Linear interpolation
+ for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
+ {
+ const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
+
+ computeCoefficients(start_state.position[id],
+ end_state.position[id],
+ duration_,
+ *coefs_it);
+ }
+ }
+ else if (!has_acceleration)
+ {
+ // Cubic interpolation
+ for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
+ {
+ const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
+
+ computeCoefficients(start_state.position[id], start_state.velocity[id],
+ end_state.position[id], end_state.velocity[id],
+ duration_,
+ *coefs_it);
+ }
+ }
+ else
+ {
+ // Quintic interpolation
+ for(Iterator coefs_it = coefs_.begin(); coefs_it != coefs_.end(); ++coefs_it)
+ {
+ const typename std::vector<Scalar>::size_type id = std::distance(coefs_.begin(), coefs_it);
+
+ computeCoefficients(start_state.position[id], start_state.velocity[id], start_state.acceleration[id],
+ end_state.position[id], end_state.velocity[id], end_state.acceleration[id],
+ duration_,
+ *coefs_it);
+ }
+ }
+}
+
+template<class ScalarType>
+inline void QuinticSplineSegment<ScalarType>::generatePowers(int n, const Scalar& x, Scalar* powers)
+{
+ powers[0] = 1.0;
+ for (int i=1; i<=n; ++i)
+ {
+ powers[i] = powers[i-1]*x;
+ }
+}
+
+template<class ScalarType>
+void QuinticSplineSegment<ScalarType>::
+computeCoefficients(const Scalar& start_pos,
+ const Scalar& end_pos,
+ const Scalar& time,
+ SplineCoefficients& coefficients)
+{
+ coefficients[0] = start_pos;
+ coefficients[1] = (time == 0.0) ? 0.0 : (end_pos - start_pos) / time;
+ coefficients[2] = 0.0;
+ coefficients[3] = 0.0;
+ coefficients[4] = 0.0;
+ coefficients[5] = 0.0;
+}
+
+template<class ScalarType>
+void QuinticSplineSegment<ScalarType>::
+computeCoefficients(const Scalar& start_pos, const Scalar& start_vel,
+ const Scalar& end_pos, const Scalar& end_vel,
+ const Scalar& time,
+ SplineCoefficients& coefficients)
+{
+ if (time == 0.0)
+ {
+ coefficients[0] = start_pos;
+ coefficients[1] = start_vel;
+ coefficients[2] = 0.0;
+ coefficients[3] = 0.0;
+ }
+ else
+ {
+ Scalar T[4];
+ generatePowers(3, time, T);
+
+ coefficients[0] = start_pos;
+ coefficients[1] = start_vel;
+ coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
+ coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
+ }
+ coefficients[4] = 0.0;
+ coefficients[5] = 0.0;
+}
+
+template<class ScalarType>
+void QuinticSplineSegment<ScalarType>::
+computeCoefficients(const Scalar& start_pos, const Scalar& start_vel, const Scalar& start_acc,
+ const Scalar& end_pos, const Scalar& end_vel, const Scalar& end_acc,
+ const Scalar& time,
+ SplineCoefficients& coefficients)
+{
+ if (time == 0.0)
+ {
+ coefficients[0] = start_pos;
+ coefficients[1] = start_vel;
+ coefficients[2] = 0.5*start_acc;
+ coefficients[3] = 0.0;
+ coefficients[4] = 0.0;
+ coefficients[5] = 0.0;
+ }
+ else
+ {
+ Scalar T[6];
+ generatePowers(5, time, T);
+
+ coefficients[0] = start_pos;
+ coefficients[1] = start_vel;
+ coefficients[2] = 0.5*start_acc;
+ coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
+ 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
+ coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
+ 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
+ coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
+ 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
+ }
+}
+
+template<class ScalarType>
+void QuinticSplineSegment<ScalarType>::
+sample(const SplineCoefficients& coefficients, const Scalar& time,
+ Scalar& position, Scalar& velocity, Scalar& acceleration)
+{
+ // create powers of time:
+ Scalar t[6];
+ generatePowers(5, time, t);
+
+ position = t[0]*coefficients[0] +
+ t[1]*coefficients[1] +
+ t[2]*coefficients[2] +
+ t[3]*coefficients[3] +
+ t[4]*coefficients[4] +
+ t[5]*coefficients[5];
+
+ velocity = t[0]*coefficients[1] +
+ 2.0*t[1]*coefficients[2] +
+ 3.0*t[2]*coefficients[3] +
+ 4.0*t[3]*coefficients[4] +
+ 5.0*t[4]*coefficients[5];
+
+ acceleration = 2.0*t[0]*coefficients[2] +
+ 6.0*t[1]*coefficients[3] +
+ 12.0*t[2]*coefficients[4] +
+ 20.0*t[3]*coefficients[5];
+}
+
+template<class ScalarType>
+void QuinticSplineSegment<ScalarType>::
+sampleWithTimeBounds(const SplineCoefficients& coefficients, const Scalar& duration, const Scalar& time,
+ Scalar& position, Scalar& velocity, Scalar& acceleration)
+{
+ if (time < 0)
+ {
+ Scalar _;
+ sample(coefficients, 0.0, position, _, _);
+ velocity = 0;
+ acceleration = 0;
+ }
+ else if (time > duration)
+ {
+ Scalar _;
+ sample(coefficients, duration, position, _, _);
+ velocity = 0;
+ acceleration = 0;
+ }
+ else
+ {
+ sample(coefficients, time,
+ position, velocity, acceleration);
+ }
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/include/trajectory_interface/trajectory_interface.h b/joint_trajectory_controller/include/trajectory_interface/trajectory_interface.h
new file mode 100644
index 0000000..1ec68eb
--- /dev/null
+++ b/joint_trajectory_controller/include/trajectory_interface/trajectory_interface.h
@@ -0,0 +1,156 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#ifndef TRAJECTORY_INTERFACE_TRAJECTORY_INTERFACE_H
+#define TRAJECTORY_INTERFACE_TRAJECTORY_INTERFACE_H
+
+#include <algorithm>
+#include <iterator>
+#include <vector>
+
+// Things to generalize:
+// - State specification: What values (pos, vel, acc, ...), and their dimension (4 for quats, 1 for scalars)
+// - State might be different for derivatives: Quat == 4, ang vel == 3: Solve doing w=[w0, w1, w2, 0]
+// - Sample state vs. sample pos and derivatives separately
+// - Manage and find active segments of heretogeneous types
+
+// NOTES:
+// - Segments in original implementation are an open interval on the start time. We are making them closed
+// - If an action goal superceeds another and the new trajectory starts in the future, until that time no constraint
+// enforcement will take place. Odd.
+
+namespace trajectory_interface
+{
+
+namespace internal
+{
+
+// NOTE: Implement as a lambda expression when C++11 becomes mainstream
+template<class Time, class Segment>
+inline bool isBeforeSegment(const Time& time, const Segment& segment)
+{
+ return time < segment.startTime();
+}
+
+} // namespace
+
+/**
+ * \brief Find an iterator to the segment containing a specified \p time.
+ *
+ * \param first Input iterator to the initial position in the segment sequence.
+ * \param last Input iterator to the final position in the segment sequence.
+ * The range searched is <tt>[first,last)</tt>.
+ * \param time Time to search for in the range.
+ *
+ * \return Iterator to the trajectory segment containing \p time. If no segment contains \p time (ie. it's earlier
+ * than the start time of \p first), then \p last is returned.
+ *
+ * \pre The range <tt>[first,last)</tt> should be sorted by segment start time.
+ *
+ * \note On average, this method has logarithmic time complexity when used on \b random-access iterators.
+ * On \b non-random-access iterators, iterator advances incur an additional linear time cost.
+ */
+template<class TrajectoryIterator, class Time>
+inline TrajectoryIterator findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time& time)
+{
+ typedef typename std::iterator_traits<TrajectoryIterator>::value_type Segment;
+ return (first == last || internal::isBeforeSegment(time, *first))
+ ? last // Optimization when time preceeds all segments, or when an empty range is passed
+ : --std::upper_bound(first, last, time, internal::isBeforeSegment<Time, Segment>); // Notice decrement operator
+}
+
+/**
+ * \brief Find an iterator to the segment containing a specified \p time.
+ *
+ * This is a convenience method wrapping the iterator-based
+ * \ref findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time& time) "findSegment" overload.
+ *
+ * \tparam Trajectory Trajectory type. Should be a \e sequence container \e sorted by segment start time.
+ *
+ * \sa findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time& time)
+ */
+template<class Trajectory, class Time>
+inline typename Trajectory::const_iterator findSegment(const Trajectory& trajectory, const Time& time)
+{
+ return findSegment(trajectory.begin(), trajectory.end(), time);
+}
+
+/**
+ * \brief Equivalent to \ref findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time& time) "findSegment"
+ * but returning a non-const iterator.
+ *
+ * \note Although \p is passed by non-const reference, it is not modified by this function. It's a workaround to allow
+ * overloads of this function to coexist (they need different signatures to prevent ambiguities).
+ */
+template<class Trajectory, class Time>
+inline typename Trajectory::iterator findSegment(Trajectory& trajectory, const Time& time)
+{
+ return findSegment(trajectory.begin(), trajectory.end(), time);
+}
+
+/**
+ * \brief Sample a trajectory at a specified time.
+ *
+ * This is a convenience function that combines finding the segment associated to a specified time (see \ref findSegment)
+ * and sampling it.
+ *
+ * \tparam Trajectory Trajectory type. Should be a \e sequence container \e sorted by segment start time.
+ * \param[in] trajectory Holds a sequence of segments.
+ * \param[in] time Where the trajectory is to be sampled.
+ * \param[out] state Segment state at \p time.
+ *
+ * \return Iterator to the trajectory segment containing \p time. If no segment contains \p time, then
+ * <tt>trajectory.end()</tt> is returned.
+ *
+ * \note The segment implementation should allow sampling outside the trajectory time interval, implementing a policy
+ * such as holding the first/last position. In such a case, it is possible to get a valid sample for all time inputs,
+ * even for the cases when this function returns <tt>trajectory.end()</tt>.
+ *
+ * \sa findSegment
+ */
+template<class Trajectory>
+inline typename Trajectory::const_iterator sample(const Trajectory& trajectory,
+ const typename Trajectory::value_type::Time& time,
+ typename Trajectory::value_type::State& state)
+{
+ typename Trajectory::const_iterator it = findSegment(trajectory, time);
+ if (it != trajectory.end())
+ {
+ it->sample(time, state); // Segment found at specified time
+ }
+ else if (!trajectory.empty())
+ {
+ trajectory.front().sample(time, state); // Specified time preceeds trajectory start time
+ }
+ return it;
+}
+
+} // namespace
+
+#endif // header guard
diff --git a/joint_trajectory_controller/mainpage.dox b/joint_trajectory_controller/mainpage.dox
new file mode 100644
index 0000000..5be7dd8
--- /dev/null
+++ b/joint_trajectory_controller/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b joint_trajectory_controller
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
new file mode 100644
index 0000000..d853ff1
--- /dev/null
+++ b/joint_trajectory_controller/package.xml
@@ -0,0 +1,58 @@
+<package>
+ <name>joint_trajectory_controller</name>
+ <description> Controller for executing joint-space trajectories on a group of joints. </description>
+ <version>0.10.0</version>
+
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Adolfo Rodriguez Tsouroukdissian</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>actionlib</build_depend>
+ <build_depend>angles</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>urdf</build_depend>
+
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>hardware_interface</build_depend>
+ <build_depend>realtime_tools</build_depend>
+
+ <build_depend>control_msgs</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+
+ <build_depend>rostest</build_depend> <!--Tests-only!-->
+ <build_depend>controller_manager</build_depend> <!--Tests-only!-->
+ <build_depend>xacro</build_depend> <!--Tests-only!-->
+
+
+ <run_depend>actionlib</run_depend>
+ <run_depend>angles</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>urdf</run_depend>
+
+ <run_depend>control_toolbox</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>hardware_interface</run_depend>
+ <run_depend>realtime_tools</run_depend>
+
+ <run_depend>control_msgs</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+
+ <run_depend>rostest</run_depend> <!--Tests-only!-->
+ <run_depend>controller_manager</run_depend> <!--Tests-only!-->
+ <run_depend>xacro</run_depend> <!--Tests-only!-->
+ <run_depend>rqt_plot</run_depend> <!--Tests-only!-->
+
+ <export>
+ <controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
+ </export>
+</package>
diff --git a/joint_trajectory_controller/ros_control_plugins.xml b/joint_trajectory_controller/ros_control_plugins.xml
new file mode 100644
index 0000000..8639bd8
--- /dev/null
+++ b/joint_trajectory_controller/ros_control_plugins.xml
@@ -0,0 +1,30 @@
+<library path="lib/libjoint_trajectory_controller">
+
+ <class name="position_controllers/JointTrajectoryController"
+ type="position_controllers::JointTrajectoryController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointTrajectoryController executes joint-space trajectories on a set of joints.
+ This variant represents trajectory segments as quintic splines and sends commands to a position interface.
+ </description>
+ </class>
+
+ <class name="velocity_controllers/JointTrajectoryController"
+ type="velocity_controllers::JointTrajectoryController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointTrajectoryController executes joint-space trajectories on a set of joints.
+ This variant represents trajectory segments as quintic splines and sends commands to a velocity interface.
+ </description>
+ </class>
+
+ <class name="effort_controllers/JointTrajectoryController"
+ type="effort_controllers::JointTrajectoryController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointTrajectoryController executes joint-space trajectories on a set of joints.
+ This variant represents trajectory segments as quintic splines and sends commands to an effort interface.
+ </description>
+ </class>
+
+</library>
diff --git a/joint_trajectory_controller/rosdoc.yaml b/joint_trajectory_controller/rosdoc.yaml
new file mode 100644
index 0000000..78d926a
--- /dev/null
+++ b/joint_trajectory_controller/rosdoc.yaml
@@ -0,0 +1,5 @@
+- builder: doxygen
+ name: C++ API
+ output_dir: c++
+ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
+ exclude_patterns: 'test'
\ No newline at end of file
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
new file mode 100644
index 0000000..5b9fe70
--- /dev/null
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -0,0 +1,71 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// 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 S.L. 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.
+//////////////////////////////////////////////////////////////////////////////
+
+// Pluginlib
+#include <pluginlib/class_list_macros.h>
+
+// Project
+#include <trajectory_interface/quintic_spline_segment.h>
+#include <joint_trajectory_controller/joint_trajectory_controller.h>
+
+namespace position_controllers
+{
+ /**
+ * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
+ * commands to a \b position interface.
+ */
+ typedef joint_trajectory_controller::JointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
+ hardware_interface::PositionJointInterface>
+ JointTrajectoryController;
+}
+
+namespace velocity_controllers
+{
+ /**
+ * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
+ * commands to a \b velocity interface.
+ */
+ typedef joint_trajectory_controller::JointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
+ hardware_interface::VelocityJointInterface>
+ JointTrajectoryController;
+}
+
+namespace effort_controllers
+{
+ /**
+ * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
+ * commands to an \b effort interface.
+ */
+ typedef joint_trajectory_controller::JointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
+ hardware_interface::EffortJointInterface>
+ JointTrajectoryController;
+}
+
+PLUGINLIB_EXPORT_CLASS(position_controllers::JointTrajectoryController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointTrajectoryController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(effort_controllers::JointTrajectoryController, controller_interface::ControllerBase)
diff --git a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
new file mode 100644
index 0000000..b26985d
--- /dev/null
+++ b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
@@ -0,0 +1,843 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <cmath>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <actionlib/server/action_server.h>
+#include <trajectory_interface/quintic_spline_segment.h>
+#include <joint_trajectory_controller/init_joint_trajectory.h>
+
+using namespace joint_trajectory_controller;
+using namespace trajectory_msgs;
+using std::vector;
+using std::string;
+
+// Floating-point value comparison threshold
+const double EPS = 1e-9;
+
+typedef JointTrajectorySegment<trajectory_interface::QuinticSplineSegment<double> > Segment;
+typedef vector<Segment> Trajectory;
+
+TEST(PermutationTest, Permutation)
+{
+ vector<string> t1(4);
+ t1[0] = "A";
+ t1[1] = "B";
+ t1[2] = "C";
+ t1[3] = "D";
+
+ vector<string> t2(4);
+ t2[0] = "B";
+ t2[1] = "D";
+ t2[2] = "A";
+ t2[3] = "C";
+
+ typedef vector<unsigned int> PermutationType;
+ PermutationType ground_truth(4);
+ ground_truth[0] = 2;
+ ground_truth[1] = 0;
+ ground_truth[2] = 3;
+ ground_truth[3] = 1;
+
+ // Mismatching sizes: Return empty permutation vector
+ {
+ vector<string> t2_bad(1,"A");
+ PermutationType perm = internal::permutation(t1, t2_bad);
+ EXPECT_TRUE(perm.empty());
+ }
+
+ // Mismatching contents: Return empty permutation vector
+ {
+ vector<string> t2_bad(3,"A");
+ PermutationType perm = internal::permutation(t1, t2_bad);
+ EXPECT_TRUE(perm.empty());
+ }
+
+ // Valid parameters
+ {
+ PermutationType perm = internal::permutation(t1, t2);
+ EXPECT_EQ(ground_truth.size(), perm.size());
+ EXPECT_EQ(ground_truth[0], perm[0]);
+ EXPECT_EQ(ground_truth[1], perm[1]);
+ EXPECT_EQ(ground_truth[2], perm[2]);
+ EXPECT_EQ(ground_truth[3], perm[3]);
+ }
+
+ // Valid parameters, inverse parameter order yields inverse permutation vector
+ {
+ PermutationType perm = internal::permutation(t2, t1);
+ EXPECT_EQ(ground_truth.size(), perm.size());
+ EXPECT_EQ(ground_truth[3], perm[0]);
+ EXPECT_EQ(ground_truth[2], perm[1]);
+ EXPECT_EQ(ground_truth[1], perm[2]);
+ EXPECT_EQ(ground_truth[0], perm[3]);
+ }
+}
+
+class InitTrajectoryTest : public ::testing::Test
+{
+public:
+ InitTrajectoryTest()
+ : points(3)
+ {
+ // Trajectory message
+ points[0].positions.resize(1, 2.0);
+ points[0].velocities.resize(1, 0.0);
+ points[0].accelerations.resize(1, 0.0);
+ points[0].time_from_start = ros::Duration(1.0);
+
+ points[1].positions.resize(1, 4.0);
+ points[1].velocities.resize(1, 0.0);
+ points[1].accelerations.resize(1, 0.0);
+ points[1].time_from_start = ros::Duration(2.0);
+
+ points[2].positions.resize(1, 3.0);
+ points[2].velocities.resize(1, 0.0);
+ points[2].accelerations.resize(1, 0.0);
+ points[2].time_from_start = ros::Duration(4.0);
+
+ trajectory_msg.header.stamp = ros::Time(0.5);
+ trajectory_msg.joint_names.resize(1, "foo_joint");
+ trajectory_msg.points = points;
+
+ // Current trajectory
+ typename Segment::State state[2];
+
+ state[0].position.push_back(0.0);
+ state[0].velocity.push_back(0.0);
+ state[0].acceleration.push_back(0.0);
+
+ state[1].position.push_back(1.0);
+ state[1].velocity.push_back(1.0);
+ state[1].acceleration.push_back(1.0);
+
+ curr_traj.push_back(Segment(0.0, state[0],
+ 1.0, state[1]));
+
+ curr_traj.push_back(Segment(3.0, state[1],
+ 10.0, state[0])); // This is just junk that should be discarded in the tests
+ }
+
+protected:
+ // Trajectory message to parse
+ vector<JointTrajectoryPoint> points;
+ trajectory_msgs::JointTrajectory trajectory_msg;
+
+ // Currently executed trajectory
+ Trajectory curr_traj;
+};
+
+TEST_F(InitTrajectoryTest, InitException)
+{
+ // Invalid input should throw an exception
+ trajectory_msg.points.back().positions.push_back(0.0); // Inconsistent size in last element
+ EXPECT_THROW(initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp), std::invalid_argument);
+}
+
+// Test logic of parsing a trajectory message. No current trajectory is specified
+TEST_F(InitTrajectoryTest, InitLogic)
+{
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Empty trajectory message: Return empty trajectory
+ {
+ const ros::Time time = msg_start_time;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msgs::JointTrajectory(), msg_start_time);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // Input time is...
+
+ // Before first point: Return full trajectory (2 segments)
+ {
+ const ros::Time time = msg_start_time;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_EQ(points.size() - 1, trajectory.size());
+ }
+
+ // First point: Return partial trajectory (last segment)
+ {
+ const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_EQ(points.size() - 2, trajectory.size());
+ }
+
+ // Between the first and second points: Return partial trajectory (last segment)
+ {
+ const ros::Time time = msg_start_time +
+ ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_EQ(msg_points.size() - 2, trajectory.size());
+ }
+
+ // Second point: Return empty trajectory
+ {
+ const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // Between the second and third points: Return empty trajectory
+ {
+ const ros::Time time = msg_start_time +
+ ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0);
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // Last point: Return empty trajectory
+ {
+ const ros::Time time = msg_start_time + (--msg_points.end())->time_from_start;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // After the last point: Return empty trajectory
+ {
+ const ros::Time time(msg_start_time + msg_points.back().time_from_start + ros::Duration(1.0));
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ EXPECT_TRUE(trajectory.empty());
+ }
+}
+
+// Test logic of parsing a trajectory message. Current trajectory is specified, hence trajectory combination takes place
+TEST_F(InitTrajectoryTest, InitLogicCombine)
+{
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.current_trajectory = &curr_traj;
+
+ // Empty trajectory message: Return empty trajectory
+ {
+ const ros::Time time = msg_start_time;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msgs::JointTrajectory(), msg_start_time, options);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // Input time is...
+
+ // Before first point, starting the current trajectory: Return 4 segments: Last of current + bridge + full message
+ {
+ const ros::Time time(curr_traj[0].startTime());
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_EQ(points.size() + 1, trajectory.size());
+ }
+
+ // Before first point: Return 4 segments: Last of current + bridge + full message
+ {
+ const ros::Time time = msg_start_time;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_EQ(points.size() + 1, trajectory.size());
+ }
+
+ // First point: Return partial trajectory, 3 segments: Last of current + bridge + last of message
+ {
+ const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_EQ(points.size(), trajectory.size());
+ }
+
+ // Between the first and second points: Same as before
+ {
+ const ros::Time time = msg_start_time +
+ ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_EQ(msg_points.size(), trajectory.size());
+ }
+
+ // Second point: Return partial trajectory, 2 segments: Last of current + bridge (only one point of message made it)
+ {
+ const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_EQ(msg_points.size() - 1, trajectory.size());
+ }
+
+ // Between the second and third points: Same as before
+ {
+ const ros::Time time = msg_start_time +
+ ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0);
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_EQ(msg_points.size() - 1, trajectory.size());
+ }
+
+ // Last point: Return empty trajectory
+ {
+ const ros::Time time = msg_start_time + (--msg_points.end())->time_from_start;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // After the last point: Return empty trajectory
+ {
+ const ros::Time time(msg_start_time + msg_points.back().time_from_start + ros::Duration(1.0));
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ EXPECT_TRUE(trajectory.empty());
+ }
+}
+
+TEST_F(InitTrajectoryTest, InitValues)
+{
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Input time is before first point: Return full trajectory (2 segments)
+ const ros::Time time = msg_start_time;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
+ ASSERT_EQ(points.size() - 1, trajectory.size());
+
+ // Check all segment start/end times and states
+ for (unsigned int i = 0; i < trajectory.size(); ++i)
+ {
+ const Segment& segment = trajectory[i];
+ const JointTrajectoryPoint& p_start = msg_points[i];
+ const JointTrajectoryPoint& p_end = msg_points[i + 1];
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (msg_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (msg_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_start.accelerations[0], state.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(p_end.positions[0], state.position[0]);
+ EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_end.accelerations[0], state.acceleration[0]);
+ }
+ }
+}
+
+TEST_F(InitTrajectoryTest, InitValuesCombine)
+{
+ // Before first point: Return 4 segments: Last of current + bridge + full message
+ const ros::Time time(curr_traj[0].startTime());
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.current_trajectory = &curr_traj;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ ASSERT_EQ(points.size() + 1, trajectory.size());
+
+ // Check current trajectory segment start/end times and states (only one)
+ {
+ const Segment& ref_segment = curr_traj[0];
+ const Segment& segment = trajectory[0];
+
+ // Check start/end times
+ {
+ EXPECT_EQ(ref_segment.startTime(), segment.startTime());
+ EXPECT_EQ(ref_segment.endTime(), segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State ref_state, state;
+ ref_segment.sample(ref_segment.startTime(), ref_state);
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(ref_state.position[0], state.position[0]);
+ EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State ref_state, state;
+ ref_segment.sample(ref_segment.endTime(), ref_state);
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(ref_state.position[0], state.position[0]);
+ EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
+ }
+ }
+
+ // Check bridge trajectory segment start/end times and states (only one)
+ {
+ const Segment& ref_segment = curr_traj[0];
+ const Segment& segment = trajectory[1];
+
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Segment start time should correspond to message start time
+ // Segment end time should correspond to first trajectory message point
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const typename Segment::Time start_time = msg_start_time.toSec();
+ const typename Segment::Time end_time = (msg_start_time + msg_points[0].time_from_start).toSec();
+
+ // Check start/end times
+ {
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state. Corresponds to current trajectory sampled at start_time
+ {
+ typename Segment::State ref_state, state;
+ ref_segment.sample(start_time, ref_state);
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(ref_state.position[0], state.position[0]);
+ EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
+ }
+
+ // Check end state. Corresponds to first trajectory message point
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(msg_points[0].positions[0], state.position[0]);
+ EXPECT_EQ(msg_points[0].velocities[0], state.velocity[0]);
+ EXPECT_EQ(msg_points[0].accelerations[0], state.acceleration[0]);
+ }
+ }
+
+ // Check all segment start/end times and states (2 segments)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it)
+ {
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ const Segment& segment = trajectory[traj_it];
+ const JointTrajectoryPoint& p_start = msg_points[msg_it];
+ const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (msg_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (msg_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_start.accelerations[0], state.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(p_end.positions[0], state.position[0]);
+ EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_end.accelerations[0], state.acceleration[0]);
+ }
+ }
+}
+
+TEST_F(InitTrajectoryTest, JointPermutation)
+{
+ // Add an extra joint to the trajectory message created in the fixture
+ trajectory_msg.points[0].positions.push_back(-2.0);
+ trajectory_msg.points[0].velocities.push_back(0.0);
+ trajectory_msg.points[0].accelerations.push_back(0.0);
+
+ trajectory_msg.points[1].positions.push_back(-4.0);
+ trajectory_msg.points[1].velocities.push_back(0.0);
+ trajectory_msg.points[1].accelerations.push_back(0.0);
+
+ trajectory_msg.points[2].positions.push_back(-3.0);
+ trajectory_msg.points[2].velocities.push_back(0.0);
+ trajectory_msg.points[2].accelerations.push_back(0.0);
+
+ trajectory_msg.joint_names.push_back("bar_joint");
+
+ // No reference joint names: Original message order is preserved
+ {
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp);
+
+ // Check position values only
+ const Segment& segment = trajectory[0];
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[0]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[1]);
+ }
+
+ // Reference joint names vector that reverses joint order
+ {
+ vector<string> joint_names; // Joints are reversed
+ joint_names.push_back(trajectory_msg.joint_names[1]);
+ joint_names.push_back(trajectory_msg.joint_names[0]);
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.joint_names = &joint_names;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
+
+ // Check position values only
+ const Segment& segment = trajectory[0];
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_NE(trajectory_msg.points[0].positions[0], state.position[0]);
+ EXPECT_NE(trajectory_msg.points[0].positions[1], state.position[1]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[1]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]);
+ }
+
+ // Reference joint names size mismatch
+ {
+ vector<string> joint_names;
+ joint_names.push_back(trajectory_msg.joint_names[0]);
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.joint_names = &joint_names;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
+ EXPECT_TRUE(trajectory.empty());
+ }
+
+ // Reference joint names value mismatch
+ {
+ vector<string> joint_names;
+ joint_names.push_back(trajectory_msg.joint_names[0]);
+ joint_names.push_back("baz_joint");
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.joint_names = &joint_names;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
+ EXPECT_TRUE(trajectory.empty());
+ }
+}
+
+TEST_F(InitTrajectoryTest, WrappingSpec)
+{
+ // Modify trajectory message created in the fixture to wrap around
+ const double wrap = 4 * M_PI;
+ for (unsigned int i = 0; i < trajectory_msg.points.size(); ++i)
+ {
+ trajectory_msg.points[i].positions[0] += wrap; // Add wrapping value to all points
+ }
+
+ // Before first point: Return 4 segments: Last of current + bridge + full message
+ const ros::Time time(curr_traj[0].startTime());
+ std::vector<bool> angle_wraparound(1, true);
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.current_trajectory = &curr_traj;
+ options.angle_wraparound = &angle_wraparound;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ ASSERT_EQ(points.size() + 1, trajectory.size());
+
+ // Check current trajectory segment start/end times and states (only one)
+ {
+ const Segment& ref_segment = curr_traj[0];
+ const Segment& segment = trajectory[0];
+
+ // Check start/end times
+ {
+ EXPECT_EQ(ref_segment.startTime(), segment.startTime());
+ EXPECT_EQ(ref_segment.endTime(), segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State ref_state, state;
+ ref_segment.sample(ref_segment.startTime(), ref_state);
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(ref_state.position[0], state.position[0]);
+ EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State ref_state, state;
+ ref_segment.sample(ref_segment.endTime(), ref_state);
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(ref_state.position[0], state.position[0]);
+ EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
+ }
+ }
+
+ // Check bridge trajectory segment start/end times and states (only one)
+ {
+ const Segment& ref_segment = curr_traj[0];
+ const Segment& segment = trajectory[1];
+
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Segment start time should correspond to message start time
+ // Segment end time should correspond to first trajectory message point
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const typename Segment::Time start_time = msg_start_time.toSec();
+ const typename Segment::Time end_time = (msg_start_time + msg_points[0].time_from_start).toSec();
+
+ // Check start/end times
+ {
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state. Corresponds to current trajectory sampled at start_time
+ {
+ typename Segment::State ref_state, state;
+ ref_segment.sample(start_time, ref_state);
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(ref_state.position[0], state.position[0]);
+ EXPECT_EQ(ref_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(ref_state.acceleration[0], state.acceleration[0]);
+ }
+
+ // Check end state. Corresponds to first trajectory message point
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(msg_points[0].positions[0] - wrap, state.position[0]); // NOTE: Wrap used
+ EXPECT_EQ(msg_points[0].velocities[0], state.velocity[0]);
+ EXPECT_EQ(msg_points[0].accelerations[0], state.acceleration[0]);
+ }
+ }
+
+ // Check all segment start/end times and states (2 segments)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it)
+ {
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ const Segment& segment = trajectory[traj_it];
+ const JointTrajectoryPoint& p_start = msg_points[msg_it];
+ const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (msg_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (msg_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0] - wrap, state.position[0]); // NOTE: Wrap used
+ EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_start.accelerations[0], state.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(p_end.positions[0] - wrap, state.position[0]); // NOTE: Wrap used
+ EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_end.accelerations[0], state.acceleration[0]);
+ }
+ }
+
+ // Reference joint names size mismatch
+ {
+ std::vector<bool> angle_wraparound(2, true);
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.current_trajectory = &curr_traj;
+ options.angle_wraparound = &angle_wraparound;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp, options);
+ EXPECT_TRUE(trajectory.empty());
+ }
+}
+
+// If current trajectory is not specified, but wrapping joints are, then the wrapping spec is ignored
+TEST_F(InitTrajectoryTest, IgnoreWrappingSpec)
+{
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Modify trajectory message created in the fixture to wrap around
+ const double wrap = 4 * M_PI;
+ for (unsigned int i = 0; i < trajectory_msg.points.size(); ++i)
+ {
+ trajectory_msg.points[i].positions[0] += wrap; // Add wrapping value to all points
+ }
+
+ // Before first point: Return 2 segments: Full message
+ const ros::Time time(curr_traj[0].startTime());
+ std::vector<bool> angle_wraparound(1, true);
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.angle_wraparound = &angle_wraparound;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ ASSERT_EQ(points.size() - 1, trajectory.size());
+
+ // Check all segment start/end times and states (2 segments)
+ for (unsigned int i = 0; i < trajectory.size(); ++i)
+ {
+ const Segment& segment = trajectory[i];
+ const JointTrajectoryPoint& p_start = msg_points[i];
+ const JointTrajectoryPoint& p_end = msg_points[i + 1];
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (msg_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (msg_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_start.accelerations[0], state.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(p_end.positions[0], state.position[0]);
+ EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
+ EXPECT_EQ(p_end.accelerations[0], state.acceleration[0]);
+ }
+ }
+}
+
+// If current trajectory is not specified, but wrapping joints are, then the wrapping spec is ignored
+TEST_F(InitTrajectoryTest, GoalHandleTest)
+{
+ typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> ActionServer;
+ typedef ActionServer::GoalHandle GoalHandle;
+ typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
+ typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
+
+ GoalHandle gh;
+ RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
+
+ // Before first point: Return 4 segments: Last of current + bridge + full message
+ const ros::Time time(curr_traj[0].startTime());
+ InitJointTrajectoryOptions<Trajectory> options;
+ options.current_trajectory = &curr_traj;
+ options.rt_goal_handle = rt_goal;
+
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ ASSERT_EQ(points.size() + 1, trajectory.size());
+
+ // Segment of current trajectory preserves existing null goal handle
+ ASSERT_FALSE(trajectory[0].getGoalHandle());
+
+ // All further segments should be associated to the new goal handle
+ for (unsigned int i = 1; i < trajectory.size(); ++i) // NOTE: Start index != 0
+ {
+ ASSERT_EQ(rt_goal, trajectory[i].getGoalHandle());
+ }
+}
+
+TEST_F(InitTrajectoryTest, OtherTimeBase)
+{
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+
+ // Options with a different time base
+ InitJointTrajectoryOptions<Trajectory> options;
+ ros::Duration time_offset(1.0);
+ ros::Time other_time_base = msg_start_time + time_offset;
+ options.current_trajectory = &curr_traj;
+ options.other_time_base = &other_time_base;
+
+ // Before first point: Return 4 segments: Last of current + bridge + full message
+ const ros::Time time = msg_start_time;
+ Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
+ ASSERT_EQ(points.size() + 1, trajectory.size());
+
+ // Check current trajectory segment start/end times (only one). Time offset does NOT apply
+ {
+ const Segment& ref_segment = curr_traj[0];
+ const Segment& segment = trajectory[0];
+
+ // Check start/end times
+ {
+ EXPECT_EQ(ref_segment.startTime(), segment.startTime());
+ EXPECT_EQ(ref_segment.endTime(), segment.endTime());
+ }
+ }
+
+ // Check bridge trajectory segment start/end times and states (only one). Time offset applies to end state only
+ {
+ const Segment& segment = trajectory[1];
+
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Segment start time should correspond to message start time
+ // Segment end time should correspond to first trajectory message point
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const typename Segment::Time start_time = msg_start_time.toSec();
+ const typename Segment::Time end_time = (msg_start_time + msg_points[0].time_from_start).toSec();
+
+ // Check start/end times
+ {
+ EXPECT_EQ(start_time + time_offset.toSec(), segment.startTime()); // NOTE: offsets applied
+ EXPECT_EQ(end_time + time_offset.toSec(), segment.endTime());
+ }
+ }
+
+ // Check all segment start/end times and states (2 segments). Time offset does apply
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it)
+ {
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ const Segment& segment = trajectory[traj_it];
+ const JointTrajectoryPoint& p_start = msg_points[msg_it];
+ const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (msg_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (msg_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time + time_offset.toSec(), segment.startTime()); // NOTE: offsets applied
+ EXPECT_EQ(end_time + time_offset.toSec(), segment.endTime());
+ }
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/joint_trajectory_controller/test/joint_trajectory_controller.test b/joint_trajectory_controller/test/joint_trajectory_controller.test
new file mode 100644
index 0000000..499e34d
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_trajectory_controller.test
@@ -0,0 +1,35 @@
+<launch>
+ <!-- Load RRbot model -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro.py '$(find joint_trajectory_controller)/test/rrbot.xacro'" />
+
+ <!-- Start RRbot -->
+ <node name="rrbot"
+ pkg="joint_trajectory_controller"
+ type="rrbot"/>
+
+ <!-- Load controller config -->
+ <rosparam command="load" file="$(find joint_trajectory_controller)/test/rrbot_controllers.yaml" />
+
+ <!-- Spawn controller -->
+ <node name="controller_spawner"
+ pkg="controller_manager" type="spawner" output="screen"
+ args="rrbot_controller" />
+
+ <!-- Rxplot monitoring -->
+ <node name="rrbot_pos_monitor"
+ pkg="rqt_plot"
+ type="rqt_plot"
+ args="/rrbot_controller/state/desired/positions[0]:positions[1],/rrbot_controller/state/actual/positions[0]:positions[1]" />
+
+ <node name="rrbot_vel_monitor"
+ pkg="rqt_plot"
+ type="rqt_plot"
+ args="/rrbot_controller/state/desired/velocities[0]:velocities[1],/rrbot_controller/state/actual/velocities[0]:velocities[1]" />
+
+ <!-- Controller test -->
+ <test test-name="joint_trajectory_controller_test"
+ pkg="joint_trajectory_controller"
+ type="joint_trajectory_controller_test"
+ time-limit="80.0"/>
+</launch>
diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
new file mode 100644
index 0000000..bb357c5
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
@@ -0,0 +1,940 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <algorithm>
+#include <cmath>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+#include <actionlib/client/simple_action_client.h>
+
+#include <std_msgs/Float64.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <control_msgs/QueryTrajectoryState.h>
+
+// Floating-point value comparison threshold
+const double EPS = 0.01;
+
+using actionlib::SimpleClientGoalState;
+
+class JointTrajectoryControllerTest : public ::testing::Test
+{
+public:
+ JointTrajectoryControllerTest()
+ : nh("rrbot_controller"),
+ short_timeout(1.0),
+ long_timeout(10.0),
+ controller_state()
+ {
+ n_joints = (2);
+ joint_names.resize(n_joints);
+ joint_names[0] = "joint1";
+ joint_names[1] = "joint2";
+
+ trajectory_msgs::JointTrajectoryPoint point;
+ point.positions.resize(n_joints, 0.0);
+ point.velocities.resize(n_joints, 0.0);
+ point.accelerations.resize(n_joints, 0.0);
+
+ // Go home trajectory
+ traj_home.joint_names = joint_names;
+ traj_home.points.resize(1, point);
+ traj_home.points[0].time_from_start = ros::Duration(1.0);
+
+ // Three-point trajectory
+ points.resize(3, point);
+ points[0].positions[0] = M_PI / 4.0;
+ points[0].positions[1] = 0.0;
+ points[0].time_from_start = ros::Duration(1.0);
+
+ points[1].positions[0] = 0.0;
+ points[1].positions[1] = -M_PI / 4.0;
+ points[1].time_from_start = ros::Duration(2.0);
+
+ points[2].positions[0] = -M_PI / 4.0;
+ points[2].positions[1] = M_PI / 4.0;
+ points[2].time_from_start = ros::Duration(4.0);
+
+ traj.joint_names = joint_names;
+ traj.points = points;
+
+ // Action goals
+ traj_home_goal.trajectory = traj_home;
+ traj_goal.trajectory = traj;
+
+ // Smoothing publisher (determines how well the robot follows a trajectory)
+ smoothing_pub = ros::NodeHandle().advertise<std_msgs::Float64>("smoothing", 1);
+
+ // Trajectory publisher
+ traj_pub = nh.advertise<trajectory_msgs::JointTrajectory>("command", 1);
+
+ // State subscriber
+ state_sub = nh.subscribe<control_msgs::JointTrajectoryControllerState>("state",
+ 1,
+ &JointTrajectoryControllerTest::stateCB,
+ this);
+
+ // Query state service client
+ query_state_service = nh.serviceClient<control_msgs::QueryTrajectoryState>("query_state");
+
+ // Action client
+ const std::string action_server_name = nh.getNamespace() + "/follow_joint_trajectory";
+ action_client.reset(new ActionClient(action_server_name));
+ action_client2.reset(new ActionClient(action_server_name));
+ }
+
+ ~JointTrajectoryControllerTest()
+ {
+ state_sub.shutdown(); // This is important, to make sure that the callback is not woken up later in the destructor
+ }
+
+protected:
+ typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ActionClient;
+ typedef boost::shared_ptr<ActionClient> ActionClientPtr;
+ typedef control_msgs::FollowJointTrajectoryGoal ActionGoal;
+ typedef control_msgs::JointTrajectoryControllerStateConstPtr StateConstPtr;
+
+ boost::mutex mutex;
+ ros::NodeHandle nh;
+
+ unsigned int n_joints;
+ std::vector<std::string> joint_names;
+ std::vector<trajectory_msgs::JointTrajectoryPoint> points;
+
+ trajectory_msgs::JointTrajectory traj_home;
+ trajectory_msgs::JointTrajectory traj;
+ ActionGoal traj_home_goal;
+ ActionGoal traj_goal;
+
+ ros::Duration short_timeout;
+ ros::Duration long_timeout;
+
+ ros::Publisher smoothing_pub;
+ ros::Publisher traj_pub;
+ ros::Subscriber state_sub;
+ ros::ServiceClient query_state_service;
+ ActionClientPtr action_client;
+ ActionClientPtr action_client2;
+
+
+ StateConstPtr controller_state;
+
+ void stateCB(const StateConstPtr& state)
+ {
+ boost::mutex::scoped_lock lock(mutex);
+ controller_state = state;
+ }
+
+ StateConstPtr getState()
+ {
+ boost::mutex::scoped_lock lock(mutex);
+ return controller_state;
+ }
+
+ bool initState(const ros::Duration& timeout = ros::Duration(5.0))
+ {
+ bool init_ok = false;
+ ros::Time start_time = ros::Time::now();
+ while (!init_ok && (ros::Time::now() - start_time) < timeout)
+ {
+ {
+ boost::mutex::scoped_lock lock(mutex);
+ init_ok = controller_state && !controller_state->joint_names.empty();
+ }
+ ros::Duration(0.1).sleep();
+ }
+ return init_ok;
+ }
+
+ static bool waitForState(const ActionClientPtr& action_client,
+ const actionlib::SimpleClientGoalState& state,
+ const ros::Duration& timeout)
+ {
+ using ros::Time;
+ using ros::Duration;
+
+ Time start_time = Time::now();
+ while (action_client->getState() != state && ros::ok())
+ {
+ if (timeout >= Duration(0.0) && (Time::now() - start_time) > timeout) {return false;} // Timed-out
+ ros::Duration(0.01).sleep();
+ }
+ return true;
+ }
+};
+
+// Controller state ROS API ////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, stateTopicConsistency)
+{
+ // Get current controller state
+ ASSERT_TRUE(initState());
+ StateConstPtr state = getState();
+
+ // Checks that are valid for all state messages
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_EQ(joint_names[i], state->joint_names[i]);
+ }
+
+ EXPECT_EQ(n_joints, state->desired.positions.size());
+ EXPECT_EQ(n_joints, state->desired.velocities.size());
+ EXPECT_EQ(n_joints, state->desired.accelerations.size());
+
+ EXPECT_EQ(n_joints, state->actual.positions.size());
+ EXPECT_EQ(n_joints, state->actual.velocities.size());
+ EXPECT_TRUE(state->actual.accelerations.empty());
+
+ EXPECT_EQ(n_joints, state->error.positions.size());
+ EXPECT_EQ(n_joints, state->error.velocities.size());
+ EXPECT_TRUE(state->error.accelerations.empty());
+}
+
+TEST_F(JointTrajectoryControllerTest, queryStateServiceConsistency)
+{
+ query_state_service.waitForExistence(ros::Duration(2.0));
+ ASSERT_TRUE(query_state_service.isValid());
+
+ control_msgs::QueryTrajectoryState srv;
+ srv.request.time = ros::Time::now();
+ ASSERT_TRUE(query_state_service.call(srv));
+
+ // Checks that are valid for all queries
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_EQ(joint_names[i], srv.response.name[i]);
+ }
+
+ EXPECT_EQ(n_joints, srv.response.position.size());
+ EXPECT_EQ(n_joints, srv.response.velocity.size());
+ EXPECT_EQ(n_joints, srv.response.acceleration.size());
+}
+
+// Invalid messages ////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, invalidMessages)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Invalid size
+ {
+ trajectory_msgs::JointTrajectoryPoint point;
+ point.positions.resize(1, 0.0);
+ point.velocities.resize(1, 0.0);
+ point.accelerations.resize(1, 0.0);
+
+ trajectory_msgs::JointTrajectory bad_traj;
+ bad_traj.joint_names.resize(1, "joint1");
+ bad_traj.points.resize(1, point);
+ bad_traj.points[0].time_from_start = ros::Duration(1.0);
+ ActionGoal bad_goal;
+ bad_goal.trajectory = bad_traj;
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS);
+ }
+
+ // Incompatible joint names
+ {
+ ActionGoal bad_goal = traj_home_goal;
+ bad_goal.trajectory.joint_names[0] = "bad_name";
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS);
+ }
+
+ // No position data
+ {
+ ActionGoal bad_goal = traj_home_goal;
+ bad_goal.trajectory.points[0].positions.clear();
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL);
+ }
+
+ // Incompatible data sizes
+ {
+ ActionGoal bad_goal = traj_home_goal;
+ bad_goal.trajectory.points[0].positions.pop_back();
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL);
+ }
+
+ // Non-strictly increasing waypoint times
+ {
+ ActionGoal bad_goal = traj_goal;
+ bad_goal.trajectory.points[2].time_from_start = bad_goal.trajectory.points[1].time_from_start;
+
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL);
+ }
+
+ // Empty trajectory through action interface
+ // NOTE: Sending an empty trajectory through the topic interface cancels execution of all queued segments, but
+ // an empty trajectory is interpreted by the action interface as not having the correct joint names.
+ {
+ ActionGoal empty_goal;
+ action_client->sendGoal(empty_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS);
+ }
+}
+
+// Uninterrupted trajectory execution //////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, topicSingleTraj)
+{
+ ASSERT_TRUE(initState());
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+
+ EXPECT_NEAR(traj.points.back().positions[i], state->actual.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->actual.velocities[i], EPS);
+
+ EXPECT_NEAR(0.0, state->error.positions[i], EPS);
+ EXPECT_NEAR(0.0, state->error.velocities[i], EPS);
+ }
+
+ // Validate query state service
+ control_msgs::QueryTrajectoryState srv;
+ srv.request.time = ros::Time::now();
+ ASSERT_TRUE(query_state_service.call(srv));
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(state->desired.positions[i], srv.response.position[i], EPS);
+ EXPECT_NEAR(state->desired.velocities[i], srv.response.velocity[i], EPS);
+ EXPECT_NEAR(state->desired.accelerations[i], srv.response.acceleration[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, actionSingleTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::SUCCESSFUL);
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+
+ EXPECT_NEAR(traj.points.back().positions[i], state->actual.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->actual.velocities[i], EPS);
+
+ EXPECT_NEAR(0.0, state->error.positions[i], EPS);
+ EXPECT_NEAR(0.0, state->error.velocities[i], EPS);
+ }
+}
+
+// Joint reordering ////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, jointReordering)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Message joints are ordered differently than in controller
+ ActionGoal reorder_goal = traj_home_goal;
+ std::swap(reorder_goal.trajectory.joint_names[0], reorder_goal.trajectory.joint_names[1]);
+ reorder_goal.trajectory.points.front().positions[0] = M_PI / 4.0;
+ reorder_goal.trajectory.points.front().positions[1] = 0.0;
+
+
+ reorder_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(reorder_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+ EXPECT_NEAR(reorder_goal.trajectory.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(reorder_goal.trajectory.points.back().positions[1], state->desired.positions[0], EPS);
+}
+
+// Joint wraparound ////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, jointWraparound)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Go to home configuration, we need known initial conditions
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Trajectory goals that trigger wrapping. Between the first and second goals:
+ // - First joint command has a wraparound of -2 loops
+ // - Second joint command has a wraparound of +1 loop
+ // - Both joints should end up doing an angular displacement of |pi/4| for the second goal
+ trajectory_msgs::JointTrajectoryPoint point;
+ point.positions.resize(n_joints, 0.0);
+ point.velocities.resize(n_joints, 0.0);
+ point.accelerations.resize(n_joints, 0.0);
+
+ ActionGoal goal1;
+ goal1.trajectory.joint_names = joint_names;
+ goal1.trajectory.points.resize(1, point);
+ goal1.trajectory.points[0].positions[0] = 0.25 * M_PI; //-M_PI / 2.0;
+ goal1.trajectory.points[0].positions[1] = 0.50 * M_PI; // M_PI / 2.0;
+ goal1.trajectory.points[0].time_from_start = ros::Duration(1.0);
+
+ ActionGoal goal2;
+ goal2.trajectory.joint_names = joint_names;
+ goal2.trajectory.points.resize(1, point);
+ goal2.trajectory.points[0].positions[0] = 4.50 * M_PI;//M_PI / 2.0;
+ goal2.trajectory.points[0].positions[1] = -1.75 * M_PI; //0.0;
+ goal2.trajectory.points[0].time_from_start = ros::Duration(1.0);
+
+ // Send trajectory
+ goal1.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(goal1);
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Send trajectory
+ goal2.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(goal2);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+ EXPECT_NEAR(goal1.trajectory.points[0].positions[0] + 0.25 * M_PI, state->desired.positions[0], EPS);
+ EXPECT_NEAR(goal1.trajectory.points[0].positions[1] - 0.25 * M_PI, state->desired.positions[1], EPS);
+}
+
+TEST_F(JointTrajectoryControllerTest, jointWraparoundPiSingularity)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Go to home configuration, we need known initial conditions
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Trajectory goals that trigger wrapping.
+ // When moving a wrapping joint pi radians (eg. from -pi/2 to pi/2), numeric errors can make the controller think
+ // that it has to wrap when in fact it doesn't. This comes from a call to angles::shortest_angular_distance, which
+ // sometimes wields pi, and sometimes -pi.
+ trajectory_msgs::JointTrajectoryPoint point;
+ point.positions.resize(n_joints, 0.0);
+ point.velocities.resize(n_joints, 0.0);
+ point.accelerations.resize(n_joints, 0.0);
+
+ ActionGoal goal1;
+ goal1.trajectory.joint_names = joint_names;
+ goal1.trajectory.points.resize(1, point);
+ goal1.trajectory.points[0].positions[0] = -M_PI / 2.0;
+ goal1.trajectory.points[0].positions[1] = 0.0;
+ goal1.trajectory.points[0].time_from_start = ros::Duration(1.0);
+
+ ActionGoal goal2;
+ goal2.trajectory.joint_names = joint_names;
+ goal2.trajectory.points.resize(1, point);
+ goal2.trajectory.points[0].positions[0] = M_PI / 2.0;
+ goal2.trajectory.points[0].positions[1] = 0.0;
+ goal2.trajectory.points[0].time_from_start = ros::Duration(2.0);
+
+ // Send trajectory
+ goal1.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(goal1);
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Send trajectory
+ goal2.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(goal2);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(goal2.trajectory.points[0].positions[i], state->desired.positions[i], EPS);
+ }
+}
+
+// Trajectory replacement //////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, topicReplacesTopicTraj)
+{
+ ASSERT_TRUE(initState());
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory that preempts the previous one
+ traj_home.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_home);
+ wait_duration = traj_home.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Check that we're back home
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj_home.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, actionReplacesActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory that preempts the previous one
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client2->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're back home
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj_home.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, actionReplacesTopicTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory that preempts the previous one
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're back home
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj_home.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, topicReplacesActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory that preempts the previous one
+ traj_home.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_home);
+
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+
+ wait_duration = traj_home.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Check that we're back home
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj_home.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj_home.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+// Cancel execution ////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsTopicTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Go to home configuration, we need known initial conditions
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Start state
+ StateConstPtr start_state = getState();
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send an empty trajectory that preempts the previous one and stops the robot where it is
+ trajectory_msgs::JointTrajectory traj_empty;
+ traj_pub.publish(traj_empty);
+ ros::Duration(2.0).sleep(); // Stopping takes some time
+
+ // Check that we're not on the start state
+ StateConstPtr state1 = getState();
+ EXPECT_LT(traj.points.front().positions[0] * 0.9,
+ std::abs(start_state->desired.positions[0] - state1->desired.positions[0]));
+
+ // Check that we're not moving
+ ros::Duration(0.5).sleep(); // Wait
+ StateConstPtr state2 = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(state1->desired.positions[i], state2->desired.positions[i], EPS);
+ EXPECT_NEAR(state1->desired.velocities[i], state2->desired.velocities[i], EPS);
+ EXPECT_NEAR(state1->desired.accelerations[i], state2->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, emptyTopicCancelsActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Go to home configuration, we need known initial conditions
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Start state
+ StateConstPtr start_state = getState();
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send an empty trajectory that preempts the previous one and stops the robot where it is
+ trajectory_msgs::JointTrajectory traj_empty;
+ traj_pub.publish(traj_empty);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ ros::Duration(2.0).sleep(); // Stopping takes some time
+
+ // Check that we're not on the start state
+ StateConstPtr state1 = getState();
+ EXPECT_LT(traj.points.front().positions[0] * 0.9,
+ std::abs(start_state->desired.positions[0] - state1->desired.positions[0]));
+
+ // Check that we're not moving
+ ros::Duration(0.5).sleep(); // Wait
+ StateConstPtr state2 = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(state1->desired.positions[i], state2->desired.positions[i], EPS);
+ EXPECT_NEAR(state1->desired.velocities[i], state2->desired.velocities[i], EPS);
+ EXPECT_NEAR(state1->desired.accelerations[i], state2->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, cancelActionGoal)
+{
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ action_client->cancelGoal();
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+}
+
+// Ignore old trajectory points ////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, ignoreOldTopicTraj)
+{
+ ASSERT_TRUE(initState());
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory with all points occuring in the past. Should not preempts the previous one
+ traj_home.header.stamp = ros::Time::now() - traj_home.points.back().time_from_start - ros::Duration(0.5);
+ traj_pub.publish(traj_home);
+
+ wait_duration = traj.points.back().time_from_start - traj.points.front().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until first trajectory is done
+
+ // Check that we're at the original trajectory end (NOT back home)
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, ignoreOldActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory with all points occuring in the past. Should not preempts the previous one
+ traj_home_goal.trajectory.header.stamp = ros::Time::now() - traj_home.points.back().time_from_start - ros::Duration(0.5);
+ action_client2->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::REJECTED, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the original trajectory end (NOT back home)
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, ignorePartiallyOldActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ ActionGoal first_goal = traj_goal;
+ first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ first_goal.trajectory.points.pop_back(); // Remove last point
+ action_client->sendGoal(first_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send another trajectory with only the last point not occuring in the past. Only the last point should be executed
+ traj_goal.trajectory.header.stamp = ros::Time::now() - traj.points.back().time_from_start + ros::Duration(1.0);
+ action_client2->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the trajectory end
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+// Tolerance checking //////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointTrajectoryControllerTest, pathToleranceViolation)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Go to home configuration, we need known initial conditions
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Make robot respond with a delay
+ {
+ std_msgs::Float64 smoothing;
+ smoothing.data = 0.9;
+ smoothing_pub.publish(smoothing);
+ ros::Duration(0.5).sleep();
+ }
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until done
+ EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED);
+
+ // Restore perfect control
+ {
+ std_msgs::Float64 smoothing;
+ smoothing.data = 0.0;
+ smoothing_pub.publish(smoothing);
+ ros::Duration(0.5).sleep();
+ }
+}
+
+TEST_F(JointTrajectoryControllerTest, goalToleranceViolation)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Go to home configuration, we need known initial conditions
+ traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+ // Make robot respond with a delay
+ {
+ std_msgs::Float64 smoothing;
+ smoothing.data = 0.95;
+ smoothing_pub.publish(smoothing);
+ ros::Duration(0.5).sleep();
+ }
+
+ // Disable path constraints
+ traj_goal.path_tolerance.resize(2);
+ traj_goal.path_tolerance[0].name = "joint1";
+ traj_goal.path_tolerance[0].position = -1.0;
+ traj_goal.path_tolerance[0].velocity = -1.0;
+ traj_goal.path_tolerance[1].name = "joint2";
+ traj_goal.path_tolerance[1].position = -1.0;
+ traj_goal.path_tolerance[1].velocity = -1.0;
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until done
+ EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED);
+
+ // Restore perfect control
+ {
+ std_msgs::Float64 smoothing;
+ smoothing.data = 0.0;
+ smoothing_pub.publish(smoothing);
+ ros::Duration(0.5).sleep();
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "joint_trajectory_controller_test");
+
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ int ret = RUN_ALL_TESTS();
+ spinner.stop();
+ ros::shutdown();
+ return ret;
+}
diff --git a/joint_trajectory_controller/test/joint_trajectory_msg_utils_test.cpp b/joint_trajectory_controller/test/joint_trajectory_msg_utils_test.cpp
new file mode 100644
index 0000000..c7f506d
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_trajectory_msg_utils_test.cpp
@@ -0,0 +1,239 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <cmath>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <joint_trajectory_controller/joint_trajectory_msg_utils.h>
+
+using namespace joint_trajectory_controller;
+using namespace trajectory_msgs;
+using std::vector;
+using std::string;
+
+// Floating-point value comparison threshold
+const double EPS = 1e-9;
+
+TEST(StartTimeTest, StartTime)
+{
+ const ros::Time now(1.0);
+ // Zero start time
+ {
+ JointTrajectory msg;
+ msg.header.stamp = ros::Time(0.0);
+ EXPECT_NE(msg.header.stamp, internal::startTime(msg, now));
+ EXPECT_EQ(now, internal::startTime(msg, now));
+ }
+
+ // Nonzero start time
+ {
+ JointTrajectory msg;
+ msg.header.stamp = ros::Time(2.0);
+ EXPECT_EQ(msg.header.stamp, internal::startTime(msg, now));
+ EXPECT_NE(now, internal::startTime(msg, now));
+ }
+}
+
+class TrajectoryInterfaceRosTest : public ::testing::Test
+{
+public:
+ TrajectoryInterfaceRosTest()
+ : points(3)
+ {
+ points[0].positions.resize(1, 2.0);
+ points[0].velocities.resize(1, 0.0);
+ points[0].accelerations.resize(1, 0.0);
+ points[0].time_from_start = ros::Duration(1.0);
+
+ points[1].positions.resize(1, 4.0);
+ points[1].velocities.resize(1, 0.0);
+ points[1].accelerations.resize(1, 0.0);
+ points[1].time_from_start = ros::Duration(2.0);
+
+ points[2].positions.resize(1, 3.0);
+ points[2].velocities.resize(1, 0.0);
+ points[2].accelerations.resize(1, 0.0);
+ points[2].time_from_start = ros::Duration(4.0);
+
+ trajectory_msg.header.stamp = ros::Time(0.5);
+ trajectory_msg.joint_names.resize(1, "foo_joint");
+ trajectory_msg.points = points;
+ }
+
+protected:
+ vector<JointTrajectoryPoint> points;
+ trajectory_msgs::JointTrajectory trajectory_msg;
+};
+
+TEST_F(TrajectoryInterfaceRosTest, IsValid)
+{
+ // Empty trajectory
+ {
+ JointTrajectory msg;
+ EXPECT_TRUE(isValid(msg));
+ }
+
+ // Joint names size mismatch
+ {
+ JointTrajectory msg = trajectory_msg;
+ msg.joint_names.push_back("bar_joint");
+ EXPECT_FALSE(isValid(msg));
+ }
+
+ // Position size mismatch
+ {
+ JointTrajectory msg = trajectory_msg;
+ msg.points.front().positions.push_back(0.0);
+ EXPECT_FALSE(isValid(msg));
+
+ msg.points.front().positions.clear(); // Empty container is not a size mismatch
+ EXPECT_TRUE(isValid(msg));
+ }
+
+ // Velocity size mismatch
+ {
+ JointTrajectory msg = trajectory_msg;
+ msg.points.front().velocities.push_back(0.0);
+ EXPECT_FALSE(isValid(msg));
+
+ msg.points.front().velocities.clear(); // Empty container is not a size mismatch
+ EXPECT_TRUE(isValid(msg));
+ }
+
+ // Acceleration size mismatch
+ {
+ JointTrajectory msg = trajectory_msg;
+ msg.points.front().accelerations.push_back(0.0);
+ EXPECT_FALSE(isValid(msg));
+
+ msg.points.front().accelerations.clear(); // Empty container is not a size mismatch
+ EXPECT_TRUE(isValid(msg));
+ }
+
+ // Valid trajectory
+ {
+ EXPECT_TRUE(isValid(trajectory_msg));
+ }
+}
+
+TEST_F(TrajectoryInterfaceRosTest, IsTimeStrictlyIncreasing)
+{
+ // Empty trajectory
+ {
+ JointTrajectory msg;
+ EXPECT_TRUE(isTimeStrictlyIncreasing(msg));
+ }
+
+ // Single-waypoint trajectory
+ {
+ JointTrajectory msg;
+ msg.points.push_back(points[0]);
+ EXPECT_TRUE(isTimeStrictlyIncreasing(msg));
+ }
+
+ // Multi-waypoint tajectory with strictly increasing times
+ {
+ EXPECT_TRUE(isTimeStrictlyIncreasing(trajectory_msg));
+ }
+
+ // Multi-waypoint tajectory with monotonically increasing (non-decreasing) times
+ {
+ JointTrajectory msg;
+ msg = trajectory_msg;
+ msg.points[2].time_from_start = msg.points[1].time_from_start;
+ EXPECT_FALSE(isTimeStrictlyIncreasing(msg));
+ }
+
+ // Multi-waypoint tajectory with non-monotonic times
+ {
+ JointTrajectory msg;
+ msg = trajectory_msg;
+ msg.points[2].time_from_start = msg.points[0].time_from_start;
+ EXPECT_FALSE(isTimeStrictlyIncreasing(msg));
+ }
+}
+
+TEST_F(TrajectoryInterfaceRosTest, FindPoint)
+{
+ const ros::Time msg_start_time = trajectory_msg.header.stamp;
+ const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
+
+ // Before first point: No points found
+ {
+ const ros::Time time = msg_start_time;
+ EXPECT_EQ(msg_points.end(), findPoint(trajectory_msg, time));
+ }
+
+ // First point
+ {
+ const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
+ EXPECT_EQ(msg_points.begin(), findPoint(trajectory_msg, time));
+ }
+
+ // Between the first and second points
+ {
+ const ros::Time time = msg_start_time +
+ ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
+ EXPECT_EQ(msg_points.begin(), findPoint(trajectory_msg, time));
+ }
+
+ // Second point
+ {
+ const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start;
+ EXPECT_EQ(++msg_points.begin(), findPoint(trajectory_msg, time));
+ }
+
+ // Between the second and third points
+ {
+ const ros::Time time = msg_start_time +
+ ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0);
+ EXPECT_EQ(++msg_points.begin(), findPoint(trajectory_msg, time));
+ }
+
+ // Last point
+ {
+ const ros::Time time = msg_start_time + (--msg_points.end())->time_from_start;
+ EXPECT_EQ(--msg_points.end(), findPoint(trajectory_msg, time));
+ }
+
+ // After the last point
+ {
+ const ros::Time time = msg_start_time + (--msg_points.end())->time_from_start + ros::Duration(1.0);
+ EXPECT_EQ(--msg_points.end(), findPoint(trajectory_msg, time));
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
new file mode 100644
index 0000000..546dec4
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
@@ -0,0 +1,439 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <cmath>
+#include <stdexcept>
+#include <gtest/gtest.h>
+#include <ros/console.h>
+#include <trajectory_interface/quintic_spline_segment.h>
+#include <joint_trajectory_controller/joint_trajectory_segment.h>
+
+using namespace joint_trajectory_controller;
+using namespace trajectory_msgs;
+
+// Floating-point value comparison threshold
+const double EPS = 1e-9;
+
+typedef JointTrajectorySegment<trajectory_interface::QuinticSplineSegment<double> > Segment;
+typedef std::vector<unsigned int> PermutationType;
+
+TEST(WraparoundOffsetTest, WrappingPositions)
+{
+ // Setup with state increments that cause multi-loop wrapping
+ const double half_pi = M_PI / 2.0;
+ const double two_pi = 2.0 * M_PI;
+
+ std::vector<double> pos1(2);
+ pos1[0] = half_pi / 2.0;
+ pos1[1] = half_pi / 2.0;
+
+ std::vector<double> pos2(2);
+ pos2[0] = two_pi + half_pi;
+ pos2[1] = 2.0 * two_pi + half_pi;
+
+ // No wrapping joints
+ {
+ std::vector<bool> angle_wraparound(2, false);
+ std::vector<double> wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ }
+
+ // Single wrapping joint
+ {
+ std::vector<bool> angle_wraparound(2);
+ angle_wraparound[0] = true;
+ angle_wraparound[1] = false;
+
+ // From state1 to state2
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos1, pos2, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ }
+
+ // From state2 to state1
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos2, pos1, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(two_pi, wraparound_offset[0], EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ }
+ }
+
+ // Both wrapping joints
+ {
+ std::vector<bool> angle_wraparound(2, true);
+
+ // From state1 to state2
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos1, pos2, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS);
+ EXPECT_NEAR(-2.0 * two_pi, wraparound_offset[1], EPS);
+ }
+
+ // From state2 to state1
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos2, pos1, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(two_pi, wraparound_offset[0], EPS);
+ EXPECT_NEAR(2.0 * two_pi, wraparound_offset[1], EPS);
+ }
+ }
+}
+
+TEST(WraparoundOffsetTest, WrappingPositionsPiSingularity)
+{
+ // Setup with state increments that cause multi-loop wrapping
+ const double half_pi = M_PI / 2.0;
+
+ std::vector<double> pos1(1, -half_pi);
+ std::vector<double> pos2(1, half_pi);
+
+ std::vector<bool> angle_wraparound(1, true);
+ // From state1 to state2
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos1, pos2, angle_wraparound);
+ EXPECT_EQ(pos1.size(), wraparound_offset.size());
+ EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ }
+
+ // From state2 to state1
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos2, pos1, angle_wraparound);
+ EXPECT_EQ(pos1.size(), wraparound_offset.size());
+ EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ }
+}
+
+TEST(WraparoundOffsetTest, NonWrappingPositions)
+{
+ // Setup with state increments that don't cause multi-loop wrapping
+ const double half_pi = M_PI / 2.0;
+
+ std::vector<double> pos1(2);
+ pos1[0] = half_pi / 2.0;
+ pos1[1] = half_pi / 2.0;
+
+ std::vector<double> pos2(2);
+ pos2[0] = 0.0;
+ pos2[1] = 2.0 * half_pi;
+
+ // Both wrapping joints
+ {
+ std::vector<bool> angle_wraparound(2, true);
+
+ // From state1 to state2
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos1, pos2, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ }
+
+ // From state2 to state1
+ {
+ std::vector<double> wraparound_offset = wraparoundOffset<double>(pos2, pos1, angle_wraparound);
+ EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ }
+ }
+}
+
+class JointTrajectorySegmentTest : public ::testing::Test
+{
+public:
+ JointTrajectorySegmentTest()
+ : traj_start_time(0.5)
+ {
+ p_start.positions.resize(1, 2.0);
+ p_start.velocities.resize(1, -1.0);
+ p_start.time_from_start = ros::Duration(1.0);
+
+ p_end.positions.resize(1, 4.0);
+ p_end.velocities.resize(1, -2.0);
+ p_end.time_from_start = ros::Duration(2.0);
+
+ start_time = (traj_start_time + p_start.time_from_start).toSec();
+ start_state.position.push_back(p_start.positions.front());
+ start_state.velocity.push_back(p_start.velocities.front());
+
+ end_time = (traj_start_time + p_end.time_from_start).toSec();
+ end_state.position.push_back(p_end.positions.front());
+ end_state.velocity.push_back(p_end.velocities.front());
+ }
+
+protected:
+ ros::Time traj_start_time;
+ JointTrajectoryPoint p_start;
+ JointTrajectoryPoint p_end;
+
+ typename Segment::Time start_time, end_time;
+ typename Segment::State start_state, end_state;
+};
+
+TEST_F(JointTrajectorySegmentTest, InvalidSegmentConstruction)
+{
+ // Empty start point
+ {
+ JointTrajectoryPoint p_start_bad;
+ EXPECT_THROW(Segment(traj_start_time, p_start_bad, p_end), std::invalid_argument);
+ try {Segment(traj_start_time, JointTrajectoryPoint(), p_end);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Start/end data size mismatch
+ {
+ JointTrajectoryPoint p_start_bad = p_start;
+ p_start_bad.positions.push_back(0.0);
+ EXPECT_THROW(Segment(traj_start_time, p_start_bad, p_end), std::invalid_argument);
+ try {Segment(traj_start_time, p_start_bad, p_end);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Invalid start state
+ {
+ JointTrajectoryPoint p_start_bad = p_start;
+ p_start_bad.velocities.push_back(0.0);
+ EXPECT_THROW(Segment(traj_start_time, p_start_bad, p_end), std::invalid_argument);
+ try {Segment(traj_start_time, p_start_bad, p_end);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Invalid end state
+ {
+ JointTrajectoryPoint p_end_bad = p_end;
+ p_end_bad.velocities.push_back(0.0);
+ EXPECT_THROW(Segment(traj_start_time, p_start, p_end_bad), std::invalid_argument);
+ try {Segment(traj_start_time, p_start, p_end_bad);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Invalid permutation vector size
+ {
+ PermutationType permutation(2, 1);
+ EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument);
+ try {Segment(traj_start_time, p_start, p_end, permutation);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Invalid permutation vector indices
+ {
+ PermutationType permutation(1, 1);
+ EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument);
+ try {Segment(traj_start_time, p_start, p_end, permutation);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Invalid joint wraparound specification
+ {
+ PermutationType permutation;
+ std::vector<double> pos_offset(2);
+ EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation, pos_offset), std::invalid_argument);
+ try {Segment(traj_start_time, p_start, p_end, permutation, pos_offset);}
+ catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+}
+
+TEST_F(JointTrajectorySegmentTest, ValidSegmentConstructionRos)
+{
+ // Construct segment from ROS message
+ EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end));
+ Segment segment(traj_start_time, p_start, p_end);
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (traj_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (traj_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(p_end.positions[0], state.position[0]);
+ EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
+ }
+}
+
+TEST_F(JointTrajectorySegmentTest, ValidSegmentConstruction)
+{
+ // Construct segment from time/state data
+ EXPECT_NO_THROW(Segment(start_time, start_state, end_time, end_state));
+ Segment segment(start_time, start_state, end_time, end_state);
+
+ // Check start/end times
+ {
+ const typename Segment::Time start_time = (traj_start_time + p_start.time_from_start).toSec();
+ const typename Segment::Time end_time = (traj_start_time + p_end.time_from_start).toSec();
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.velocities[0], state.velocity[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state;
+ segment.sample(segment.endTime(), state);
+ EXPECT_EQ(p_end.positions[0], state.position[0]);
+ EXPECT_EQ(p_end.velocities[0], state.velocity[0]);
+ }
+}
+
+TEST_F(JointTrajectorySegmentTest, CrossValidateSegmentConstruction)
+{
+ // Compare the output of initializing a segment from a ROS message, or from time/state data structures
+ // This test also checks that empty accelerations in a ROS message are honored and not initialized to zero.
+
+ // Construct segment from time/state data
+ EXPECT_NO_THROW(Segment(start_time, start_state, end_time, end_state));
+ Segment segment_no_ros(start_time, start_state, end_time, end_state);
+
+ // Construct segment from ROS message
+ EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end));
+ Segment segment_ros(traj_start_time, p_start, p_end);
+
+ // Check start/end times
+ {
+ EXPECT_EQ(segment_no_ros.startTime(), segment_ros.startTime());
+ EXPECT_EQ(segment_no_ros.endTime(), segment_ros.endTime());
+ }
+
+ // Check start state
+ {
+ typename Segment::State state_no_ros;
+ typename Segment::State state_ros;
+ segment_no_ros.sample(segment_no_ros.startTime(), state_no_ros);
+ segment_ros.sample(segment_ros.startTime(), state_ros);
+
+ EXPECT_EQ(state_ros.position[0], state_no_ros.position[0]);
+ EXPECT_EQ(state_ros.velocity[0], state_no_ros.velocity[0]);
+ EXPECT_EQ(state_ros.acceleration[0], state_no_ros.acceleration[0]);
+ }
+
+ // Check end state
+ {
+ typename Segment::State state_no_ros;
+ typename Segment::State state_ros;
+ segment_no_ros.sample(segment_no_ros.endTime(), state_no_ros);
+ segment_ros.sample(segment_ros.endTime(), state_ros);
+
+ EXPECT_EQ(state_ros.position[0], state_no_ros.position[0]);
+ EXPECT_EQ(state_ros.velocity[0], state_no_ros.velocity[0]);
+ EXPECT_EQ(state_ros.acceleration[0], state_no_ros.acceleration[0]);
+ }
+}
+
+// TODO: Test with dimension four data
+TEST_F(JointTrajectorySegmentTest, PermutationTest)
+{
+ // Add an extra joint to the trajectory point messages created in the fixture
+ p_start.positions.push_back(-p_start.positions[0]);
+ p_start.velocities.push_back(-p_start.velocities[0]);
+
+ p_end.positions.push_back(-p_end.positions[0]);
+ p_end.velocities.push_back(-p_end.velocities[0]);
+
+ // No permutation vector
+ {
+ // Construct segment from ROS message
+ EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end));
+ Segment segment(traj_start_time, p_start, p_end);
+
+ // Check position values of start state only
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.positions[1], state.position[1]);
+ }
+
+ // Permutation vector preserving trajectory message joint order
+ {
+ PermutationType permutation(2);
+ permutation[0] = 0;
+ permutation[1] = 1;
+
+ // Construct segment from ROS message
+ EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation));
+ Segment segment(traj_start_time, p_start, p_end, permutation);
+
+ // Check position values of start state only
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_EQ(p_start.positions[0], state.position[0]);
+ EXPECT_EQ(p_start.positions[1], state.position[1]);
+ }
+
+ // Permutation vector reversing trajectory message joint order
+ {
+ PermutationType permutation(2);
+ permutation[0] = 1;
+ permutation[1] = 0;
+
+ // Construct segment from ROS message
+ EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation));
+ Segment segment(traj_start_time, p_start, p_end, permutation);
+
+ // Check position values of start state only
+ typename Segment::State state;
+ segment.sample(segment.startTime(), state);
+ EXPECT_NE(p_start.positions[0], state.position[0]);
+ EXPECT_NE(p_start.positions[1], state.position[1]);
+ EXPECT_EQ(p_start.positions[0], state.position[1]);
+ EXPECT_EQ(p_start.positions[1], state.position[0]);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/joint_trajectory_controller/test/quintic_spline_segment_test.cpp b/joint_trajectory_controller/test/quintic_spline_segment_test.cpp
new file mode 100644
index 0000000..97d420e
--- /dev/null
+++ b/joint_trajectory_controller/test/quintic_spline_segment_test.cpp
@@ -0,0 +1,559 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <cmath>
+#include <stdexcept>
+#include <gtest/gtest.h>
+#include <ros/console.h>
+#include <trajectory_interface/quintic_spline_segment.h>
+
+using namespace trajectory_interface;
+
+// Floating-point value comparison threshold
+const double EPS = 1e-9;
+
+typedef QuinticSplineSegment<double> Segment;
+typedef typename Segment::State State;
+typedef typename Segment::Time Time;
+
+TEST(QuinticSplineSegmentTest, StateConstructor)
+{
+ const unsigned int size = 5;
+ State state(size);
+ EXPECT_EQ(size, state.position.size());
+ EXPECT_EQ(size, state.velocity.size());
+ EXPECT_EQ(size, state.acceleration.size());
+
+ for (unsigned int i = 0; i < size; ++i)
+ {
+ EXPECT_EQ(0.0, state.position[i]);
+ EXPECT_EQ(0.0, state.velocity[i]);
+ EXPECT_EQ(0.0, state.acceleration[i]);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, Accessors)
+{
+ const Time start_time = 1.0;
+ State start_state(1);
+ start_state.position[0] = 0.0;
+
+ const Time end_time = 2.0;
+ State end_state(1);
+ end_state.position[0] = 2.0;
+
+ Segment segment(start_time, start_state, end_time, end_state);
+ EXPECT_EQ(start_time, segment.startTime());
+ EXPECT_EQ(end_time, segment.endTime());
+}
+
+TEST(QuinticSplineSegmentTest, InvalidSegmentConstruction)
+{
+ State valid_state(1);
+ valid_state.position[0] = 0.0;
+ valid_state.velocity[0] = 0.0;
+ valid_state.acceleration[0] = 0.0;
+
+ const Time start_time = 1.0;
+ const Time valid_end_time = 2.0;
+
+ // Empty state
+ {
+ State empty_state;
+ EXPECT_THROW(Segment(start_time, empty_state, valid_end_time, valid_state), std::invalid_argument);
+ EXPECT_THROW(Segment(start_time, valid_state, valid_end_time, empty_state), std::invalid_argument);
+ EXPECT_THROW(Segment(start_time, empty_state, valid_end_time, empty_state), std::invalid_argument);
+ try{Segment(start_time, empty_state, valid_end_time, empty_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Start/end state size mismatch
+ {
+ State bad_size_state(2);
+ EXPECT_THROW(Segment(start_time, bad_size_state, valid_end_time, valid_state), std::invalid_argument);
+ EXPECT_THROW(Segment(start_time, valid_state, valid_end_time, bad_size_state), std::invalid_argument);
+ EXPECT_THROW(Segment(start_time, bad_size_state, valid_end_time, valid_state), std::invalid_argument);
+ try{Segment(start_time, bad_size_state, valid_end_time, valid_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Start/end state velocity size mismatch
+ {
+ State bad_vel_state = valid_state;
+ bad_vel_state.velocity.push_back(0.0);
+ EXPECT_THROW(Segment(start_time, bad_vel_state, valid_end_time, valid_state), std::invalid_argument);
+ EXPECT_THROW(Segment(start_time, valid_state, valid_end_time, bad_vel_state), std::invalid_argument);
+ try{Segment(start_time, bad_vel_state, valid_end_time, valid_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ try{Segment(start_time, valid_state, valid_end_time, bad_vel_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Start/end state acceleration size mismatch
+ {
+ State bad_acc_state = valid_state;
+ bad_acc_state.acceleration.push_back(0.0);
+ EXPECT_THROW(Segment(start_time, bad_acc_state, valid_end_time, valid_state), std::invalid_argument);
+ EXPECT_THROW(Segment(start_time, valid_state, valid_end_time, bad_acc_state), std::invalid_argument);
+ try{Segment(start_time, bad_acc_state, valid_end_time, valid_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ try{Segment(start_time, valid_state, valid_end_time, bad_acc_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+
+ // Invalid duration triggers an exception
+ {
+ const Time invalid_end_time = -1.0;
+ EXPECT_THROW(Segment(start_time, valid_state, invalid_end_time, valid_state), std::invalid_argument);
+ try{Segment(start_time, valid_state, invalid_end_time, valid_state);}
+ catch(const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
+ }
+}
+
+TEST(QuinticSplineSegmentTest, DefaultConstructor)
+{
+ EXPECT_NO_THROW(Segment());
+
+ Segment segment;
+ State state(1);
+
+ EXPECT_FALSE(state.position.empty());
+ EXPECT_FALSE(state.velocity.empty());
+ EXPECT_FALSE(state.acceleration.empty());
+
+ segment.sample(1.0, state);
+
+ EXPECT_TRUE(state.position.empty());
+ EXPECT_TRUE(state.velocity.empty());
+ EXPECT_TRUE(state.acceleration.empty());
+}
+
+TEST(QuinticSplineSegmentTest, ZeroDurationPosEnpointsSampler)
+{
+ const Time start_time = 1.0;
+ State start_state;
+ start_state.position.push_back(1.0);
+
+ State end_state;
+ end_state.position.push_back(2.0);
+
+ // Construct segment
+ EXPECT_NO_THROW(Segment(start_time, start_state, start_time, end_state));
+ Segment segment(start_time, start_state, start_time, end_state);
+
+ // Sample before segment start
+ {
+ State state;
+ segment.sample(-start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+
+ // Sample at segment start
+ {
+ State state;
+ segment.sample(start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+
+ // Sample after segment end
+ {
+ State state;
+ segment.sample(2.0 * start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, ZeroDurationPosVelEnpointsSampler)
+{
+ const Time start_time = 1.0;
+ State start_state(1);
+ start_state.position[0] = 1.0;
+ start_state.velocity[0] = 2.0;
+ start_state.acceleration.clear();
+
+ State end_state(1);
+ end_state.position[0] = 2.0;
+ end_state.velocity[0] = 4.0;
+ end_state.acceleration.clear();
+
+ // Construct segment
+ EXPECT_NO_THROW(Segment(start_time, start_state, start_time, end_state));
+ Segment segment(start_time, start_state, start_time, end_state);
+
+ // Sample before segment start
+ {
+ State state;
+ segment.sample(-start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+
+ // Sample at segment start
+ {
+ State state;
+ segment.sample(start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(start_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+
+ // Sample after segment end
+ {
+ State state;
+ segment.sample(2.0 * start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, ZeroDurationPosVelAccEnpointsSampler)
+{
+ const Time start_time = 1.0;
+ State start_state(1);
+ start_state.position[0] = 1.0;
+ start_state.velocity[0] = 2.0;
+ start_state.acceleration[0] = 3.0;
+
+ State end_state(1);
+ end_state.position[0] = 2.0;
+ end_state.velocity[0] = 4.0;
+ end_state.acceleration[0] = 6.0;
+
+ // Construct segment
+ EXPECT_NO_THROW(Segment(start_time, start_state, start_time, end_state));
+ Segment segment(start_time, start_state, start_time, end_state);
+
+ // Sample before segment start
+ {
+ State state;
+ segment.sample(-start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+
+ // Sample at segment start
+ {
+ State state;
+ segment.sample(start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(start_state.velocity[0], state.velocity[0]);
+ EXPECT_EQ(start_state.acceleration[0], state.acceleration[0]);
+ }
+
+ // Sample after segment end
+ {
+ State state;
+ segment.sample(2.0 * start_time, state);
+ EXPECT_EQ(start_state.position[0], state.position[0]);
+ EXPECT_EQ(0.0, state.velocity[0]);
+ EXPECT_EQ(0.0, state.acceleration[0]);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, PosEnpointsSampler)
+{
+ const Time start_time = 1.0;
+ State start_state(1);
+ start_state.position[0] = 0.0;
+ start_state.velocity.clear();
+ start_state.acceleration.clear();
+
+ const Time end_time = 3.0;
+ State end_state(1);
+ end_state.position[0] = 2.0;
+ end_state.velocity[0] = 0.0; // Should be ignored, as start state does not specify it
+ end_state.acceleration[0] = 0.0; // Should be ignored, as start state does not specify it
+
+ const Time duration = end_time - start_time;
+
+ const double velocity = (end_state.position[0] - start_state.position[0]) / duration;
+
+ // Construct segment
+ EXPECT_NO_THROW(Segment(start_time, start_state, end_time, end_state));
+ Segment segment(start_time, start_state, end_time, end_state);
+
+ // Sample before segment start
+ {
+ State state;
+ segment.sample(start_time - duration, state);
+ EXPECT_NEAR(start_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at segment start
+ {
+ State state;
+ segment.sample(start_time, state);
+ EXPECT_NEAR(start_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(velocity, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at mid-segment
+ {
+ State state;
+ segment.sample(start_time + duration / 2.0, state);
+ EXPECT_NEAR(end_state.position[0] / 2.0, state.position[0], EPS);
+ EXPECT_NEAR(velocity, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at segment end
+ {
+ State state;
+ segment.sample(end_time, state);
+ EXPECT_NEAR(end_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(velocity, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample past segment end
+ {
+ State state;
+ segment.sample(end_time + duration, state);
+ EXPECT_NEAR(end_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, PosVelEnpointsSampler)
+{
+ // Start and end state taken from x^3 - 2x
+ const Time start_time = 1.0;
+ State start_state(1);
+ start_state.position[0] = 0.0;
+ start_state.velocity[0] = -2.0;
+ start_state.acceleration.clear();
+
+ const Time end_time = 3.0;
+ State end_state(1);
+ end_state.position[0] = 4.0;
+ end_state.velocity[0] = 10.0;
+ end_state.acceleration[0] = 0.0; // Should be ignored, as start state does not specify it
+
+ const Time duration = end_time - start_time;
+
+ // Construct segment
+ EXPECT_NO_THROW(Segment(start_time, start_state, end_time, end_state));
+ Segment segment(start_time, start_state, end_time, end_state);
+
+ // Sample before segment start
+ {
+ State state;
+ segment.sample(start_time - duration, state);
+ EXPECT_NEAR(start_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at segment start
+ {
+ State state;
+ segment.sample(start_time, state);
+ EXPECT_NEAR(start_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(start_state.velocity[0], state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at mid-segment: Zero-crossing
+ {
+ const double time = start_time + std::sqrt(2.0);
+
+ State state;
+ segment.sample(time, state);
+ EXPECT_NEAR(0.0, state.position[0], EPS);
+ EXPECT_NEAR(4.0, state.velocity[0], EPS);
+ EXPECT_NEAR(6.0 * std::sqrt(2.0), state.acceleration[0], EPS);
+ }
+
+ // Sample at segment end
+ {
+ State state;
+ segment.sample(end_time, state);
+ EXPECT_NEAR(end_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(end_state.velocity[0], state.velocity[0], EPS);
+ EXPECT_NEAR(12.0, state.acceleration[0], EPS);
+ }
+
+ // Sample past segment end
+ {
+ State state;
+ segment.sample(end_time + duration, state);
+ EXPECT_NEAR(end_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, PosVeAcclEnpointsSampler)
+{
+ // Start and end state taken from x(x-1)(x-2)(x-3)(x-4) = x^5 -10x^4 + 35x^3 -50x^2 + 24x
+ const Time start_time = 1.0;
+ State start_state(1);
+ start_state.position[0] = 0.0;
+ start_state.velocity[0] = 24.0;
+ start_state.acceleration[0] = -100.0;
+
+ const Time end_time = 3.0;
+ State end_state(1);
+ end_state.position[0] = 0.0;
+ end_state.velocity[0] = 4.0;
+ end_state.acceleration[0] = 0.0;
+
+ const Time duration = end_time - start_time;
+
+ // Construct segment
+ EXPECT_NO_THROW(Segment(start_time, start_state, end_time, end_state));
+ Segment segment(start_time, start_state, end_time, end_state);
+
+ // Sample before segment start
+ {
+ State state;
+ segment.sample(start_time - duration, state);
+ EXPECT_NEAR(start_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at segment start
+ {
+ State state;
+ segment.sample(start_time, state);
+ EXPECT_NEAR(start_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(start_state.velocity[0], state.velocity[0], EPS);
+ EXPECT_NEAR(start_state.acceleration[0], state.acceleration[0], EPS);
+ }
+
+ // Sample at mid-segment: Zero-crossing
+ {
+ State state;
+ segment.sample(start_time + 1.0, state);
+ EXPECT_NEAR( 0.0, state.position[0], EPS);
+ EXPECT_NEAR(-6.0, state.velocity[0], EPS);
+ EXPECT_NEAR(10.0, state.acceleration[0], EPS);
+ }
+
+ // Sample at segment end
+ {
+ State state;
+ segment.sample(end_time, state);
+ EXPECT_NEAR(end_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(end_state.velocity[0], state.velocity[0], EPS);
+ EXPECT_NEAR(end_state.acceleration[0], state.acceleration[0], EPS);
+ }
+
+ // Sample past segment end
+ {
+ State state;
+ segment.sample(end_time + duration, state);
+ EXPECT_NEAR(end_state.position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[0], EPS);
+ }
+}
+
+TEST(QuinticSplineSegmentTest, MultiDofTest)
+{
+ const unsigned int dim = 2;
+
+ const Time start_time = 1.0;
+ State start_state;
+ start_state.position.push_back( 0.0);
+ start_state.position.push_back(-1.0);
+
+ const Time end_time = 2.0;
+ State end_state;
+ end_state.position.push_back( 1.0);
+ end_state.position.push_back(-2.0);
+
+ Segment segment(start_time, start_state, end_time, end_state);
+ const Time duration = segment.endTime() - segment.startTime();
+
+ // Sample before segments start
+ {
+ State state;
+ segment.sample(start_time - duration, state);
+ EXPECT_EQ(dim, segment.size());
+
+ for (unsigned int i = 0; i < segment.size(); ++i)
+ {
+ EXPECT_NEAR(start_state.position[i], state.position[i], EPS);
+ EXPECT_NEAR(0.0, state.velocity[i], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[i], EPS);
+ }
+ }
+
+ // Sample at mid-segment
+ {
+ State state;
+ segment.sample(start_time + duration / 2.0, state);
+ EXPECT_EQ(dim, segment.size());
+
+ for (unsigned int i = 0; i < segment.size(); ++i)
+ {
+ const double position = (end_state.position[i] + start_state.position[i]) / 2.0;
+ const double velocity = (end_state.position[i] - start_state.position[i]) / duration;
+ EXPECT_NEAR(position, state.position[i], EPS);
+ EXPECT_NEAR(velocity, state.velocity[i], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[i], EPS);
+ }
+ }
+
+ // Sample past segment end
+ {
+ State state;
+ segment.sample(end_time + duration, state);
+ EXPECT_EQ(dim, segment.size());
+
+ for (unsigned int i = 0; i < segment.size(); ++i)
+ {
+ EXPECT_NEAR(end_state.position[i], state.position[i], EPS);
+ EXPECT_NEAR(0.0, state.velocity[i], EPS);
+ EXPECT_NEAR(0.0, state.acceleration[i], EPS);
+ }
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/joint_trajectory_controller/test/rrbot.cpp b/joint_trajectory_controller/test/rrbot.cpp
new file mode 100644
index 0000000..47c7597
--- /dev/null
+++ b/joint_trajectory_controller/test/rrbot.cpp
@@ -0,0 +1,127 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
+
+// ROS
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+
+// 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>
+
+class RRbot : public hardware_interface::RobotHW
+{
+public:
+ RRbot()
+ {
+ // Intialize raw data
+ pos_[0] = 0.0; pos_[1] = 0.0;
+ vel_[0] = 0.0; vel_[1] = 0.0;
+ eff_[0] = 0.0; eff_[1] = 0.0;
+ cmd_[0] = 0.0; cmd_[1] = 0.0;
+
+ // Connect and register the joint state interface
+ hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]);
+ jnt_state_interface_.registerHandle(state_handle_1);
+
+ hardware_interface::JointStateHandle state_handle_2("joint2", &pos_[1], &vel_[1], &eff_[1]);
+ jnt_state_interface_.registerHandle(state_handle_2);
+
+ registerInterface(&jnt_state_interface_);
+
+ // Connect and register the joint position interface
+ hardware_interface::JointHandle pos_handle_1(jnt_state_interface_.getHandle("joint1"), &cmd_[0]);
+ jnt_pos_interface_.registerHandle(pos_handle_1);
+
+ hardware_interface::JointHandle pos_handle_2(jnt_state_interface_.getHandle("joint2"), &cmd_[1]);
+ jnt_pos_interface_.registerHandle(pos_handle_2);
+
+ registerInterface(&jnt_pos_interface_);
+
+ // Smoothing subscriber
+ smoothing_sub_ = ros::NodeHandle().subscribe("smoothing", 1, &RRbot::smoothingCB, this);
+ smoothing_.initRT(0.0);
+ }
+
+ ros::Time getTime() const {return ros::Time::now();}
+ ros::Duration getPeriod() const {return ros::Duration(0.01);}
+
+ void read() {}
+
+ void write()
+ {
+ const double smoothing = *(smoothing_.readFromRT());
+ for (unsigned int i = 0; i < 2; ++i)
+ {
+ vel_[i] = (cmd_[i] - pos_[i]) / getPeriod().toSec();
+
+ const double next_pos = smoothing * pos_[i] + (1.0 - smoothing) * cmd_[i];
+ pos_[i] = next_pos;
+ }
+ }
+
+private:
+ hardware_interface::JointStateInterface jnt_state_interface_;
+ hardware_interface::PositionJointInterface jnt_pos_interface_;
+ double cmd_[2];
+ double pos_[2];
+ double vel_[2];
+ double eff_[2];
+
+ realtime_tools::RealtimeBuffer<double> smoothing_;
+ void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);}
+
+ ros::Subscriber smoothing_sub_;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "rrbot");
+ ros::NodeHandle nh;
+
+ RRbot robot;
+ controller_manager::ControllerManager cm(&robot, nh);
+
+ ros::Rate rate(1.0 / robot.getPeriod().toSec());
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ while (ros::ok())
+ {
+ robot.read();
+ cm.update(robot.getTime(), robot.getPeriod());
+ robot.write();
+ rate.sleep();
+ }
+ spinner.stop();
+
+ return 0;
+}
diff --git a/joint_trajectory_controller/test/rrbot.xacro b/joint_trajectory_controller/test/rrbot.xacro
new file mode 100644
index 0000000..ed1c9d3
--- /dev/null
+++ b/joint_trajectory_controller/test/rrbot.xacro
@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+<!-- Revolute-Revolute Manipulator -->
+<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <!-- Constants for robot dimensions -->
+ <xacro:property name="PI" value="3.1415926535897931"/>
+ <xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
+ <xacro:property name="height1" value="2" /> <!-- Link 1 -->
+ <xacro:property name="height2" value="1" /> <!-- Link 2 -->
+ <xacro:property name="height3" value="1" /> <!-- Link 3 -->
+ <xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
+ <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
+
+ <material name="black">
+ <color rgba="0.0 0.0 0.0 1.0"/>
+ </material>
+
+ <material name="orange">
+ <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+ </material>
+
+ <!-- Used for fixing robot to Gazebo 'base_link' -->
+ <link name="world"/>
+
+ <joint name="fixed" type="fixed">
+ <parent link="world"/>
+ <child link="link1"/>
+ </joint>
+
+ <!-- Base Link -->
+ <link name="link1">
+ <collision>
+ <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
+ <geometry>
+ <box size="${width} ${width} ${height1}"/>
+ </geometry>
+ </collision>
+
+ <visual>
+ <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
+ <geometry>
+ <box size="${width} ${width} ${height1}"/>
+ </geometry>
+ <material name="orange"/>
+ </visual>
+
+ <inertial>
+ <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
+ <mass value="1"/>
+ <inertia
+ ixx="1.0" ixy="0.0" ixz="0.0"
+ iyy="1.0" iyz="0.0"
+ izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="joint1" type="continuous">
+ <parent link="link1"/>
+ <child link="link2"/>
+ <origin xyz="0 ${width} ${height1 - axel_offset}" rpy="0 0 0"/>
+ <axis xyz="0 1 0"/>
+ <dynamics damping="0.7"/>
+ <limit effort="20" velocity="${2*PI}" />
+ </joint>
+
+ <!-- Middle Link -->
+ <link name="link2">
+ <collision>
+ <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
+ <geometry>
+ <box size="${width} ${width} ${height2}"/>
+ </geometry>
+ </collision>
+
+ <visual>
+ <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
+ <geometry>
+ <box size="${width} ${width} ${height2}"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+
+ <inertial>
+ <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
+ <mass value="1"/>
+ <inertia
+ ixx="1.0" ixy="0.0" ixz="0.0"
+ iyy="1.0" iyz="0.0"
+ izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="joint2" type="continuous">
+ <parent link="link2"/>
+ <child link="link3"/>
+ <origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
+ <axis xyz="0 1 0"/>
+ <dynamics damping="0.7"/>
+ <limit effort="20" velocity="${2*PI}" />
+ </joint>
+
+ <!-- Top Link -->
+ <link name="link3">
+ <collision>
+ <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
+ <geometry>
+ <box size="${width} ${width} ${height3}"/>
+ </geometry>
+ </collision>
+
+ <visual>
+ <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
+ <geometry>
+ <box size="${width} ${width} ${height3}"/>
+ </geometry>
+ <material name="orange"/>
+ </visual>
+
+ <inertial>
+ <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
+ <mass value="1"/>
+ <inertia
+ ixx="1.0" ixy="0.0" ixz="0.0"
+ iyy="1.0" iyz="0.0"
+ izz="1.0"/>
+ </inertial>
+ </link>
+
+</robot>
diff --git a/joint_trajectory_controller/test/rrbot_controllers.yaml b/joint_trajectory_controller/test/rrbot_controllers.yaml
new file mode 100644
index 0000000..4623945
--- /dev/null
+++ b/joint_trajectory_controller/test/rrbot_controllers.yaml
@@ -0,0 +1,14 @@
+rrbot_controller:
+ type: "position_controllers/JointTrajectoryController"
+ joints:
+ - joint1
+ - joint2
+
+ constraints:
+ goal_time: 0.5
+ joint1:
+ goal: 0.01
+ trajectory: 0.05
+ joint2:
+ goal: 0.01
+ trajectory: 0.05
diff --git a/joint_trajectory_controller/test/tolerances.test b/joint_trajectory_controller/test/tolerances.test
new file mode 100644
index 0000000..83e7169
--- /dev/null
+++ b/joint_trajectory_controller/test/tolerances.test
@@ -0,0 +1,4 @@
+<launch>
+ <rosparam command="load" ns="test" file="$(find joint_trajectory_controller)/test/tolerances.yaml" />
+ <test test-name="tolerances_test" pkg="joint_trajectory_controller" type="tolerances_test"/>
+</launch>
diff --git a/joint_trajectory_controller/test/tolerances.yaml b/joint_trajectory_controller/test/tolerances.yaml
new file mode 100644
index 0000000..fe90f87
--- /dev/null
+++ b/joint_trajectory_controller/test/tolerances.yaml
@@ -0,0 +1,14 @@
+constraints:
+ goal_time: 1.0 # Defaults to zero
+ stopped_velocity_tolerance: 0.02 # Defaults to 0.01
+ foo_joint:
+ trajectory: 0.05 # Not enforced if unspecified
+ goal: 0.03 # Not enforced if unspecified
+ bar_joint:
+ goal: 0.01
+
+ baz_joint: # Not part of joints we're looking for
+ trajectory: 1.0
+ goal: 2.0
+
+
diff --git a/joint_trajectory_controller/test/tolerances_test.cpp b/joint_trajectory_controller/test/tolerances_test.cpp
new file mode 100644
index 0000000..00f7a1f
--- /dev/null
+++ b/joint_trajectory_controller/test/tolerances_test.cpp
@@ -0,0 +1,320 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <trajectory_interface/pos_vel_acc_state.h>
+#include <joint_trajectory_controller/tolerances.h>
+
+using namespace joint_trajectory_controller;
+typedef trajectory_interface::PosVelAccState<double> State;
+typedef StateTolerances<double> StateTols;
+
+TEST(TolerancesTest, CheckStateTolerance)
+{
+ const double tol = 1.0;
+
+ State state_error_ok;
+ state_error_ok.position.resize(2, -tol);
+ state_error_ok.velocity.resize(2, -tol);
+ state_error_ok.acceleration.resize(2, -tol);
+
+ // Empty tolerances: No checks take place
+ {
+ std::vector<StateTols> state_tols(2);
+ EXPECT_TRUE(checkStateTolerance(state_error_ok, state_tols));
+ }
+
+ // Check position-only tolerances
+ {
+ State state_error = state_error_ok;
+ StateTols state_tol;
+ state_tol.position = tol;
+ std::vector<StateTols> state_tols(2, state_tol);
+
+ // Tolerances OK
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ // Increase non-checked variable errors
+ state_error.velocity.back() *= 2.0;
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ state_error.acceleration.back() *= 2.0;
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ // Increase position errors
+ state_error.position.back() *= 2.0;
+ EXPECT_FALSE(checkStateTolerance(state_error, state_tols));
+ }
+
+ // Check velocity-only tolerances
+ {
+ State state_error = state_error_ok;
+ StateTols state_tol;
+ state_tol.velocity = tol;
+ std::vector<StateTols> state_tols(2, state_tol);
+
+ // Tolerances OK
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ // Increase non-checked variable errors
+ state_error.position.back() *= 2.0;
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ state_error.acceleration.back() *= 2.0;
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ // Increase velocity errors
+ state_error.velocity.back() *= 2.0;
+ EXPECT_FALSE(checkStateTolerance(state_error, state_tols));
+ }
+
+ // Check acceleration-only tolerances
+ {
+ State state_error = state_error_ok;
+ StateTols state_tol;
+ state_tol.acceleration = tol;
+ std::vector<StateTols> state_tols(2, state_tol);
+
+ // Tolerances OK
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ // Increase non-checked variable errors
+ state_error.position.back() *= 2.0;
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ state_error.velocity.back() *= 2.0;
+ EXPECT_TRUE(checkStateTolerance(state_error, state_tols));
+
+ // Increase acceleration errors
+ state_error.acceleration.back() *= 2.0;
+ EXPECT_FALSE(checkStateTolerance(state_error, state_tols));
+ }
+}
+
+TEST(TolerancesTest, UpdateStateTolerances)
+{
+ StateTols default_state_tols(1.0, 2.0, 3.0);
+
+ control_msgs::JointTolerance default_tol_msg;
+ default_tol_msg.name = "foo_joint";
+ default_tol_msg.position = 0.0;
+ default_tol_msg.velocity = 0.0;
+ default_tol_msg.acceleration = 0.0;
+
+ // Zero tolerances: No-op
+ {
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(default_state_tols.position, state_tols.position);
+ EXPECT_EQ(default_state_tols.velocity, state_tols.velocity);
+ EXPECT_EQ(default_state_tols.acceleration, state_tols.acceleration);
+ }
+
+ // Negative tolerances: Reset existing values
+ {
+ // Position
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ tol_msg.position = -1.0;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(0.0, state_tols.position);
+ EXPECT_EQ(default_state_tols.velocity, state_tols.velocity);
+ EXPECT_EQ(default_state_tols.acceleration, state_tols.acceleration);
+ }
+ {
+ // Velocity
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ tol_msg.velocity = -1.0;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(default_state_tols.position, state_tols.position);
+ EXPECT_EQ(0.0, state_tols.velocity);
+ EXPECT_EQ(default_state_tols.acceleration, state_tols.acceleration);
+ }
+ {
+ // Acceleration
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ tol_msg.acceleration = -1.0;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(default_state_tols.position, state_tols.position);
+ EXPECT_EQ(default_state_tols.velocity, state_tols.velocity);
+ EXPECT_EQ(0.0, state_tols.acceleration);
+ }
+
+ // Positive tolerances: Override existing values
+ {
+ // Position
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ tol_msg.position = 0.5;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(tol_msg.position, state_tols.position);
+ EXPECT_EQ(default_state_tols.velocity, state_tols.velocity);
+ EXPECT_EQ(default_state_tols.acceleration, state_tols.acceleration);
+ }
+ {
+ // Velocity
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ tol_msg.velocity = 0.5;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(default_state_tols.position, state_tols.position);
+ EXPECT_EQ(tol_msg.velocity, state_tols.velocity);
+ EXPECT_EQ(default_state_tols.acceleration, state_tols.acceleration);
+ }
+ {
+ // Acceleration
+ StateTols state_tols = default_state_tols;
+ control_msgs::JointTolerance tol_msg = default_tol_msg;
+ tol_msg.acceleration = 0.5;
+ updateStateTolerances(tol_msg, state_tols);
+ EXPECT_EQ(default_state_tols.position, state_tols.position);
+ EXPECT_EQ(default_state_tols.velocity, state_tols.velocity);
+ EXPECT_EQ(tol_msg.acceleration, state_tols.acceleration);
+ }
+}
+
+TEST(TolerancesTest, UpdateSegmentTolerances)
+{
+ // Joint names
+ std::vector<std::string> joint_names(2);
+ joint_names[0] = "foo_joint";
+ joint_names[1] = "bar_joint";
+
+ // Tolerances to update from message data
+ SegmentTolerances<double> ref_segment_tols(2);
+ ref_segment_tols.state_tolerance[0] = StateTolerances<double>(1.0, 1.0, 1.0);
+ ref_segment_tols.state_tolerance[1] = StateTolerances<double>(2.0, 2.0, 2.0);
+ ref_segment_tols.goal_state_tolerance[0] = StateTolerances<double>(3.0, 3.0, 3.0);
+ ref_segment_tols.goal_state_tolerance[1] = StateTolerances<double>(4.0, 4.0, 4.0);
+ ref_segment_tols.goal_time_tolerance = 1.0;
+
+ // Message data
+ control_msgs::JointTolerance invalid_tol_msg;
+ invalid_tol_msg.name = "invalid_joint";
+ invalid_tol_msg.position = -1.0;
+ invalid_tol_msg.velocity = -1.0;
+ invalid_tol_msg.acceleration = -1.0;
+
+ control_msgs::JointTolerance state_tol_msg;
+ state_tol_msg.name = joint_names[0];
+ state_tol_msg.position = 0.5;
+ state_tol_msg.velocity = 0.0;
+ state_tol_msg.acceleration = -1.0;
+
+ control_msgs::JointTolerance goal_state_tol_msg;
+ goal_state_tol_msg.name = joint_names[1];
+ goal_state_tol_msg.position = 0.25;
+ goal_state_tol_msg.velocity = 0.0;
+ goal_state_tol_msg.acceleration = -1.0;
+
+ control_msgs::FollowJointTrajectoryGoal goal;
+ goal.path_tolerance.push_back(invalid_tol_msg); // Useless data that should be ignored
+ goal.path_tolerance.push_back(state_tol_msg); // Only first joint has state tolerances
+ goal.path_tolerance.push_back(invalid_tol_msg); // Useless data that should be ignored
+
+ goal.goal_tolerance.push_back(invalid_tol_msg); // Useless data that should be ignored
+ goal.goal_tolerance.push_back(invalid_tol_msg); // Useless data that should be ignored
+ goal.goal_tolerance.push_back(goal_state_tol_msg); // Only second joint has goal state tolerances
+
+ goal.goal_time_tolerance = ros::Duration(0.0); // No-op
+
+ // Update tolerances from message
+ SegmentTolerances<double> segment_tols = ref_segment_tols;
+ updateSegmentTolerances(goal, joint_names, segment_tols);
+
+ // First joint should get only state tolerances updated
+ EXPECT_EQ(state_tol_msg.position, segment_tols.state_tolerance[0].position); // Update
+ EXPECT_EQ(ref_segment_tols.state_tolerance[0].velocity, segment_tols.state_tolerance[0].velocity); // No-op
+ EXPECT_EQ(0.0, segment_tols.state_tolerance[0].acceleration); // Reset
+
+ EXPECT_EQ(ref_segment_tols.state_tolerance[1].position, segment_tols.state_tolerance[1].position); // No-op
+ EXPECT_EQ(ref_segment_tols.state_tolerance[1].velocity, segment_tols.state_tolerance[1].velocity); // No-op
+ EXPECT_EQ(ref_segment_tols.state_tolerance[1].acceleration, segment_tols.state_tolerance[1].acceleration); // No-op
+
+ // Second joint should get only goal state tolerances updated
+ EXPECT_EQ(ref_segment_tols.goal_state_tolerance[0].position, segment_tols.goal_state_tolerance[0].position); // No-op
+ EXPECT_EQ(ref_segment_tols.goal_state_tolerance[0].velocity, segment_tols.goal_state_tolerance[0].velocity); // No-op
+ EXPECT_EQ(ref_segment_tols.goal_state_tolerance[0].acceleration, segment_tols.goal_state_tolerance[0].acceleration); // No-op
+
+ EXPECT_EQ(goal_state_tol_msg.position, segment_tols.goal_state_tolerance[1].position); // Update
+ EXPECT_EQ(ref_segment_tols.goal_state_tolerance[1].velocity, segment_tols.goal_state_tolerance[1].velocity); // No-op
+ EXPECT_EQ(0.0, segment_tols.goal_state_tolerance[1].acceleration); // Reset
+
+ // Goal time constraint
+ EXPECT_EQ(ref_segment_tols.goal_time_tolerance, segment_tols.goal_time_tolerance); // No-op
+
+ goal.goal_time_tolerance = ros::Duration(1.0);
+ updateSegmentTolerances(goal, joint_names, segment_tols);
+ EXPECT_EQ(goal.goal_time_tolerance.toSec(), segment_tols.goal_time_tolerance); // Update
+
+ goal.goal_time_tolerance = ros::Duration(-1.0);
+ updateSegmentTolerances(goal, joint_names, segment_tols);
+ EXPECT_EQ(0.0, segment_tols.goal_time_tolerance); // Reset
+}
+
+TEST(TolerancesTest, getSegmentTolerances)
+{
+ ros::NodeHandle nh("test/constraints");
+
+ std::vector<std::string> joint_names(2);
+ joint_names[0] = "foo_joint";
+ joint_names[1] = "bar_joint";
+
+ SegmentTolerances<double> segment_tols = getSegmentTolerances<double>(nh, joint_names);
+
+ EXPECT_EQ(joint_names.size(), segment_tols.state_tolerance.size());
+ EXPECT_EQ(joint_names.size(), segment_tols.goal_state_tolerance.size());
+
+ EXPECT_EQ(0.05, segment_tols.state_tolerance[0].position);
+ EXPECT_EQ(0.0, segment_tols.state_tolerance[0].velocity);
+ EXPECT_EQ(0.0, segment_tols.state_tolerance[0].acceleration);
+
+ EXPECT_EQ(0.0, segment_tols.state_tolerance[1].position);
+ EXPECT_EQ(0.0, segment_tols.state_tolerance[1].velocity);
+ EXPECT_EQ(0.0, segment_tols.state_tolerance[1].acceleration);
+
+ EXPECT_EQ(0.03, segment_tols.goal_state_tolerance[0].position);
+ EXPECT_EQ(0.02, segment_tols.goal_state_tolerance[0].velocity);
+ EXPECT_EQ(0.0, segment_tols.goal_state_tolerance[0].acceleration);
+
+ EXPECT_EQ(0.01, segment_tols.goal_state_tolerance[1].position);
+ EXPECT_EQ(0.02, segment_tols.goal_state_tolerance[1].velocity);
+ EXPECT_EQ(0.0, segment_tols.goal_state_tolerance[1].acceleration);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "tolerances_test");
+ return RUN_ALL_TESTS();
+}
diff --git a/joint_trajectory_controller/test/trajectory_interface_test.cpp b/joint_trajectory_controller/test/trajectory_interface_test.cpp
new file mode 100644
index 0000000..b5ac81b
--- /dev/null
+++ b/joint_trajectory_controller/test/trajectory_interface_test.cpp
@@ -0,0 +1,364 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 S.L. 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 Adolfo Rodriguez Tsouroukdissian
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+#include <trajectory_interface/quintic_spline_segment.h>
+#include <trajectory_interface/trajectory_interface.h>
+
+using namespace trajectory_interface;
+
+// Floating-point value comparison threshold
+const double EPS = 1e-9;
+
+typedef QuinticSplineSegment<double> Segment;
+typedef typename Segment::State State;
+typedef typename Segment::Time Time;
+typedef std::vector<Segment> Trajectory;
+
+class TrajectoryInterfaceTest : public ::testing::Test
+{
+public:
+ TrajectoryInterfaceTest()
+ {
+ times[0] = 1.0;
+ states[0].position.push_back(2.0);
+ states[0].velocity.push_back(0.0);
+
+ times[1] = 2.0;
+ states[1].position.push_back(4.0);
+ states[1].velocity.push_back(0.0);
+
+ times[2] = 4.0;
+ states[2].position.push_back(3.0);
+ states[2].velocity.push_back(0.0);
+
+ times[3] = 5.0;
+ states[3].position.push_back(4.0);
+ states[3].velocity.push_back(0.0);
+
+ times[4] = 6.0;
+ states[4].position.push_back(2.0);
+ states[4].velocity.push_back(0.0);
+
+ trajectory.push_back(Segment(times[0], states[0], times[1], states[1]));
+ trajectory.push_back(Segment(times[1], states[1], times[2], states[2]));
+ // There is a period without spline interpolation here, between times[2] and times[3]
+ // Position hold should be used when sampling in this interval
+ trajectory.push_back(Segment(times[3], states[3], times[4], states[4]));
+ // After this segment position hold should be used
+ }
+
+protected:
+ State states[5];
+ Time times[5];
+
+ Trajectory trajectory;
+};
+
+TEST(EmptyTrajectoryInterfaceTest, FindSegment)
+{
+ Trajectory trajectory;
+ EXPECT_EQ(trajectory.end(), findSegment(trajectory, 0.0));
+}
+
+TEST(EmptyTrajectoryInterfaceTest, SampleTrajectory)
+{
+ Trajectory trajectory;
+ State state;
+
+ EXPECT_EQ(trajectory.end(), sample(trajectory, 0.0, state));
+ EXPECT_TRUE(state.position.empty());
+ EXPECT_TRUE(state.velocity.empty());
+ EXPECT_TRUE(state.acceleration.empty());
+}
+
+TEST_F(TrajectoryInterfaceTest, FindSegmentOverloads)
+{
+ // The important thing here is that the test builds
+ // We want to make sure that we can get both const and non-const iterators
+ const Time time = times[0];
+ Trajectory::const_iterator const_it = findSegment(trajectory, time);
+ Trajectory::iterator it = findSegment(trajectory, time);
+ EXPECT_EQ(const_it, it); // This is here only to silence the compiler for unused variables
+}
+
+TEST_F(TrajectoryInterfaceTest, FindSegment)
+{
+ // Before trajectory start: No segments found
+ {
+ const Time time = times[0] - 1.0;
+ EXPECT_EQ(trajectory.end(), findSegment(trajectory, time));
+ }
+
+ // First segment start
+ {
+ const Time time = times[0];
+ EXPECT_EQ(trajectory.begin(), findSegment(trajectory, time));
+ }
+
+ // During the first segment
+ {
+ const Time time = (times[0] + times[1]) / 2.0;
+ EXPECT_EQ(trajectory.begin(), findSegment(trajectory, time));
+ }
+
+ // Second segment start
+ {
+ const Time time = times[1];
+ EXPECT_EQ(++trajectory.begin(), findSegment(trajectory, time));
+ }
+
+ // During the second segment
+ {
+ const Time time = (times[1] + times[2]) / 2.0;
+ EXPECT_EQ(++trajectory.begin(), findSegment(trajectory, time));
+ }
+
+ // After the second segment end time, but before the third segment start time (there is a gap)
+ // The second segment should be still returned
+ {
+ const Time time = (times[2] + times[3]) / 2.0;
+ EXPECT_EQ(++trajectory.begin(), findSegment(trajectory, time));
+ }
+
+ // Last segment start
+ {
+ const Time time = times[3];
+ EXPECT_EQ(--trajectory.end(), findSegment(trajectory, time));
+ }
+
+ // During the last segment
+ {
+ const Time time = (times[3] + times[4]) / 2.0;
+ EXPECT_EQ(--trajectory.end(), findSegment(trajectory, time));
+ }
+
+ // After the last segment end time
+ // The last segment should be still returned
+ {
+ const Time time = times[4] + 1.0;
+ EXPECT_EQ(--trajectory.end(), findSegment(trajectory, time));
+ }
+}
+
+TEST_F(TrajectoryInterfaceTest, SampleTrajectory)
+{
+ // Before trajectory start: No segments found
+ {
+ const Time time = times[0] - 1.0;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[0].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[0].velocity[0], state.velocity[0], EPS);
+ }
+
+ // First segment start
+ {
+ const Time time = times[0];
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[0].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[0].velocity[0], state.velocity[0], EPS);
+ }
+
+ // During the first segment
+ {
+ const Time time = (times[0] + times[1]) / 2.0;
+ const double position = (states[0].position[0] + states[1].position[0]) / 2.0;
+ const double velocity = 3.0;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(position, state.position[0], EPS);
+ EXPECT_NEAR(velocity, state.velocity[0], EPS);
+ }
+
+ // Second segment start
+ {
+ const Time time = times[1];
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[1].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[1].velocity[0], state.velocity[0], EPS);
+ }
+
+ // During the second segment
+ {
+ const Time time = (times[1] + times[2]) / 2.0;
+ const double position = (states[1].position[0] + states[2].position[0]) / 2.0;
+ const double velocity = -0.75;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(position, state.position[0], EPS);
+ EXPECT_NEAR(velocity, state.velocity[0], EPS);
+ }
+
+ // After the second segment end time, but before the third segment start time (there is a gap)
+ // The second segment should be still returned
+ {
+ const Time time = (times[2] + times[3]) / 2.0;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[2].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[2].velocity[0], state.velocity[0], EPS);
+ }
+
+ // Just before last segment start
+ // There should be a discontinuity between this point and the last segment start. We make it explicit here
+ // The discontinuity arises by the way in which the trajectory segments were specified
+ {
+ const Time time = times[3] - EPS;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[2].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[2].velocity[0], state.velocity[0], EPS);
+ }
+
+ // Last segment start
+ {
+ const Time time = times[3];
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[3].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[3].velocity[0], state.velocity[0], EPS);
+ }
+
+ // During the last segment
+ {
+ const Time time = (times[3] + times[4]) / 2.0;
+ const double position = (states[3].position[0] + states[4].position[0]) / 2.0;
+ const double velocity = -3.0;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(position, state.position[0], EPS);
+ EXPECT_NEAR(velocity, state.velocity[0], EPS);
+ }
+
+ // After the last segment end time
+ // The last segment should be still returned
+ {
+ const Time time = times[4] + 1.0;
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[4].position[0], state.position[0], EPS);
+ EXPECT_NEAR(states[4].velocity[0], state.velocity[0], EPS);
+ }
+}
+
+TEST(OverlappingTrajectoryInterfaceTest, SampleTrajectory)
+{
+ State states[4];
+ Time times[4];
+
+ Trajectory trajectory;
+
+ times[0] = 0.0;
+ states[0].position.push_back(0.0);
+
+ times[1] = 2.0;
+ states[1].position.push_back(2.0);
+
+ times[2] = 1.0;
+ states[2].position.push_back(1.0);
+
+ times[3] = 3.0;
+ states[3].position.push_back(5.0);
+
+ // Both segments are defined in the [1.0, 2.0) interval. The second one should take precedence
+ trajectory.push_back(Segment(times[0], states[0], times[1], states[1]));
+ trajectory.push_back(Segment(times[2], states[2], times[3], states[3]));
+
+ const double velocity1 = (states[1].position[0] - states[0].position[0]) / (times[1] - times[0]);
+ const double velocity2 = (states[3].position[0] - states[2].position[0]) / (times[3] - times[2]);
+
+ // First segment start
+ {
+ const Time time = times[0];
+ State state;
+ sample(trajectory, time, state);
+ EXPECT_NEAR(states[0].position[0], state.position[0], EPS);
+ EXPECT_NEAR(velocity1, state.velocity[0], EPS);
+ }
+
+ // During the first segment
+ {
+ const Time time = (times[0] + times[2]) / 2.0; // Midway between first and second segment start
+ const double position = (states[0].position[0] + states[2].position[0]) / 2.0;
+ State state;
+ EXPECT_EQ(trajectory.begin(), sample(trajectory, time, state));
+ EXPECT_NEAR(position, state.position[0], EPS);
+ EXPECT_NEAR(velocity1, state.velocity[0], EPS);
+ }
+
+ // Second segment start
+ {
+ const Time time = times[2];
+ State state;
+ EXPECT_EQ(++trajectory.begin(), sample(trajectory, time, state));
+ EXPECT_NEAR(states[2].position[0], state.position[0], EPS);
+ EXPECT_NEAR(velocity2, state.velocity[0], EPS);
+ }
+
+ // During the second segment
+ {
+ const Time time = (times[2] + times[3]) / 2.0; // Midway between second segment start and end
+ const double position = (states[2].position[0] + states[3].position[0]) / 2.0;
+ State state;
+ EXPECT_EQ(++trajectory.begin(), sample(trajectory, time, state));
+ EXPECT_NEAR(position, state.position[0], EPS);
+ EXPECT_NEAR(velocity2, state.velocity[0], EPS);
+ }
+
+ // Second segment end
+ {
+ const Time time = times[3];
+ State state;
+ EXPECT_EQ(++trajectory.begin(), sample(trajectory, time, state));
+ EXPECT_NEAR(states[3].position[0], state.position[0], EPS);
+ EXPECT_NEAR(velocity2, state.velocity[0], EPS);
+ }
+
+ // After the second segment end time (and specified first segment end time)
+ // The second segment should be still returned
+ {
+ const Time time = times[3] + 1.0;
+ State state;
+ EXPECT_EQ(++trajectory.begin(), sample(trajectory, time, state));
+ EXPECT_NEAR(states[3].position[0], state.position[0], EPS);
+ EXPECT_NEAR(0.0, state.velocity[0], EPS);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/position_controllers/.gitignore b/position_controllers/.gitignore
new file mode 100644
index 0000000..61fd441
--- /dev/null
+++ b/position_controllers/.gitignore
@@ -0,0 +1,2 @@
+build
+lib
\ No newline at end of file
diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst
new file mode 100644
index 0000000..34c6889
--- /dev/null
+++ b/position_controllers/CHANGELOG.rst
@@ -0,0 +1,87 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package position_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+
+0.9.2 (2015-05-04)
+------------------
+* Thread-safe and realtime-safe forward controllers.
+* Contributors: Mathias Lüdtke
+
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* New controller: position_controllers/JointGroupPositionController (multi-joint)
+* Hold position when starting JointPositionController and JointGroupPositionController
+* Buildsystem fixes
+* Contributors: Dave Coleman, ipa-fxm
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+* Link shared libraries to catkin libraries
+ GCC is quite lenient with missing symbols on shared libraries and
+ doesn't event output any warning about it.
+ When building with other compilers, missing symbols result in build
+ errors.
+* Contributors: Paul Mathieu
+
+0.5.4 (2013-09-30)
+------------------
+
+0.5.3 (2013-09-04)
+------------------
+* Removed manifest.xml from all packages to prevent rosdep heirarchy issues in Groovy and Hydro
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-19)
+------------------
+* Added maintainer
+
+0.5.0 (2013-07-16)
+------------------
+* Merged
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+* Restore "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors""
+ This reverts commit 0862ad93696b0d736b565cd65ea36690dde0eaa7.
+* Adding install targets for plugin xml files
+* Revert "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors"
+ This reverts commit 2314b8b434e35dc9c1c298140118a004e00febd8.
+
+0.4.0 (2013-06-26)
+------------------
+* Version 0.4.0
+* Fixed PLUGINLIB_DECLARE_CLASS depreacated errors
+* adding install targets
+* adding switches for hybrid buildsystem
+* adding these packages which weren't seen by catkinize_stack
+* Extend joint_effort_controller to other interfaces
+ - Factor-out implementation of simple command-forwarding controller.
+ - Provide specializations (typedefs really) for effort, velocity and position
+ interfaces.
diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt
new file mode 100644
index 0000000..e7f535c
--- /dev/null
+++ b/position_controllers/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(position_controllers)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS controller_interface forward_command_controller)
+
+# Declare catkin project
+catkin_package(
+ CATKIN_DEPENDS controller_interface forward_command_controller
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+add_library(${PROJECT_NAME}
+ src/joint_position_controller.cpp
+ src/joint_group_position_controller.cpp
+)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+install(FILES position_controllers_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.h b/position_controllers/include/position_controllers/joint_group_position_controller.h
new file mode 100644
index 0000000..30f0213
--- /dev/null
+++ b/position_controllers/include/position_controllers/joint_group_position_controller.h
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef POSITION_CONTROLLERS_JOINT_GROUP_POSITION_CONTROLLER_H
+#define POSITION_CONTROLLERS_JOINT_GROUP_POSITION_CONTROLLER_H
+
+#include <forward_command_controller/forward_joint_group_command_controller.h>
+
+namespace position_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of position controlled joints (linear or angular).
+ *
+ * This class forwards the commanded positions down to a set of joints.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointGroupPositionController".
+ * \param joints List of names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint positions to apply
+ */
+typedef forward_command_controller::ForwardJointGroupCommandController<hardware_interface::PositionJointInterface>
+ JointGroupPositionController;
+}
+
+#endif
diff --git a/position_controllers/include/position_controllers/joint_position_controller.h b/position_controllers/include/position_controllers/joint_position_controller.h
new file mode 100644
index 0000000..3dd0a35
--- /dev/null
+++ b/position_controllers/include/position_controllers/joint_position_controller.h
@@ -0,0 +1,62 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef POSITION_CONTROLLERS_JOINT_POSITION_CONTROLLER_H
+#define POSITION_CONTROLLERS_JOINT_POSITION_CONTROLLER_H
+
+#include <forward_command_controller/forward_command_controller.h>
+
+namespace position_controllers
+{
+
+/**
+ * \brief Joint Position Controller (linear or angular)
+ *
+ * This class passes the commanded position down to the joint
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointPositionController".
+ * \param joint Name of the joint to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64) : The joint position to apply
+ */
+typedef forward_command_controller::ForwardCommandController<hardware_interface::PositionJointInterface>
+ JointPositionController;
+}
+
+#endif
diff --git a/position_controllers/mainpage.dox b/position_controllers/mainpage.dox
new file mode 100644
index 0000000..0563999
--- /dev/null
+++ b/position_controllers/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b position_controllers
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/position_controllers/package.xml b/position_controllers/package.xml
new file mode 100644
index 0000000..bff4d99
--- /dev/null
+++ b/position_controllers/package.xml
@@ -0,0 +1,26 @@
+<package>
+ <name>position_controllers</name>
+ <version>0.10.0</version>
+ <description>position_controllers</description>
+
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="davetcoleman at gmail.com">Dave Coleman</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>Vijay Pradeep</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>forward_command_controller</build_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>forward_command_controller</run_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/position_controllers_plugins.xml"/>
+ </export>
+</package>
diff --git a/position_controllers/position_controllers_plugins.xml b/position_controllers/position_controllers_plugins.xml
new file mode 100644
index 0000000..0543308
--- /dev/null
+++ b/position_controllers/position_controllers_plugins.xml
@@ -0,0 +1,15 @@
+<library path="lib/libposition_controllers">
+
+ <class name="position_controllers/JointPositionController" type="position_controllers::JointPositionController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointPositionController tracks position commands. It expects a PositionJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="position_controllers/JointGroupPositionController" type="position_controllers::JointGroupPositionController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointGroupPositionController tracks position commands for a set of joints. It expects a PositionJointInterface type of hardware interface.
+ </description>
+ </class>
+
+</library>
diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp
new file mode 100644
index 0000000..ee88966
--- /dev/null
+++ b/position_controllers/src/joint_group_position_controller.cpp
@@ -0,0 +1,53 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <position_controllers/joint_group_position_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+{
+ // Start controller with current joint positions
+ std::vector<double> & commands = *commands_buffer_.readFromRT();
+ for(unsigned int i=0; i<joints_.size(); i++)
+ {
+ commands[i]=joints_[i].getPosition();
+ }
+}
+
+
+PLUGINLIB_EXPORT_CLASS(position_controllers::JointGroupPositionController,controller_interface::ControllerBase)
diff --git a/position_controllers/src/joint_position_controller.cpp b/position_controllers/src/joint_position_controller.cpp
new file mode 100644
index 0000000..0590458
--- /dev/null
+++ b/position_controllers/src/joint_position_controller.cpp
@@ -0,0 +1,47 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <position_controllers/joint_position_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardCommandController<T>::starting(const ros::Time& time)
+{
+ // Start controller with current joint position
+ command_buffer_.writeFromNonRT(joint_.getPosition());
+}
+
+PLUGINLIB_EXPORT_CLASS(position_controllers::JointPositionController,controller_interface::ControllerBase)
diff --git a/ros_controllers/CHANGELOG.rst b/ros_controllers/CHANGELOG.rst
new file mode 100644
index 0000000..f27d19b
--- /dev/null
+++ b/ros_controllers/CHANGELOG.rst
@@ -0,0 +1,66 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ros_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+
+0.9.2 (2015-05-04)
+------------------
+
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Add rqt_joint_trajectory_controller to metapackage
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+* Add diff_drive_controller and gripper_action_controller dependencies.
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Dave Coleman
+
+0.6.0 (2014-02-05)
+------------------
+* Add self as ros_controllers maintainer.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.5.4 (2013-09-30)
+------------------
+* Add joint_trajectory_controller to metapackage.
+
+0.5.3 (2013-09-04)
+------------------
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-16)
+------------------
+* Added to maintainer list
+
+0.5.0 (2013-07-16)
+------------------
+* Added force_torque_sensor_controller
+* Removed controller_msgs, switched to depend on control_msgs
+* Added imu_sensor_controller
+* Updates to effort_controllers
+
+
+0.4.0 (2013-06-26)
+------------------
+* Initial release of ros_controllers into bloom/ROS distro
diff --git a/ros_controllers/CMakeLists.txt b/ros_controllers/CMakeLists.txt
new file mode 100644
index 0000000..959fe1d
--- /dev/null
+++ b/ros_controllers/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_controllers)
+find_package(catkin REQUIRED)
+catkin_metapackage()
diff --git a/ros_controllers/package.xml b/ros_controllers/package.xml
new file mode 100644
index 0000000..58e3ee1
--- /dev/null
+++ b/ros_controllers/package.xml
@@ -0,0 +1,35 @@
+<package>
+ <name>ros_controllers</name>
+ <version>0.10.0</version>
+ <description>Library of ros controllers</description>
+
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="davetcoleman at gmail.com">Dave Coleman</maintainer>
+
+ <license>BSD</license>
+
+ <url type="website">http://ros.org/wiki/ros_controllers</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>Wim Meeussen</author>
+
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <run_depend>imu_sensor_controller</run_depend>
+ <run_depend>force_torque_sensor_controller</run_depend>
+ <run_depend>position_controllers</run_depend>
+ <run_depend>velocity_controllers</run_depend>
+ <run_depend>effort_controllers</run_depend>
+ <run_depend>forward_command_controller</run_depend>
+ <run_depend>joint_state_controller</run_depend>
+ <run_depend>joint_trajectory_controller</run_depend>
+ <run_depend>diff_drive_controller</run_depend>
+ <run_depend>gripper_action_controller</run_depend>
+ <run_depend>rqt_joint_trajectory_controller</run_depend>
+
+ <export>
+ <metapackage/>
+ </export>
+</package>
diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst
new file mode 100644
index 0000000..a165356
--- /dev/null
+++ b/rqt_joint_trajectory_controller/CHANGELOG.rst
@@ -0,0 +1,87 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rqt_joint_trajectory_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+* Adapt to new controller_manager_msgs/ControllerState message definition
+* Add vertical scrollbar to joints list
+ - Add vertical scrollbar to joints list that appears only when required,
+ i.e., when the plugin size cannot accommodate all controller joints.
+ - Remove vertical spacer at the bottom of the plugin.
+* Clear controllers combo on cm change
+ Clear the list of running joint trajectory controllers when the
+ controller manager selection changes. This prevents potential conflicts when
+ multiple controller managers have controllers with the same names.
+* Fail gracefully if URDF is not loaded
+ Implement lazy loading of joint limits from URDF.
+ This allows to start rqt_joint_trajectory_controller on an otherwise empty ROS
+ node graph without crashing.
+* Save and restore plugin settings
+ - Save current controller_manager and controller selection on plugin close
+ - Restore last selection if controller manager and controller are running
+* Stricter controller validation
+ Only display in the controller combo box those controllers that are running
+ _and\_ have position and velocity limits specified in the URDF. In the absence
+ of limits information, it's not posible to properly initialize the GUI sliders.
+* Fix broken URDF joint limits parsing
+* Don't choke on missing URDF vel limits
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.2 (2015-05-04)
+------------------
+* rqt_joint_traj_controller: Add missing runtime dep
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.1 (2014-11-03)
+------------------
+
+0.9.0 (2014-10-31)
+------------------
+* New rqt plugin: joint_trajectory_controller rqt plugin.
+ - Allows to select any running joint trajectory controller from any active
+ controller manager
+ - Two modes:
+ - Monitor: Joint display shows actual positions of controller joints
+ - Control: Joint display sends controller commands
+ - Max joint speed is read from the URDF, but can be scaled down for safety
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+
+0.7.3 (2014-10-28)
+------------------
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+
+0.5.4 (2013-09-30)
+------------------
+
+0.5.3 (2013-09-04)
+------------------
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+
+0.4.0 (2013-06-26)
+------------------
diff --git a/rqt_joint_trajectory_controller/CMakeLists.txt b/rqt_joint_trajectory_controller/CMakeLists.txt
new file mode 100644
index 0000000..c6384e3
--- /dev/null
+++ b/rqt_joint_trajectory_controller/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rqt_joint_trajectory_controller)
+
+find_package(catkin REQUIRED COMPONENTS)
+catkin_python_setup()
+catkin_package()
+
+install(FILES plugin.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY resource
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(PROGRAMS
+ scripts/rqt_joint_trajectory_controller
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml
new file mode 100644
index 0000000..919a0c0
--- /dev/null
+++ b/rqt_joint_trajectory_controller/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package>
+ <name>rqt_joint_trajectory_controller</name>
+ <version>0.10.0</version>
+ <description>Graphical frontend for interacting with joint_trajectory_controller instances.</description>
+
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <author email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</author>
+ <license>Modified BSD</license>
+ <url type="website">http://wiki.ros.org/rqt_joint_trajectory_controller</url>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <run_depend>control_msgs</run_depend>
+ <run_depend>controller_manager_msgs</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+ <run_depend>rospy</run_depend>
+ <run_depend>rqt_gui</run_depend>
+ <run_depend>rqt_gui_py</run_depend>
+
+ <export>
+ <rqt_gui plugin="${prefix}/plugin.xml"/>
+ </export>
+</package>
diff --git a/rqt_joint_trajectory_controller/plugin.xml b/rqt_joint_trajectory_controller/plugin.xml
new file mode 100644
index 0000000..ebd5d73
--- /dev/null
+++ b/rqt_joint_trajectory_controller/plugin.xml
@@ -0,0 +1,21 @@
+<library path="src">
+ <class name="JointTrajectoryController"
+ type="rqt_joint_trajectory_controller.joint_trajectory_controller.JointTrajectoryController"
+ base_class_type="rqt_gui_py::Plugin">
+ <description>
+ Graphical frontend for interacting with joint_trajectory_controller instances.
+ </description>
+ <qtgui>
+ <group>
+ <label>Robot Tools</label>
+ <icon type="theme">folder</icon>
+ <statustip>Plugins related to robot tools.</statustip>
+ </group>
+ <label>Joint trajectory controller</label>
+ <icon>resource/scope.png</icon>
+ <statustip>
+ Monitor and send commands to running joint trajectory controllers.
+ </statustip>
+ </qtgui>
+ </class>
+</library>
\ No newline at end of file
diff --git a/rqt_joint_trajectory_controller/resource/double_editor.ui b/rqt_joint_trajectory_controller/resource/double_editor.ui
new file mode 100644
index 0000000..828a1e1
--- /dev/null
+++ b/rqt_joint_trajectory_controller/resource/double_editor.ui
@@ -0,0 +1,92 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>double_editor</class>
+ <widget class="QWidget" name="double_editor">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>249</width>
+ <height>121</height>
+ </rect>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout" stretch="1,0">
+ <item>
+ <widget class="QSlider" name="slider">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="baseSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="invertedControls">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QDoubleSpinBox" name="spin_box">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="baseSize">
+ <size>
+ <width>100</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="buttonSymbols">
+ <enum>QAbstractSpinBox::UpDownArrows</enum>
+ </property>
+ <property name="keyboardTracking">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui
new file mode 100644
index 0000000..3f3b6c1
--- /dev/null
+++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui
@@ -0,0 +1,202 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>joint_trajectory_controller</class>
+ <widget class="QWidget" name="joint_trajectory_controller">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>336</width>
+ <height>317</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>Joint trajectory controller</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <widget class="QGroupBox" name="controller_group">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="1">
+ <widget class="QComboBox" name="jtc_combo"/>
+ </item>
+ <item row="1" column="0">
+ <widget class="QComboBox" name="cm_combo"/>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="cm_list_label">
+ <property name="toolTip">
+ <string><html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html></string>
+ </property>
+ <property name="text">
+ <string>controller manager ns</string>
+ </property>
+ <property name="buddy">
+ <cstring>cm_combo</cstring>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="controller_list_label">
+ <property name="text">
+ <string>controller</string>
+ </property>
+ <property name="buddy">
+ <cstring>jtc_combo</cstring>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="toolTip">
+ <string><html><head/><body><p>Enable/disable sending commands to the controller.</p></body></html></string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="icon">
+ <iconset>
+ <normaloff>off.svg</normaloff>
+ <normalon>on.svg</normalon>
+ <activeon>on.svg</activeon>off.svg</iconset>
+ </property>
+ <property name="iconSize">
+ <size>
+ <width>48</width>
+ <height>48</height>
+ </size>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="joint_group_outer">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="title">
+ <string>joints</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="flat">
+ <bool>false</bool>
+ </property>
+ <property name="checkable">
+ <bool>false</bool>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <item>
+ <widget class="QScrollArea" name="joint_group_scroll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::NoFrame</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Plain</enum>
+ </property>
+ <property name="verticalScrollBarPolicy">
+ <enum>Qt::ScrollBarAsNeeded</enum>
+ </property>
+ <property name="horizontalScrollBarPolicy">
+ <enum>Qt::ScrollBarAlwaysOff</enum>
+ </property>
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="QWidget" name="joint_group">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>314</width>
+ <height>109</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGroupBox" name="speed_scaling_group">
+ <property name="title">
+ <string>speed scaling</string>
+ </property>
+ <layout class="QVBoxLayout" name="speed_scaling_layout"/>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <tabstops>
+ <tabstop>cm_combo</tabstop>
+ <tabstop>jtc_combo</tabstop>
+ <tabstop>enable_button</tabstop>
+ </tabstops>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/rqt_joint_trajectory_controller/resource/off.svg b/rqt_joint_trajectory_controller/resource/off.svg
new file mode 100644
index 0000000..4c91a11
--- /dev/null
+++ b/rqt_joint_trajectory_controller/resource/off.svg
@@ -0,0 +1,267 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="54.122021"
+ height="54.122028"
+ id="svg2633"
+ sodipodi:version="0.32"
+ inkscape:version="0.48.3.1 r9886"
+ version="1.0"
+ sodipodi:docname="off.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ sodipodi:modified="true">
+ <defs
+ id="defs2635">
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3762"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3764"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3766"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ id="linearGradient3257"
+ inkscape:collect="always">
+ <stop
+ id="stop3259"
+ offset="0"
+ style="stop-color:#ffffff;stop-opacity:1" />
+ <stop
+ id="stop3261"
+ offset="1"
+ style="stop-color:#ffffff;stop-opacity:0" />
+ </linearGradient>
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3768"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient2714"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient2716"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient2718"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient2720"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient2243"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="4"
+ inkscape:cx="4.2775602"
+ inkscape:cy="17.232002"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ width="82px"
+ height="60px"
+ inkscape:window-width="739"
+ inkscape:window-height="573"
+ inkscape:window-x="250"
+ inkscape:window-y="223"
+ showgrid="false"
+ inkscape:window-maximized="0"
+ fit-margin-top="5"
+ fit-margin-left="5"
+ fit-margin-right="5"
+ fit-margin-bottom="5" />
+ <metadata
+ id="metadata2638">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title>red power button</dc:title>
+ <dc:date>08 12 2006</dc:date>
+ <dc:creator>
+ <cc:Agent>
+ <dc:title>molumen</dc:title>
+ </cc:Agent>
+ </dc:creator>
+ <dc:description>red power button</dc:description>
+ <cc:license
+ rdf:resource="http://web.resource.org/cc/PublicDomain" />
+ <dc:subject>
+ <rdf:Bag>
+ <rdf:li>icon</rdf:li>
+ <rdf:li>button</rdf:li>
+ <rdf:li>design</rdf:li>
+ <rdf:li>UI</rdf:li>
+ <rdf:li>interface</rdf:li>
+ <rdf:li>power</rdf:li>
+ <rdf:li>switch</rdf:li>
+ <rdf:li>on</rdf:li>
+ <rdf:li>off</rdf:li>
+ <rdf:li>red</rdf:li>
+ <rdf:li>glossy</rdf:li>
+ <rdf:li>toggle</rdf:li>
+ </rdf:Bag>
+ </dc:subject>
+ </cc:Work>
+ <cc:License
+ rdf:about="http://web.resource.org/cc/PublicDomain">
+ <cc:permits
+ rdf:resource="http://web.resource.org/cc/Reproduction" />
+ <cc:permits
+ rdf:resource="http://web.resource.org/cc/Distribution" />
+ <cc:permits
+ rdf:resource="http://web.resource.org/cc/DerivativeWorks" />
+ </cc:License>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1"
+ transform="translate(-33.253876,-23.858592)">
+ <path
+ sodipodi:type="arc"
+ style="fill:#ef2929;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ id="path3676"
+ sodipodi:cx="655.00006"
+ sodipodi:cy="908.54846"
+ sodipodi:rx="35.10146"
+ sodipodi:ry="35.10146"
+ d="m 690.10152,908.54846 c 0,19.386 -15.71546,35.10146 -35.10146,35.10146 -19.386,0 -35.10146,-15.71546 -35.10146,-35.10146 0,-19.386 15.71546,-35.10146 35.10146,-35.10146 19.386,0 35.10146,15.71546 35.10146,35.10146 z"
+ transform="matrix(0.6284927,0,0,0.6284928,-351.34787,-520.09656)" />
+ <g
+ id="g3750"
+ transform="matrix(0.5824089,0,0,0.6244504,-298.35727,-439.02821)"
+ style="fill:#a40000;stroke:none">
+ <path
+ sodipodi:type="inkscape:offset"
+ inkscape:radius="-0.66131908"
+ inkscape:original="M 649.75 891.0625 C 641.40779 893.35756 635.28126 900.99292 635.28125 910.0625 C 635.28127 920.94545 644.11705 929.78124 655 929.78125 C 665.88295 929.78122 674.71874 920.94545 674.71875 910.0625 C 674.71873 900.98968 668.56549 893.35456 660.21875 891.0625 L 660.21875 899.15625 C 664.28467 901.11041 667.125 905.25095 667.125 910.0625 C 667.12501 916.75034 661.68785 922.1875 655 922.1875 C 648.31215 922.18751 642.875 916.75035 642.875 910.0625 C 642.87498 905.2 [...]
+ xlink:href="#path22091"
+ style="fill:#a40000;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ id="path3752"
+ inkscape:href="#path22091"
+ d="m 649.09375,892 c -7.61853,2.48999 -13.15624,9.60548 -13.15625,18.0625 2e-5,10.525 8.5375,19.06249 19.0625,19.0625 10.525,-3e-5 19.06249,-8.5375 19.0625,-19.0625 -2e-5,-8.45792 -5.56444,-15.57409 -13.1875,-18.0625 l 0,6.8125 c 4.06728,2.13572 6.90625,6.34059 6.90625,11.25 10e-6,7.04627 -5.73497,12.78125 -12.78125,12.78125 -7.04628,10e-6 -12.78125,-5.73497 -12.78125,-12.78125 -2e-5,-4.90659 2.80869,-9.1136 6.875,-11.25 l 0,-6.8125 z"
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)" />
+ <path
+ sodipodi:type="inkscape:offset"
+ inkscape:radius="-0.60243881"
+ inkscape:original="M 651.1875 887.3125 L 651.1875 911.125 L 658.8125 911.125 L 658.8125 887.3125 L 651.1875 887.3125 z "
+ xlink:href="#path22093"
+ style="fill:#a40000;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ id="path3754"
+ inkscape:href="#path22093"
+ d="m 651.78125,887.90625 0,22.625 6.4375,0 0,-22.625 -6.4375,0 z"
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)" />
+ </g>
+ <g
+ style="fill:#eeeeec;stroke:none"
+ transform="matrix(0.5824089,0,0,0.5824089,-298.35727,-406.81819)"
+ id="g3756">
+ <path
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)"
+ d="m 649.09375,892 c -7.61853,2.48999 -13.15624,9.60548 -13.15625,18.0625 2e-5,10.525 8.5375,19.06249 19.0625,19.0625 10.525,-3e-5 19.06249,-8.5375 19.0625,-19.0625 -2e-5,-8.45792 -5.56444,-15.57409 -13.1875,-18.0625 l 0,6.8125 c 4.06728,2.13572 6.90625,6.34059 6.90625,11.25 10e-6,7.04627 -5.73497,12.78125 -12.78125,12.78125 -7.04628,10e-6 -12.78125,-5.73497 -12.78125,-12.78125 -2e-5,-4.90659 2.80869,-9.1136 6.875,-11.25 l 0,-6.8125 z"
+ inkscape:href="#path22091"
+ id="path3758"
+ style="fill:#eeeeec;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ xlink:href="#path22091"
+ inkscape:original="M 649.75 891.0625 C 641.40779 893.35756 635.28126 900.99292 635.28125 910.0625 C 635.28127 920.94545 644.11705 929.78124 655 929.78125 C 665.88295 929.78122 674.71874 920.94545 674.71875 910.0625 C 674.71873 900.98968 668.56549 893.35456 660.21875 891.0625 L 660.21875 899.15625 C 664.28467 901.11041 667.125 905.25095 667.125 910.0625 C 667.12501 916.75034 661.68785 922.1875 655 922.1875 C 648.31215 922.18751 642.875 916.75035 642.875 910.0625 C 642.87498 905.2 [...]
+ inkscape:radius="-0.66131908"
+ sodipodi:type="inkscape:offset" />
+ <path
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)"
+ d="m 651.78125,887.90625 0,22.625 6.4375,0 0,-22.625 -6.4375,0 z"
+ inkscape:href="#path22093"
+ id="path3760"
+ style="fill:#eeeeec;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ xlink:href="#path22093"
+ inkscape:original="M 651.1875 887.3125 L 651.1875 911.125 L 658.8125 911.125 L 658.8125 887.3125 L 651.1875 887.3125 z "
+ inkscape:radius="-0.60243881"
+ sodipodi:type="inkscape:offset" />
+ </g>
+ <path
+ transform="matrix(0.7015559,0,0,0.7362531,-28.921634,17.30468)"
+ d="m 149.62659,34.778526 c 0,8.993293 -10.04164,16.283797 -22.42863,16.283797 -12.38699,0 -22.42863,-7.290504 -22.42863,-16.283797 0,-8.993292 10.04164,-16.283796 22.42863,-16.283796 12.38699,0 22.42863,7.290504 22.42863,16.283796 z"
+ sodipodi:ry="16.283796"
+ sodipodi:rx="22.428625"
+ sodipodi:cy="34.778526"
+ sodipodi:cx="127.19796"
+ id="path3678"
+ style="opacity:0.75;fill:url(#linearGradient2243);fill-opacity:1;fill-rule:evenodd;stroke:none"
+ sodipodi:type="arc" />
+ </g>
+</svg>
diff --git a/rqt_joint_trajectory_controller/resource/on.svg b/rqt_joint_trajectory_controller/resource/on.svg
new file mode 100644
index 0000000..233d356
--- /dev/null
+++ b/rqt_joint_trajectory_controller/resource/on.svg
@@ -0,0 +1,231 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="54.122021"
+ height="54.122028"
+ id="svg2633"
+ sodipodi:version="0.32"
+ inkscape:version="0.48.3.1 r9886"
+ version="1.0"
+ sodipodi:docname="on.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ sodipodi:modified="true">
+ <defs
+ id="defs2635">
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3762"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3764"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3766"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ id="linearGradient3257"
+ inkscape:collect="always">
+ <stop
+ id="stop3259"
+ offset="0"
+ style="stop-color:#ffffff;stop-opacity:1" />
+ <stop
+ id="stop3261"
+ offset="1"
+ style="stop-color:#ffffff;stop-opacity:0" />
+ </linearGradient>
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient3768"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3257"
+ id="linearGradient2243"
+ gradientUnits="userSpaceOnUse"
+ x1="127.19796"
+ y1="18.49473"
+ x2="127.19796"
+ y2="47.546875" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="4"
+ inkscape:cx="31.011235"
+ inkscape:cy="24.729413"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ width="82px"
+ height="60px"
+ inkscape:window-width="804"
+ inkscape:window-height="594"
+ inkscape:window-x="250"
+ inkscape:window-y="223"
+ showgrid="false"
+ fit-margin-top="5"
+ fit-margin-left="5"
+ fit-margin-right="5"
+ fit-margin-bottom="5"
+ inkscape:window-maximized="0" />
+ <metadata
+ id="metadata2638">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title>green power button</dc:title>
+ <dc:date>08 12 2006</dc:date>
+ <dc:creator>
+ <cc:Agent>
+ <dc:title>molumen</dc:title>
+ </cc:Agent>
+ </dc:creator>
+ <dc:description>green power button</dc:description>
+ <cc:license
+ rdf:resource="http://web.resource.org/cc/PublicDomain" />
+ <dc:subject>
+ <rdf:Bag>
+ <rdf:li>icon</rdf:li>
+ <rdf:li>button</rdf:li>
+ <rdf:li>design</rdf:li>
+ <rdf:li>UI</rdf:li>
+ <rdf:li>interface</rdf:li>
+ <rdf:li>power</rdf:li>
+ <rdf:li>switch</rdf:li>
+ <rdf:li>on</rdf:li>
+ <rdf:li>off</rdf:li>
+ <rdf:li>green</rdf:li>
+ <rdf:li>glossy</rdf:li>
+ <rdf:li>toggle</rdf:li>
+ </rdf:Bag>
+ </dc:subject>
+ </cc:Work>
+ <cc:License
+ rdf:about="http://web.resource.org/cc/PublicDomain">
+ <cc:permits
+ rdf:resource="http://web.resource.org/cc/Reproduction" />
+ <cc:permits
+ rdf:resource="http://web.resource.org/cc/Distribution" />
+ <cc:permits
+ rdf:resource="http://web.resource.org/cc/DerivativeWorks" />
+ </cc:License>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1"
+ transform="translate(-33.253876,-23.858592)">
+ <path
+ sodipodi:type="arc"
+ style="fill:#73d216;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ id="path3676"
+ sodipodi:cx="655.00006"
+ sodipodi:cy="908.54846"
+ sodipodi:rx="35.10146"
+ sodipodi:ry="35.10146"
+ d="m 690.10152,908.54846 c 0,19.386 -15.71546,35.10146 -35.10146,35.10146 -19.386,0 -35.10146,-15.71546 -35.10146,-35.10146 0,-19.386 15.71546,-35.10146 35.10146,-35.10146 19.386,0 35.10146,15.71546 35.10146,35.10146 z"
+ transform="matrix(0.6284927,0,0,0.6284928,-351.34787,-520.09656)" />
+ <g
+ id="g3750"
+ transform="matrix(0.5824089,0,0,0.6244504,-298.35727,-439.02821)"
+ style="fill:#a40000;stroke:none">
+ <path
+ sodipodi:type="inkscape:offset"
+ inkscape:radius="-0.66131908"
+ inkscape:original="M 649.75 891.0625 C 641.40779 893.35756 635.28126 900.99292 635.28125 910.0625 C 635.28127 920.94545 644.11705 929.78124 655 929.78125 C 665.88295 929.78122 674.71874 920.94545 674.71875 910.0625 C 674.71873 900.98968 668.56549 893.35456 660.21875 891.0625 L 660.21875 899.15625 C 664.28467 901.11041 667.125 905.25095 667.125 910.0625 C 667.12501 916.75034 661.68785 922.1875 655 922.1875 C 648.31215 922.18751 642.875 916.75035 642.875 910.0625 C 642.87498 905.2 [...]
+ xlink:href="#path22091"
+ style="fill:#4e9a06;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ id="path3752"
+ inkscape:href="#path22091"
+ d="m 649.09375,892 c -7.61853,2.48999 -13.15624,9.60548 -13.15625,18.0625 2e-5,10.525 8.5375,19.06249 19.0625,19.0625 10.525,-3e-5 19.06249,-8.5375 19.0625,-19.0625 -2e-5,-8.45792 -5.56444,-15.57409 -13.1875,-18.0625 l 0,6.8125 c 4.06728,2.13572 6.90625,6.34059 6.90625,11.25 10e-6,7.04627 -5.73497,12.78125 -12.78125,12.78125 -7.04628,10e-6 -12.78125,-5.73497 -12.78125,-12.78125 -2e-5,-4.90659 2.80869,-9.1136 6.875,-11.25 l 0,-6.8125 z"
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)" />
+ <path
+ sodipodi:type="inkscape:offset"
+ inkscape:radius="-0.60243881"
+ inkscape:original="M 651.1875 887.3125 L 651.1875 911.125 L 658.8125 911.125 L 658.8125 887.3125 L 651.1875 887.3125 z "
+ xlink:href="#path22093"
+ style="fill:#4e9a06;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ id="path3754"
+ inkscape:href="#path22093"
+ d="m 651.78125,887.90625 0,22.625 6.4375,0 0,-22.625 -6.4375,0 z"
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)" />
+ </g>
+ <g
+ style="fill:#eeeeec;stroke:none"
+ transform="matrix(0.5824089,0,0,0.5824089,-298.35727,-406.81819)"
+ id="g3756">
+ <path
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)"
+ d="m 649.09375,892 c -7.61853,2.48999 -13.15624,9.60548 -13.15625,18.0625 2e-5,10.525 8.5375,19.06249 19.0625,19.0625 10.525,-3e-5 19.06249,-8.5375 19.0625,-19.0625 -2e-5,-8.45792 -5.56444,-15.57409 -13.1875,-18.0625 l 0,6.8125 c 4.06728,2.13572 6.90625,6.34059 6.90625,11.25 10e-6,7.04627 -5.73497,12.78125 -12.78125,12.78125 -7.04628,10e-6 -12.78125,-5.73497 -12.78125,-12.78125 -2e-5,-4.90659 2.80869,-9.1136 6.875,-11.25 l 0,-6.8125 z"
+ inkscape:href="#path22091"
+ id="path3758"
+ style="fill:#eeeeec;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ xlink:href="#path22091"
+ inkscape:original="M 649.75 891.0625 C 641.40779 893.35756 635.28126 900.99292 635.28125 910.0625 C 635.28127 920.94545 644.11705 929.78124 655 929.78125 C 665.88295 929.78122 674.71874 920.94545 674.71875 910.0625 C 674.71873 900.98968 668.56549 893.35456 660.21875 891.0625 L 660.21875 899.15625 C 664.28467 901.11041 667.125 905.25095 667.125 910.0625 C 667.12501 916.75034 661.68785 922.1875 655 922.1875 C 648.31215 922.18751 642.875 916.75035 642.875 910.0625 C 642.87498 905.2 [...]
+ inkscape:radius="-0.66131908"
+ sodipodi:type="inkscape:offset" />
+ <path
+ transform="matrix(0.935483,0,0,0.935483,3.100892,-64.47165)"
+ d="m 651.78125,887.90625 0,22.625 6.4375,0 0,-22.625 -6.4375,0 z"
+ inkscape:href="#path22093"
+ id="path3760"
+ style="fill:#eeeeec;fill-opacity:1;fill-rule:evenodd;stroke:none"
+ xlink:href="#path22093"
+ inkscape:original="M 651.1875 887.3125 L 651.1875 911.125 L 658.8125 911.125 L 658.8125 887.3125 L 651.1875 887.3125 z "
+ inkscape:radius="-0.60243881"
+ sodipodi:type="inkscape:offset" />
+ </g>
+ <path
+ transform="matrix(0.7015559,0,0,0.7362531,-28.921634,17.30468)"
+ d="m 149.62659,34.778526 c 0,8.993293 -10.04164,16.283797 -22.42863,16.283797 -12.38699,0 -22.42863,-7.290504 -22.42863,-16.283797 0,-8.993292 10.04164,-16.283796 22.42863,-16.283796 12.38699,0 22.42863,7.290504 22.42863,16.283796 z"
+ sodipodi:ry="16.283796"
+ sodipodi:rx="22.428625"
+ sodipodi:cy="34.778526"
+ sodipodi:cx="127.19796"
+ id="path3678"
+ style="opacity:0.75;fill:url(#linearGradient2243);fill-opacity:1;fill-rule:evenodd;stroke:none"
+ sodipodi:type="arc" />
+ </g>
+</svg>
diff --git a/rqt_joint_trajectory_controller/resource/scope.png b/rqt_joint_trajectory_controller/resource/scope.png
new file mode 100644
index 0000000..24c089b
Binary files /dev/null and b/rqt_joint_trajectory_controller/resource/scope.png differ
diff --git a/rqt_joint_trajectory_controller/resource/scope.svg b/rqt_joint_trajectory_controller/resource/scope.svg
new file mode 100644
index 0000000..ee4d4da
--- /dev/null
+++ b/rqt_joint_trajectory_controller/resource/scope.svg
@@ -0,0 +1,269 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="48px"
+ height="48px"
+ id="svg4815"
+ version="1.1"
+ inkscape:version="0.48.3.1 r9886"
+ sodipodi:docname="scope.svg"
+ inkscape:export-filename="/home/adolfo/branches_svn/4.4_AUTOMATICA_ANACONDA/catkin_ws/src/anaconda_robot/rqt_joint_trajectory_controller/resource/scope.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4817">
+ <linearGradient
+ id="linearGradient5977">
+ <stop
+ style="stop-color:#7f7f7f;stop-opacity:1;"
+ offset="0"
+ id="stop5979" />
+ <stop
+ style="stop-color:#3c3c3c;stop-opacity:1;"
+ offset="1"
+ id="stop5981" />
+ </linearGradient>
+ <linearGradient
+ id="linearGradient5969">
+ <stop
+ style="stop-color:#006837;stop-opacity:1;"
+ offset="0"
+ id="stop5971" />
+ <stop
+ style="stop-color:#179f31;stop-opacity:1;"
+ offset="1"
+ id="stop5973" />
+ </linearGradient>
+ <filter
+ id="filter5678"
+ style="color-interpolation-filters:sRGB;"
+ inkscape:label="Drop Shadow">
+ <feFlood
+ id="feFlood5680"
+ flood-opacity="1"
+ flood-color="rgb(255,255,255)"
+ result="flood" />
+ <feComposite
+ id="feComposite5682"
+ in2="SourceGraphic"
+ in="flood"
+ operator="in"
+ result="composite1" />
+ <feGaussianBlur
+ id="feGaussianBlur5684"
+ in="composite"
+ stdDeviation="1.5"
+ result="blur" />
+ <feOffset
+ id="feOffset5686"
+ dx="0"
+ dy="0"
+ result="offset" />
+ <feComposite
+ id="feComposite5688"
+ in2="offset"
+ in="SourceGraphic"
+ operator="over"
+ result="composite2" />
+ </filter>
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5969"
+ id="linearGradient5975"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489"
+ gradientUnits="userSpaceOnUse" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5977"
+ id="linearGradient5983"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489"
+ gradientUnits="userSpaceOnUse" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5969"
+ id="linearGradient3977"
+ gradientUnits="userSpaceOnUse"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5977"
+ id="linearGradient3979"
+ gradientUnits="userSpaceOnUse"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5969"
+ id="linearGradient3994"
+ gradientUnits="userSpaceOnUse"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5977"
+ id="linearGradient3996"
+ gradientUnits="userSpaceOnUse"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5969"
+ id="linearGradient4013"
+ gradientUnits="userSpaceOnUse"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489" />
+ <linearGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient5977"
+ id="linearGradient4015"
+ gradientUnits="userSpaceOnUse"
+ x1="24.978645"
+ y1="1.5048902"
+ x2="24.55007"
+ y2="46.50489" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="9.8994949"
+ inkscape:cx="0.38356625"
+ inkscape:cy="29.522249"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:grid-bbox="true"
+ inkscape:document-units="px"
+ inkscape:window-width="1609"
+ inkscape:window-height="1030"
+ inkscape:window-x="69"
+ inkscape:window-y="-3"
+ inkscape:window-maximized="1" />
+ <metadata
+ id="metadata4820">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ id="layer1"
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer">
+ <g
+ id="g3998"
+ transform="matrix(0.89828164,0,0,0.89828164,2.2136621,2.3351326)">
+ <rect
+ rx="5.2105126"
+ y="0.83511418"
+ x="0.88029718"
+ height="46.48241"
+ width="46.482407"
+ id="rect4830"
+ style="fill:url(#linearGradient4013);fill-opacity:1;stroke:url(#linearGradient4015);stroke-width:2.5;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:5.4" />
+ <g
+ transform="translate(0.22212881,0.04712212)"
+ id="g5925">
+ <path
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="M 12.770311,42.210053 13.078452,5.8840568"
+ id="path5690"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cc" />
+ <rect
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:5.4"
+ id="rect5763"
+ width="36.241539"
+ height="36.332306"
+ x="5.7185159"
+ y="5.8777475"
+ rx="0" />
+ <path
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="M 19.880247,42.210053 20.188388,5.8483422"
+ id="path5690-8"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cc" />
+ <path
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="M 26.990184,42.210053 27.298325,5.8840566"
+ id="path5690-9"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cc" />
+ <path
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="M 34.10012,42.210053 34.408261,5.8483411"
+ id="path5690-91"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cc" />
+ <g
+ id="g5883"
+ transform="matrix(0,-1,1,0,6.9708005,73.186757)">
+ <path
+ sodipodi:nodetypes="cc"
+ inkscape:connector-curvature="0"
+ id="path5690-3"
+ d="M 38.323882,35.109427 38.632023,-1.2165689"
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <path
+ sodipodi:nodetypes="cc"
+ inkscape:connector-curvature="0"
+ id="path5690-8-4"
+ d="M 45.433818,35.109427 45.741959,-1.2522835"
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <path
+ sodipodi:nodetypes="cc"
+ inkscape:connector-curvature="0"
+ id="path5690-9-5"
+ d="M 52.543755,35.109427 52.851896,-1.2165691"
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ <path
+ sodipodi:nodetypes="cc"
+ inkscape:connector-curvature="0"
+ id="path5690-91-5"
+ d="M 59.653691,35.109427 59.961832,-1.2522846"
+ style="fill:none;stroke:#57ec52;stroke-width:0.25;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" />
+ </g>
+ </g>
+ <path
+ sodipodi:nodetypes="cszzzzsc"
+ inkscape:connector-curvature="0"
+ id="path4854"
+ d="m 2.105887,25.675629 6.177364,0 c 3.139418,0 2.566433,11.792558 5.656855,11.818785 3.090421,0.02623 4.504254,-27.8519726 8.889342,-27.8802111 4.385088,-0.028239 3.045578,12.0742221 6.262946,12.1218311 3.217368,0.04761 2.903628,-6.667007 5.252793,-6.667007 2.349165,0 2.948238,10.909648 6.262946,10.909648 l 5.49396,0"
+ style="fill:none;stroke:#95ea59;stroke-width:1.6698549;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;filter:url(#filter5678);stroke-miterlimit:4;stroke-dasharray:none" />
+ </g>
+ </g>
+</svg>
diff --git a/rqt_joint_trajectory_controller/rosdoc.yaml b/rqt_joint_trajectory_controller/rosdoc.yaml
new file mode 100644
index 0000000..ea15d49
--- /dev/null
+++ b/rqt_joint_trajectory_controller/rosdoc.yaml
@@ -0,0 +1,2 @@
+ - builder: epydoc
+ output_dir: python
\ No newline at end of file
diff --git a/rqt_joint_trajectory_controller/scripts/rqt_joint_trajectory_controller b/rqt_joint_trajectory_controller/scripts/rqt_joint_trajectory_controller
new file mode 100755
index 0000000..9645908
--- /dev/null
+++ b/rqt_joint_trajectory_controller/scripts/rqt_joint_trajectory_controller
@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+
+import sys
+
+from rqt_gui.main import Main
+
+main = Main()
+sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_controller'))
diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py
new file mode 100644
index 0000000..1894f64
--- /dev/null
+++ b/rqt_joint_trajectory_controller/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+ packages=['rqt_joint_trajectory_controller'],
+ package_dir={'': 'src'}
+)
+
+setup(**d)
\ No newline at end of file
diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/__init__.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/double_editor.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/double_editor.py
new file mode 100644
index 0000000..7f07f7f
--- /dev/null
+++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/double_editor.py
@@ -0,0 +1,121 @@
+#!/usr/bin/env python
+
+# 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 S.L. 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.
+
+import os
+import rospkg
+
+from python_qt_binding import loadUi
+from python_qt_binding.QtCore import Signal
+from python_qt_binding.QtGui import QWidget
+
+
+class DoubleEditor(QWidget):
+ # TODO:
+ # - Actually make bounds optional
+ #
+ # - Support wrapping mode
+ #
+ # - Support unspecified (+-Inf) lower and upper bounds (both, or one)
+ #
+ # - Allow to specify the step and page increment sizes
+ # (right-click context menu?)
+ #
+ # - Use alternative widget to slider for values that wrap, or are
+ # unbounded.
+ # QwtWheel could be a good choice, dials are not so good because they
+ # use lots of vertical (premium) screen space, and are fine for wrapping
+ # values, but not so much for unbounded ones
+ #
+ # - Merge with existing similar code such as rqt_reconfigure's
+ # DoubleEditor?
+ """
+ Widget that allows to edit the value of a floating-point value, optionally
+ subject to lower and upper bounds.
+ """
+ valueChanged = Signal(float)
+
+ def __init__(self, min_val, max_val):
+ super(DoubleEditor, self).__init__()
+
+ # Preconditions
+ assert min_val < max_val
+
+ # Cache values
+ self._min_val = min_val
+ self._max_val = max_val
+
+ # Load editor UI
+ rp = rospkg.RosPack()
+ ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
+ 'resource', 'double_editor.ui')
+ loadUi(ui_file, self)
+
+ # Setup widget ranges and slider scale factor
+ self.slider.setRange(0, 100)
+ self.slider.setSingleStep(1)
+ self._scale = (max_val - min_val) / \
+ (self.slider.maximum() - self.slider.minimum())
+ self.spin_box.setRange(min_val, max_val)
+ self.spin_box.setSingleStep(self._scale)
+
+ # Couple slider and spin box together
+ self.slider.valueChanged.connect(self._on_slider_changed)
+ self.spin_box.valueChanged.connect(self._on_spinbox_changed)
+
+ # Ensure initial sync of slider and spin box
+ self._on_spinbox_changed()
+
+ def _slider_to_val(self, sval):
+ return self._min_val + self._scale * (sval - self.slider.minimum())
+
+ def _val_to_slider(self, val):
+ return round(self.slider.minimum() + (val - self._min_val) /
+ self._scale)
+
+ def _on_slider_changed(self):
+ val = self._slider_to_val(self.slider.value())
+ self.spin_box.blockSignals(True) # Prevents updating the command twice
+ self.spin_box.setValue(val)
+ self.spin_box.blockSignals(False)
+ self.valueChanged.emit(val)
+
+ def _on_spinbox_changed(self):
+ val = self.spin_box.value()
+ self.slider.blockSignals(True) # Prevents updating the command twice
+ self.slider.setValue(self._val_to_slider(val))
+ self.slider.blockSignals(False)
+ self.valueChanged.emit(val)
+
+ def setValue(self, val):
+ if val != self.spin_box.value():
+ self.spin_box.blockSignals(True)
+ self.spin_box.setValue(val) # Update spin box first
+ self._on_spinbox_changed() # Sync slider with spin box
+ self.spin_box.blockSignals(False)
+
+ def value(self):
+ return self.spin_box.value()
diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py
new file mode 100644
index 0000000..7cdd52c
--- /dev/null
+++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+
+# TODO: Use urdf_parser_py.urdf instead. I gave it a try, but got
+# Exception: Required attribute not set in XML: upper
+# upper is an optional attribute, so I don't understand what's going on
+# See comments in https://github.com/ros/urdfdom/issues/36
+
+import xml.dom.minidom
+from math import pi
+
+import rospy
+
+
+def get_joint_limits(key='robot_description', use_smallest_joint_limits=True):
+ use_small = use_smallest_joint_limits
+ use_mimic = True
+
+ # Code inspired on the joint_state_publisher package by David Lu!!!
+ # https://github.com/ros/robot_model/blob/indigo-devel/
+ # joint_state_publisher/joint_state_publisher/joint_state_publisher
+ description = rospy.get_param(key)
+ robot = xml.dom.minidom.parseString(description)\
+ .getElementsByTagName('robot')[0]
+ free_joints = {}
+ dependent_joints = {}
+
+ # Find all non-fixed joints
+ for child in robot.childNodes:
+ if child.nodeType is child.TEXT_NODE:
+ continue
+ if child.localName == 'joint':
+ jtype = child.getAttribute('type')
+ if jtype == 'fixed':
+ continue
+ name = child.getAttribute('name')
+ try:
+ limit = child.getElementsByTagName('limit')[0]
+ except:
+ continue
+ if jtype == 'continuous':
+ minval = -pi
+ maxval = pi
+ else:
+ try:
+ minval = float(limit.getAttribute('lower'))
+ maxval = float(limit.getAttribute('upper'))
+ except:
+ continue
+ try:
+ maxvel = float(limit.getAttribute('velocity'))
+ except:
+ continue
+ safety_tags = child.getElementsByTagName('safety_controller')
+ if use_small and len(safety_tags) == 1:
+ tag = safety_tags[0]
+ if tag.hasAttribute('soft_lower_limit'):
+ minval = max(minval,
+ float(tag.getAttribute('soft_lower_limit')))
+ if tag.hasAttribute('soft_upper_limit'):
+ maxval = min(maxval,
+ float(tag.getAttribute('soft_upper_limit')))
+
+ mimic_tags = child.getElementsByTagName('mimic')
+ if use_mimic and len(mimic_tags) == 1:
+ tag = mimic_tags[0]
+ entry = {'parent': tag.getAttribute('joint')}
+ if tag.hasAttribute('multiplier'):
+ entry['factor'] = float(tag.getAttribute('multiplier'))
+ if tag.hasAttribute('offset'):
+ entry['offset'] = float(tag.getAttribute('offset'))
+
+ dependent_joints[name] = entry
+ continue
+
+ if name in dependent_joints:
+ continue
+
+ joint = {'min_position': minval, 'max_position': maxval}
+ joint["has_position_limits"] = jtype != 'continuous'
+ joint['max_velocity'] = maxvel
+ free_joints[name] = joint
+ return free_joints
diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py
new file mode 100644
index 0000000..42d4678
--- /dev/null
+++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py
@@ -0,0 +1,482 @@
+#!/usr/bin/env python
+
+# 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 S.L. 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.
+
+import os
+import rospy
+import rospkg
+
+from qt_gui.plugin import Plugin
+from python_qt_binding import loadUi
+from python_qt_binding.QtCore import QTimer, Signal
+from python_qt_binding.QtGui import QWidget, QFormLayout
+
+from control_msgs.msg import JointTrajectoryControllerState
+from controller_manager_msgs.utils\
+ import ControllerLister, ControllerManagerLister
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+from double_editor import DoubleEditor
+from joint_limits_urdf import get_joint_limits
+from update_combo import update_combo
+
+# TODO:
+# - Better UI suppor for continuous joints (see DoubleEditor TODO)
+# - Can we load controller joints faster?, it's currently pretty slow
+# - If URDF is reloaded, allow to reset the whole plugin?
+# - Allow to configure:
+# - URDF location
+# - Command publishing and state update frequency
+# - Controller manager and jtc monitor frequency
+# - Min trajectory duration
+# - Fail gracefully when the URDF or some other requisite is not set
+# - Could users confuse the enable/disable button with controller start/stop
+# (in a controller manager sense)?
+# - Better decoupling between model and view
+# - Tab order is wrong. For the record, this did not work:
+# QWidget.setTabOrder(self._widget.controller_group,
+# self._widget.joint_group)
+# QWidget.setTabOrder(self._widget.joint_group,
+# self._widget.speed_scaling_group)
+
+# NOTE:
+# Controller enable/disable icons are in the public domain, and available here:
+# freestockphotos.biz/photos.php?c=all&o=popular&s=0&lic=all&a=all&set=powocab_u2
+
+
+class JointTrajectoryController(Plugin):
+ """
+ Graphical frontend for a C{JointTrajectoryController}.
+
+ There are two modes for interacting with a controller:
+ 1. B{Monitor mode} Joint displays are updated with the state reported
+ by the controller. This is a read-only mode and does I{not} send
+ control commands. Every time a new controller is selected, it starts
+ in monitor mode for safety reasons.
+
+ 2. B{Control mode} Joint displays update the control command that is
+ sent to the controller. Commands are sent periodically evan if the
+ displays are not being updated by the user.
+
+ To control the aggressiveness of the motions, the maximum speed of the
+ sent commands can be scaled down using the C{Speed scaling control}
+
+ This plugin is able to detect and keep track of all active controller
+ managers, as well as the JointTrajectoryControllers that are I{running}
+ in each controller manager.
+ For a controller to be compatible with this plugin, it must comply with
+ the following requisites:
+ - The controller type contains the C{JointTrajectoryController}
+ substring, e.g., C{position_controllers/JointTrajectoryController}
+ - The controller exposes the C{command} and C{state} topics in its
+ ROS interface.
+
+ Additionally, there must be a URDF loaded with a valid joint limit
+ specification, namely position (when applicable) and velocity limits.
+
+ A reference implementation of the C{JointTrajectoryController} is
+ available in the C{joint_trajectory_controller} ROS package.
+ """
+ _cmd_pub_freq = 10.0 # Hz
+ _widget_update_freq = 30.0 # Hz
+ _ctrlrs_update_freq = 1 # Hz
+ _min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
+
+ jointStateChanged = Signal([dict])
+
+ def __init__(self, context):
+ super(JointTrajectoryController, self).__init__(context)
+ self.setObjectName('JointTrajectoryController')
+
+ # Create QWidget and extend it with all the attributes and children
+ # from the UI file
+ self._widget = QWidget()
+ rp = rospkg.RosPack()
+ ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
+ 'resource',
+ 'joint_trajectory_controller.ui')
+ loadUi(ui_file, self._widget)
+ self._widget.setObjectName('JointTrajectoryControllerUi')
+
+ # Setup speed scaler
+ speed_scaling = DoubleEditor(1.0, 100.0)
+ speed_scaling.spin_box.setSuffix('%')
+ speed_scaling.spin_box.setValue(50.0)
+ speed_scaling.spin_box.setDecimals(0)
+ speed_scaling.setEnabled(False)
+ self._widget.speed_scaling_layout.addWidget(speed_scaling)
+ self._speed_scaling_widget = speed_scaling
+ speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
+ self._on_speed_scaling_change(speed_scaling.value())
+
+ # Show _widget.windowTitle on left-top of each plugin (when
+ # it's set in _widget). This is useful when you open multiple
+ # plugins at once. Also if you open multiple instances of your
+ # plugin at once, these lines add number to make it easy to
+ # tell from pane to pane.
+ if context.serial_number() > 1:
+ self._widget.setWindowTitle(self._widget.windowTitle() +
+ (' (%d)' % context.serial_number()))
+ # Add widget to the user interface
+ context.add_widget(self._widget)
+
+ # Initialize members
+ self._jtc_name = [] # Name of selected joint trajectory controller
+ self._cm_ns = [] # Namespace of the selected controller manager
+ self._joint_pos = {} # name->pos map for joints of selected controller
+ self._joint_names = [] # Ordered list of selected controller joints
+ self._robot_joint_limits = {} # Lazily evaluated on first use
+
+ # Timer for sending commands to active controller
+ self._update_cmd_timer = QTimer(self)
+ self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
+ self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
+
+ # Timer for updating the joint widgets from the controller state
+ self._update_act_pos_timer = QTimer(self)
+ self._update_act_pos_timer.setInterval(1000.0 /
+ self._widget_update_freq)
+ self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
+
+ # Timer for controller manager updates
+ self._list_cm = ControllerManagerLister()
+ self._update_cm_list_timer = QTimer(self)
+ self._update_cm_list_timer.setInterval(1000.0 /
+ self._ctrlrs_update_freq)
+ self._update_cm_list_timer.timeout.connect(self._update_cm_list)
+ self._update_cm_list_timer.start()
+
+ # Timer for running controller updates
+ self._update_jtc_list_timer = QTimer(self)
+ self._update_jtc_list_timer.setInterval(1000.0 /
+ self._ctrlrs_update_freq)
+ self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
+ self._update_jtc_list_timer.start()
+
+ # Signal connections
+ w = self._widget
+ w.enable_button.toggled.connect(self._on_jtc_enabled)
+ w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
+ w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
+
+ self._cmd_pub = None # Controller command publisher
+ self._state_sub = None # Controller state subscriber
+
+ self._list_controllers = None
+
+ def shutdown_plugin(self):
+ self._update_cmd_timer.stop()
+ self._update_act_pos_timer.stop()
+ self._update_cm_list_timer.stop()
+ self._update_jtc_list_timer.stop()
+ self._unregister_state_sub()
+ self._unregister_cmd_pub()
+
+ def save_settings(self, plugin_settings, instance_settings):
+ instance_settings.set_value('cm_ns', self._cm_ns)
+ instance_settings.set_value('jtc_name', self._jtc_name)
+
+ def restore_settings(self, plugin_settings, instance_settings):
+ # Restore last session's controller_manager, if present
+ self._update_cm_list()
+ cm_ns = instance_settings.value('cm_ns')
+ cm_combo = self._widget.cm_combo
+ cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
+ try:
+ idx = cm_list.index(cm_ns)
+ cm_combo.setCurrentIndex(idx)
+ # Resore last session's controller, if running
+ self._update_jtc_list()
+ jtc_name = instance_settings.value('jtc_name')
+ jtc_combo = self._widget.jtc_combo
+ jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
+ try:
+ idx = jtc_list.index(jtc_name)
+ jtc_combo.setCurrentIndex(idx)
+ except (ValueError):
+ pass
+ except (ValueError):
+ pass
+
+ # def trigger_configuration(self):
+ # Comment in to signal that the plugin has a way to configure
+ # This will enable a setting button (gear icon) in each dock widget
+ # title bar
+ # Usually used to open a modal configuration dialog
+
+ def _update_cm_list(self):
+ update_combo(self._widget.cm_combo, self._list_cm())
+
+ def _update_jtc_list(self):
+ # Clear controller list if no controller information is available
+ if not self._list_controllers:
+ self._widget.jtc_combo.clear()
+ return
+
+ # List of running controllers with a valid joint limits specification
+ # for _all_ their joints
+ running_jtc = self._running_jtc_info()
+ if running_jtc and not self._robot_joint_limits:
+ self._robot_joint_limits = get_joint_limits() # Lazy evaluation
+ valid_jtc = []
+ for jtc_info in running_jtc:
+ has_limits = all(name in self._robot_joint_limits
+ for name in _jtc_joint_names(jtc_info))
+ if has_limits:
+ valid_jtc.append(jtc_info);
+ valid_jtc_names = [data.name for data in valid_jtc]
+
+ # Update widget
+ update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
+
+ def _on_speed_scaling_change(self, val):
+ self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
+
+ def _on_joint_state_change(self, actual_pos):
+ assert(len(actual_pos) == len(self._joint_pos))
+ for name in actual_pos.keys():
+ self._joint_pos[name]['position'] = actual_pos[name]
+
+ def _on_cm_change(self, cm_ns):
+ self._cm_ns = cm_ns
+ if cm_ns:
+ self._list_controllers = ControllerLister(cm_ns)
+ # NOTE: Clear below is important, as different controller managers
+ # might have controllers with the same name but different
+ # configurations. Clearing forces controller re-discovery
+ self._widget.jtc_combo.clear()
+ self._update_jtc_list()
+ else:
+ self._list_controllers = None
+
+ def _on_jtc_change(self, jtc_name):
+ self._unload_jtc()
+ self._jtc_name = jtc_name
+ if self._jtc_name:
+ self._load_jtc()
+
+ def _on_jtc_enabled(self, val):
+ # Don't allow enabling if there are no controllers selected
+ if not self._jtc_name:
+ self._widget.enable_button.setChecked(False)
+ return
+
+ # Enable/disable joint displays
+ for joint_widget in self._joint_widgets():
+ joint_widget.setEnabled(val)
+
+ # Enable/disable speed scaling
+ self._speed_scaling_widget.setEnabled(val)
+
+ if val:
+ # Widgets send desired position commands to controller
+ self._update_act_pos_timer.stop()
+ self._update_cmd_timer.start()
+ else:
+ # Controller updates widgets with actual position
+ self._update_cmd_timer.stop()
+ self._update_act_pos_timer.start()
+
+ def _load_jtc(self):
+ # Initialize joint data corresponding to selected controller
+ running_jtc = self._running_jtc_info()
+ self._joint_names = next(_jtc_joint_names(x) for x in running_jtc
+ if x.name == self._jtc_name)
+ for name in self._joint_names:
+ self._joint_pos[name] = {}
+
+ # Update joint display
+ try:
+ layout = self._widget.joint_group.layout()
+ for name in self._joint_names:
+ limits = self._robot_joint_limits[name]
+ joint_widget = DoubleEditor(limits['min_position'],
+ limits['max_position'])
+ layout.addRow(name, joint_widget)
+ # NOTE: Using partial instead of a lambda because lambdas
+ # "will not evaluate/look up the argument values before it is
+ # effectively called, breaking situations like using a loop
+ # variable inside it"
+ from functools import partial
+ par = partial(self._update_single_cmd_cb, name=name)
+ joint_widget.valueChanged.connect(par)
+ except:
+ # TODO: Can we do better than swallow the exception?
+ from sys import exc_info
+ print 'Unexpected error:', exc_info()[0]
+
+ # Enter monitor mode (sending commands disabled)
+ self._on_jtc_enabled(False)
+
+ # Setup ROS interfaces
+ jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
+ state_topic = jtc_ns + '/state'
+ cmd_topic = jtc_ns + '/command'
+ self._state_sub = rospy.Subscriber(state_topic,
+ JointTrajectoryControllerState,
+ self._state_cb,
+ queue_size=1)
+ self._cmd_pub = rospy.Publisher(cmd_topic,
+ JointTrajectory,
+ queue_size=1)
+
+ # Start updating the joint positions
+ self.jointStateChanged.connect(self._on_joint_state_change)
+
+ def _unload_jtc(self):
+ # Stop updating the joint positions
+ try:
+ self.jointStateChanged.disconnect(self._on_joint_state_change)
+ except:
+ pass
+
+ # Reset ROS interfaces
+ self._unregister_state_sub()
+ self._unregister_cmd_pub()
+
+ # Clear joint widgets
+ # NOTE: Implementation is a workaround for:
+ # https://bugreports.qt-project.org/browse/QTBUG-15990 :(
+ layout = self._widget.joint_group.layout()
+ if layout is not None:
+ while layout.count():
+ layout.takeAt(0).widget().deleteLater()
+ # Delete existing layout by reparenting to temporary
+ QWidget().setLayout(layout)
+ self._widget.joint_group.setLayout(QFormLayout())
+
+ # Reset joint data
+ self._joint_names = []
+ self._joint_pos = {}
+
+ # Enforce monitor mode (sending commands disabled)
+ self._widget.enable_button.setChecked(False)
+
+ def _running_jtc_info(self):
+ from controller_manager_msgs.utils\
+ import filter_by_type, filter_by_state
+
+ controller_list = self._list_controllers()
+ jtc_list = filter_by_type(controller_list,
+ 'JointTrajectoryController',
+ match_substring=True)
+ running_jtc_list = filter_by_state(jtc_list, 'running')
+ return running_jtc_list
+
+ def _unregister_cmd_pub(self):
+ if self._cmd_pub is not None:
+ self._cmd_pub.unregister()
+ self._cmd_pub = None
+
+ def _unregister_state_sub(self):
+ if self._state_sub is not None:
+ self._state_sub.unregister()
+ self._state_sub = None
+
+ def _state_cb(self, msg):
+ actual_pos = {}
+ for i in range(len(msg.joint_names)):
+ joint_name = msg.joint_names[i]
+ joint_pos = msg.actual.positions[i]
+ actual_pos[joint_name] = joint_pos
+ self.jointStateChanged.emit(actual_pos)
+
+ def _update_single_cmd_cb(self, val, name):
+ self._joint_pos[name]['command'] = val
+
+ def _update_cmd_cb(self):
+ dur = []
+ traj = JointTrajectory()
+ traj.joint_names = self._joint_names
+ point = JointTrajectoryPoint()
+ for name in traj.joint_names:
+ pos = self._joint_pos[name]['position']
+ cmd = pos
+ try:
+ cmd = self._joint_pos[name]['command']
+ except (KeyError):
+ pass
+ max_vel = self._robot_joint_limits[name]['max_velocity']
+ dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
+ point.positions.append(cmd)
+ point.time_from_start = rospy.Duration(max(dur) / self._speed_scale)
+ traj.points.append(point)
+
+ self._cmd_pub.publish(traj)
+
+ def _update_joint_widgets(self):
+ joint_widgets = self._joint_widgets()
+ for id in range(len(joint_widgets)):
+ joint_name = self._joint_names[id]
+ try:
+ joint_pos = self._joint_pos[joint_name]['position']
+ joint_widgets[id].setValue(joint_pos)
+ except (KeyError):
+ pass # Can happen when first connected to controller
+
+ def _joint_widgets(self): # TODO: Cache instead of compute every time?
+ widgets = []
+ layout = self._widget.joint_group.layout()
+ for row_id in range(layout.rowCount()):
+ widgets.append(layout.itemAt(row_id,
+ QFormLayout.FieldRole).widget())
+ return widgets
+
+def _jtc_joint_names(jtc_info):
+ # NOTE: We assume that there is at least one hardware interface that
+ # claims resources (there should be), and the resource list is fetched
+ # from the first available interface
+ return jtc_info.claimed_resources[0].resources
+
+def _resolve_controller_ns(cm_ns, controller_name):
+ """
+ Resolve a controller's namespace from that of the controller manager.
+ Controllers are assumed to live one level above the controller
+ manager, e.g.
+
+ >>> _resolve_controller_ns('/path/to/controller_manager', 'foo')
+ '/path/to/foo'
+
+ In the particular case in which the controller manager is not
+ namespaced, the controller is assumed to live in the root namespace
+
+ >>> _resolve_controller_ns('/', 'foo')
+ '/foo'
+ >>> _resolve_controller_ns('', 'foo')
+ '/foo'
+ @param cm_ns Controller manager namespace (can be an empty string)
+ @type cm_ns str
+ @param controller_name Controller name (non-empty string)
+ @type controller_name str
+ @return Controller namespace
+ @rtype str
+ """
+ assert(controller_name)
+ ns = cm_ns.rsplit('/', 1)[0]
+ if ns != '/':
+ ns += '/'
+ ns += controller_name
+ return ns
diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/update_combo.py
new file mode 100644
index 0000000..b06e38e
--- /dev/null
+++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/update_combo.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+
+# 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 S.L. 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.
+
+def update_combo(combo, new_vals):
+ """
+ Update the contents of a combo box with a set of new values.
+
+ If the previously selected element is still present in the new values, it
+ will remain as active selection, even if its index has changed. This will
+ not trigger any signal.
+
+ If the previously selected element is no longer present in the new values,
+ the combo will unset its selection. This will trigger signals for changed
+ element.
+ """
+ selected_val = combo.currentText()
+ old_vals = [combo.itemText(i) for i in range(combo.count())]
+
+ # Check if combo items changed
+ if not _is_permutation(old_vals, new_vals):
+ # Determine if selected value is in the new list
+ selected_id = -1
+ try:
+ selected_id = new_vals.index(selected_val)
+ except (ValueError):
+ combo.setCurrentIndex(-1)
+
+ # Re-populate items
+ combo.blockSignals(True) # No need to notify these changes
+ combo.clear()
+ combo.insertItems(0, new_vals)
+ combo.setCurrentIndex(selected_id) # Restore selection
+ combo.blockSignals(False)
+
+def _is_permutation(a, b):
+ """
+ @type a []
+ @type b []
+ @return True if C{a} is a permutation of C{b}, false otherwise
+ @rtype bool
+ """
+ return len(a) == len(b) and sorted(a) == sorted(b)
diff --git a/velocity_controllers/.gitignore b/velocity_controllers/.gitignore
new file mode 100644
index 0000000..61fd441
--- /dev/null
+++ b/velocity_controllers/.gitignore
@@ -0,0 +1,2 @@
+build
+lib
\ No newline at end of file
diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst
new file mode 100644
index 0000000..e9edbc2
--- /dev/null
+++ b/velocity_controllers/CHANGELOG.rst
@@ -0,0 +1,86 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package velocity_controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.10.0 (2015-11-20)
+-------------------
+
+0.9.2 (2015-05-04)
+------------------
+* Thread-safe and realtime-safe forward controllers.
+* Contributors: Mathias Lüdtke
+
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Add missing dependencies
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* New controller: velocity_controllers/JointGroupVelocityController (multi-joint)
+* New controller: velocity_controllers/JointPositionController
+* Contributors: ipa-fxm, Dave Coleman
+
+0.8.1 (2014-07-11)
+------------------
+
+0.8.0 (2014-05-12)
+------------------
+* Remove rosbuild artifacts. Fix `#90 <https://github.com/ros-controls/ros_controllers/issues/90>`_.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.7.2 (2014-04-01)
+------------------
+
+0.7.1 (2014-03-31)
+------------------
+
+0.7.0 (2014-03-28)
+------------------
+
+0.6.0 (2014-02-05)
+------------------
+* Link shared libraries to catkin libraries
+ GCC is quite lenient with missing symbols on shared libraries and
+ doesn't event output any warning about it.
+ When building with other compilers, missing symbols result in build
+ errors.
+* Contributors: Paul Mathieu
+
+0.5.4 (2013-09-30)
+------------------
+
+0.5.3 (2013-09-04)
+------------------
+* Removed manifest.xml from all packages to prevent rosdep heirarchy issues in Groovy and Hydro
+* Added ignored manifest.xml files, added rule to .gitignore
+
+0.5.2 (2013-08-06)
+------------------
+
+0.5.1 (2013-07-19)
+------------------
+
+0.5.0 (2013-07-16)
+------------------
+* Merged
+* Add meta tags to packages not specifying them.
+ - Website, bugtracker, repository.
+* Restore "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors""
+ This reverts commit 0862ad93696b0d736b565cd65ea36690dde0eaa7.
+* Adding install targets for plugin xml files
+* Revert "Fixed PLUGINLIB_DECLARE_CLASS depreacated errors"
+ This reverts commit 2314b8b434e35dc9c1c298140118a004e00febd8.
+
+0.4.0 (2013-06-26)
+------------------
+* Version 0.4.0
+* Fixed PLUGINLIB_DECLARE_CLASS depreacated errors
+* adding install targets
+* adding switches for hybrid buildsystem
+* adding these packages which weren't seen by catkinize_stack
+* Extend joint_effort_controller to other interfaces
+ - Factor-out implementation of simple command-forwarding controller.
+ - Provide specializations (typedefs really) for effort, velocity and position
+ interfaces.
diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt
new file mode 100644
index 0000000..b82caf1
--- /dev/null
+++ b/velocity_controllers/CMakeLists.txt
@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(velocity_controllers)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS
+ angles
+ control_msgs
+ control_toolbox
+ controller_interface
+ forward_command_controller
+ realtime_tools
+ urdf
+)
+
+# Declare catkin package
+catkin_package(
+ CATKIN_DEPENDS
+ angles
+ control_msgs
+ control_toolbox
+ controller_interface
+ forward_command_controller
+ realtime_tools
+ urdf
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+ src/joint_velocity_controller.cpp
+ src/joint_position_controller.cpp
+ src/joint_group_velocity_controller.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES velocity_controllers_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h
new file mode 100644
index 0000000..6b6167e
--- /dev/null
+++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.h
@@ -0,0 +1,64 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef VELOCITY_CONTROLLERS_JOINT_GROUP_VELOCITY_CONTROLLER_H
+#define VELOCITY_CONTROLLERS_JOINT_GROUP_VELOCITY_CONTROLLER_H
+
+#include <forward_command_controller/forward_joint_group_command_controller.h>
+
+namespace velocity_controllers
+{
+
+/**
+ * \brief Forward command controller for a set of velocity controlled joints (linear or angular).
+ *
+ * This class forwards the commanded velocities down to a set of joints.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointGroupVelocityController".
+ * \param joints List of names of the joints to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64MultiArray) : The joint velocities to apply
+ */
+typedef forward_command_controller::ForwardJointGroupCommandController<hardware_interface::VelocityJointInterface>
+ JointGroupVelocityController;
+
+}
+
+#endif
diff --git a/velocity_controllers/include/velocity_controllers/joint_position_controller.h b/velocity_controllers/include/velocity_controllers/joint_position_controller.h
new file mode 100644
index 0000000..9d10bb7
--- /dev/null
+++ b/velocity_controllers/include/velocity_controllers/joint_position_controller.h
@@ -0,0 +1,202 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, University of Colorado, Boulder
+ * 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 Willow Garage 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: Dave Coleman
+ Contributors: Jonathan Bohren, Wim Meeussen, Vijay Pradeep
+ Desc: Velocity-based position controller using basic PID loop
+*/
+
+#ifndef VELOCITY_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
+#define VELOCITY_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
+
+/**
+ @class velocity_controllers::JointPositionController
+ @brief Joint Position Controller
+
+ This class controls positon using a pid loop.
+
+ @section ROS ROS interface
+
+ @param type Must be "velocity_controllers::JointPositionController"
+ @param joint Name of the joint to control.
+ @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid
+
+ Subscribes to:
+
+ - @b command (std_msgs::Float64) : The joint position to achieve.
+
+ Publishes:
+
+ - @b state (control_msgs::JointControllerState) :
+ Current state of the controller, including pid error and gains.
+
+*/
+
+#include <ros/node_handle.h>
+#include <urdf/model.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_msgs/JointControllerState.h>
+#include <realtime_tools/realtime_buffer.h>
+
+namespace velocity_controllers
+{
+
+class JointPositionController: public controller_interface::Controller<hardware_interface::VelocityJointInterface>
+{
+public:
+
+ /**
+ * \brief Store position and velocity command in struct to allow easier realtime buffer usage
+ */
+ struct Commands
+ {
+ double position_; // Last commanded position
+ double velocity_; // Last commanded velocity
+ bool has_velocity_; // false if no velocity command has been specified
+ };
+
+ JointPositionController();
+ ~JointPositionController();
+
+ /** \brief The init function is called to initialize the controller from a
+ * non-realtime thread with a pointer to the hardware interface, itself,
+ * instead of a pointer to a RobotHW.
+ *
+ * \param robot The specific hardware interface used by this controller.
+ *
+ * \param n A NodeHandle in the namespace from which the controller
+ * should read its configuration, and where it should set up its ROS
+ * interface.
+ *
+ * \returns True if initialization was successful and the controller
+ * is ready to be started.
+ */
+ bool init(hardware_interface::VelocityJointInterface *robot, ros::NodeHandle &n);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ *
+ * \param command
+ */
+ void setCommand(double pos_target);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ * Also supports a target velocity
+ *
+ * \param pos_target - position setpoint
+ * \param vel_target - velocity setpoint
+ */
+ void setCommand(double pos_target, double vel_target);
+
+ /** \brief This is called from within the realtime thread just before the
+ * first call to \ref update
+ *
+ * \param time The current time
+ */
+ void starting(const ros::Time& time);
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+ void update(const ros::Time& time, const ros::Duration& period);
+
+ /**
+ * \brief Get the PID parameters
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+ /**
+ * \brief Print debug info to console
+ */
+ void printDebug();
+
+ /**
+ * \brief Get the PID parameters
+ */
+ void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
+
+ /**
+ * \brief Get the name of the joint this controller uses
+ */
+ std::string getJointName();
+
+ /**
+ * \brief Get the current position of the joint
+ * \return current position
+ */
+ double getPosition();
+
+ hardware_interface::JointHandle joint_;
+ boost::shared_ptr<const urdf::Joint> joint_urdf_;
+ realtime_tools::RealtimeBuffer<Commands> command_;
+ Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
+
+private:
+ int loop_count_;
+ control_toolbox::Pid pid_controller_; /**< Internal PID controller. */
+
+ boost::scoped_ptr<
+ realtime_tools::RealtimePublisher<
+ control_msgs::JointControllerState> > controller_state_publisher_ ;
+
+ ros::Subscriber sub_command_;
+
+ /**
+ * \brief Callback from /command subscriber for setpoint
+ */
+ void setCommandCB(const std_msgs::Float64ConstPtr& msg);
+
+ /**
+ * \brief Check that the command is within the hard limits of the joint. Checks for joint
+ * type first. Sets command to limit if out of bounds.
+ * \param command - the input to test
+ */
+ void enforceJointLimits(double &command);
+
+};
+
+} // namespace
+
+#endif
diff --git a/velocity_controllers/include/velocity_controllers/joint_velocity_controller.h b/velocity_controllers/include/velocity_controllers/joint_velocity_controller.h
new file mode 100644
index 0000000..b6b8782
--- /dev/null
+++ b/velocity_controllers/include/velocity_controllers/joint_velocity_controller.h
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef VELOCITY_CONTROLLERS_JOINT_VELOCITY_CONTROLLER_H
+#define VELOCITY_CONTROLLERS_JOINT_VELOCITY_CONTROLLER_H
+
+#include <forward_command_controller/forward_command_controller.h>
+
+namespace velocity_controllers
+{
+
+/**
+ * \brief Joint Velocity Controller (linear or angular)
+ *
+ * This class passes the commanded velocity down to the joint
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointVelocityController".
+ * \param joint Name of the joint to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64) : The joint velocity to apply
+ */
+typedef forward_command_controller::ForwardCommandController<hardware_interface::VelocityJointInterface>
+ JointVelocityController;
+
+}
+
+#endif
diff --git a/velocity_controllers/mainpage.dox b/velocity_controllers/mainpage.dox
new file mode 100644
index 0000000..00196d2
--- /dev/null
+++ b/velocity_controllers/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b velocity_controllers
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml
new file mode 100644
index 0000000..aceddc8
--- /dev/null
+++ b/velocity_controllers/package.xml
@@ -0,0 +1,37 @@
+<package>
+ <name>velocity_controllers</name>
+ <version>0.10.0</version>
+ <description>velocity_controllers</description>
+
+ <maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</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>Vijay Pradeep</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>angles</build_depend>
+ <build_depend>control_msgs</build_depend>
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>forward_command_controller</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>urdf</build_depend>
+
+ <run_depend>angles</run_depend>
+ <run_depend>control_msgs</run_depend>
+ <run_depend>control_toolbox</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>forward_command_controller</run_depend>
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>urdf</run_depend>
+
+ <export>
+ <controller_interface plugin="${prefix}/velocity_controllers_plugins.xml"/>
+ </export>
+</package>
diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp
new file mode 100644
index 0000000..715ca74
--- /dev/null
+++ b/velocity_controllers/src/joint_group_velocity_controller.cpp
@@ -0,0 +1,49 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, PAL Robotics, S.L.
+ * Copyright (c) 2014, Fraunhofer IPA
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <velocity_controllers/joint_group_velocity_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+{
+ // Start controller with 0.0 velocities
+ commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
+}
+
+
+PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase)
diff --git a/velocity_controllers/src/joint_position_controller.cpp b/velocity_controllers/src/joint_position_controller.cpp
new file mode 100644
index 0000000..928b21f
--- /dev/null
+++ b/velocity_controllers/src/joint_position_controller.cpp
@@ -0,0 +1,263 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * Copyright (c) 2013, University of Colorado, Boulder
+ * 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 Willow Garage 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: Dave Coleman
+ Contributors: Jonathan Bohren, Wim Meeussen, Vijay Pradeep
+ Desc: Velocity-based position controller using basic PID loop
+*/
+
+#include <velocity_controllers/joint_position_controller.h>
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace velocity_controllers {
+
+JointPositionController::JointPositionController()
+ : loop_count_(0)
+{}
+
+JointPositionController::~JointPositionController()
+{
+ sub_command_.shutdown();
+}
+
+bool JointPositionController::init(hardware_interface::VelocityJointInterface *robot, ros::NodeHandle &n)
+{
+ // Get joint name from parameter server
+ std::string joint_name;
+ if (!n.getParam("joint", joint_name))
+ {
+ ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ // Load PID Controller using gains set on parameter server
+ if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
+ return false;
+
+ // Start realtime state publisher
+ controller_state_publisher_.reset(
+ new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1));
+
+ // Start command subscriber
+ sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);
+
+ // Get joint handle from hardware interface
+ joint_ = robot->getHandle(joint_name);
+
+ // Get URDF info about joint
+ urdf::Model urdf;
+ if (!urdf.initParam("robot_description"))
+ {
+ ROS_ERROR("Failed to parse urdf file");
+ return false;
+ }
+ joint_urdf_ = urdf.getJoint(joint_name);
+ if (!joint_urdf_)
+ {
+ ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
+ return false;
+ }
+
+ return true;
+}
+
+void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
+{
+ pid_controller_.setGains(p,i,d,i_max,i_min);
+}
+
+void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
+{
+ pid_controller_.getGains(p,i,d,i_max,i_min);
+}
+
+void JointPositionController::printDebug()
+{
+ pid_controller_.printValues();
+}
+
+std::string JointPositionController::getJointName()
+{
+ return joint_.getName();
+}
+
+double JointPositionController::getPosition()
+{
+ return joint_.getPosition();
+}
+
+// Set the joint position command
+void JointPositionController::setCommand(double pos_command)
+{
+ command_struct_.position_ = pos_command;
+ command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it
+
+ // the writeFromNonRT can be used in RT, if you have the guarantee that
+ // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
+ // * there is only one single rt thread
+ command_.writeFromNonRT(command_struct_);
+}
+
+// Set the joint position command with a velocity command as well
+void JointPositionController::setCommand(double pos_command, double vel_command)
+{
+ command_struct_.position_ = pos_command;
+ command_struct_.velocity_ = vel_command;
+ command_struct_.has_velocity_ = true;
+
+ command_.writeFromNonRT(command_struct_);
+}
+
+void JointPositionController::starting(const ros::Time& time)
+{
+ double pos_command = joint_.getPosition();
+
+ // Make sure joint is within limits if applicable
+ enforceJointLimits(pos_command);
+
+ command_struct_.position_ = pos_command;
+ command_struct_.has_velocity_ = false;
+
+ command_.initRT(command_struct_);
+
+ pid_controller_.reset();
+}
+
+void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
+{
+ command_struct_ = *(command_.readFromRT());
+ double command_position = command_struct_.position_;
+ double command_velocity = command_struct_.velocity_;
+ bool has_velocity_ = command_struct_.has_velocity_;
+
+ double error, vel_error;
+ double commanded_velocity;
+
+ double current_position = joint_.getPosition();
+
+ // Make sure joint is within limits if applicable
+ enforceJointLimits(command_position);
+
+ // Compute position error
+ if (joint_urdf_->type == urdf::Joint::REVOLUTE)
+ {
+ angles::shortest_angular_distance_with_limits(
+ current_position,
+ command_position,
+ joint_urdf_->limits->lower,
+ joint_urdf_->limits->upper,
+ error);
+ }
+ else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
+ {
+ error = angles::shortest_angular_distance(current_position, command_position);
+ }
+ else //prismatic
+ {
+ error = command_position - current_position;
+ }
+
+ // Decide which of the two PID computeCommand() methods to call
+ if (has_velocity_)
+ {
+ // Compute velocity error if a non-zero velocity command was given
+ vel_error = command_velocity - joint_.getVelocity();
+
+ // Set the PID error and compute the PID command with nonuniform
+ // time step size. This also allows the user to pass in a precomputed derivative error.
+ commanded_velocity = pid_controller_.computeCommand(error, vel_error, period);
+ }
+ else
+ {
+ // Set the PID error and compute the PID command with nonuniform
+ // time step size.
+ commanded_velocity = pid_controller_.computeCommand(error, period);
+ }
+
+ joint_.setCommand(commanded_velocity);
+
+ // publish state
+ if (loop_count_ % 10 == 0)
+ {
+ if(controller_state_publisher_ && controller_state_publisher_->trylock())
+ {
+ controller_state_publisher_->msg_.header.stamp = time;
+ controller_state_publisher_->msg_.set_point = command_position;
+ controller_state_publisher_->msg_.process_value = current_position;
+ controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
+ controller_state_publisher_->msg_.error = error;
+ controller_state_publisher_->msg_.time_step = period.toSec();
+ controller_state_publisher_->msg_.command = commanded_velocity;
+
+ double dummy;
+ getGains(controller_state_publisher_->msg_.p,
+ controller_state_publisher_->msg_.i,
+ controller_state_publisher_->msg_.d,
+ controller_state_publisher_->msg_.i_clamp,
+ dummy);
+ controller_state_publisher_->unlockAndPublish();
+ }
+ }
+ loop_count_++;
+}
+
+void JointPositionController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
+{
+ setCommand(msg->data);
+}
+
+// Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved
+void JointPositionController::enforceJointLimits(double &command)
+{
+ // Check that this joint has applicable limits
+ if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC)
+ {
+ if( command > joint_urdf_->limits->upper ) // above upper limnit
+ {
+ command = joint_urdf_->limits->upper;
+ }
+ else if( command < joint_urdf_->limits->lower ) // below lower limit
+ {
+ command = joint_urdf_->limits->lower;
+ }
+ }
+}
+
+} // namespace
+
+PLUGINLIB_EXPORT_CLASS( velocity_controllers::JointPositionController, controller_interface::ControllerBase)
diff --git a/velocity_controllers/src/joint_velocity_controller.cpp b/velocity_controllers/src/joint_velocity_controller.cpp
new file mode 100644
index 0000000..3b1970a
--- /dev/null
+++ b/velocity_controllers/src/joint_velocity_controller.cpp
@@ -0,0 +1,48 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * Copyright (c) 2012, hiDOF, Inc.
+ * 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 Willow Garage 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.
+ *********************************************************************/
+
+#include <velocity_controllers/joint_velocity_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+template <class T>
+void forward_command_controller::ForwardCommandController<T>::starting(const ros::Time& time)
+{
+ // Start controller with 0.0 velocity
+ command_buffer_.writeFromNonRT(0.0);
+}
+
+
+PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointVelocityController,controller_interface::ControllerBase)
diff --git a/velocity_controllers/velocity_controllers_plugins.xml b/velocity_controllers/velocity_controllers_plugins.xml
new file mode 100644
index 0000000..994e30f
--- /dev/null
+++ b/velocity_controllers/velocity_controllers_plugins.xml
@@ -0,0 +1,21 @@
+<library path="lib/libvelocity_controllers">
+
+ <class name="velocity_controllers/JointVelocityController" type="velocity_controllers::JointVelocityController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointVelocityController tracks velocity commands. It expects a VelocityJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="velocity_controllers/JointPositionController" type="velocity_controllers::JointPositionController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointPositionController converts position commands to velocity commands. It expects a VelocityJointInterface type of hardware interface.
+ </description>
+ </class>
+
+ <class name="velocity_controllers/JointGroupVelocityController" type="velocity_controllers::JointGroupVelocityController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The JointGroupVelocityController tracks velocity commands for a set of joints. It expects a VelocityJointInterface type of hardware interface.
+ </description>
+ </class>
+
+</library>
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-ros-controllers.git
More information about the debian-science-commits
mailing list