[ros-ros-controllers] 07/07: New upstream version 0.12.3
Leopold Palomo-Avellaneda
leo at alaxarxa.net
Mon Jul 17 12:58:39 UTC 2017
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 31c65918b9a0296219019af284937bf352bbf1c8
Author: Leopold Palomo-Avellaneda <leo at alaxarxa.net>
Date: Fri Jul 14 15:16:03 2017 +0200
New upstream version 0.12.3
---
README.md | 8 +-
diff_drive_controller/CHANGELOG.rst | 24 +
diff_drive_controller/CMakeLists.txt | 10 +-
diff_drive_controller/package.xml | 27 +-
.../src/diff_drive_controller.cpp | 12 +-
.../test/diff_drive_bad_urdf.test | 2 +-
.../test/diff_drive_common.launch | 2 +-
.../test/diff_drive_limits_test.cpp | 2 +
.../test/diff_drive_radius_param.test | 2 +-
.../test/diff_drive_radius_param_fail.test | 2 +-
.../test/diff_drive_separation_param.test | 2 +-
.../test/skid_steer_common.launch | 2 +-
diff_drive_controller/test/sphere_wheel.xacro | 6 +-
diff_drive_controller/test/view_diffbot.launch | 2 +-
.../test/view_skidsteerbot.launch | 2 +-
diff_drive_controller/test/wheel.xacro | 6 +-
effort_controllers/CHANGELOG.rst | 20 +
effort_controllers/CMakeLists.txt | 4 +-
.../effort_controllers/joint_position_controller.h | 2 +-
effort_controllers/package.xml | 30 +-
.../src/joint_position_controller.cpp | 2 +-
force_torque_sensor_controller/CHANGELOG.rst | 17 +
force_torque_sensor_controller/CMakeLists.txt | 21 +-
force_torque_sensor_controller/package.xml | 25 +-
forward_command_controller/CHANGELOG.rst | 15 +
forward_command_controller/package.xml | 18 +-
gripper_action_controller/CHANGELOG.rst | 18 +
gripper_action_controller/CMakeLists.txt | 144 +----
.../gripper_action_controller_impl.h | 21 +-
gripper_action_controller/package.xml | 76 +--
imu_sensor_controller/CHANGELOG.rst | 16 +
imu_sensor_controller/CMakeLists.txt | 22 +-
imu_sensor_controller/package.xml | 25 +-
joint_state_controller/CHANGELOG.rst | 14 +
joint_state_controller/package.xml | 4 +-
joint_trajectory_controller/CHANGELOG.rst | 27 +
joint_trajectory_controller/CMakeLists.txt | 18 +-
.../init_joint_trajectory.h | 280 ++++++---
.../joint_trajectory_controller.h | 44 +-
.../joint_trajectory_controller_impl.h | 318 +++++-----
.../joint_trajectory_segment.h | 89 +--
.../joint_trajectory_controller/tolerances.h | 61 ++
joint_trajectory_controller/package.xml | 54 +-
.../test/init_joint_trajectory_test.cpp | 198 ++++---
.../test/joint_partial_trajectory_controller.test | 39 ++
.../joint_partial_trajectory_controller_test.cpp | 637 +++++++++++++++++++++
.../test/joint_trajectory_controller.test | 24 +-
.../test/joint_trajectory_controller_test.cpp | 15 +-
.../test/joint_trajectory_controller_vel.test | 39 ++
.../test/joint_trajectory_segment_test.cpp | 129 ++---
joint_trajectory_controller/test/rrbot.cpp | 70 ++-
.../test/rrbot_partial_controllers.yaml | 15 +
.../test/rrbot_vel_controllers.yaml | 18 +
position_controllers/CHANGELOG.rst | 15 +
position_controllers/package.xml | 12 +-
ros_controllers/CHANGELOG.rst | 16 +
ros_controllers/package.xml | 29 +-
rqt_joint_trajectory_controller/CHANGELOG.rst | 15 +
rqt_joint_trajectory_controller/package.xml | 19 +-
velocity_controllers/CHANGELOG.rst | 18 +
.../joint_position_controller.h | 2 +-
velocity_controllers/package.xml | 28 +-
.../src/joint_position_controller.cpp | 2 +-
63 files changed, 1907 insertions(+), 929 deletions(-)
diff --git a/README.md b/README.md
index b43ba8d..f0fd810 100644
--- a/README.md
+++ b/README.md
@@ -3,12 +3,6 @@ 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
+[](https://travis-ci.org/ros-controls/ros_controllers)
diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst
index 6b6f607..6fe2aa1 100644
--- a/diff_drive_controller/CHANGELOG.rst
+++ b/diff_drive_controller/CHANGELOG.rst
@@ -2,6 +2,30 @@
Changelog for package diff_drive_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+* Add exporting include dirs
+* Contributors: Bence Magyar
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Add urdf compatibility header
+* Add --inorder to xacro calls
+* Add missing xacro tags
+* Use xacro instead of xacro.py
+* Disable angular jerk limit test
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt
index b0389ca..101d86f 100644
--- a/diff_drive_controller/CMakeLists.txt
+++ b/diff_drive_controller/CMakeLists.txt
@@ -3,10 +3,11 @@ project(diff_drive_controller)
find_package(catkin REQUIRED COMPONENTS
controller_interface
- urdf
+ nav_msgs
realtime_tools
tf
- nav_msgs)
+ urdf
+)
catkin_package(
INCLUDE_DIRS include
@@ -20,6 +21,9 @@ include_directories(
add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+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}
@@ -30,7 +34,7 @@ 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)
+ find_package(catkin REQUIRED COMPONENTS controller_manager rostest std_srvs tf)
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(diffbot test/diffbot.cpp)
diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml
index 3115d2c..274581f 100644
--- a/diff_drive_controller/package.xml
+++ b/diff_drive_controller/package.xml
@@ -1,8 +1,9 @@
-<package>
+<package format="2">
<name>diff_drive_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>Controller for a differential drive mobile base.</description>
- <maintainer email="bence.magyar at pal-robotics.com">Bence Magyar</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -14,21 +15,13 @@
<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>
+ <depend>controller_interface</depend>
+ <depend>nav_msgs</depend>
+ <depend>realtime_tools</depend>
+ <depend>tf</depend>
+ <depend>urdf</depend>
+ <test_depend>rostest</test_depend>
<test_depend>std_srvs</test_depend>
<test_depend>controller_manager</test_depend>
<test_depend>xacro</test_depend>
diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
index b581b04..2565dcf 100644
--- a/diff_drive_controller/src/diff_drive_controller.cpp
+++ b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -42,6 +42,8 @@
#include <urdf_parser/urdf_parser.h>
+#include <urdf/urdfdom_compatibility.h>
+
#include <boost/assign.hpp>
#include <diff_drive_controller/diff_drive_controller.h>
@@ -58,7 +60,7 @@ static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3&
* \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)
+static bool isCylinder(const urdf::LinkConstSharedPtr& link)
{
if (!link)
{
@@ -93,7 +95,7 @@ static bool isCylinder(const boost::shared_ptr<const urdf::Link>& 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)
+static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius)
{
if (!isCylinder(wheel_link))
{
@@ -464,10 +466,10 @@ namespace diff_drive_controller{
return false;
}
- boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
+ urdf::ModelInterfaceSharedPtr 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));
+ urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name));
+ urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name));
if (lookup_wheel_separation)
{
diff --git a/diff_drive_controller/test/diff_drive_bad_urdf.test b/diff_drive_controller/test/diff_drive_bad_urdf.test
index 2932c6f..fc774f9 100644
--- a/diff_drive_controller/test/diff_drive_bad_urdf.test
+++ b/diff_drive_controller/test/diff_drive_bad_urdf.test
@@ -4,7 +4,7 @@
<!-- Override robot_description with bad urdf -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find diff_drive_controller)/test/diffbot_bad.xacro'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_bad.xacro' --inorder" />
<!-- Controller test -->
<test test-name="diff_drive_fail_test"
diff --git a/diff_drive_controller/test/diff_drive_common.launch b/diff_drive_controller/test/diff_drive_common.launch
index b63aaa8..8be22b5 100644
--- a/diff_drive_controller/test/diff_drive_common.launch
+++ b/diff_drive_controller/test/diff_drive_common.launch
@@ -1,7 +1,7 @@
<launch>
<!-- Load diffbot model -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find diff_drive_controller)/test/diffbot.xacro'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro' --inorder" />
<!-- Start diffbot -->
<node name="diffbot"
diff --git a/diff_drive_controller/test/diff_drive_limits_test.cpp b/diff_drive_controller/test/diff_drive_limits_test.cpp
index 6f69b8f..f2d5f9d 100644
--- a/diff_drive_controller/test/diff_drive_limits_test.cpp
+++ b/diff_drive_controller/test/diff_drive_limits_test.cpp
@@ -123,6 +123,7 @@ TEST_F(DiffDriveControllerTest, testLinearVelocityLimits)
publish(cmd_vel);
}
+/* This test has been failing on Travis for a long time due to timing issues however it works well when ran manually
TEST_F(DiffDriveControllerTest, testAngularJerkLimits)
{
// wait for ROS
@@ -153,6 +154,7 @@ TEST_F(DiffDriveControllerTest, testAngularJerkLimits)
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
}
+*/
TEST_F(DiffDriveControllerTest, testAngularAccelerationLimits)
{
diff --git a/diff_drive_controller/test/diff_drive_radius_param.test b/diff_drive_controller/test/diff_drive_radius_param.test
index cfd6a6e..c0e4f85 100644
--- a/diff_drive_controller/test/diff_drive_radius_param.test
+++ b/diff_drive_controller/test/diff_drive_radius_param.test
@@ -4,7 +4,7 @@
<!-- 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'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro' --inorder" />
<!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
<param name="diffbot_controller/wheel_radius" value="0.11"/>
diff --git a/diff_drive_controller/test/diff_drive_radius_param_fail.test b/diff_drive_controller/test/diff_drive_radius_param_fail.test
index 8f99587..28a3b4d 100644
--- a/diff_drive_controller/test/diff_drive_radius_param_fail.test
+++ b/diff_drive_controller/test/diff_drive_radius_param_fail.test
@@ -4,7 +4,7 @@
<!-- Override robot_description with bad urdf -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro' --inorder" />
<!-- Don't provide the radius parameter, so the controller should break -->
<!-- <param name="diffbot_controller/wheel_radius" value="0.11"/> -->
diff --git a/diff_drive_controller/test/diff_drive_separation_param.test b/diff_drive_controller/test/diff_drive_separation_param.test
index e39af27..4120242 100644
--- a/diff_drive_controller/test/diff_drive_separation_param.test
+++ b/diff_drive_controller/test/diff_drive_separation_param.test
@@ -4,7 +4,7 @@
<!-- 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'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro' --inorder" />
<!-- 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 -->
diff --git a/diff_drive_controller/test/skid_steer_common.launch b/diff_drive_controller/test/skid_steer_common.launch
index fe7e948..0c61789 100644
--- a/diff_drive_controller/test/skid_steer_common.launch
+++ b/diff_drive_controller/test/skid_steer_common.launch
@@ -1,7 +1,7 @@
<launch>
<!-- Load skidsteerbot model -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro' --inorder" />
<!-- Start skidsteerbot -->
<node name="skidsteerbot"
diff --git a/diff_drive_controller/test/sphere_wheel.xacro b/diff_drive_controller/test/sphere_wheel.xacro
index 0694a30..bd8dd47 100644
--- a/diff_drive_controller/test/sphere_wheel.xacro
+++ b/diff_drive_controller/test/sphere_wheel.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <macro name="sphere_wheel" params="name parent radius *origin">
+ <xacro:macro name="sphere_wheel" params="name parent radius *origin">
<link name="${name}_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
@@ -24,7 +24,7 @@
<joint name="${name}_joint" type="continuous">
<parent link="${parent}_link"/>
<child link="${name}_link"/>
- <insert_block name="origin"/>
+ <xacro:insert_block name="origin"/>
<axis xyz="0 0 1"/>
</joint>
@@ -39,5 +39,5 @@
<gazebo reference="${name}_link">
<material>Gazebo/Red</material>
</gazebo>
- </macro>
+ </xacro:macro>
</robot>
diff --git a/diff_drive_controller/test/view_diffbot.launch b/diff_drive_controller/test/view_diffbot.launch
index cdc362e..5ce4247 100644
--- a/diff_drive_controller/test/view_diffbot.launch
+++ b/diff_drive_controller/test/view_diffbot.launch
@@ -4,7 +4,7 @@
<!-- load robot -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find diff_drive_controller)/test/diffbot.xacro'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro' --inorder" />
<!-- Start diffbot -->
<node name="diffbot"
diff --git a/diff_drive_controller/test/view_skidsteerbot.launch b/diff_drive_controller/test/view_skidsteerbot.launch
index 66d8d5c..ce82994 100644
--- a/diff_drive_controller/test/view_skidsteerbot.launch
+++ b/diff_drive_controller/test/view_skidsteerbot.launch
@@ -4,7 +4,7 @@
<!-- load robot -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
+ command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro' --inorder" />
<!-- Start skidsteerbot -->
<node name="skidsteerbot"
diff --git a/diff_drive_controller/test/wheel.xacro b/diff_drive_controller/test/wheel.xacro
index 6fa1bd6..1e8dd89 100644
--- a/diff_drive_controller/test/wheel.xacro
+++ b/diff_drive_controller/test/wheel.xacro
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <macro name="wheel" params="name parent radius thickness *origin">
+ <xacro:macro name="wheel" params="name parent radius thickness *origin">
<link name="${name}_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
@@ -24,7 +24,7 @@
<joint name="${name}_joint" type="continuous">
<parent link="${parent}_link"/>
<child link="${name}_link"/>
- <insert_block name="origin"/>
+ <xacro:insert_block name="origin"/>
<axis xyz="0 0 1"/>
</joint>
@@ -39,5 +39,5 @@
<gazebo reference="${name}_link">
<material>Gazebo/Red</material>
</gazebo>
- </macro>
+ </xacro:macro>
</robot>
diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst
index 7f4ffdc..4cfd52d 100644
--- a/effort_controllers/CHANGELOG.rst
+++ b/effort_controllers/CHANGELOG.rst
@@ -2,6 +2,26 @@
Changelog for package effort_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+* Supply NodeHandle to urdf::Model. Closes `#244 <https://github.com/ros-controls/ros_controllers/issues/244>`_
+* Contributors: Piyush Khandelwal
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Remove unused dependency
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
* Included angles in dependencies
diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt
index 68b844e..8b1bf40 100644
--- a/effort_controllers/CMakeLists.txt
+++ b/effort_controllers/CMakeLists.txt
@@ -6,8 +6,8 @@ find_package(catkin REQUIRED COMPONENTS
angles
controller_interface
control_msgs
- forward_command_controller
control_toolbox
+ forward_command_controller
realtime_tools
urdf
)
@@ -21,9 +21,9 @@ catkin_package(
controller_interface
control_msgs
control_toolbox
+ forward_command_controller
realtime_tools
urdf
- forward_command_controller
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
diff --git a/effort_controllers/include/effort_controllers/joint_position_controller.h b/effort_controllers/include/effort_controllers/joint_position_controller.h
index 15267c8..4846078 100644
--- a/effort_controllers/include/effort_controllers/joint_position_controller.h
+++ b/effort_controllers/include/effort_controllers/joint_position_controller.h
@@ -167,7 +167,7 @@ public:
double getPosition();
hardware_interface::JointHandle joint_;
- boost::shared_ptr<const urdf::Joint> joint_urdf_;
+ urdf::JointConstSharedPtr joint_urdf_;
realtime_tools::RealtimeBuffer<Commands> command_;
Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml
index d529412..13bf50d 100644
--- a/effort_controllers/package.xml
+++ b/effort_controllers/package.xml
@@ -1,8 +1,10 @@
-<package>
+<package format="2">
<name>effort_controllers</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>effort_controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -14,23 +16,13 @@
<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>
+ <depend>angles</depend>
+ <depend>controller_interface</depend>
+ <depend>control_msgs</depend>
+ <depend>control_toolbox</depend>
+ <depend>realtime_tools</depend>
+ <depend>urdf</depend>
+ <depend>forward_command_controller</depend>
<export>
<controller_interface plugin="${prefix}/effort_controllers_plugins.xml"/>
diff --git a/effort_controllers/src/joint_position_controller.cpp b/effort_controllers/src/joint_position_controller.cpp
index a9f4be5..1cad388 100644
--- a/effort_controllers/src/joint_position_controller.cpp
+++ b/effort_controllers/src/joint_position_controller.cpp
@@ -80,7 +80,7 @@ bool JointPositionController::init(hardware_interface::EffortJointInterface *rob
// Get URDF info about joint
urdf::Model urdf;
- if (!urdf.initParam("robot_description"))
+ if (!urdf.initParamWithNodeHandle("robot_description", n))
{
ROS_ERROR("Failed to parse urdf file");
return false;
diff --git a/force_torque_sensor_controller/CHANGELOG.rst b/force_torque_sensor_controller/CHANGELOG.rst
index 3fcb2bb..7787b9b 100644
--- a/force_torque_sensor_controller/CHANGELOG.rst
+++ b/force_torque_sensor_controller/CHANGELOG.rst
@@ -2,6 +2,23 @@
Changelog for package force_torque_sensor_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Sort dependencies
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/force_torque_sensor_controller/CMakeLists.txt b/force_torque_sensor_controller/CMakeLists.txt
index 437f097..47fefa2 100644
--- a/force_torque_sensor_controller/CMakeLists.txt
+++ b/force_torque_sensor_controller/CMakeLists.txt
@@ -2,11 +2,24 @@ 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)
+find_package(catkin REQUIRED COMPONENTS
+ controller_interface
+ geometry_msgs
+ hardware_interface
+ pluginlib
+ realtime_tools
+ roscpp
+)
# Declare catkin package
catkin_package(
- CATKIN_DEPENDS controller_interface hardware_interface geometry_msgs realtime_tools
+ CATKIN_DEPENDS
+ controller_interface
+ geometry_msgs
+ hardware_interface
+ pluginlib
+ realtime_tools
+ roscpp
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
@@ -14,7 +27,9 @@ catkin_package(
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)
+ src/force_torque_sensor_controller.cpp
+ include/force_torque_sensor_controller/force_torque_sensor_controller.h
+)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Install
diff --git a/force_torque_sensor_controller/package.xml b/force_torque_sensor_controller/package.xml
index aaaff75..3031e2c 100644
--- a/force_torque_sensor_controller/package.xml
+++ b/force_torque_sensor_controller/package.xml
@@ -1,8 +1,10 @@
-<package>
+<package format="2">
<name>force_torque_sensor_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>Controller to publish state of force-torque sensors</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -14,19 +16,12 @@
<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>
+ <depend>controller_interface</depend>
+ <depend>geometry_msgs</depend>
+ <depend>hardware_interface</depend>
+ <depend>pluginlib</depend>
+ <depend>realtime_tools</depend>
+ <depend>roscpp</depend>
<export>
<controller_interface plugin="${prefix}/force_torque_sensor_plugin.xml"/>
diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst
index 1ecb380..283254d 100644
--- a/forward_command_controller/CHANGELOG.rst
+++ b/forward_command_controller/CHANGELOG.rst
@@ -2,6 +2,21 @@
Changelog for package forward_command_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml
index 45c361d..1d40baa 100644
--- a/forward_command_controller/package.xml
+++ b/forward_command_controller/package.xml
@@ -1,8 +1,10 @@
-<package>
+<package format="2">
<name>forward_command_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>forward_command_controller</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -14,14 +16,10 @@
<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>
+ <depend>controller_interface</depend>
+ <depend>hardware_interface</depend>
+ <depend>std_msgs</depend>
+ <depend>realtime_tools</depend>
<export>
<cpp cflags="-I${prefix}/include"/>
diff --git a/gripper_action_controller/CHANGELOG.rst b/gripper_action_controller/CHANGELOG.rst
index 84296c3..1f1e212 100644
--- a/gripper_action_controller/CHANGELOG.rst
+++ b/gripper_action_controller/CHANGELOG.rst
@@ -2,6 +2,24 @@
Changelog for package gripper_action_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Change for format2
+* Add Enrique and Bence to maintainers
+* urdf::Model typedefs had to be added to a different repo first
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/gripper_action_controller/CMakeLists.txt b/gripper_action_controller/CMakeLists.txt
index 2b925a9..b35a0c7 100644
--- a/gripper_action_controller/CMakeLists.txt
+++ b/gripper_action_controller/CMakeLists.txt
@@ -1,114 +1,40 @@
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
+ controller_manager
+ control_msgs
+ control_toolbox
hardware_interface
realtime_tools
- control_msgs
+ roscpp
trajectory_msgs
- controller_manager
+ urdf
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
+CATKIN_DEPENDS
+ actionlib
+ cmake_modules
+ controller_interface
+ control_msgs
+ hardware_interface
+ realtime_tools
+ trajectory_msgs
)
-###########
-## 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
@@ -118,60 +44,16 @@ 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_impl.h b/gripper_action_controller/include/gripper_action_controller/gripper_action_controller_impl.h
index 2900741..6a868b3 100644
--- 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
@@ -41,9 +41,9 @@ std::string getLeafNamespace(const ros::NodeHandle& nh)
return complete_ns.substr(id + 1);
}
-boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+urdf::ModelSharedPtr getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
{
- boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+ urdf::ModelSharedPtr urdf(new urdf::Model);
std::string urdf_str;
// Check for robot_description in proper namespace
@@ -53,25 +53,24 @@ boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::str
{
ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
nh.getNamespace() << ").");
- return boost::shared_ptr<urdf::Model>();
+ return urdf::ModelSharedPtr();
}
}
// 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::ModelSharedPtr();
}
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<urdf::JointConstSharedPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
{
- std::vector<UrdfJointConstPtr> out;
+ std::vector<urdf::JointConstSharedPtr> out;
for (unsigned int i = 0; i < joint_names.size(); ++i)
{
- UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+ urdf::JointConstSharedPtr urdf_joint = urdf.getJoint(joint_names[i]);
if (urdf_joint)
{
out.push_back(urdf_joint);
@@ -79,7 +78,7 @@ std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std:
else
{
ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
- return std::vector<UrdfJointConstPtr>();
+ return std::vector<urdf::JointConstSharedPtr>();
}
}
return out;
@@ -157,7 +156,7 @@ bool GripperActionController<HardwareInterface>::init(HardwareInterface* hw,
}
// URDF joints
- boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
+ urdf::ModelSharedPtr urdf = getUrdf(root_nh, "robot_description");
if (!urdf)
{
return false;
@@ -165,7 +164,7 @@ bool GripperActionController<HardwareInterface>::init(HardwareInterface* hw,
std::vector<std::string> joint_names;
joint_names.push_back(joint_name_);
- std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names);
+ std::vector<urdf::JointConstSharedPtr> urdf_joints = getUrdfJoints(*urdf, joint_names);
if (urdf_joints.empty())
{
return false;
diff --git a/gripper_action_controller/package.xml b/gripper_action_controller/package.xml
index 37bbd33..c2871ea 100644
--- a/gripper_action_controller/package.xml
+++ b/gripper_action_controller/package.xml
@@ -1,74 +1,32 @@
<?xml version="1.0"?>
-<package>
+<package format="2">
<name>gripper_action_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</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>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</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>
+ <depend>actionlib</depend>
+ <depend>angles</depend>
+ <depend>cmake_modules</depend>
+ <depend>control_msgs</depend>
+ <depend>control_toolbox</depend>
+ <depend>controller_interface</depend>
+ <depend>controller_manager</depend>
+ <depend>hardware_interface</depend>
+ <depend>realtime_tools</depend>
+ <depend>roscpp</depend>
+ <depend>trajectory_msgs</depend>
+ <depend>urdf</depend>
+ <depend>xacro</depend>
- <!-- The export tag contains other, unspecified, tags -->
<export>
<controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
</export>
diff --git a/imu_sensor_controller/CHANGELOG.rst b/imu_sensor_controller/CHANGELOG.rst
index b23e3c2..9bdf13e 100644
--- a/imu_sensor_controller/CHANGELOG.rst
+++ b/imu_sensor_controller/CHANGELOG.rst
@@ -2,6 +2,22 @@
Changelog for package imu_sensor_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix most catkin lint issues
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/imu_sensor_controller/CMakeLists.txt b/imu_sensor_controller/CMakeLists.txt
index 2f44b7d..df712fb 100644
--- a/imu_sensor_controller/CMakeLists.txt
+++ b/imu_sensor_controller/CMakeLists.txt
@@ -1,12 +1,23 @@
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)
+find_package(catkin REQUIRED COMPONENTS
+ controller_interface
+ hardware_interface
+ pluginlib
+ realtime_tools
+ roscpp
+ sensor_msgs
+)
-# Declare catkin package
catkin_package(
- CATKIN_DEPENDS controller_interface hardware_interface sensor_msgs realtime_tools
+ CATKIN_DEPENDS
+ controller_interface
+ hardware_interface
+ pluginlib
+ realtime_tools
+ roscpp
+ sensor_msgs
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
@@ -14,7 +25,8 @@ catkin_package(
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)
+ src/imu_sensor_controller.cpp
+ include/imu_sensor_controller/imu_sensor_controller.h)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
# Install
diff --git a/imu_sensor_controller/package.xml b/imu_sensor_controller/package.xml
index 121170d..b4bd718 100644
--- a/imu_sensor_controller/package.xml
+++ b/imu_sensor_controller/package.xml
@@ -1,8 +1,10 @@
-<package>
+<package format="2">
<name>imu_sensor_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>Controller to publish state of IMU sensors</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -14,19 +16,12 @@
<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>
+ <depend>realtime_tools</depend>
+ <depend>roscpp</depend>
+ <depend>hardware_interface</depend>
+ <depend>pluginlib</depend>
+ <depend>controller_interface</depend>
+ <depend>sensor_msgs</depend>
<export>
<controller_interface plugin="${prefix}/imu_sensor_plugin.xml"/>
diff --git a/joint_state_controller/CHANGELOG.rst b/joint_state_controller/CHANGELOG.rst
index 7dfc4e7..055f2fb 100644
--- a/joint_state_controller/CHANGELOG.rst
+++ b/joint_state_controller/CHANGELOG.rst
@@ -2,6 +2,20 @@
Changelog for package joint_state_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/joint_state_controller/package.xml b/joint_state_controller/package.xml
index a3941e2..f5b02c5 100644
--- a/joint_state_controller/package.xml
+++ b/joint_state_controller/package.xml
@@ -1,8 +1,10 @@
<package format="2">
<name>joint_state_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>Controller to publish joint state</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst
index d1d4f56..8c82b5a 100644
--- a/joint_trajectory_controller/CHANGELOG.rst
+++ b/joint_trajectory_controller/CHANGELOG.rst
@@ -2,6 +2,33 @@
Changelog for package joint_trajectory_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+* Remove rqt_plot test_depend & make plots optional
+* Contributors: Bence Magyar
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Fix missing controller_manager include
+* Ordered dependencies & cleanup
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Add test that sends trajectory entirely in past
+* Use xacro instead of xacro.py
+* urdf::Model typedefs had to be added to a different repo first
+* Updated copyright info
+* jtc: Enable sending trajectories with a partial set of joints
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Infrastructure for testing the velocity_controllers::JointTrajectoryController.
+* jtc: Enable sending trajectories with a partial set of joints
+* Contributors: Beatriz Leon, Bence Magyar, Miguel Prada
+
0.11.2 (2016-08-16)
-------------------
diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
index 985e042..87ef18a 100644
--- a/joint_trajectory_controller/CMakeLists.txt
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -15,8 +15,6 @@ find_package(catkin
realtime_tools
control_msgs
trajectory_msgs
- controller_manager
- xacro
)
# Declare catkin package
@@ -32,8 +30,6 @@ catkin_package(
realtime_tools
control_msgs
trajectory_msgs
- controller_manager
- xacro
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)
@@ -54,9 +50,16 @@ add_library(${PROJECT_NAME} src/joint_trajectory_controller.cpp
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
-
if(CATKIN_ENABLE_TESTING)
+ find_package(catkin
+ REQUIRED COMPONENTS
+ actionlib
+ controller_manager
+ xacro
+ )
+
find_package(rostest REQUIRED)
+ include_directories(include ${catkin_INCLUDE_DIRS})
catkin_add_gtest(quintic_spline_segment_test test/quintic_spline_segment_test.cpp)
target_link_libraries(quintic_spline_segment_test ${catkin_LIBRARIES})
@@ -88,6 +91,11 @@ if(CATKIN_ENABLE_TESTING)
test/joint_trajectory_controller.test
test/joint_trajectory_controller_test.cpp)
target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES})
+
+ add_rostest_gtest(joint_trajectory_controller_vel_test
+ test/joint_trajectory_controller_vel.test
+ test/joint_trajectory_controller_test.cpp)
+ target_link_libraries(joint_trajectory_controller_vel_test ${catkin_LIBRARIES})
endif()
# Install
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
index 75cf188..37269ef 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
@@ -57,19 +57,19 @@ 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>.
+ * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices.
+ * If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
+ * <tt>"{2, 1}"</tt>.
*/
template <class T>
-inline std::vector<unsigned int> permutation(const T& t1, const T& t2)
+inline std::vector<unsigned int> mapping(const T& t1, const T& t2)
{
typedef unsigned int SizeType;
+
+ // t1 must be a subset of t2
+ if (t1.size() > t2.size()) {return std::vector<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
+ std::vector<SizeType> mapping_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);
@@ -78,10 +78,10 @@ inline std::vector<unsigned int> permutation(const T& t1, const T& t2)
{
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;
+ mapping_vector[t1_dist] = t2_dist;
}
}
- return permutation_vector;
+ return mapping_vector;
}
} // namespace
@@ -95,7 +95,9 @@ struct InitJointTrajectoryOptions
{
typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
- typedef typename Trajectory::value_type::Scalar Scalar;
+ typedef typename Trajectory::value_type TrajectoryPerJoint;
+ typedef typename TrajectoryPerJoint::value_type Segment;
+ typedef typename Segment::Scalar Scalar;
InitJointTrajectoryOptions()
: current_trajectory(0),
@@ -103,7 +105,8 @@ struct InitJointTrajectoryOptions
angle_wraparound(0),
rt_goal_handle(),
default_tolerances(0),
- other_time_base(0)
+ other_time_base(0),
+ allow_partial_joints_goal(false)
{}
Trajectory* current_trajectory;
@@ -112,6 +115,13 @@ struct InitJointTrajectoryOptions
RealtimeGoalHandlePtr rt_goal_handle;
SegmentTolerances<Scalar>* default_tolerances;
ros::Time* other_time_base;
+ bool allow_partial_joints_goal;
+};
+
+template <class Trajectory>
+bool isNotEmpty(typename Trajectory::value_type trajPerJoint)
+{
+ return !trajPerJoint.empty();
};
/**
@@ -164,7 +174,6 @@ struct InitJointTrajectoryOptions
* 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:
@@ -184,8 +193,10 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
const InitJointTrajectoryOptions<Trajectory>& options =
InitJointTrajectoryOptions<Trajectory>())
{
- typedef typename Trajectory::value_type Segment;
+ typedef typename Trajectory::value_type TrajectoryPerJoint;
+ typedef typename TrajectoryPerJoint::value_type Segment;
typedef typename Segment::Scalar Scalar;
+ typedef typename TrajectoryPerJoint::const_iterator TrajIter;
const unsigned int n_joints = msg.joint_names.size();
@@ -239,13 +250,35 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
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 (has_angle_wraparound)
+ {
+ // Preconditions
+ const unsigned int n_angle_wraparound = options.angle_wraparound->size();
+ if (n_angle_wraparound != joint_names.size())
+ {
+ ROS_ERROR("Cannot create trajectory from message. "
+ "Vector specifying whether joints wrap around has an invalid size.");
+ return Trajectory();
+ }
+ }
+
+ // If partial joints goals are not allowed, goal should specify all controller joints
+ if (!options.allow_partial_joints_goal)
+ {
+ if (msg.joint_names.size() != joint_names.size())
+ {
+ ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints.");
+ return Trajectory();
+ }
+ }
+
+ // Mapping vector contains the map between the message joint order and the expected joint order
+ // If unspecified, a trivial map is computed
+ std::vector<unsigned int> mapping_vector = internal::mapping(msg.joint_names,joint_names);
- if (permutation_vector.empty())
+ if (mapping_vector.empty())
{
ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints.");
return Trajectory();
@@ -264,17 +297,17 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
// 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())
+ msg_it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time)
+ if (msg_it == msg.points.end())
{
- it = msg.points.begin(); // Entire trajectory is after current time
+ msg_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())
+ ++msg_it; // Points to first point after current time OR sequence end
+ if (msg_it == msg.points.end())
{
- ros::Duration last_point_dur = time - (msg_start_time + (--it)->time_from_start);
+ ros::Duration last_point_dur = time - (msg_start_time + (--msg_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() <<
@@ -283,8 +316,8 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
}
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) <<
+ ros::Duration next_point_dur = msg_start_time + msg_it->time_from_start - time;
+ ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), msg_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) <<
@@ -297,91 +330,156 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
// - 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
+ // Set active goal to segments after current time
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
+ result_traj = *(options.current_trajectory);
- // Compute offsets due to wrapping joints
- if (has_angle_wraparound)
+ //Iterate to all segments after current time to set the new goal handler
+ for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++)
{
- position_offset = wraparoundOffset(last_curr_state.position,
- first_new_state.position,
- *(options.angle_wraparound));
- if (position_offset.empty())
+ const TrajectoryPerJoint& curr_joint_traj = result_traj[joint_id];
+ TrajIter active_seg = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment
+
+ while (std::distance(active_seg, curr_joint_traj.end())>=1)
{
- ROS_ERROR("Cannot create trajectory from message. "
- "Vector specifying whether joints wrap around has an invalid size.");
- return Trajectory();
+ (result_traj[joint_id])[std::distance(curr_joint_traj.begin(),active_seg)].setGoalHandle(options.rt_goal_handle);
+ ++active_seg;
}
}
+ }
+ else
+ result_traj.resize(joint_names.size());
+
+ //Iterate through the joints that are in the message, in the order of the mapping vector
+ //for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++)
+ for (unsigned int msg_joint_it=0; msg_joint_it < mapping_vector.size();msg_joint_it++)
+ {
+ std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator it = msg_it;
+ if (!isValid(*it, it->positions.size()))
+ throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
- // 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
+ TrajectoryPerJoint result_traj_per_joint; // Currently empty
+ unsigned int joint_id = mapping_vector[msg_joint_it];
- // Add useful segments of current trajectory to result
+ // Initialize offsets due to wrapping joints to zero
+ std::vector<Scalar> position_offset(1, 0.0);
+
+ //Initialize segment tolerance per joint
+ SegmentTolerancesPerJoint<Scalar> tolerances_per_joint;
+ tolerances_per_joint.state_tolerance = tolerances.state_tolerance[joint_id];
+ tolerances_per_joint.goal_state_tolerance = tolerances.goal_state_tolerance[joint_id];
+ tolerances_per_joint.goal_time_tolerance = tolerances.goal_time_tolerance;
+
+ // Bridge current trajectory to new one
+ if (has_current_trajectory)
{
- 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())
+ const TrajectoryPerJoint& curr_joint_traj = (*options.current_trajectory)[joint_id];
+
+ // 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_joint_traj, last_curr_time, last_curr_state);
+
+ // Get the first time and state that will be executed from the new trajectory
+ trajectory_msgs::JointTrajectoryPoint point_per_joint;
+ if (!it->positions.empty()) {point_per_joint.positions.resize(1, it->positions[msg_joint_it]);}
+ if (!it->velocities.empty()) {point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);}
+ if (!it->accelerations.empty()) {point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);}
+ point_per_joint.time_from_start = it->time_from_start;
+
+ const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec();
+ typename Segment::State first_new_state(point_per_joint); // Here offsets are not yet applied
+
+ // Compute offsets due to wrapping joints
+ if (has_angle_wraparound)
+ {
+ position_offset[0] = wraparoundJointOffset(last_curr_state.position[0],
+ first_new_state.position[0],
+ (*options.angle_wraparound)[joint_id]);
+ }
+
+ // Apply offset to first state that will be executed from the new trajectory
+ first_new_state = typename Segment::State(point_per_joint, position_offset); // Now offsets are applied
+
+ // Add useful segments of current trajectory to result
{
- ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer.");
- return Trajectory();
+ TrajIter first = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment
+ TrajIter last = findSegment(curr_joint_traj, last_curr_time); // Segment active when new trajectory starts
+ if (first == curr_joint_traj.end() || last == curr_joint_traj.end())
+ {
+ ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer.");
+ return Trajectory();
+ }
+ result_traj_per_joint.insert(result_traj_per_joint.begin(), first, ++last); // Range [first,last) will still be executed
}
- 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_per_joint);}
+ result_traj_per_joint.push_back(bridge_seg);
}
- // 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_per_joint.size() -1;
+ const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1;
- // 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;
+
+ trajectory_msgs::JointTrajectoryPoint it_point_per_joint, next_it_point_per_joint;
+
+ if (!isValid(*it, it->positions.size()))
+ throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
+ if (!it->positions.empty()) {it_point_per_joint.positions.resize(1, it->positions[msg_joint_it]);}
+ if (!it->velocities.empty()) {it_point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);}
+ if (!it->accelerations.empty()) {it_point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);}
+ it_point_per_joint.time_from_start = it->time_from_start;
+
+ if (!isValid(*next_it, next_it->positions.size()))
+ throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
+ if (!next_it->positions.empty()) {next_it_point_per_joint.positions.resize(1, next_it->positions[msg_joint_it]);}
+ if (!next_it->velocities.empty()) {next_it_point_per_joint.velocities.resize(1, next_it->velocities[msg_joint_it]);}
+ if (!next_it->accelerations.empty()) {next_it_point_per_joint.accelerations.resize(1, next_it->accelerations[msg_joint_it]);}
+ next_it_point_per_joint.time_from_start = next_it->time_from_start;
+
+ Segment segment(o_msg_start_time, it_point_per_joint, next_it_point_per_joint, position_offset);
+ segment.setGoalHandle(options.rt_goal_handle);
+ if (has_rt_goal_handle) {segment.setTolerances(tolerances_per_joint);}
+ result_traj_per_joint.push_back(segment);
+ ++it;
+ }
- // 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 of joint " << joint_names[joint_id] << "has " << result_traj_per_joint.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());
+
+ if (result_traj_per_joint.size() > 0)
+ result_traj[joint_id] = result_traj_per_joint;
}
- // Useful debug info
- std::stringstream log_str;
- log_str << "Trajectory has " << result_traj.size() << " segments";
- if (has_current_trajectory)
+ // If the trajectory for all joints is empty, empty the trajectory vector
+ typename Trajectory::const_iterator trajIter = std::find_if (result_traj.begin(), result_traj.end(), isNotEmpty<Trajectory>);
+ if (trajIter == result_traj.end())
{
- 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.";}
+ result_traj.clear();
}
- else {log_str << ".";}
- ROS_DEBUG_STREAM(log_str.str());
return result_traj;
}
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
index 4aaf291..7b5f469 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
@@ -40,6 +40,7 @@
// Boost
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
+#include <boost/dynamic_bitset.hpp>
// ROS
#include <ros/node_handle.h>
@@ -166,8 +167,10 @@ private:
typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;
typedef JointTrajectorySegment<SegmentImpl> Segment;
- typedef std::vector<Segment> Trajectory;
+ typedef std::vector<Segment> TrajectoryPerJoint;
+ typedef std::vector<TrajectoryPerJoint> Trajectory;
typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
+ typedef boost::shared_ptr<TrajectoryPerJoint> TrajectoryPerJointPtr;
typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
typedef typename Segment::Scalar Scalar;
@@ -194,11 +197,11 @@ private:
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.
+ 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 desired_joint_state_; ///< Preallocated workspace variable.
+ typename Segment::State state_joint_error_; ///< Preallocated workspace variable.
realtime_tools::RealtimeBuffer<TimeData> time_data_;
@@ -206,6 +209,8 @@ private:
ros::Duration action_monitor_period_;
typename Segment::Time stop_trajectory_duration_;
+ boost::dynamic_bitset<> successful_joint_traj_;
+ bool allow_partial_joints_goal_;
// ROS API
ros::NodeHandle controller_nh_;
@@ -241,33 +246,6 @@ private:
*/
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
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
index 25f913f..cda6841 100644
--- 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
@@ -76,9 +76,9 @@ std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string
return out;
}
-boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+urdf::ModelSharedPtr getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
{
- boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+ urdf::ModelSharedPtr urdf(new urdf::Model);
std::string urdf_str;
// Check for robot_description in proper namespace
@@ -88,25 +88,24 @@ boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::str
{
ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
nh.getNamespace() << ").");
- return boost::shared_ptr<urdf::Model>();
+ return urdf::ModelSharedPtr();
}
}
// 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::ModelSharedPtr();
}
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<urdf::JointConstSharedPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
{
- std::vector<UrdfJointConstPtr> out;
+ std::vector<urdf::JointConstSharedPtr> out;
for (unsigned int i = 0; i < joint_names.size(); ++i)
{
- UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+ urdf::JointConstSharedPtr urdf_joint = urdf.getJoint(joint_names[i]);
if (urdf_joint)
{
out.push_back(urdf_joint);
@@ -114,7 +113,7 @@ std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std:
else
{
ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
- return std::vector<UrdfJointConstPtr>();
+ return std::vector<urdf::JointConstSharedPtr>();
}
}
return out;
@@ -180,60 +179,6 @@ preemptActiveGoal()
}
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
@@ -277,16 +222,23 @@ bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInt
}
ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s.");
+ // Checking if partial trajectories are allowed
+ controller_nh_.param<bool>("allow_partial_joints_goal", allow_partial_joints_goal_, false);
+ if (allow_partial_joints_goal_)
+ {
+ ROS_DEBUG_NAMED(name_, "Goals with partial set of joints are allowed");
+ }
+
// 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");
+ urdf::ModelSharedPtr urdf = getUrdf(root_nh, "robot_description");
if (!urdf) {return false;}
- std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
+ std::vector<urdf::JointConstSharedPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
if (urdf_joints.empty()) {return false;}
assert(n_joints == urdf_joints.size());
@@ -344,14 +296,26 @@ bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInt
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);
+ current_state_ = typename Segment::State(n_joints);
+ desired_state_ = typename Segment::State(n_joints);
+ state_error_ = typename Segment::State(n_joints);
+ desired_joint_state_ = typename Segment::State(1);
+ state_joint_error_ = typename Segment::State(1);
- Segment hold_segment(0.0, current_state_, 0.0, current_state_);
- hold_trajectory_ptr_->resize(1, hold_segment);
+ successful_joint_traj_ = boost::dynamic_bitset<>(joints_.size());
+
+ // Initialize trajectory with all joints
+ typename Segment::State current_joint_state_ = typename Segment::State(1);
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ current_joint_state_.position[0]= current_state_.position[i];
+ current_joint_state_.velocity[0]= current_state_.velocity[i];
+ Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_);
+
+ TrajectoryPerJoint joint_segment;
+ joint_segment.resize(1, hold_segment);
+ hold_trajectory_ptr_->push_back(joint_segment);
+ }
{
state_publisher_->lock();
@@ -393,16 +357,6 @@ update(const ros::Time& time, const ros::Duration& period)
// 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)
{
@@ -410,31 +364,95 @@ update(const ros::Time& time, const ros::Duration& period)
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];
+ typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], time_data.uptime.toSec(), desired_joint_state_);
+ if (curr_traj[i].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;
+ }
+ desired_state_.position[i] = desired_joint_state_.position[0];
+ desired_state_.velocity[i] = desired_joint_state_.velocity[0];
+ desired_state_.acceleration[i] = 0.0;
+
+ state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[i];
+ state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[i];
+ state_joint_error_.acceleration[0] = 0.0;
+
+ state_error_.position[i] = desired_joint_state_.position[0] - current_state_.position[i];
+ state_error_.velocity[i] = desired_joint_state_.velocity[0] - 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())
+ //Check tolerances
+ const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
+ if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
{
- // Currently executing a segment: check path tolerances
- checkPathTolerances(state_error_,
- *segment_it);
+ // Check tolerances
+ if (time_data.uptime.toSec() < segment_it->endTime())
+ {
+ // Currently executing a segment: check path tolerances
+ const SegmentTolerancesPerJoint<Scalar>& joint_tolerances = segment_it->getTolerances();
+ if (!checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance))
+ {
+ if (verbose_)
+ {
+ ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]);
+ checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true);
+ }
+ 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();
+ successful_joint_traj_.reset();
+ }
+ }
+ else if (segment_it == --curr_traj[i].end())
+ {
+ if (verbose_)
+ ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segment, checking goal tolerances");
+
+ // Controller uptime
+ const ros::Time uptime = time_data_.readFromRT()->uptime;
+
+ // Checks that we have ended inside the goal tolerances
+ const SegmentTolerancesPerJoint<Scalar>& tolerances = segment_it->getTolerances();
+ const bool inside_goal_tolerances = checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance);
+
+ if (inside_goal_tolerances)
+ {
+ successful_joint_traj_[i] = 1;
+ }
+ else if (uptime.toSec() < segment_it->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 for joint: "<< joint_names_[i]);
+ // Check the tolerances one more time to output the errors that occurs
+ checkStateTolerancePerJoint(state_joint_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();
+ successful_joint_traj_.reset();
+ }
+ }
}
- 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);
- }
+ //If there is an active goal and all segments finished successfully then set goal as succeeded
+ RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+ if (current_active_goal and successful_joint_traj_.count() == joints_.size())
+ {
+ current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
+ current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ successful_joint_traj_.reset();
}
// Hardware interface adapter: Generate and send commands
@@ -500,12 +518,13 @@ updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePt
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_;
+ 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_;
+ options.allow_partial_joints_goal = allow_partial_joints_goal_;
// Update currently executing trajectory
try
@@ -540,7 +559,7 @@ template <class SegmentImpl, class HardwareInterface>
void JointTrajectoryController<SegmentImpl, HardwareInterface>::
goalCB(GoalHandle gh)
{
- ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
+ ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal");
// Precondition: Running controller
if (!this->isRunning())
@@ -552,11 +571,24 @@ goalCB(GoalHandle gh)
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 partial joints goals are not allowed, goal should specify all controller joints
+ if (!allow_partial_joints_goal_)
+ {
+ if (gh.getGoal()->trajectory.joint_names.size() != joint_names_.size())
+ {
+ 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;
+ }
+ }
- if (permutation_vector.empty())
+ // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case
+ using internal::mapping;
+ std::vector<unsigned int> mapping_vector = mapping(gh.getGoal()->trajectory.joint_names, joint_names_);
+
+ if (mapping_vector.empty())
{
ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
control_msgs::FollowJointTrajectoryResult result;
@@ -639,19 +671,28 @@ queryStateService(control_msgs::QueryTrajectoryState::Request& req,
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)
+ typename Segment::State response_point = typename Segment::State(joint_names_.size());
+
+ for (unsigned int i = 0; i < joints_.size(); ++i)
{
- ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time.");
- return false;
- }
+ typename Segment::State state;
+ typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], sample_time.toSec(), state);
+ if (curr_traj[i].end() == segment_it)
+ {
+ ROS_ERROR_STREAM_NAMED(name_, "Requested sample time precedes trajectory start time.");
+ return false;
+ }
+ response_point.position[i] = state.position[0];
+ response_point.velocity[i] = state.velocity[0];
+ response_point.acceleration[i] = state.acceleration[0];
+ }
+
// Populate response
resp.name = joint_names_;
- resp.position = state.position;
- resp.velocity = state.velocity;
- resp.acceleration = state.acceleration;
+ resp.position = response_point.position;
+ resp.velocity = response_point.velocity;
+ resp.acceleration = response_point.acceleration;
return true;
}
@@ -691,7 +732,10 @@ setHoldPosition(const ros::Time& time)
// - 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());
+ assert(joint_names_.size() == hold_trajectory_ptr_->size());
+
+ typename Segment::State hold_start_state_ = typename Segment::State(1);
+ typename Segment::State hold_end_state_ = typename Segment::State(1);
const typename Segment::Time start_time = time.toSec();
const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_;
@@ -701,24 +745,24 @@ setHoldPosition(const ros::Time& time)
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_start_state_.position[0] = joints_[i].getPosition();
+ hold_start_state_.velocity[0] = joints_[i].getVelocity();
+ hold_start_state_.acceleration[0] = 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_);
+ hold_end_state_.position[0] = joints_[i].getPosition();
+ hold_end_state_.velocity[0] = -joints_[i].getVelocity();
+ hold_end_state_.acceleration[0] = 0.0;
- // Sample segment at its midpoint, that should have zero velocity
- hold_trajectory_ptr_->front().sample(end_time, hold_end_state_);
+ (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
+ end_time_2x, 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_);
+ // Sample segment at its midpoint, that should have zero velocity
+ (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
+ // Now create segment that goes from current state to one with zero end velocity
+ (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
+ end_time, hold_end_state_);
+ }
curr_trajectory_box_.set(hold_trajectory_ptr_);
}
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
index 3c024c7..6915a79 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
@@ -85,31 +85,18 @@ public:
/**
* \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
+ * \param position_offset Position offset to apply 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);
+ init(point, 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;
@@ -121,17 +108,6 @@ public:
{
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 "
@@ -145,15 +121,12 @@ public:
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];}
+ if (!point.positions.empty()) {this->position[i] = point.positions[i] + offset;}
+ if (!point.velocities.empty()) {this->velocity[i] = point.velocities[i];}
+ if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[i];}
}
}
};
@@ -171,7 +144,7 @@ public:
const Time& end_time,
const State& end_state)
: rt_goal_handle_(),
- tolerances_(start_state.position.size())
+ tolerances_()
{
Segment::init(start_time, start_state, end_time, end_state);
}
@@ -183,7 +156,6 @@ public:
* 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.
@@ -191,10 +163,9 @@ public:
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())
+ tolerances_()
{
if (start_point.positions.size() != end_point.positions.size())
{
@@ -207,8 +178,8 @@ public:
try
{
- const State start_state(start_point, permutation, position_offset);
- const State end_state(end_point, permutation, position_offset);
+ const State start_state(start_point, position_offset);
+ const State end_state(end_point, position_offset);
this->init(start_time, start_state,
end_time, end_state);
@@ -227,51 +198,45 @@ public:
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_;}
+ const SegmentTolerancesPerJoint<Scalar>& getTolerances() const {return tolerances_;}
/** \brief Set the tolerances this segment is associated to. */
- void setTolerances(const SegmentTolerances<Scalar>& tolerances) {tolerances_ = tolerances;}
+ void setTolerances(const SegmentTolerancesPerJoint<Scalar>& tolerances) {tolerances_ = tolerances;}
private:
RealtimeGoalHandlePtr rt_goal_handle_;
- SegmentTolerances<Scalar> tolerances_;
+ SegmentTolerancesPerJoint<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
+ * \param angle_wraparound Boolean where true value corresponds to a joint that wrap around
+ * (ie. is continuous). Offset will be computed only for this joint, otherwise it is set to zero.
+ * \return Wraparound offset 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)
+Scalar wraparoundJointOffset(const Scalar& prev_position,
+ const Scalar& next_position,
+ const 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);
+ Scalar pos_offset = 0.0;
- for (unsigned int i = 0; i < angle_wraparound.size(); ++i)
+ if (angle_wraparound)
{
- if (angle_wraparound[i])
- {
- Scalar dist = angles::shortest_angular_distance(prev_position[i], next_position[i]);
+ Scalar dist = angles::shortest_angular_distance(prev_position, next_position);
- // 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];
+ // Deal with singularity at M_PI shortest distance
+ if (std::abs(dist) - M_PI < 1e-9)
+ {
+ dist = next_position > prev_position ? std::abs(dist) : -std::abs(dist);
}
+ pos_offset = (prev_position + dist) - next_position;
}
+
return pos_offset;
}
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
index efc0658..02e03e0 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
@@ -89,6 +89,28 @@ struct SegmentTolerances
};
/**
+ * \brief Trajectory segment tolerances per Joint
+ */
+template<class Scalar>
+struct SegmentTolerancesPerJoint
+{
+ SegmentTolerancesPerJoint()
+ : state_tolerance(static_cast<Scalar>(0.0)),
+ goal_state_tolerance(static_cast<Scalar>(0.0)),
+ goal_time_tolerance(static_cast<Scalar>(0.0))
+ {}
+
+ /** State tolerances that appply during segment execution. */
+ StateTolerances<Scalar> state_tolerance;
+
+ /** State tolerances that apply for the goal state only.*/
+ 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
@@ -137,6 +159,45 @@ inline bool checkStateTolerance(const State&
}
/**
+ * \param state_error State error to check.
+ * \param state_tolerance State tolerances to check \p state_error against.
+ * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true
+ * \return True if \p state_error fulfills \p state_tolerance.
+ */
+template <class State>
+inline bool checkStateTolerancePerJoint(const State& state_error,
+ const StateTolerances<typename State::Scalar>& state_tolerance,
+ bool show_errors = false)
+{
+
+ using std::abs;
+
+ const bool is_valid = !(state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) &&
+ !(state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) &&
+ !(state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration);
+
+ if (!is_valid)
+ {
+ if( show_errors )
+ {
+ ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed:");
+
+ if (state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position)
+ ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[0] <<
+ " Position Tolerance: " << state_tolerance.position);
+ if (state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity)
+ ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[0] <<
+ " Velocity Tolerance: " << state_tolerance.velocity);
+ if (state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration)
+ ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[0] <<
+ " Acceleration Tolerance: " << state_tolerance.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.
diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
index ea0dbbf..05d3467 100644
--- a/joint_trajectory_controller/package.xml
+++ b/joint_trajectory_controller/package.xml
@@ -1,9 +1,11 @@
-<package>
+<package format="2">
<name>joint_trajectory_controller</name>
<description> Controller for executing joint-space trajectories on a group of joints. </description>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -16,41 +18,21 @@
<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!-->
+ <depend>actionlib</depend>
+ <depend>angles</depend>
+ <depend>control_msgs</depend>
+ <depend>control_toolbox</depend>
+ <depend>controller_interface</depend>
+ <depend>hardware_interface</depend>
+ <depend>realtime_tools</depend>
+ <depend>roscpp</depend>
+ <depend>trajectory_msgs</depend>
+ <depend>urdf</depend>
+
+ <test_depend>rostest</test_depend>
+ <test_depend>controller_manager</test_depend>
+ <test_depend>xacro</test_depend>
<export>
<controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
diff --git a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
index b26985d..c72035c 100644
--- a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
+++ b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
@@ -45,61 +45,73 @@ using std::string;
const double EPS = 1e-9;
typedef JointTrajectorySegment<trajectory_interface::QuinticSplineSegment<double> > Segment;
-typedef vector<Segment> Trajectory;
+typedef vector<Segment> TrajectoryPerJoint;
+typedef vector<TrajectoryPerJoint> Trajectory;
-TEST(PermutationTest, Permutation)
+TEST(MappingTest, Mapping)
{
vector<string> t1(4);
- t1[0] = "A";
- t1[1] = "B";
- t1[2] = "C";
- t1[3] = "D";
+ t1[0] = "B";
+ t1[1] = "D";
+ t1[2] = "A";
+ t1[3] = "C";
vector<string> t2(4);
- t2[0] = "B";
- t2[1] = "D";
- t2[2] = "A";
- t2[3] = "C";
+ t2[0] = "A";
+ t2[1] = "B";
+ t2[2] = "C";
+ t2[3] = "D";
- typedef vector<unsigned int> PermutationType;
- PermutationType ground_truth(4);
+ typedef vector<unsigned int> MappingType;
+ MappingType 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
+ // Mismatching sizes (t2 smaller than t1): Return empty mapping vector
{
vector<string> t2_bad(1,"A");
- PermutationType perm = internal::permutation(t1, t2_bad);
- EXPECT_TRUE(perm.empty());
+ MappingType mapping = internal::mapping(t1, t2_bad);
+ EXPECT_TRUE(mapping.empty());
}
- // Mismatching contents: Return empty permutation vector
+ // Mismatching contents: Return empty mapping vector
{
vector<string> t2_bad(3,"A");
- PermutationType perm = internal::permutation(t1, t2_bad);
- EXPECT_TRUE(perm.empty());
+ MappingType mapping = internal::mapping(t1, t2_bad);
+ EXPECT_TRUE(mapping.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]);
+ MappingType mapping = internal::mapping(t1, t2);
+ EXPECT_EQ(ground_truth.size(), mapping.size());
+ EXPECT_EQ(ground_truth[3], mapping[0]);
+ EXPECT_EQ(ground_truth[2], mapping[1]);
+ EXPECT_EQ(ground_truth[1], mapping[2]);
+ EXPECT_EQ(ground_truth[0], mapping[3]);
}
- // Valid parameters, inverse parameter order yields inverse permutation vector
+ // Valid parameters, inverse parameter order yields inverse mapping 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]);
+ MappingType mapping = internal::mapping(t2, t1);
+ EXPECT_EQ(ground_truth.size(), mapping.size());
+ EXPECT_EQ(ground_truth[0], mapping[0]);
+ EXPECT_EQ(ground_truth[1], mapping[1]);
+ EXPECT_EQ(ground_truth[2], mapping[2]);
+ EXPECT_EQ(ground_truth[3], mapping[3]);
+ }
+
+ // Valid parameters, partial trajectory
+ {
+ vector<string> t1_partial(2);
+ t1_partial[0] = "B";
+ t1_partial[1] = "D";
+ MappingType mapping = internal::mapping(t1_partial, t2);
+ EXPECT_EQ(t1_partial.size(), mapping.size());
+ EXPECT_EQ(1, mapping[0]);
+ EXPECT_EQ(3, mapping[1]);
}
}
@@ -140,11 +152,15 @@ public:
state[1].velocity.push_back(1.0);
state[1].acceleration.push_back(1.0);
- curr_traj.push_back(Segment(0.0, state[0],
+ TrajectoryPerJoint joint_segment;
+
+ joint_segment.push_back(Segment(0.0, state[0],
1.0, state[1]));
- curr_traj.push_back(Segment(3.0, state[1],
+ joint_segment.push_back(Segment(3.0, state[1],
10.0, state[0])); // This is just junk that should be discarded in the tests
+
+ curr_traj.push_back(joint_segment);
}
protected:
@@ -182,14 +198,14 @@ TEST_F(InitTrajectoryTest, InitLogic)
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time);
- EXPECT_EQ(points.size() - 1, trajectory.size());
+ EXPECT_EQ(points.size() - 1, trajectory[0].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());
+ EXPECT_EQ(points.size() - 2, trajectory[0].size());
}
// Between the first and second points: Return partial trajectory (last segment)
@@ -197,7 +213,7 @@ TEST_F(InitTrajectoryTest, InitLogic)
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());
+ EXPECT_EQ(msg_points.size() - 2, trajectory[0].size());
}
// Second point: Return empty trajectory
@@ -249,23 +265,23 @@ TEST_F(InitTrajectoryTest, InitLogicCombine)
// Before first point, starting the current trajectory: Return 4 segments: Last of current + bridge + full message
{
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][0].startTime());
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, time, options);
- EXPECT_EQ(points.size() + 1, trajectory.size());
+ EXPECT_EQ(points.size() + 1, trajectory[0].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());
+ EXPECT_EQ(points.size() + 1, trajectory[0].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());
+ EXPECT_EQ(points.size(), trajectory[0].size());
}
// Between the first and second points: Same as before
@@ -273,14 +289,14 @@ TEST_F(InitTrajectoryTest, InitLogicCombine)
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());
+ EXPECT_EQ(msg_points.size(), trajectory[0].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());
+ EXPECT_EQ(msg_points.size() - 1, trajectory[0].size());
}
// Between the second and third points: Same as before
@@ -288,7 +304,7 @@ TEST_F(InitTrajectoryTest, InitLogicCombine)
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());
+ EXPECT_EQ(msg_points.size() - 1, trajectory[0].size());
}
// Last point: Return empty trajectory
@@ -314,12 +330,12 @@ TEST_F(InitTrajectoryTest, InitValues)
// 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());
+ ASSERT_EQ(points.size() - 1, trajectory[0].size());
// Check all segment start/end times and states
- for (unsigned int i = 0; i < trajectory.size(); ++i)
+ for (unsigned int i = 0; i < trajectory[0].size(); ++i)
{
- const Segment& segment = trajectory[i];
+ const Segment& segment = trajectory[0][i];
const JointTrajectoryPoint& p_start = msg_points[i];
const JointTrajectoryPoint& p_end = msg_points[i + 1];
@@ -354,17 +370,17 @@ TEST_F(InitTrajectoryTest, InitValues)
TEST_F(InitTrajectoryTest, InitValuesCombine)
{
// Before first point: Return 4 segments: Last of current + bridge + full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][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());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Check current trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[0];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][0];
// Check start/end times
{
@@ -395,8 +411,8 @@ TEST_F(InitTrajectoryTest, InitValuesCombine)
// Check bridge trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[1];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][1];
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
@@ -433,12 +449,12 @@ TEST_F(InitTrajectoryTest, InitValuesCombine)
}
// 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)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].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 Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
@@ -470,7 +486,7 @@ TEST_F(InitTrajectoryTest, InitValuesCombine)
}
}
-TEST_F(InitTrajectoryTest, JointPermutation)
+TEST_F(InitTrajectoryTest, JointMapping)
{
// Add an extra joint to the trajectory message created in the fixture
trajectory_msg.points[0].positions.push_back(-2.0);
@@ -492,11 +508,13 @@ TEST_F(InitTrajectoryTest, JointPermutation)
Trajectory trajectory = initJointTrajectory<Trajectory>(trajectory_msg, trajectory_msg.header.stamp);
// Check position values only
- const Segment& segment = trajectory[0];
+ const Segment& segment_foo_joint = trajectory[0][0];
+ const Segment& segment_bar_joint = trajectory[1][0];
typename Segment::State state;
- segment.sample(segment.startTime(), state);
+ segment_foo_joint.sample(segment_foo_joint.startTime(), state);
EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[0]);
- EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[1]);
+ segment_bar_joint.sample(segment_bar_joint.startTime(), state);
+ EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]);
}
// Reference joint names vector that reverses joint order
@@ -510,13 +528,17 @@ TEST_F(InitTrajectoryTest, JointPermutation)
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]);
+ const Segment& segment_bar_joint = trajectory[0][0];
+ const Segment& segment_foo_joint = trajectory[1][0];
+
+ typename Segment::State state_foo_joint;
+ typename Segment::State state_bar_joint;
+ segment_foo_joint.sample(segment_foo_joint.startTime(), state_foo_joint);
+ segment_bar_joint.sample(segment_bar_joint.startTime(), state_bar_joint);
+ EXPECT_NE(trajectory_msg.points[0].positions[0], state_bar_joint.position[0]);
+ EXPECT_NE(trajectory_msg.points[0].positions[1], state_foo_joint.position[0]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[0], state_foo_joint.position[0]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[1], state_bar_joint.position[0]);
}
// Reference joint names size mismatch
@@ -553,19 +575,19 @@ TEST_F(InitTrajectoryTest, WrappingSpec)
}
// Before first point: Return 4 segments: Last of current + bridge + full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][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());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Check current trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[0];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][0];
// Check start/end times
{
@@ -596,8 +618,8 @@ TEST_F(InitTrajectoryTest, WrappingSpec)
// Check bridge trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[1];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][1];
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
@@ -634,12 +656,12 @@ TEST_F(InitTrajectoryTest, WrappingSpec)
}
// 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)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].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 Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
@@ -696,18 +718,18 @@ TEST_F(InitTrajectoryTest, IgnoreWrappingSpec)
}
// Before first point: Return 2 segments: Full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][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());
+ ASSERT_EQ(points.size() - 1, trajectory[0].size());
// Check all segment start/end times and states (2 segments)
- for (unsigned int i = 0; i < trajectory.size(); ++i)
+ for (unsigned int i = 0; i < trajectory[0].size(); ++i)
{
- const Segment& segment = trajectory[i];
+ const Segment& segment = trajectory[0][i];
const JointTrajectoryPoint& p_start = msg_points[i];
const JointTrajectoryPoint& p_end = msg_points[i + 1];
@@ -751,21 +773,21 @@ TEST_F(InitTrajectoryTest, GoalHandleTest)
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());
+ const ros::Time time(curr_traj[0][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());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Segment of current trajectory preserves existing null goal handle
- ASSERT_FALSE(trajectory[0].getGoalHandle());
+ ASSERT_FALSE(trajectory[0][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
+ for (unsigned int i = 1; i < trajectory[0].size(); ++i) // NOTE: Start index != 0
{
- ASSERT_EQ(rt_goal, trajectory[i].getGoalHandle());
+ ASSERT_EQ(rt_goal, trajectory[0][i].getGoalHandle());
}
}
@@ -783,12 +805,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase)
// 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());
+ ASSERT_EQ(points.size() + 1, trajectory[0].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];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][0];
// Check start/end times
{
@@ -799,7 +821,7 @@ TEST_F(InitTrajectoryTest, OtherTimeBase)
// Check bridge trajectory segment start/end times and states (only one). Time offset applies to end state only
{
- const Segment& segment = trajectory[1];
+ const Segment& segment = trajectory[0][1];
const vector<JointTrajectoryPoint>& msg_points = trajectory_msg.points;
@@ -817,12 +839,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase)
}
// 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)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].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 Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller.test b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test
new file mode 100644
index 0000000..5c314d2
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test
@@ -0,0 +1,39 @@
+<launch>
+ <arg name="display_plots" default="false"/>
+
+ <!-- Load RRbot model -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro '$(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_partial_controllers.yaml" />
+
+ <!-- Spawn controller -->
+ <node name="controller_spawner"
+ pkg="controller_manager" type="spawner" output="screen"
+ args="rrbot_controller" />
+
+ <group if="$(arg display_plots)">
+ <!-- rqt_plot 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]" />
+ </group>
+
+ <!-- Controller test -->
+ <test test-name="joint_partial_trajectory_controller_test"
+ pkg="joint_trajectory_controller"
+ type="joint_partial_trajectory_controller_test"
+ time-limit="80.0"/>
+</launch>
diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp
new file mode 100644
index 0000000..ad45a71
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp
@@ -0,0 +1,637 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2016, Shadow Robot Company Ltd.
+//
+// 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 Beatriz Leon
+
+#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 JointPartialTrajectoryControllerTest : public ::testing::Test
+{
+public:
+ JointPartialTrajectoryControllerTest()
+ : nh("rrbot_controller"),
+ short_timeout(1.0),
+ long_timeout(10.0),
+ controller_state()
+ {
+ n_joints = (2);
+ n_partial_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;
+
+ // Partial trajectory
+ traj_partial.joint_names.resize(1, "joint2");
+ points.resize(3, point);
+ points[0].positions[0] = M_PI / 4.0;
+ points[0].time_from_start = ros::Duration(1.0);
+
+ points[1].positions[0] = M_PI / 2.0;
+ points[1].time_from_start = ros::Duration(2.0);
+
+ points[2].positions[0] = -M_PI / 2.0;
+ points[2].time_from_start = ros::Duration(4.0);
+ traj_partial.points = points;
+
+ // Action goals
+ traj_home_goal.trajectory = traj_home;
+ traj_goal.trajectory = traj;
+ traj_partial_goal.trajectory = traj_partial;
+
+ // 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,
+ &JointPartialTrajectoryControllerTest::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));
+ }
+
+ ~JointPartialTrajectoryControllerTest()
+ {
+ 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;
+ unsigned int n_partial_joints;
+ std::vector<std::string> joint_names;
+ std::vector<trajectory_msgs::JointTrajectoryPoint> points;
+
+ trajectory_msgs::JointTrajectory traj_home;
+ trajectory_msgs::JointTrajectory traj;
+ trajectory_msgs::JointTrajectory traj_partial;
+ ActionGoal traj_home_goal;
+ ActionGoal traj_goal;
+ ActionGoal traj_partial_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;
+ }
+};
+
+// Invalid messages ////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, invalidMessages)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // 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);
+ }
+}
+
+// Partial trajectory execution //////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, allowPartialTrajIfSpecified)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send partial trajectory.
+ action_client->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, long_timeout));
+}
+
+// Uninterrupted Partial trajectory execution //////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, topicSinglePartialTraj)
+{
+ ASSERT_TRUE(initState());
+
+ // Send partial trajectory
+ traj_partial.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_partial);
+ ros::Duration wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS);
+
+ EXPECT_NEAR(0.0, state->error.positions[1], EPS);
+ EXPECT_NEAR(0.0, state->error.velocities[1], 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(JointPartialTrajectoryControllerTest, actionSinglePartialTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send partial trajectory
+ traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_partial_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();
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS);
+
+ EXPECT_NEAR(0.0, state->error.positions[1], EPS);
+ EXPECT_NEAR(0.0, state->error.velocities[1], EPS);
+}
+
+// Trajectory replacement //////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, topicReplacesTopicTraj)
+{
+ 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 partial trajectory that preempts the previous one
+ traj_partial.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_partial);
+ wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, 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 partial trajectory that preempts the previous one
+ traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client2->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ //ROS_ERROR_STREAM("state: "<< action_client->getState().toString());
+
+ // 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 original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, 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 partial trajectory that preempts the previous one
+ traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_partial_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 at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, 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 partial trajectory that preempts the previous one
+ traj_partial.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_partial);
+
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+
+ wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+// Ignore old trajectory points ////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, 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 partial trajectory with all points occuring in the past. Should not preempts the previous one
+ traj_partial.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5);
+ traj_pub.publish(traj_partial);
+
+ 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 (Joint2 has not moved according to traj_partial)
+ 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(JointPartialTrajectoryControllerTest, 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 partial trajectory with all points occuring in the past. Should not preempts the previous one
+ traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5);
+ action_client2->sendGoal(traj_partial_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 (Joint2 has not moved according to traj_partial)
+ 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(JointPartialTrajectoryControllerTest, 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
+ 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 partial trajectory with only the last point not occuring in the past. Only the last point should be executed
+ traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj.points.back().time_from_start + ros::Duration(1.0);
+ action_client2->sendGoal(traj_partial_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 original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, executeParitalActionTrajInFuture)
+{
+ 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
+ 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 partial trajectory with only the last point not occuring in the past. Only the last point should be executed
+ traj_partial_goal.trajectory.header.stamp = ros::Time::now() + traj.points.back().time_from_start + ros::Duration(2.0);
+ action_client2->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until first traj is done
+ wait_duration = first_goal.trajectory.points.back().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ 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);
+ }
+
+ // 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 original trajectory for joint1 and on the partial trajectory for joint2
+ state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "joint_partial_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_controller.test b/joint_trajectory_controller/test/joint_trajectory_controller.test
index 499e34d..d943488 100644
--- a/joint_trajectory_controller/test/joint_trajectory_controller.test
+++ b/joint_trajectory_controller/test/joint_trajectory_controller.test
@@ -1,7 +1,9 @@
<launch>
+ <arg name="display_plots" default="false"/>
+
<!-- Load RRbot model -->
<param name="robot_description"
- command="$(find xacro)/xacro.py '$(find joint_trajectory_controller)/test/rrbot.xacro'" />
+ command="$(find xacro)/xacro '$(find joint_trajectory_controller)/test/rrbot.xacro'" />
<!-- Start RRbot -->
<node name="rrbot"
@@ -16,16 +18,18 @@
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]" />
+ <group if="$(arg display_plots)">
+ <!-- rqt_plot 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]" />
+ <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]" />
+ </group>
<!-- Controller test -->
<test test-name="joint_trajectory_controller_test"
diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
index bb357c5..ce4f4bc 100644
--- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
+++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
@@ -248,7 +248,7 @@ TEST_F(JointTrajectoryControllerTest, invalidMessages)
ASSERT_TRUE(initState());
ASSERT_TRUE(action_client->waitForServer(long_timeout));
- // Invalid size
+ // Invalid size (No partial joints goals allowed)
{
trajectory_msgs::JointTrajectoryPoint point;
point.positions.resize(1, 0.0);
@@ -776,7 +776,7 @@ TEST_F(JointTrajectoryControllerTest, ignoreOldTopicTraj)
}
}
-TEST_F(JointTrajectoryControllerTest, ignoreOldActionTraj)
+TEST_F(JointTrajectoryControllerTest, ignorePreemptOfOldActionTraj)
{
ASSERT_TRUE(initState());
ASSERT_TRUE(action_client->waitForServer(long_timeout));
@@ -808,6 +808,17 @@ TEST_F(JointTrajectoryControllerTest, ignoreOldActionTraj)
}
}
+TEST_F(JointTrajectoryControllerTest, ignoreSingleOldActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_home_goal.trajectory.header.stamp = ros::Time::now() - ros::Duration(10000);
+ action_client->sendGoal(traj_home_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, short_timeout));
+}
+
TEST_F(JointTrajectoryControllerTest, ignorePartiallyOldActionTraj)
{
ASSERT_TRUE(initState());
diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_vel.test b/joint_trajectory_controller/test/joint_trajectory_controller_vel.test
new file mode 100644
index 0000000..b0b352d
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_trajectory_controller_vel.test
@@ -0,0 +1,39 @@
+<launch>
+ <arg name="display_plots" default="false"/>
+
+ <!-- Load RRbot model -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro '$(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_vel_controllers.yaml" />
+
+ <!-- Spawn controller -->
+ <node name="controller_spawner"
+ pkg="controller_manager" type="spawner" output="screen"
+ args="rrbot_controller" />
+
+ <group if="$(arg display_plots)">
+ <!-- rqt_plot 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]" />
+ </group>
+
+ <!-- Controller test -->
+ <test test-name="joint_trajectory_controller_vel_test"
+ pkg="joint_trajectory_controller"
+ type="joint_trajectory_controller_test"
+ time-limit="80.0"/>
+</launch>
diff --git a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
index 546dec4..14ac71c 100644
--- a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
+++ b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
@@ -41,7 +41,7 @@ using namespace trajectory_msgs;
const double EPS = 1e-9;
typedef JointTrajectorySegment<trajectory_interface::QuinticSplineSegment<double> > Segment;
-typedef std::vector<unsigned int> PermutationType;
+typedef std::vector<unsigned int> MappingType;
TEST(WraparoundOffsetTest, WrappingPositions)
{
@@ -60,10 +60,10 @@ TEST(WraparoundOffsetTest, WrappingPositions)
// 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
// Single wrapping joint
@@ -74,18 +74,18 @@ TEST(WraparoundOffsetTest, WrappingPositions)
// 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(-two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]);
+ EXPECT_NEAR(two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
}
@@ -95,18 +95,18 @@ TEST(WraparoundOffsetTest, WrappingPositions)
// 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(-two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(-2.0 * two_pi, wraparound_offset2, 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]);
+ EXPECT_NEAR(two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(2.0 * two_pi, wraparound_offset2, EPS);
}
}
}
@@ -122,16 +122,14 @@ TEST(WraparoundOffsetTest, WrappingPositionsPiSingularity)
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);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ EXPECT_NEAR(0.0, wraparound_offset1, 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
}
}
@@ -154,18 +152,18 @@ TEST(WraparoundOffsetTest, NonWrappingPositions)
// 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, 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);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
}
}
@@ -239,28 +237,11 @@ TEST_F(JointTrajectorySegmentTest, InvalidSegmentConstruction)
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);}
+ EXPECT_THROW(Segment(traj_start_time, p_start, p_end, pos_offset), std::invalid_argument);
+ try {Segment(traj_start_time, p_start, p_end, pos_offset);}
catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
}
}
@@ -372,7 +353,7 @@ TEST_F(JointTrajectorySegmentTest, CrossValidateSegmentConstruction)
}
// TODO: Test with dimension four data
-TEST_F(JointTrajectorySegmentTest, PermutationTest)
+TEST_F(JointTrajectorySegmentTest, MappingTest)
{
// Add an extra joint to the trajectory point messages created in the fixture
p_start.positions.push_back(-p_start.positions[0]);
@@ -381,7 +362,7 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest)
p_end.positions.push_back(-p_end.positions[0]);
p_end.velocities.push_back(-p_end.velocities[0]);
- // No permutation vector
+ // No mapping vector
{
// Construct segment from ROS message
EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end));
@@ -393,42 +374,6 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest)
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)
diff --git a/joint_trajectory_controller/test/rrbot.cpp b/joint_trajectory_controller/test/rrbot.cpp
index 47c7597..f304f9a 100644
--- a/joint_trajectory_controller/test/rrbot.cpp
+++ b/joint_trajectory_controller/test/rrbot.cpp
@@ -47,7 +47,8 @@ public:
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;
+ pos_cmd_[0] = 0.0; pos_cmd_[1] = 0.0;
+ vel_cmd_[0] = 0.0; vel_cmd_[1] = 0.0;
// Connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]);
@@ -59,14 +60,23 @@ public:
registerInterface(&jnt_state_interface_);
// Connect and register the joint position interface
- hardware_interface::JointHandle pos_handle_1(jnt_state_interface_.getHandle("joint1"), &cmd_[0]);
+ hardware_interface::JointHandle pos_handle_1(jnt_state_interface_.getHandle("joint1"), &pos_cmd_[0]);
jnt_pos_interface_.registerHandle(pos_handle_1);
- hardware_interface::JointHandle pos_handle_2(jnt_state_interface_.getHandle("joint2"), &cmd_[1]);
+ hardware_interface::JointHandle pos_handle_2(jnt_state_interface_.getHandle("joint2"), &pos_cmd_[1]);
jnt_pos_interface_.registerHandle(pos_handle_2);
registerInterface(&jnt_pos_interface_);
+ // Connect and register the joint velocity interface
+ hardware_interface::JointHandle vel_handle_1(jnt_state_interface_.getHandle("joint1"), &vel_cmd_[0]);
+ jnt_vel_interface_.registerHandle(vel_handle_1);
+
+ hardware_interface::JointHandle vel_handle_2(jnt_state_interface_.getHandle("joint2"), &vel_cmd_[1]);
+ jnt_vel_interface_.registerHandle(vel_handle_2);
+
+ registerInterface(&jnt_vel_interface_);
+
// Smoothing subscriber
smoothing_sub_ = ros::NodeHandle().subscribe("smoothing", 1, &RRbot::smoothingCB, this);
smoothing_.initRT(0.0);
@@ -82,21 +92,67 @@ public:
const double smoothing = *(smoothing_.readFromRT());
for (unsigned int i = 0; i < 2; ++i)
{
- vel_[i] = (cmd_[i] - pos_[i]) / getPeriod().toSec();
+ if(active_interface_[i] == "hardware_interface::PositionJointInterface")
+ {
+ vel_[i] = (pos_cmd_[i] - pos_[i]) / getPeriod().toSec();
+
+ const double next_pos = smoothing * pos_[i] + (1.0 - smoothing) * pos_cmd_[i];
+ pos_[i] = next_pos;
+ }
+ else if(active_interface_[i] == "hardware_interface::VelocityJointInterface")
+ {
+ vel_[i] = (1.0 - smoothing) * vel_cmd_[i];
+ pos_[i] = pos_[i] + vel_[i] * getPeriod().toSec();
+ }
+ }
+ }
+
+ typedef std::list<hardware_interface::ControllerInfo> ControllerList;
- const double next_pos = smoothing * pos_[i] + (1.0 - smoothing) * cmd_[i];
- pos_[i] = next_pos;
+ bool prepareSwitch(const ControllerList& start_list,
+ const ControllerList& stop_list)
+ {
+ for(ControllerList::const_iterator it = start_list.begin(); it != start_list.end(); ++it)
+ {
+ for(std::vector<hardware_interface::InterfaceResources>::const_iterator iface_it = it->claimed_resources.begin(); iface_it != it->claimed_resources.end(); ++iface_it)
+ {
+ for(std::set<std::string>::const_iterator res_it = iface_it->resources.begin(); res_it != iface_it->resources.end(); ++res_it)
+ {
+ if(*res_it == "joint1")
+ {
+ next_active_interface_[0] = iface_it->hardware_interface;
+ }
+ else if(*res_it == "joint2")
+ {
+ next_active_interface_[1] = iface_it->hardware_interface;
+ }
+ }
+ }
}
+
+ return true;
+ }
+
+ void doSwitch(const ControllerList& start_list,
+ const ControllerList& stop_list)
+ {
+ active_interface_[0] = next_active_interface_[0];
+ active_interface_[1] = next_active_interface_[1];
}
private:
hardware_interface::JointStateInterface jnt_state_interface_;
hardware_interface::PositionJointInterface jnt_pos_interface_;
- double cmd_[2];
+ hardware_interface::VelocityJointInterface jnt_vel_interface_;
+ double pos_cmd_[2];
+ double vel_cmd_[2];
double pos_[2];
double vel_[2];
double eff_[2];
+ std::string active_interface_[2];
+ std::string next_active_interface_[2];
+
realtime_tools::RealtimeBuffer<double> smoothing_;
void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);}
diff --git a/joint_trajectory_controller/test/rrbot_partial_controllers.yaml b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml
new file mode 100644
index 0000000..e7a3195
--- /dev/null
+++ b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml
@@ -0,0 +1,15 @@
+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
+ allow_partial_joints_goal: true
diff --git a/joint_trajectory_controller/test/rrbot_vel_controllers.yaml b/joint_trajectory_controller/test/rrbot_vel_controllers.yaml
new file mode 100644
index 0000000..0a660f5
--- /dev/null
+++ b/joint_trajectory_controller/test/rrbot_vel_controllers.yaml
@@ -0,0 +1,18 @@
+rrbot_controller:
+ type: "velocity_controllers/JointTrajectoryController"
+ joints:
+ - joint1
+ - joint2
+
+ constraints:
+ goal_time: 0.5
+ joint1:
+ goal: 0.01
+ trajectory: 0.05
+ joint2:
+ goal: 0.01
+ trajectory: 0.05
+
+ gains:
+ joint1: {p: 60.0, i: 0.0, d: 0.0, i_clamp: 1}
+ joint2: {p: 60.0, i: 0.0, d: 0.0, i_clamp: 1}
diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst
index ae51d81..fd122e8 100644
--- a/position_controllers/CHANGELOG.rst
+++ b/position_controllers/CHANGELOG.rst
@@ -2,6 +2,21 @@
Changelog for package position_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/position_controllers/package.xml b/position_controllers/package.xml
index 9c5b7c8..2e62760 100644
--- a/position_controllers/package.xml
+++ b/position_controllers/package.xml
@@ -1,10 +1,12 @@
-<package>
+<package format="2">
<name>position_controllers</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>position_controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
<maintainer email="davetcoleman at gmail.com">Dave Coleman</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -15,10 +17,8 @@
<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>
+ <depend>controller_interface</depend>
+ <depend>forward_command_controller</depend>
<export>
<controller_interface plugin="${prefix}/position_controllers_plugins.xml"/>
diff --git a/ros_controllers/CHANGELOG.rst b/ros_controllers/CHANGELOG.rst
index 9e8a46d..00034d1 100644
--- a/ros_controllers/CHANGELOG.rst
+++ b/ros_controllers/CHANGELOG.rst
@@ -2,6 +2,22 @@
Changelog for package ros_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Ordered dependencies & cleanup
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/ros_controllers/package.xml b/ros_controllers/package.xml
index 7b43b90..1ae5992 100644
--- a/ros_controllers/package.xml
+++ b/ros_controllers/package.xml
@@ -1,10 +1,12 @@
-<package>
+<package format="2">
<name>ros_controllers</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>Library of ros controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
<maintainer email="davetcoleman at gmail.com">Dave Coleman</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -14,20 +16,19 @@
<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>
+ <exec_depend>diff_drive_controller</exec_depend>
+ <exec_depend>effort_controllers</exec_depend>
+ <exec_depend>force_torque_sensor_controller</exec_depend>
+ <exec_depend>forward_command_controller</exec_depend>
+ <exec_depend>gripper_action_controller</exec_depend>
+ <exec_depend>imu_sensor_controller</exec_depend>
+ <exec_depend>joint_state_controller</exec_depend>
+ <exec_depend>joint_trajectory_controller</exec_depend>
+ <exec_depend>position_controllers</exec_depend>
+ <exec_depend>rqt_joint_trajectory_controller</exec_depend>
+ <exec_depend>velocity_controllers</exec_depend>
<export>
<metapackage/>
diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst
index 3b8de4d..5a3b52a 100644
--- a/rqt_joint_trajectory_controller/CHANGELOG.rst
+++ b/rqt_joint_trajectory_controller/CHANGELOG.rst
@@ -2,6 +2,21 @@
Changelog for package rqt_joint_trajectory_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
* Changes in import of Qt modules
diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml
index b20771d..728df72 100644
--- a/rqt_joint_trajectory_controller/package.xml
+++ b/rqt_joint_trajectory_controller/package.xml
@@ -1,22 +1,25 @@
<?xml version="1.0"?>
-<package>
+<package format="2">
<name>rqt_joint_trajectory_controller</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>Graphical frontend for interacting with joint_trajectory_controller instances.</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</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>
+ <exec_depend>control_msgs</exec_depend>
+ <exec_depend>controller_manager_msgs</exec_depend>
+ <exec_depend>trajectory_msgs</exec_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>rqt_gui</exec_depend>
+ <exec_depend>rqt_gui_py</exec_depend>
<export>
<rqt_gui plugin="${prefix}/plugin.xml"/>
diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst
index 36e026a..fb43735 100644
--- a/velocity_controllers/CHANGELOG.rst
+++ b/velocity_controllers/CHANGELOG.rst
@@ -2,6 +2,24 @@
Changelog for package velocity_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.12.3 (2017-04-23)
+-------------------
+* Supply NodeHandle to urdf::Model. Closes `#244 <https://github.com/ros-controls/ros_controllers/issues/244>`_
+* Contributors: Piyush Khandelwal
+
+0.12.2 (2017-04-21)
+-------------------
+
+0.12.1 (2017-03-08)
+-------------------
+
+0.12.0 (2017-02-15)
+-------------------
+* Change for format2
+* Add Enrique and Bence to maintainers
+* Replace boost::shared_ptr<urdf::XY> with urdf::XYConstSharedPtr when exists
+* Contributors: Bence Magyar
+
0.11.2 (2016-08-16)
-------------------
diff --git a/velocity_controllers/include/velocity_controllers/joint_position_controller.h b/velocity_controllers/include/velocity_controllers/joint_position_controller.h
index 716660b..91ab72e 100644
--- a/velocity_controllers/include/velocity_controllers/joint_position_controller.h
+++ b/velocity_controllers/include/velocity_controllers/joint_position_controller.h
@@ -174,7 +174,7 @@ public:
double getPosition();
hardware_interface::JointHandle joint_;
- boost::shared_ptr<const urdf::Joint> joint_urdf_;
+ urdf::JointConstSharedPtr joint_urdf_;
realtime_tools::RealtimeBuffer<Commands> command_;
Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml
index 338e4c4..f415277 100644
--- a/velocity_controllers/package.xml
+++ b/velocity_controllers/package.xml
@@ -1,9 +1,11 @@
-<package>
+<package format="2">
<name>velocity_controllers</name>
- <version>0.11.2</version>
+ <version>0.12.3</version>
<description>velocity_controllers</description>
<maintainer email="adolfo.rodriguez at pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
+ <maintainer email="bence.magyar.robotics at gmail.com">Bence Magyar</maintainer>
+ <maintainer email="enrique.fernandez.perdomo at gmail.com">Enrique Fernandez</maintainer>
<license>BSD</license>
@@ -15,21 +17,13 @@
<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>
+ <depend>angles</depend>
+ <depend>control_msgs</depend>
+ <depend>control_toolbox</depend>
+ <depend>controller_interface</depend>
+ <depend>forward_command_controller</depend>
+ <depend>realtime_tools</depend>
+ <depend>urdf</depend>
<export>
<controller_interface plugin="${prefix}/velocity_controllers_plugins.xml"/>
diff --git a/velocity_controllers/src/joint_position_controller.cpp b/velocity_controllers/src/joint_position_controller.cpp
index 3103158..cbe361f 100644
--- a/velocity_controllers/src/joint_position_controller.cpp
+++ b/velocity_controllers/src/joint_position_controller.cpp
@@ -81,7 +81,7 @@ bool JointPositionController::init(hardware_interface::VelocityJointInterface *r
// Get URDF info about joint
urdf::Model urdf;
- if (!urdf.initParam("robot_description"))
+ if (!urdf.initParamWithNodeHandle("robot_description", n))
{
ROS_ERROR("Failed to parse urdf file");
return false;
--
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