From f4e95dab40bd167f37f1abb4a1058794c1d72baa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 22 Jun 2021 23:14:01 +0200 Subject: [PATCH 01/13] Using joint limits structure for the admittance controller. --- .../include/joint_limits}/joint_limits.hpp | 32 +++++---- .../joint_limits}/joint_limits_rosparam.hpp | 65 +++++++++++++++---- 2 files changed, 74 insertions(+), 23 deletions(-) rename {joint_limits_interface/include/joint_limits_interface => hardware_interface/include/joint_limits}/joint_limits.hpp (61%) rename {joint_limits_interface/include/joint_limits_interface => hardware_interface/include/joint_limits}/joint_limits_rosparam.hpp (78%) diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp b/hardware_interface/include/joint_limits/joint_limits.hpp similarity index 61% rename from joint_limits_interface/include/joint_limits_interface/joint_limits.hpp rename to hardware_interface/include/joint_limits/joint_limits.hpp index ab69f75122..537d6305fb 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits.hpp +++ b/hardware_interface/include/joint_limits/joint_limits.hpp @@ -14,20 +14,22 @@ /// \author Adolfo Rodriguez Tsouroukdissian -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ +#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HPP_ -namespace joint_limits_interface +#include + +namespace joint_limits { struct JointLimits { JointLimits() - : min_position(0.0), - max_position(0.0), - max_velocity(0.0), - max_acceleration(0.0), - max_jerk(0.0), - max_effort(0.0), + : min_position(std::numeric_limits::quiet_NaN()), + max_position(std::numeric_limits::quiet_NaN()), + max_velocity(std::numeric_limits::quiet_NaN()), + max_acceleration(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()), + max_effort(std::numeric_limits::quiet_NaN()), has_position_limits(false), has_velocity_limits(false), has_acceleration_limits(false), @@ -54,7 +56,13 @@ struct JointLimits struct SoftJointLimits { - SoftJointLimits() : min_position(0.0), max_position(0.0), k_position(0.0), k_velocity(0.0) {} + SoftJointLimits() + : min_position(std::numeric_limits::quiet_NaN()), + max_position(std::numeric_limits::quiet_NaN()), + k_position(std::numeric_limits::quiet_NaN()), + k_velocity(std::numeric_limits::quiet_NaN()) + { + } double min_position; double max_position; @@ -62,6 +70,6 @@ struct SoftJointLimits double k_velocity; }; -} // namespace joint_limits_interface +} // namespace joint_limits -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_HPP_ +#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp similarity index 78% rename from joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp rename to hardware_interface/include/joint_limits/joint_limits_rosparam.hpp index 26298475ea..eeb96d48a2 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_rosparam.hpp +++ b/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp @@ -14,17 +14,60 @@ /// \author Adolfo Rodriguez Tsouroukdissian -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ - -#include - -#include +#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ +#include #include -namespace joint_limits_interface +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace joint_limits +{ +inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node) { + const std::string param_base_name = "joint_limits." + joint_name; + try + { + node->declare_parameter(param_base_name + ".has_position_limits", false); + node->declare_parameter( + param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + node->declare_parameter( + param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_velocity_limits", false); + node->declare_parameter( + param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + node->declare_parameter( + param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_acceleration_limits", false); + node->declare_parameter( + param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_jerk_limits", false); + node->declare_parameter( + param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".has_effort_limits", false); + node->declare_parameter( + param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + node->declare_parameter(param_base_name + ".angle_wraparound", false); + node->declare_parameter(param_base_name + ".has_soft_limits", false); + node->declare_parameter( + param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + node->declare_parameter( + param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + node->declare_parameter( + param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + node->declare_parameter( + param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + } + catch (const std::exception & ex) + { + RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); + return false; + } + return true; +} + /// Populate a JointLimits instance from the ROS parameter server. /** * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters @@ -58,7 +101,7 @@ namespace joint_limits_interface * existing values. Values in \p limits not specified in the parameter server remain unchanged. * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise. */ -inline bool getJointLimits( +inline bool get_joint_limits( const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) { const std::string param_base_name = "joint_limits." + joint_name; @@ -216,7 +259,7 @@ inline bool getJointLimits( * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ -inline bool getSoftJointLimits( +inline bool get_soft_joint_limits( const std::string & joint_name, const rclcpp::Node::SharedPtr & node, SoftJointLimits & soft_limits) { @@ -265,6 +308,6 @@ inline bool getSoftJointLimits( return false; } -} // namespace joint_limits_interface +} // namespace joint_limits -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_ROSPARAM_HPP_ +#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ From 694cc6a481c1f3940dd92c96a68e45d75a32ec1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 4 Oct 2021 16:43:01 +0200 Subject: [PATCH 02/13] Ros2 control extensions rolling joint limits plugins (#5) * Added initial structures for joint-limit plugins. * Move Ruckig limiter to package joint_limits_enforcement_plugins and make it working. Co-authored-by: AndyZe --- joint_limits/CMakeLists.txt | 97 ++++++++++ .../joint_limits/joint_limiter_interface.hpp | 93 +++++++++ .../include/joint_limits/joint_limits.hpp | 58 ++++++ .../joint_limits/joint_limits_rosparam.hpp | 2 +- .../joint_limits/simple_joint_limiter.hpp | 41 ++++ .../include/joint_limits/visibility_control.h | 49 +++++ joint_limits/joint_limiters.xml | 9 + joint_limits/package.xml | 26 +++ joint_limits/src/joint_limiter_interface.cpp | 72 +++++++ joint_limits/src/simple_joint_limiter.cpp | 170 ++++++++++++++++ .../test/test_simple_joint_limiter.cpp | 42 ++++ .../CMakeLists.txt | 85 ++++++++ .../visibility_control.h | 49 +++++ .../ruckig_joint_limiter.hpp | 64 ++++++ .../joint_limits_enforcement_plugins.xml | 9 + joint_limits_enforcement_plugins/package.xml | 29 +++ .../src/ruckig_joint_limiter.cpp | 182 ++++++++++++++++++ .../test/test_ruckig_joint_limiter.cpp | 44 +++++ ros2_control/package.xml | 2 + 19 files changed, 1122 insertions(+), 1 deletion(-) create mode 100644 joint_limits/CMakeLists.txt create mode 100644 joint_limits/include/joint_limits/joint_limiter_interface.hpp rename {hardware_interface => joint_limits}/include/joint_limits/joint_limits.hpp (54%) rename {hardware_interface => joint_limits}/include/joint_limits/joint_limits_rosparam.hpp (99%) create mode 100644 joint_limits/include/joint_limits/simple_joint_limiter.hpp create mode 100644 joint_limits/include/joint_limits/visibility_control.h create mode 100644 joint_limits/joint_limiters.xml create mode 100644 joint_limits/package.xml create mode 100644 joint_limits/src/joint_limiter_interface.cpp create mode 100644 joint_limits/src/simple_joint_limiter.cpp create mode 100644 joint_limits/test/test_simple_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/CMakeLists.txt create mode 100644 joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h create mode 100644 joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp create mode 100644 joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml create mode 100644 joint_limits_enforcement_plugins/package.xml create mode 100644 joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp create mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt new file mode 100644 index 0000000000..b50834b8e0 --- /dev/null +++ b/joint_limits/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_limits) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) + +add_library(joint_limiter_interface SHARED src/joint_limiter_interface.cpp) +target_include_directories( + joint_limiter_interface + PRIVATE + include) +ament_target_dependencies( + joint_limiter_interface + rclcpp +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_limiter_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +add_library( + simple_joint_limiter + SHARED + src/simple_joint_limiter.cpp +) +target_include_directories( + simple_joint_limiter + PRIVATE + include +) +target_link_libraries( + simple_joint_limiter + joint_limiter_interface +) +ament_target_dependencies( + simple_joint_limiter + rclcpp + rcutils +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(simple_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + joint_limiter_interface + simple_joint_limiter + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + #ament_add_gmock(joint_limits_test test/joint_limits_test.cpp) + #target_include_directories(joint_limits_test PUBLIC include) + #target_link_libraries(joint_limits_test joint_limits) + + ament_add_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp) + target_include_directories(test_simple_joint_limiter PRIVATE include) + target_link_libraries(test_simple_joint_limiter joint_limiter_interface) + ament_target_dependencies( + test_simple_joint_limiter + pluginlib + rclcpp + ) +endif() + +ament_export_dependencies( + rclcpp + rcutils +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + joint_limiter_interface + simple_joint_limiter +) + +ament_package() diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..0aaf3ee57e --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include + +#include "joint_limits/joint_limits.hpp" +#include "joint_limits/visibility_control.h" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_limits +{ +template +class JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; + + JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; + + /// Initialization of every JointLimiter. + /** + * Initialization of JointLimiter for defined joints with their names. + * Robot description topic provides a topic name where URDF of the robot can be found. + * This is needed to use joint limits from URDF (not implemented yet!). + * Override this method only if Initialization and reading joint limits should be adapted. + * Otherwise, initialize your custom limiter in `on_limit` method. + * + * \param[in] joint_names names of joints where limits should be applied. + * \param[in] node shared pointer to the node where joint limit parameters defined. + * \param[in] robot_description_topic string of a topic where robot description is accessible. + * + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & robot_description_topic = "/robot_description"); + + JOINT_LIMITS_PUBLIC virtual bool configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) + { + // TODO(destogl): add checks for position + return on_configure(current_joint_states); + } + + JOINT_LIMITS_PUBLIC virtual bool enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) + { + // TODO(destogl): add checks if sizes of vectors and number of limits correspond. + return on_enforce(current_joint_states, desired_joint_states, dt); + } + + // TODO(destogl): Make those protected? + // Methods that each limiter implementation has to implement + JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; } + + JOINT_LIMITS_PUBLIC virtual bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) + { + return true; + } + + JOINT_LIMITS_PUBLIC virtual bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) = 0; + +protected: + size_t number_of_joints_; + std::vector joint_limits_; + rclcpp::Node::SharedPtr node_; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/hardware_interface/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp similarity index 54% rename from hardware_interface/include/joint_limits/joint_limits.hpp rename to joint_limits/include/joint_limits/joint_limits.hpp index 537d6305fb..0ccb3e52a4 100644 --- a/hardware_interface/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -18,6 +18,8 @@ #define JOINT_LIMITS__JOINT_LIMITS_HPP_ #include +#include +#include namespace joint_limits { @@ -52,6 +54,62 @@ struct JointLimits bool has_jerk_limits; bool has_effort_limits; bool angle_wraparound; + + std::string to_string() + { + std::stringstream ss_output; + + if (has_position_limits) + { + ss_output << " position limits: " + << "[" << min_position << ", " << max_position << "]\n"; + } + if (has_velocity_limits) + { + ss_output << " velocity limit: " + << "[" << max_velocity << "]\n"; + } + if (has_acceleration_limits) + { + ss_output << " acceleration limit: " + << "[" << max_acceleration << "]\n"; + } + if (has_jerk_limits) + { + ss_output << " jerk limit: " + << "[" << max_acceleration << "]\n"; + } + if (has_effort_limits) + { + ss_output << " effort limit: " + << "[" << max_acceleration << "]\n"; + } + if (angle_wraparound) + { + ss_output << " angle wraparound is active."; + } + + return ss_output.str(); + } + + std::string debug_to_string() + { + std::stringstream ss_output; + + ss_output << " has position limits: " << (has_position_limits ? "true" : "false") << "[" + << min_position << ", " << max_position << "]\n"; + ss_output << " has velocity limits: " << (has_velocity_limits ? "true" : "false") << "[" + << max_velocity << "]\n"; + ss_output << " has acceleration limits: " << (has_acceleration_limits ? "true" : "false") + << " [" << max_acceleration << "]\n"; + ss_output << " has jerk limits: " << (has_jerk_limits ? "true" : "false") << "[" << max_jerk + << "]\n"; + ss_output << " has effort limits: " << (has_effort_limits ? "true" : "false") << "[" + << max_effort << "]\n"; + ss_output << " angle wraparound: " << (angle_wraparound ? "true" : "false"); + + return ss_output.str(); + } }; struct SoftJointLimits diff --git a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp similarity index 99% rename from hardware_interface/include/joint_limits/joint_limits_rosparam.hpp rename to joint_limits/include/joint_limits/joint_limits_rosparam.hpp index eeb96d48a2..05c7f3d67c 100644 --- a/hardware_interface/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -259,7 +259,7 @@ inline bool get_joint_limits( * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ -inline bool get_soft_joint_limits( +inline bool get_joint_limits( const std::string & joint_name, const rclcpp::Node::SharedPtr & node, SoftJointLimits & soft_limits) { diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp new file mode 100644 index 0000000000..2011e0d48e --- /dev/null +++ b/joint_limits/include/joint_limits/simple_joint_limiter.hpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ +#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" + +namespace joint_limits +{ +template +class SimpleJointLimiter : public JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC SimpleJointLimiter(); + + JOINT_LIMITS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h new file mode 100644 index 0000000000..0dcc0193a1 --- /dev/null +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_BUILDING_DLL +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT +#else +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT +#endif +#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#else +#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#endif +#define JOINT_LIMITS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml new file mode 100644 index 0000000000..929e7f538b --- /dev/null +++ b/joint_limits/joint_limiters.xml @@ -0,0 +1,9 @@ + + + + Simple joint limiter using clamping approach. + + + diff --git a/joint_limits/package.xml b/joint_limits/package.xml new file mode 100644 index 0000000000..b66a16e928 --- /dev/null +++ b/joint_limits/package.xml @@ -0,0 +1,26 @@ + + joint_limits + 0.0.0 + Package for with interfaces for handling of joint limits for use in controllers or in hardware. It also implements a simple, default joint limit strategy by clamping the values. + + Bence Magyar + Denis Štogl + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + rclcpp + pluginlib + rcutils + + ament_cmake_gmock + + + ament_cmake + + diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp new file mode 100644 index 0000000000..b0d109b0cd --- /dev/null +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include "joint_limits/joint_limiter_interface.hpp" + +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" + +// TODO(anyone): Add handing of SoftLimits +namespace joint_limits +{ +template <> +bool JointLimiterInterface::init( + const std::vector joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & /*robot_description_topic*/) +{ + number_of_joints_ = joint_names.size(); + joint_limits_.resize(number_of_joints_); + node_ = node; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + for (auto i = 0ul; i < number_of_joints_; ++i) + { + if (!declare_parameters(joint_names[i], node)) + { + RCLCPP_ERROR( + node->get_logger(), "JointLimiter: Joint '%s': parameter declaration has failed", + joint_names[i].c_str()); + result = false; + break; + } + if (!joint_limits::get_joint_limits(joint_names[i], node, joint_limits_[i])) + { + RCLCPP_ERROR( + node->get_logger(), "JointLimiter: Joint '%s': getting parameters has failed", + joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node->get_logger(), "Joint '%s':\n%s", joint_names[i].c_str(), + joint_limits_[i].to_string().c_str()); + } + + if (result) + { + result = on_init(); + } + + return result; +} + +} // namespace joint_limits diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp new file mode 100644 index 0000000000..5106615ea8 --- /dev/null +++ b/joint_limits/src/simple_joint_limiter.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Nathan Brooks, Denis Stogl + +#include "joint_limits/simple_joint_limiter.hpp" + +#include + +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops + +namespace joint_limits +{ +template <> +SimpleJointLimiter::SimpleJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool SimpleJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + auto num_joints = joint_limits_.size(); + auto dt_seconds = dt.seconds(); + + if (current_joint_states.velocities.empty()) + { + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(num_joints, 0.0); + } + + // Clamp velocities to limits + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_velocity_limits) + { + if (std::abs(desired_joint_states.velocities[index]) > joint_limits_[index].max_velocity) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed velocity limits, limiting"); + desired_joint_states.velocities[index] = + copysign(joint_limits_[index].max_velocity, desired_joint_states.velocities[index]); + double accel = + (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / + dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * accel * dt_seconds * dt_seconds; + } + } + } + + // Clamp acclerations to limits + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + double accel = + (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / + dt_seconds; + if (std::abs(accel) > joint_limits_[index].max_acceleration) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed acceleration limits, limiting"); + desired_joint_states.velocities[index] = + current_joint_states.velocities[index] + + copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds * dt_seconds; + } + } + } + + // Check that stopping distance is within joint limits + // - In joint mode, slow down only joints whose stopping distance isn't inside joint limits, + // at maximum decel + // - In Cartesian mode, slow down all joints at maximum decel if any don't have stopping distance + // within joint limits + bool position_limit_triggered = false; + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + // delta_x = (v2*v2 - v1*v1) / (2*a) + // stopping_distance = (- v1*v1) / (2*max_acceleration) + // Here we assume we will not trigger velocity limits while maximally decelerating. + // This is a valid assumption if we are not currently at a velocity limit since we are just + // coming to a rest. + double stopping_distance = std::abs( + (-desired_joint_states.velocities[index] * desired_joint_states.velocities[index]) / + (2 * joint_limits_[index].max_acceleration)); + // Check that joint limits are beyond stopping_distance and desired_velocity is towards + // that limit + // TODO(anyone): Should we consider sign on acceleration here? + if ( + (desired_joint_states.velocities[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + stopping_distance)) || + (desired_joint_states.velocities[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + stopping_distance))) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, + "Joint(s) would exceed position limits, limiting"); + position_limit_triggered = true; + + // We will limit all joints + break; + } + } + } + + if (position_limit_triggered) + { + // In Cartesian admittance mode, stop all joints if one would exceed limit + for (auto index = 0u; index < num_joints; ++index) + { + if (joint_limits_[index].has_acceleration_limits) + { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + double accel_to_stop = -current_joint_states.velocities[index] / dt_seconds; + double limited_accel = copysign( + std::min(std::abs(accel_to_stop), joint_limits_[index].max_acceleration), accel_to_stop); + + desired_joint_states.velocities[index] = + current_joint_states.velocities[index] + limited_accel * dt_seconds; + // Recompute position + desired_joint_states.positions[index] = + current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * limited_accel * dt_seconds * dt_seconds; + } + } + } + return true; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + joint_limits::SimpleJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp new file mode 100644 index 0000000000..e025ac0b9f --- /dev/null +++ b/joint_limits/test/test_simple_joint_limiter.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "joint_limits/SimpleJointLimiter"; + + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt new file mode 100644 index 0000000000..302c3562f3 --- /dev/null +++ b/joint_limits_enforcement_plugins/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.5) +project(joint_limits_enforcement_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(joint_limits REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(ruckig REQUIRED) + +add_library( + ruckig_joint_limiter + SHARED + src/ruckig_joint_limiter.cpp +) +target_include_directories( + ruckig_joint_limiter + PRIVATE + include +) +target_link_libraries( + ruckig_joint_limiter + ruckig::ruckig +) +ament_target_dependencies( + ruckig_joint_limiter + joint_limits + rclcpp + rcutils + ruckig +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ruckig_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_plugins.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + ruckig_joint_limiter + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(joint_limits REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) + + ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) + target_include_directories(test_ruckig_joint_limiter PRIVATE include) + ament_target_dependencies( + test_ruckig_joint_limiter + joint_limits + pluginlib + rclcpp + ) +endif() + +ament_export_dependencies( + joint_limits + rclcpp + rcutils + ruckig +) + +ament_export_include_directories( + include +) + +ament_export_libraries( + ruckig_joint_limiter +) + +ament_package() diff --git a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h new file mode 100644 index 0000000000..abd0019cf6 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL +#endif +#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp new file mode 100644 index 0000000000..fd577c32b3 --- /dev/null +++ b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp @@ -0,0 +1,64 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Andy Zelenak, Denis Stogl + +#ifndef RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ +#define RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ + +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_limits_enforcement_plugins/visibility_control.h" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +namespace // utility namespace +{ +constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s +constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 +constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 +} // namespace + +template +class RuckigJointLimiter : public joint_limits::JointLimiterInterface +{ +public: + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC RuckigJointLimiter(); + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_init() override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) override; + + JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & dt) override; + +private: + bool received_initial_state_ = false; + // Ruckig algorithm + std::shared_ptr> ruckig_; + std::shared_ptr> ruckig_input_; + std::shared_ptr> ruckig_output_; +}; + +} // namespace ruckig_joint_limiter + +#endif // RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ diff --git a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml new file mode 100644 index 0000000000..40b2a0de5e --- /dev/null +++ b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml @@ -0,0 +1,9 @@ + + + + Jerk-limited smoothing with the Ruckig library. + + + diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml new file mode 100644 index 0000000000..50fecd417d --- /dev/null +++ b/joint_limits_enforcement_plugins/package.xml @@ -0,0 +1,29 @@ + + joint_limits_enforcement_plugins + 0.0.0 + Package for handling of joint limits using external libraries for use in controllers or in hardware. + + Bence Magyar + Denis Štogl + Andy Zelenak + + Apache License 2.0 + + https://github.com/ros-controls/ros2_control/wiki + https://github.com/ros-controls/ros2_control/issues + https://github.com/ros-controls/ros2_control + + ament_cmake + + joint_limits + rclcpp + pluginlib + rcutils + ruckig + + ament_cmake_gmock + + + ament_cmake + + diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..ad264aed67 --- /dev/null +++ b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2021, PickNik Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \authors Andy Zelenak, Denis Stogl + +#include "ruckig_joint_limiter/ruckig_joint_limiter.hpp" + +#include +#include +#include + +#include "joint_limits/joint_limits_rosparam.hpp" +#include "rcutils/logging_macros.h" +#include "ruckig/brake.hpp" +#include "ruckig/input_parameter.hpp" +#include "ruckig/output_parameter.hpp" +#include "ruckig/ruckig.hpp" + +namespace ruckig_joint_limiter +{ +template <> +RuckigJointLimiter::RuckigJointLimiter() +: joint_limits::JointLimiterInterface() +{ +} + +template <> +bool RuckigJointLimiter::on_init(/*const rclcpp::Duration & dt*/) +{ + // TODO(destogl): This should be used from parameter + const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005); + + // Initialize Ruckig + ruckig_ = std::make_shared>(number_of_joints_, dt.seconds()); + ruckig_input_ = std::make_shared>(number_of_joints_); + ruckig_output_ = std::make_shared>(number_of_joints_); + + // Velocity mode works best for smoothing one waypoint at a time + ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; + ruckig_input_->synchronization = ruckig::Synchronization::Time; + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + if (joint_limits_[joint].has_jerk_limits) + { + ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; + } + else + { + ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK; + } + if (joint_limits_[joint].has_acceleration_limits) + { + ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; + } + else + { + ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; + } + if (joint_limits_[joint].has_velocity_limits) + { + ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; + } + else + { + ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY; + } + } + + return true; +} + +template <> +bool RuckigJointLimiter::on_configure( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) +{ + // Initialize Ruckig with current_joint_states + ruckig_input_->current_position = current_joint_states.positions; + + if (current_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->current_velocity = current_joint_states.velocities; + } + else + { + ruckig_input_->current_velocity = std::vector(number_of_joints_, 0.0); + } + if (current_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->current_acceleration = current_joint_states.accelerations; + } + else + { + ruckig_input_->current_acceleration = std::vector(number_of_joints_, 0.0); + } + + // Initialize output data + ruckig_output_->new_position = ruckig_input_->current_position; + ruckig_output_->new_velocity = ruckig_input_->current_velocity; + ruckig_output_->new_acceleration = ruckig_input_->current_acceleration; + + return true; +} + +template <> +bool RuckigJointLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, + const rclcpp::Duration & /*dt*/) +{ + // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig + // output back in as input for the next iteration. This assumes the robot tracks the command well. + ruckig_input_->current_position = ruckig_output_->new_position; + ruckig_input_->current_velocity = ruckig_output_->new_velocity; + ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; + + // Target state is the next waypoint + ruckig_input_->target_position = desired_joint_states.positions; + // TODO(destogl): in current use-case we use only velocity + if (desired_joint_states.velocities.size() == number_of_joints_) + { + ruckig_input_->target_velocity = desired_joint_states.velocities; + } + else + { + ruckig_input_->target_velocity = std::vector(number_of_joints_, 0.0); + } + if (desired_joint_states.accelerations.size() == number_of_joints_) + { + ruckig_input_->target_acceleration = desired_joint_states.accelerations; + } + else + { + ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); + } + + ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); + RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); + + desired_joint_states.positions = ruckig_output_->new_position; + desired_joint_states.velocities = ruckig_output_->new_velocity; + desired_joint_states.accelerations = ruckig_output_->new_acceleration; + + // Feed output from the previous timestep back as input + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", + "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.", + ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint), + ruckig_input_->target_acceleration.at(joint)); + } + + for (auto joint = 0ul; joint < number_of_joints_; ++joint) + { + RCUTILS_LOG_DEBUG_NAMED( + "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.", + ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint), + ruckig_output_->new_acceleration.at(joint)); + } + + return result == ruckig::Result::Finished; +} + +} // namespace ruckig_joint_limiter + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ruckig_joint_limiter::RuckigJointLimiter, + joint_limits::JointLimiterInterface) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp new file mode 100644 index 0000000000..b1b19d0587 --- /dev/null +++ b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Denis Stogl + +#include + +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/executor.hpp" + +TEST(TestLoadSimpleJointLimiter, load_limiter) +{ + rclcpp::init(0, nullptr); + + using JointLimiter = joint_limits::JointLimiterInterface; + pluginlib::ClassLoader joint_limiter_loader( + "joint_limits", "joint_limits::JointLimiterInterface"); + + std::unique_ptr joint_limiter; + std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; + + joint_limiter = + std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); + ASSERT_NO_THROW( + joint_limiter = std::unique_ptr( + joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); + ASSERT_NE(joint_limiter, nullptr); +} diff --git a/ros2_control/package.xml b/ros2_control/package.xml index bb3078e65a..25585bc7db 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -14,6 +14,8 @@ controller_manager controller_manager_msgs hardware_interface + joint_limits + ros2controlcli ros2_control_test_assets ros2controlcli transmission_interface From 12aed7d45830fd7bc9108330e4e68bc9158593f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 20 Apr 2022 18:46:36 +0200 Subject: [PATCH 03/13] Use auto_declare for parameters to avoid crashing. --- .../joint_limits/joint_limits_rosparam.hpp | 82 ++++++++++++------- 1 file changed, 53 insertions(+), 29 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 05c7f3d67c..844de38366 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -23,6 +23,30 @@ #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" +namespace // utilities +{ +/// Declare and initialize a parameter with a type. +/** + * + * Wrapper function for templated node's declare_parameter() which checks if + * parameter is already declared. + * For use in all components that inherit from ControllerInterface + */ +template +auto auto_declare( + const rclcpp::Node::SharedPtr & node, const std::string & name, const ParameterT & default_value) +{ + if (!node->has_parameter(name)) + { + return node->declare_parameter(name, default_value); + } + else + { + return node->get_parameter(name).get_value(); + } +} +} // namespace + namespace joint_limits { inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node) @@ -30,35 +54,35 @@ inline bool declare_parameters(const std::string & joint_name, const rclcpp::Nod const std::string param_base_name = "joint_limits." + joint_name; try { - node->declare_parameter(param_base_name + ".has_position_limits", false); - node->declare_parameter( - param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); - node->declare_parameter( - param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_velocity_limits", false); - node->declare_parameter( - param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); - node->declare_parameter( - param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_acceleration_limits", false); - node->declare_parameter( - param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_jerk_limits", false); - node->declare_parameter( - param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".has_effort_limits", false); - node->declare_parameter( - param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); - node->declare_parameter(param_base_name + ".angle_wraparound", false); - node->declare_parameter(param_base_name + ".has_soft_limits", false); - node->declare_parameter( - param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); - node->declare_parameter( - param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); - node->declare_parameter( - param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); - node->declare_parameter( - param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + auto_declare(node, param_base_name + ".has_position_limits", false); + auto_declare( + node, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + auto_declare( + node, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + auto_declare(node, param_base_name + ".has_velocity_limits", false); + auto_declare( + node, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + auto_declare( + node, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + auto_declare(node, param_base_name + ".has_acceleration_limits", false); + auto_declare( + node, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + auto_declare(node, param_base_name + ".has_jerk_limits", false); + auto_declare( + node, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + auto_declare(node, param_base_name + ".has_effort_limits", false); + auto_declare( + node, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + auto_declare(node, param_base_name + ".angle_wraparound", false); + auto_declare(node, param_base_name + ".has_soft_limits", false); + auto_declare( + node, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + auto_declare( + node, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + auto_declare( + node, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + auto_declare( + node, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); } catch (const std::exception & ex) { From d0ea4c5433b7d7162b4375d8c345269352b9b5d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 20 Apr 2022 18:54:46 +0200 Subject: [PATCH 04/13] Clean joint_limits package and copy tests into it. They are not working yet. --- joint_limits/CMakeLists.txt | 92 +++------ .../joint_limits/joint_limiter_interface.hpp | 93 --------- .../joint_limits/simple_joint_limiter.hpp | 41 ---- .../include/joint_limits/visibility_control.h | 49 ----- joint_limits/joint_limiters.xml | 9 - joint_limits/package.xml | 2 - joint_limits/src/joint_limiter_interface.cpp | 72 ------- joint_limits/src/simple_joint_limiter.cpp | 170 ---------------- .../test/joint_limits_interface_test.cpp | 0 .../test/joint_limits_rosparam.launch.py | 0 .../test/joint_limits_rosparam.yaml | 0 .../test/joint_limits_rosparam_test.cpp | 0 .../test/test_simple_joint_limiter.cpp | 42 ---- .../CMakeLists.txt | 85 -------- .../visibility_control.h | 49 ----- .../ruckig_joint_limiter.hpp | 64 ------ .../joint_limits_enforcement_plugins.xml | 9 - joint_limits_enforcement_plugins/package.xml | 29 --- .../src/ruckig_joint_limiter.cpp | 182 ------------------ .../test/test_ruckig_joint_limiter.cpp | 44 ----- 20 files changed, 22 insertions(+), 1010 deletions(-) delete mode 100644 joint_limits/include/joint_limits/joint_limiter_interface.hpp delete mode 100644 joint_limits/include/joint_limits/simple_joint_limiter.hpp delete mode 100644 joint_limits/include/joint_limits/visibility_control.h delete mode 100644 joint_limits/joint_limiters.xml delete mode 100644 joint_limits/src/joint_limiter_interface.cpp delete mode 100644 joint_limits/src/simple_joint_limiter.cpp rename {joint_limits_interface => joint_limits}/test/joint_limits_interface_test.cpp (100%) rename {joint_limits_interface => joint_limits}/test/joint_limits_rosparam.launch.py (100%) rename {joint_limits_interface => joint_limits}/test/joint_limits_rosparam.yaml (100%) rename {joint_limits_interface => joint_limits}/test/joint_limits_rosparam_test.cpp (100%) delete mode 100644 joint_limits/test/test_simple_joint_limiter.cpp delete mode 100644 joint_limits_enforcement_plugins/CMakeLists.txt delete mode 100644 joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h delete mode 100644 joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp delete mode 100644 joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml delete mode 100644 joint_limits_enforcement_plugins/package.xml delete mode 100644 joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp delete mode 100644 joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index b50834b8e0..d16070498c 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -6,92 +6,44 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(rcutils REQUIRED) - -add_library(joint_limiter_interface SHARED src/joint_limiter_interface.cpp) -target_include_directories( - joint_limiter_interface - PRIVATE - include) -ament_target_dependencies( - joint_limiter_interface - rclcpp -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(joint_limiter_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") - -add_library( - simple_joint_limiter - SHARED - src/simple_joint_limiter.cpp -) -target_include_directories( - simple_joint_limiter - PRIVATE - include -) -target_link_libraries( - simple_joint_limiter - joint_limiter_interface -) -ament_target_dependencies( - simple_joint_limiter - rclcpp - rcutils -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(simple_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") - -pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) install(DIRECTORY include/ DESTINATION include ) -install( - TARGETS - joint_limiter_interface - simple_joint_limiter - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(pluginlib REQUIRED) - find_package(rclcpp REQUIRED) - - #ament_add_gmock(joint_limits_test test/joint_limits_test.cpp) - #target_include_directories(joint_limits_test PUBLIC include) - #target_link_libraries(joint_limits_test joint_limits) + find_package(ament_cmake_gtest REQUIRED) + find_package(rclcpp) + + #ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) + #target_include_directories(joint_limits_interface_test PUBLIC include) + #ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) + + #add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) + #target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) + #target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) + #ament_target_dependencies(joint_limits_rosparam_test rclcpp) + #add_launch_test(test/joint_limits_rosparam.launch.py) + #install( + #TARGETS + #joint_limits_rosparam_test + #DESTINATION lib/${PROJECT_NAME} + #) + #install( + #FILES + #test/joint_limits_rosparam.yaml + #DESTINATION share/${PROJECT_NAME}/test + #) - ament_add_gmock(test_simple_joint_limiter test/test_simple_joint_limiter.cpp) - target_include_directories(test_simple_joint_limiter PRIVATE include) - target_link_libraries(test_simple_joint_limiter joint_limiter_interface) - ament_target_dependencies( - test_simple_joint_limiter - pluginlib - rclcpp - ) endif() ament_export_dependencies( rclcpp - rcutils ) ament_export_include_directories( include ) -ament_export_libraries( - joint_limiter_interface - simple_joint_limiter -) - ament_package() diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp deleted file mode 100644 index 0aaf3ee57e..0000000000 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Denis Stogl - -#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ -#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ - -#include -#include - -#include "joint_limits/joint_limits.hpp" -#include "joint_limits/visibility_control.h" -#include "rclcpp/node.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" - -namespace joint_limits -{ -template -class JointLimiterInterface -{ -public: - JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; - - JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; - - /// Initialization of every JointLimiter. - /** - * Initialization of JointLimiter for defined joints with their names. - * Robot description topic provides a topic name where URDF of the robot can be found. - * This is needed to use joint limits from URDF (not implemented yet!). - * Override this method only if Initialization and reading joint limits should be adapted. - * Otherwise, initialize your custom limiter in `on_limit` method. - * - * \param[in] joint_names names of joints where limits should be applied. - * \param[in] node shared pointer to the node where joint limit parameters defined. - * \param[in] robot_description_topic string of a topic where robot description is accessible. - * - */ - JOINT_LIMITS_PUBLIC virtual bool init( - const std::vector joint_names, const rclcpp::Node::SharedPtr & node, - const std::string & robot_description_topic = "/robot_description"); - - JOINT_LIMITS_PUBLIC virtual bool configure( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) - { - // TODO(destogl): add checks for position - return on_configure(current_joint_states); - } - - JOINT_LIMITS_PUBLIC virtual bool enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) - { - // TODO(destogl): add checks if sizes of vectors and number of limits correspond. - return on_enforce(current_joint_states, desired_joint_states, dt); - } - - // TODO(destogl): Make those protected? - // Methods that each limiter implementation has to implement - JOINT_LIMITS_PUBLIC virtual bool on_init() { return true; } - - JOINT_LIMITS_PUBLIC virtual bool on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) - { - return true; - } - - JOINT_LIMITS_PUBLIC virtual bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & dt) = 0; - -protected: - size_t number_of_joints_; - std::vector joint_limits_; - rclcpp::Node::SharedPtr node_; -}; - -} // namespace joint_limits - -#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/joint_limits/include/joint_limits/simple_joint_limiter.hpp b/joint_limits/include/joint_limits/simple_joint_limiter.hpp deleted file mode 100644 index 2011e0d48e..0000000000 --- a/joint_limits/include/joint_limits/simple_joint_limiter.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) 2021, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Denis Stogl - -#ifndef JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ -#define JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ - -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" - -namespace joint_limits -{ -template -class SimpleJointLimiter : public JointLimiterInterface -{ -public: - JOINT_LIMITS_PUBLIC SimpleJointLimiter(); - - JOINT_LIMITS_PUBLIC bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & dt) override; -}; - -} // namespace joint_limits - -#endif // JOINT_LIMITS__SIMPLE_JOINT_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h deleted file mode 100644 index 0dcc0193a1..0000000000 --- a/joint_limits/include/joint_limits/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_ -#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) -#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) -#else -#define JOINT_LIMITS_EXPORT __declspec(dllexport) -#define JOINT_LIMITS_IMPORT __declspec(dllimport) -#endif -#ifdef JOINT_LIMITS_BUILDING_DLL -#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT -#else -#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT -#endif -#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC -#define JOINT_LIMITS_LOCAL -#else -#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) -#define JOINT_LIMITS_IMPORT -#if __GNUC__ >= 4 -#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) -#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) -#else -#define JOINT_LIMITS_PUBLIC -#define JOINT_LIMITS_LOCAL -#endif -#define JOINT_LIMITS_PUBLIC_TYPE -#endif - -#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml deleted file mode 100644 index 929e7f538b..0000000000 --- a/joint_limits/joint_limiters.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Simple joint limiter using clamping approach. - - - diff --git a/joint_limits/package.xml b/joint_limits/package.xml index b66a16e928..f0d068f2bd 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -15,8 +15,6 @@ ament_cmake rclcpp - pluginlib - rcutils ament_cmake_gmock diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp deleted file mode 100644 index b0d109b0cd..0000000000 --- a/joint_limits/src/joint_limiter_interface.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Denis Stogl - -#include "joint_limits/joint_limiter_interface.hpp" - -#include -#include - -#include "joint_limits/joint_limits_rosparam.hpp" - -// TODO(anyone): Add handing of SoftLimits -namespace joint_limits -{ -template <> -bool JointLimiterInterface::init( - const std::vector joint_names, const rclcpp::Node::SharedPtr & node, - const std::string & /*robot_description_topic*/) -{ - number_of_joints_ = joint_names.size(); - joint_limits_.resize(number_of_joints_); - node_ = node; - - bool result = true; - - // TODO(destogl): get limits from URDF - - // Initialize and get joint limits from parameter server - for (auto i = 0ul; i < number_of_joints_; ++i) - { - if (!declare_parameters(joint_names[i], node)) - { - RCLCPP_ERROR( - node->get_logger(), "JointLimiter: Joint '%s': parameter declaration has failed", - joint_names[i].c_str()); - result = false; - break; - } - if (!joint_limits::get_joint_limits(joint_names[i], node, joint_limits_[i])) - { - RCLCPP_ERROR( - node->get_logger(), "JointLimiter: Joint '%s': getting parameters has failed", - joint_names[i].c_str()); - result = false; - break; - } - RCLCPP_INFO( - node->get_logger(), "Joint '%s':\n%s", joint_names[i].c_str(), - joint_limits_[i].to_string().c_str()); - } - - if (result) - { - result = on_init(); - } - - return result; -} - -} // namespace joint_limits diff --git a/joint_limits/src/simple_joint_limiter.cpp b/joint_limits/src/simple_joint_limiter.cpp deleted file mode 100644 index 5106615ea8..0000000000 --- a/joint_limits/src/simple_joint_limiter.cpp +++ /dev/null @@ -1,170 +0,0 @@ -// Copyright (c) 2021, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \authors Nathan Brooks, Denis Stogl - -#include "joint_limits/simple_joint_limiter.hpp" - -#include - -#include "rclcpp/duration.hpp" -#include "rcutils/logging_macros.h" - -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops - -namespace joint_limits -{ -template <> -SimpleJointLimiter::SimpleJointLimiter() -: joint_limits::JointLimiterInterface() -{ -} - -template <> -bool SimpleJointLimiter::on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) -{ - auto num_joints = joint_limits_.size(); - auto dt_seconds = dt.seconds(); - - if (current_joint_states.velocities.empty()) - { - // First update() after activating does not have velocity available, assume 0 - current_joint_states.velocities.resize(num_joints, 0.0); - } - - // Clamp velocities to limits - for (auto index = 0u; index < num_joints; ++index) - { - if (joint_limits_[index].has_velocity_limits) - { - if (std::abs(desired_joint_states.velocities[index]) > joint_limits_[index].max_velocity) - { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed velocity limits, limiting"); - desired_joint_states.velocities[index] = - copysign(joint_limits_[index].max_velocity, desired_joint_states.velocities[index]); - double accel = - (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / - dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * accel * dt_seconds * dt_seconds; - } - } - } - - // Clamp acclerations to limits - for (auto index = 0u; index < num_joints; ++index) - { - if (joint_limits_[index].has_acceleration_limits) - { - double accel = - (desired_joint_states.velocities[index] - current_joint_states.velocities[index]) / - dt_seconds; - if (std::abs(accel) > joint_limits_[index].max_acceleration) - { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed acceleration limits, limiting"); - desired_joint_states.velocities[index] = - current_joint_states.velocities[index] + - copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * copysign(joint_limits_[index].max_acceleration, accel) * dt_seconds * dt_seconds; - } - } - } - - // Check that stopping distance is within joint limits - // - In joint mode, slow down only joints whose stopping distance isn't inside joint limits, - // at maximum decel - // - In Cartesian mode, slow down all joints at maximum decel if any don't have stopping distance - // within joint limits - bool position_limit_triggered = false; - for (auto index = 0u; index < num_joints; ++index) - { - if (joint_limits_[index].has_acceleration_limits) - { - // delta_x = (v2*v2 - v1*v1) / (2*a) - // stopping_distance = (- v1*v1) / (2*max_acceleration) - // Here we assume we will not trigger velocity limits while maximally decelerating. - // This is a valid assumption if we are not currently at a velocity limit since we are just - // coming to a rest. - double stopping_distance = std::abs( - (-desired_joint_states.velocities[index] * desired_joint_states.velocities[index]) / - (2 * joint_limits_[index].max_acceleration)); - // Check that joint limits are beyond stopping_distance and desired_velocity is towards - // that limit - // TODO(anyone): Should we consider sign on acceleration here? - if ( - (desired_joint_states.velocities[index] < 0 && - (current_joint_states.positions[index] - joint_limits_[index].min_position < - stopping_distance)) || - (desired_joint_states.velocities[index] > 0 && - (joint_limits_[index].max_position - current_joint_states.positions[index] < - stopping_distance))) - { - RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joint(s) would exceed position limits, limiting"); - position_limit_triggered = true; - - // We will limit all joints - break; - } - } - } - - if (position_limit_triggered) - { - // In Cartesian admittance mode, stop all joints if one would exceed limit - for (auto index = 0u; index < num_joints; ++index) - { - if (joint_limits_[index].has_acceleration_limits) - { - // Compute accel to stop - // Here we aren't explicitly maximally decelerating, but for joints near their limits this - // should still result in max decel being used - double accel_to_stop = -current_joint_states.velocities[index] / dt_seconds; - double limited_accel = copysign( - std::min(std::abs(accel_to_stop), joint_limits_[index].max_acceleration), accel_to_stop); - - desired_joint_states.velocities[index] = - current_joint_states.velocities[index] + limited_accel * dt_seconds; - // Recompute position - desired_joint_states.positions[index] = - current_joint_states.positions[index] + - current_joint_states.velocities[index] * dt_seconds + - 0.5 * limited_accel * dt_seconds * dt_seconds; - } - } - } - return true; -} - -} // namespace joint_limits - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - joint_limits::SimpleJointLimiter, - joint_limits::JointLimiterInterface) diff --git a/joint_limits_interface/test/joint_limits_interface_test.cpp b/joint_limits/test/joint_limits_interface_test.cpp similarity index 100% rename from joint_limits_interface/test/joint_limits_interface_test.cpp rename to joint_limits/test/joint_limits_interface_test.cpp diff --git a/joint_limits_interface/test/joint_limits_rosparam.launch.py b/joint_limits/test/joint_limits_rosparam.launch.py similarity index 100% rename from joint_limits_interface/test/joint_limits_rosparam.launch.py rename to joint_limits/test/joint_limits_rosparam.launch.py diff --git a/joint_limits_interface/test/joint_limits_rosparam.yaml b/joint_limits/test/joint_limits_rosparam.yaml similarity index 100% rename from joint_limits_interface/test/joint_limits_rosparam.yaml rename to joint_limits/test/joint_limits_rosparam.yaml diff --git a/joint_limits_interface/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp similarity index 100% rename from joint_limits_interface/test/joint_limits_rosparam_test.cpp rename to joint_limits/test/joint_limits_rosparam_test.cpp diff --git a/joint_limits/test/test_simple_joint_limiter.cpp b/joint_limits/test/test_simple_joint_limiter.cpp deleted file mode 100644 index e025ac0b9f..0000000000 --- a/joint_limits/test/test_simple_joint_limiter.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Denis Stogl - -#include - -#include -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "pluginlib/class_loader.hpp" -#include "rclcpp/executor.hpp" - -TEST(TestLoadSimpleJointLimiter, load_limiter) -{ - rclcpp::init(0, nullptr); - - using JointLimiter = joint_limits::JointLimiterInterface; - pluginlib::ClassLoader joint_limiter_loader( - "joint_limits", "joint_limits::JointLimiterInterface"); - - std::unique_ptr joint_limiter; - std::string joint_limiter_type = "joint_limits/SimpleJointLimiter"; - - ASSERT_NO_THROW( - joint_limiter = std::unique_ptr( - joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); - ASSERT_NE(joint_limiter, nullptr); -} diff --git a/joint_limits_enforcement_plugins/CMakeLists.txt b/joint_limits_enforcement_plugins/CMakeLists.txt deleted file mode 100644 index 302c3562f3..0000000000 --- a/joint_limits_enforcement_plugins/CMakeLists.txt +++ /dev/null @@ -1,85 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_limits_enforcement_plugins) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) -endif() - -find_package(ament_cmake REQUIRED) -find_package(joint_limits REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rcutils REQUIRED) -find_package(ruckig REQUIRED) - -add_library( - ruckig_joint_limiter - SHARED - src/ruckig_joint_limiter.cpp -) -target_include_directories( - ruckig_joint_limiter - PRIVATE - include -) -target_link_libraries( - ruckig_joint_limiter - ruckig::ruckig -) -ament_target_dependencies( - ruckig_joint_limiter - joint_limits - rclcpp - rcutils - ruckig -) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(ruckig_joint_limiter PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") - -pluginlib_export_plugin_description_file(joint_limits joint_limits_enforcement_plugins.xml) - -install(DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - ruckig_joint_limiter - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(joint_limits REQUIRED) - find_package(pluginlib REQUIRED) - find_package(rclcpp REQUIRED) - - ament_add_gmock(test_ruckig_joint_limiter test/test_ruckig_joint_limiter.cpp) - target_include_directories(test_ruckig_joint_limiter PRIVATE include) - ament_target_dependencies( - test_ruckig_joint_limiter - joint_limits - pluginlib - rclcpp - ) -endif() - -ament_export_dependencies( - joint_limits - rclcpp - rcutils - ruckig -) - -ament_export_include_directories( - include -) - -ament_export_libraries( - ruckig_joint_limiter -) - -ament_package() diff --git a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h b/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h deleted file mode 100644 index abd0019cf6..0000000000 --- a/joint_limits_enforcement_plugins/include/joint_limits_enforcement_plugins/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((dllexport)) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __attribute__((dllimport)) -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __declspec(dllexport) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT __declspec(dllimport) -#endif -#ifdef JOINT_LIMITS_ENFORCEMENT_PLUGINS_BUILDING_DLL -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT -#endif -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_EXPORT __attribute__((visibility("default"))) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_IMPORT -#if __GNUC__ >= 4 -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC __attribute__((visibility("default"))) -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL __attribute__((visibility("hidden"))) -#else -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_LOCAL -#endif -#define JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC_TYPE -#endif - -#endif // JOINT_LIMITS_ENFORCEMENT_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp b/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp deleted file mode 100644 index fd577c32b3..0000000000 --- a/joint_limits_enforcement_plugins/include/ruckig_joint_limiter/ruckig_joint_limiter.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) 2021, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Andy Zelenak, Denis Stogl - -#ifndef RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ -#define RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ - -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "joint_limits_enforcement_plugins/visibility_control.h" -#include "ruckig/input_parameter.hpp" -#include "ruckig/output_parameter.hpp" -#include "ruckig/ruckig.hpp" - -namespace ruckig_joint_limiter -{ -namespace // utility namespace -{ -constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s -constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 -constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3 -} // namespace - -template -class RuckigJointLimiter : public joint_limits::JointLimiterInterface -{ -public: - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC RuckigJointLimiter(); - - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_init() override; - - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) override; - - JOINT_LIMITS_ENFORCEMENT_PLUGINS_PUBLIC bool on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & dt) override; - -private: - bool received_initial_state_ = false; - // Ruckig algorithm - std::shared_ptr> ruckig_; - std::shared_ptr> ruckig_input_; - std::shared_ptr> ruckig_output_; -}; - -} // namespace ruckig_joint_limiter - -#endif // RUCKIG_JOINT_LIMITER__RUCKIG_JOINT_LIMITER_HPP_ diff --git a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml b/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml deleted file mode 100644 index 40b2a0de5e..0000000000 --- a/joint_limits_enforcement_plugins/joint_limits_enforcement_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Jerk-limited smoothing with the Ruckig library. - - - diff --git a/joint_limits_enforcement_plugins/package.xml b/joint_limits_enforcement_plugins/package.xml deleted file mode 100644 index 50fecd417d..0000000000 --- a/joint_limits_enforcement_plugins/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - joint_limits_enforcement_plugins - 0.0.0 - Package for handling of joint limits using external libraries for use in controllers or in hardware. - - Bence Magyar - Denis Štogl - Andy Zelenak - - Apache License 2.0 - - https://github.com/ros-controls/ros2_control/wiki - https://github.com/ros-controls/ros2_control/issues - https://github.com/ros-controls/ros2_control - - ament_cmake - - joint_limits - rclcpp - pluginlib - rcutils - ruckig - - ament_cmake_gmock - - - ament_cmake - - diff --git a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp deleted file mode 100644 index ad264aed67..0000000000 --- a/joint_limits_enforcement_plugins/src/ruckig_joint_limiter.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright (c) 2021, PickNik Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \authors Andy Zelenak, Denis Stogl - -#include "ruckig_joint_limiter/ruckig_joint_limiter.hpp" - -#include -#include -#include - -#include "joint_limits/joint_limits_rosparam.hpp" -#include "rcutils/logging_macros.h" -#include "ruckig/brake.hpp" -#include "ruckig/input_parameter.hpp" -#include "ruckig/output_parameter.hpp" -#include "ruckig/ruckig.hpp" - -namespace ruckig_joint_limiter -{ -template <> -RuckigJointLimiter::RuckigJointLimiter() -: joint_limits::JointLimiterInterface() -{ -} - -template <> -bool RuckigJointLimiter::on_init(/*const rclcpp::Duration & dt*/) -{ - // TODO(destogl): This should be used from parameter - const rclcpp::Duration dt = rclcpp::Duration::from_seconds(0.005); - - // Initialize Ruckig - ruckig_ = std::make_shared>(number_of_joints_, dt.seconds()); - ruckig_input_ = std::make_shared>(number_of_joints_); - ruckig_output_ = std::make_shared>(number_of_joints_); - - // Velocity mode works best for smoothing one waypoint at a time - ruckig_input_->control_interface = ruckig::ControlInterface::Velocity; - ruckig_input_->synchronization = ruckig::Synchronization::Time; - - for (auto joint = 0ul; joint < number_of_joints_; ++joint) - { - if (joint_limits_[joint].has_jerk_limits) - { - ruckig_input_->max_jerk.at(joint) = joint_limits_[joint].max_acceleration; - } - else - { - ruckig_input_->max_jerk.at(joint) = DEFAULT_MAX_JERK; - } - if (joint_limits_[joint].has_acceleration_limits) - { - ruckig_input_->max_acceleration.at(joint) = joint_limits_[joint].max_acceleration; - } - else - { - ruckig_input_->max_acceleration.at(joint) = DEFAULT_MAX_ACCELERATION; - } - if (joint_limits_[joint].has_velocity_limits) - { - ruckig_input_->max_velocity.at(joint) = joint_limits_[joint].max_velocity; - } - else - { - ruckig_input_->max_velocity.at(joint) = DEFAULT_MAX_VELOCITY; - } - } - - return true; -} - -template <> -bool RuckigJointLimiter::on_configure( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) -{ - // Initialize Ruckig with current_joint_states - ruckig_input_->current_position = current_joint_states.positions; - - if (current_joint_states.velocities.size() == number_of_joints_) - { - ruckig_input_->current_velocity = current_joint_states.velocities; - } - else - { - ruckig_input_->current_velocity = std::vector(number_of_joints_, 0.0); - } - if (current_joint_states.accelerations.size() == number_of_joints_) - { - ruckig_input_->current_acceleration = current_joint_states.accelerations; - } - else - { - ruckig_input_->current_acceleration = std::vector(number_of_joints_, 0.0); - } - - // Initialize output data - ruckig_output_->new_position = ruckig_input_->current_position; - ruckig_output_->new_velocity = ruckig_input_->current_velocity; - ruckig_output_->new_acceleration = ruckig_input_->current_acceleration; - - return true; -} - -template <> -bool RuckigJointLimiter::on_enforce( - trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/, - trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, - const rclcpp::Duration & /*dt*/) -{ - // We don't use current_joint_states. For stability, it is recommended to feed previous Ruckig - // output back in as input for the next iteration. This assumes the robot tracks the command well. - ruckig_input_->current_position = ruckig_output_->new_position; - ruckig_input_->current_velocity = ruckig_output_->new_velocity; - ruckig_input_->current_acceleration = ruckig_output_->new_acceleration; - - // Target state is the next waypoint - ruckig_input_->target_position = desired_joint_states.positions; - // TODO(destogl): in current use-case we use only velocity - if (desired_joint_states.velocities.size() == number_of_joints_) - { - ruckig_input_->target_velocity = desired_joint_states.velocities; - } - else - { - ruckig_input_->target_velocity = std::vector(number_of_joints_, 0.0); - } - if (desired_joint_states.accelerations.size() == number_of_joints_) - { - ruckig_input_->target_acceleration = desired_joint_states.accelerations; - } - else - { - ruckig_input_->target_acceleration = std::vector(number_of_joints_, 0.0); - } - - ruckig::Result result = ruckig_->update(*ruckig_input_, *ruckig_output_); - RCUTILS_LOG_DEBUG_NAMED("ruckig_joint_limiter", "Rucking result %d", result); - - desired_joint_states.positions = ruckig_output_->new_position; - desired_joint_states.velocities = ruckig_output_->new_velocity; - desired_joint_states.accelerations = ruckig_output_->new_acceleration; - - // Feed output from the previous timestep back as input - for (auto joint = 0ul; joint < number_of_joints_; ++joint) - { - RCUTILS_LOG_DEBUG_NAMED( - "ruckig_joint_limiter", - "Desired position: %e \nDesired velocity: %e\n Desired acceleration: %e.", - ruckig_input_->target_position.at(joint), ruckig_input_->target_velocity.at(joint), - ruckig_input_->target_acceleration.at(joint)); - } - - for (auto joint = 0ul; joint < number_of_joints_; ++joint) - { - RCUTILS_LOG_DEBUG_NAMED( - "ruckig_joint_limiter", "New position: %e \nNew velocity: %e\nNew acceleration: %e.", - ruckig_output_->new_position.at(joint), ruckig_output_->new_velocity.at(joint), - ruckig_output_->new_acceleration.at(joint)); - } - - return result == ruckig::Result::Finished; -} - -} // namespace ruckig_joint_limiter - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ruckig_joint_limiter::RuckigJointLimiter, - joint_limits::JointLimiterInterface) diff --git a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp b/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp deleted file mode 100644 index b1b19d0587..0000000000 --- a/joint_limits_enforcement_plugins/test/test_ruckig_joint_limiter.cpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Denis Stogl - -#include - -#include -#include - -#include "joint_limits/joint_limiter_interface.hpp" -#include "joint_limits/joint_limits.hpp" -#include "pluginlib/class_loader.hpp" -#include "rclcpp/executor.hpp" - -TEST(TestLoadSimpleJointLimiter, load_limiter) -{ - rclcpp::init(0, nullptr); - - using JointLimiter = joint_limits::JointLimiterInterface; - pluginlib::ClassLoader joint_limiter_loader( - "joint_limits", "joint_limits::JointLimiterInterface"); - - std::unique_ptr joint_limiter; - std::string joint_limiter_type = "ruckig_joint_limiter/RuckigJointLimiter"; - - joint_limiter = - std::unique_ptr(joint_limiter_loader.createUnmanagedInstance(joint_limiter_type)); - ASSERT_NO_THROW( - joint_limiter = std::unique_ptr( - joint_limiter_loader.createUnmanagedInstance(joint_limiter_type))); - ASSERT_NE(joint_limiter, nullptr); -} From eb6c7fcdf4502e2eedd43cc3048906de8deb9bda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 4 May 2022 15:05:14 +0200 Subject: [PATCH 05/13] Update joint_limits/package.xml Co-authored-by: AndyZe --- joint_limits/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_limits/package.xml b/joint_limits/package.xml index f0d068f2bd..9095c15011 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,7 +1,7 @@ joint_limits 0.0.0 - Package for with interfaces for handling of joint limits for use in controllers or in hardware. It also implements a simple, default joint limit strategy by clamping the values. + Interfaces for handling of joint limits for controllers or hardware. Bence Magyar Denis Štogl From 2e0e5bdd798b2df6f106c7328f21ad809b9a9b21 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 4 May 2022 16:22:37 +0200 Subject: [PATCH 06/13] Final updates to according to reviews and trying to re-enable tests. --- joint_limits/CMakeLists.txt | 40 +- .../include/joint_limits/joint_limits.hpp | 34 +- .../joint_limits/joint_limits_rosparam.hpp | 5 +- joint_limits/package.xml | 2 +- .../test/joint_limits_interface_test.cpp | 627 ------------------ .../test/joint_limits_rosparam_test.cpp | 68 +- 6 files changed, 92 insertions(+), 684 deletions(-) delete mode 100644 joint_limits/test/joint_limits_interface_test.cpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index d16070498c..dd698b632e 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -1,6 +1,11 @@ cmake_minimum_required(VERSION 3.5) project(joint_limits) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() @@ -14,27 +19,24 @@ install(DIRECTORY include/ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake) find_package(rclcpp) - #ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) - #target_include_directories(joint_limits_interface_test PUBLIC include) - #ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) - - #add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - #target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - #target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - #ament_target_dependencies(joint_limits_rosparam_test rclcpp) - #add_launch_test(test/joint_limits_rosparam.launch.py) - #install( - #TARGETS - #joint_limits_rosparam_test - #DESTINATION lib/${PROJECT_NAME} - #) - #install( - #FILES - #test/joint_limits_rosparam.yaml - #DESTINATION share/${PROJECT_NAME}/test - #) + add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) + target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) + target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) + ament_target_dependencies(joint_limits_rosparam_test rclcpp ament_cmake_gtest) + add_launch_test(test/joint_limits_rosparam.launch.py) + install( + TARGETS + joint_limits_rosparam_test + DESTINATION lib/${PROJECT_NAME} + ) + install( + FILES + test/joint_limits_rosparam.yaml + DESTINATION share/${PROJECT_NAME}/test + ) endif() diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index 0ccb3e52a4..a6bd1edf15 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -23,6 +23,14 @@ namespace joint_limits { +/** + * JointLimits structure stores values from from yaml definition or `` tag in URDF. + * The mapping from URDF attributes to members is the following: + * lower --> min_position + * upper --> max_position + * velocity --> max_velocity + * effort --> max_effort + */ struct JointLimits { JointLimits() @@ -77,12 +85,12 @@ struct JointLimits if (has_jerk_limits) { ss_output << " jerk limit: " - << "[" << max_acceleration << "]\n"; + << "[" << max_jerk << "]\n"; } if (has_effort_limits) { ss_output << " effort limit: " - << "[" << max_acceleration << "]\n"; + << "[" << max_effort << "]\n"; } if (angle_wraparound) { @@ -112,6 +120,10 @@ struct JointLimits } }; +/** + * SoftJointLimits stores values from the `` tag of URDF. + * For details about meaning of each variable check here: https://wiki.ros.org/urdf/XML/joint + */ struct SoftJointLimits { SoftJointLimits() @@ -126,6 +138,24 @@ struct SoftJointLimits double max_position; double k_position; double k_velocity; + + std::string to_string() + { + std::stringstream ss_output; + + ss_output << " soft position limits: " + << "[" << min_position << ", " << max_position << "]\n"; + + ss_output << " k-position: " + << "[" << k_position << "]\n"; + + ss_output << " k-velocity: " + << "[" << k_velocity << "]\n"; + + return ss_output.str(); + } + + std::string debug_to_string() { return to_string(); } }; } // namespace joint_limits diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 844de38366..cf291c4a45 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -112,12 +112,13 @@ inline bool declare_parameters(const std::string & joint_name, const rclcpp::Nod * max_effort: 20.0 * bar_joint: * has_position_limits: false # Continuous joint + * angle_wraparound: true * has_velocity_limits: true * max_velocity: 4.0 * \endcode * - * This specification is similar to the one used by MoveIt!, - * but additionally supports jerk and effort limits. + * This specification is similar to the one used by MoveIt2, + * but additionally supports effort limits. * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node NodeHandle where the joint limits are specified. diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 9095c15011..85f24d8415 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -16,7 +16,7 @@ rclcpp - ament_cmake_gmock + ament_cmake_gtest ament_cmake diff --git a/joint_limits/test/joint_limits_interface_test.cpp b/joint_limits/test/joint_limits_interface_test.cpp deleted file mode 100644 index aaa3b63f70..0000000000 --- a/joint_limits/test/joint_limits_interface_test.cpp +++ /dev/null @@ -1,627 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#include - -#include -#include - -#include -#include - -#include - -#include -#include - -// Floating-point value comparison threshold -const double EPS = 1e-12; - -TEST(SaturateTest, Saturate) -{ - const double min = -1.0; - const double max = 2.0; - double val; - - val = -0.5; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = 0.5; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = -1.0; - EXPECT_NEAR(val, rcppmath::clamp(min, min, max), EPS); - - val = -2.0; - EXPECT_NEAR(min, rcppmath::clamp(val, min, max), EPS); - - val = 2.0; - EXPECT_NEAR(val, rcppmath::clamp(val, min, max), EPS); - - val = 3.0; - EXPECT_NEAR(max, rcppmath::clamp(val, min, max), EPS); -} - -class JointLimitsTest -{ -public: - JointLimitsTest() - : pos(0.0), - vel(0.0), - eff(0.0), - cmd(0.0), - name("joint_name"), - period(0, 100000000), - cmd_handle(hardware_interface::JointHandle(name, "position_command", &cmd)), - pos_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_POSITION, &pos)), - vel_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_VELOCITY, &vel)), - eff_handle(hardware_interface::JointHandle(name, hardware_interface::HW_IF_EFFORT, &eff)) - { - limits.has_position_limits = true; - limits.min_position = -1.0; - limits.max_position = 1.0; - - limits.has_velocity_limits = true; - limits.max_velocity = 2.0; - - limits.has_effort_limits = true; - limits.max_effort = 8.0; - - soft_limits.min_position = -0.8; - soft_limits.max_position = 0.8; - soft_limits.k_position = 20.0; - // TODO(anyone): Tune value - soft_limits.k_velocity = 40.0; - } - -protected: - double pos, vel, eff, cmd; - std::string name; - rclcpp::Duration period; - hardware_interface::JointHandle cmd_handle; - hardware_interface::JointHandle pos_handle, vel_handle, eff_handle; - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; -}; - -class JointLimitsHandleTest : public JointLimitsTest, public ::testing::Test -{ -}; - -TEST_F(JointLimitsHandleTest, HandleConstruction) -{ - { - joint_limits_interface::JointLimits limits_bad; - EXPECT_THROW( - joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should be - // descriptive - try - { - joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - limits_bad.has_effort_limits = true; - EXPECT_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, - // but exception message should be descriptive - try - { - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - limits_bad.has_velocity_limits = true; - EXPECT_THROW( - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should - // be descriptive - try - { - joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits_bad, soft_limits); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - { - joint_limits_interface::JointLimits limits_bad; - EXPECT_THROW( - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits_bad), - joint_limits_interface::JointLimitsInterfaceException); - - // Print error messages. Requires manual output inspection, but exception message should - // be descriptive - try - { - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits_bad); - } - catch (const joint_limits_interface::JointLimitsInterfaceException & e) - { - RCLCPP_ERROR(rclcpp::get_logger("joint_limits_interface_test"), "%s", e.what()); - } - } - - EXPECT_NO_THROW(joint_limits_interface::PositionJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); - EXPECT_NO_THROW(joint_limits_interface::EffortJointSoftLimitsHandle( - pos_handle, cmd_handle, limits, soft_limits)); - EXPECT_NO_THROW( - joint_limits_interface::VelocityJointSaturationHandle(pos_handle, cmd_handle, limits)); -} - -class PositionJointSoftLimitsHandleTest : public JointLimitsTest, public ::testing::Test -{ -}; - -TEST_F(PositionJointSoftLimitsHandleTest, EnforceVelocityBounds) -{ - // Test setup - const double max_increment = period.seconds() * limits.max_velocity; - pos = 0.0; - - double cmd; - - // Move slower than maximum velocity - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = max_increment / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = -max_increment / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - - // Move at maximum velocity - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = -max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - } - - // Try to move faster than the maximum velocity, enforce velocity limits - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = 2.0 * max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(max_increment, cmd_handle.get_value(), EPS); - } - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - cmd = -2.0 * max_increment; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(-max_increment, cmd_handle.get_value(), EPS); - } -} - -// This is a black box test and does not verify against random precomputed values, but rather that -// the expected qualitative behavior is honored -TEST_F(PositionJointSoftLimitsHandleTest, EnforcePositionBounds) -{ - // Test setup - const double workspace_center = (limits.min_position + limits.max_position) / 2.0; - - // Current position == upper soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (zero max velocity) - pos = soft_limits.max_position; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); - } - - // Current position == lower soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (zero min velocity) - pos = soft_limits.min_position; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(pos_handle.get_value(), cmd_handle.get_value(), EPS); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); - } - - // Current position > upper soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (negative max velocity) - // Halfway between soft and hard limit - pos = (soft_limits.max_position + limits.max_position) / 2.0; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); - } - - // Current position < lower soft limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Can't get any closer to hard limit (positive min velocity) - // Halfway between soft and hard limit - pos = (soft_limits.min_position + limits.min_position) / 2.0; - // Try to get closer to the hard limit - cmd_handle.set_value(limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); - - // OK to move away from hard limit - // Try to go to workspace center - cmd_handle.set_value(workspace_center); - limits_handle.enforce_limits(period); - EXPECT_LT(pos_handle.get_value(), cmd_handle.get_value()); - } -} - -TEST_F(PositionJointSoftLimitsHandleTest, PathologicalSoftBounds) -{ - // Safety limits are past the hard limits - soft_limits.min_position = - limits.min_position * (1.0 - 0.5 * limits.min_position / std::abs(limits.min_position)); - soft_limits.max_position = - limits.max_position * (1.0 + 0.5 * limits.max_position / std::abs(limits.max_position)); - - // Current position == higher hard limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Hit hard limit - // On hard limit - pos = limits.max_position; - // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.max_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_position, cmd_handle.get_value(), EPS); - } - - // Current position == lower hard limit - { - joint_limits_interface::PositionJointSoftLimitsHandle limits_handle( - pos_handle, cmd_handle, limits, soft_limits); - - // Hit hard limit - // On hard limit - pos = limits.min_position; - // Way beyond hard limit - cmd_handle.set_value(2.0 * limits.min_position); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.min_position, cmd_handle.get_value(), EPS); - } -} - -class VelocityJointSaturationHandleTest : public JointLimitsTest, public ::testing::Test -{ -}; - -TEST_F(VelocityJointSaturationHandleTest, EnforceVelocityBounds) -{ - // Test setup - joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); - - pos = 0.0; - double cmd; - - // Velocity within bounds - cmd = limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - cmd = -limits.max_velocity / 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - // Velocity at bounds - cmd = limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - cmd = -limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(cmd, cmd_handle.get_value(), EPS); - - // Velocity beyond bounds - cmd = 2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); - - cmd = -2.0 * limits.max_velocity; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); -} - -TEST_F(VelocityJointSaturationHandleTest, EnforceAccelerationBounds) -{ - // Test setup - limits.has_acceleration_limits = true; - limits.max_acceleration = limits.max_velocity / period.seconds(); - joint_limits_interface::VelocityJointSaturationHandle limits_handle( - pos_handle, cmd_handle, limits); - - pos = 0.0; - double cmd; - // An arbitrarily long time, sufficient to suppress acceleration limits - const rclcpp::Duration long_enough(1000, 0); - - // Positive velocity - // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond +max velocity - cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by velocity limit - EXPECT_NEAR(limits.max_velocity, cmd_handle.get_value(), EPS); - - // register last command - cmd_handle.set_value(limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond -max velocity - cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by acceleration limit - EXPECT_NEAR(-limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); - - // Negative velocity - // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond +max velocity - cmd = limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by acceleration limit - EXPECT_NEAR(limits.max_velocity / 2.0, cmd_handle.get_value(), EPS); - - // register last command - cmd_handle.set_value(-limits.max_velocity / 2.0); - // make sure the prev_cmd is registered - // without triggering the acceleration limits - limits_handle.enforce_limits(long_enough); - - // Try to go beyond -max velocity - cmd = -limits.max_velocity * 2.0; - cmd_handle.set_value(cmd); - limits_handle.enforce_limits(period); - // Max velocity bounded by velocity limit - EXPECT_NEAR(-limits.max_velocity, cmd_handle.get_value(), EPS); -} - -class JointLimitsInterfaceTest : public JointLimitsTest, public ::testing::Test -{ -public: - JointLimitsInterfaceTest() - : JointLimitsTest(), - pos2(0.0), - vel2(0.0), - eff2(0.0), - cmd2(0.0), - name2("joint2_name"), - cmd2_handle( - std::make_shared(name2, "position_command", &cmd2)), - pos2_handle(std::make_shared( - name2, hardware_interface::HW_IF_POSITION, &pos2)), - vel2_handle(std::make_shared( - name2, hardware_interface::HW_IF_VELOCITY, &vel2)), - eff2_handle(std::make_shared( - name2, hardware_interface::HW_IF_EFFORT, &eff2)) - { - } - -protected: - double pos2, vel2, eff2, cmd2; - std::string name2; - std::shared_ptr cmd2_handle; - std::shared_ptr pos2_handle, vel2_handle, eff2_handle; -}; - -// TEST_F(JointLimitsInterfaceTest, InterfaceRegistration) -// { -// // Populate interface -// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle1( -// pos_handle, cmd_handle, limits, soft_limits); -// joint_limits_interface::PositionJointSoftLimitsHandle limits_handle2( -// pos_handle2, cmd_handle2, limits, soft_limits); -// -// joint_limits_interface::PositionJointSoftLimitsInterface iface; -// iface.registerHandle(limits_handle1); -// iface.registerHandle(limits_handle2); -// -// // Get handles -// EXPECT_NO_THROW(iface.getHandle(name)); -// EXPECT_NO_THROW(iface.getHandle(name2)); -// -// PositionJointSoftLimitsHandle h1_tmp = iface.getHandle(name); -// EXPECT_EQ(name, h1_tmp.getName()); -// -// PositionJointSoftLimitsHandle h2_tmp = iface.getHandle(name2); -// EXPECT_EQ(name2, h2_tmp.getName()); -// -// // Print error message -// // Requires manual output inspection, but exception message should contain the interface name -// // (not its base class) -// try { -// iface.getHandle("unknown_name"); -// } catch (const JointLimitsInterfaceException & e) { -// ROS_ERROR("%s", e.what()); -// } -// -// // Enforce limits of all managed joints -// // Halfway between soft and hard limit -// pos = pos2 = (soft_limits.max_position + limits.max_position) / 2.0; -// // Try to get closer to the hard limit -// cmd_handle.set_value(limits.max_position); -// cmd_handle2.set_cmd(limits.max_position); -// iface.enforce_limits(period); -// EXPECT_GT(pos_handle.get_value(), cmd_handle.get_value()); -// EXPECT_GT(cmd_handle2.getPosition(), cmd_handle2.get_cmd()); -// } -// -#if 0 // todo: implement the interfaces -TEST_F(JointLimitsHandleTest, ResetSaturationInterface) -{ - // Populate interface - joint_limits_interface::PositionJointSaturationHandle limits_handle1 - (pos_handle, cmd_handle, limits); - - PositionJointSaturationInterface iface; - iface.registerHandle(limits_handle1); - - iface.enforce_limits(period); // initialize limit handles - - const double max_increment = period.seconds() * limits.max_velocity; - - cmd_handle.set_value(limits.max_position); - iface.enforce_limits(period); - - EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); - - iface.reset(); - pos = limits.max_position; - cmd_handle.set_value(limits.max_position); - iface.enforce_limits(period); - - EXPECT_NEAR(cmd_handle.get_value(), limits.max_position, EPS); -} -#endif - -// TEST_F(JointLimitsHandleTest, ResetSoftLimitsInterface) -// { -// // Populate interface -// PositionJointSoftLimitsHandle limits_handle1(cmd_handle, limits, soft_limits); -// -// PositionJointSoftLimitsInterface iface; -// iface.registerHandle(limits_handle1); -// -// iface.enforce_limits(period); // initialize limit handles -// -// const double max_increment = period.seconds() * limits.max_velocity; -// -// cmd_handle.set_value(limits.max_position); -// iface.enforce_limits(period); -// -// EXPECT_NEAR(cmd_handle.get_value(), max_increment, EPS); -// -// iface.reset(); -// pos = limits.max_position; -// cmd_handle.set_value(soft_limits.max_position); -// iface.enforce_limits(period); -// -// EXPECT_NEAR(cmd_handle.get_value(), soft_limits.max_position, EPS); -// -// } - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index 066f98ccf8..47f6488018 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -14,17 +14,16 @@ /// \author Adolfo Rodriguez Tsouroukdissian -#include - -#include +#include -#include +#include "gtest/gtest.h" -#include +#include "joint_limits/joint_limits_rosparam.hpp" +#include "rclcpp/rclcpp.hpp" class JointLimitsRosParamTest : public ::testing::Test { -protected: +public: void SetUp() { rclcpp::NodeOptions node_options; @@ -34,16 +33,19 @@ class JointLimitsRosParamTest : public ::testing::Test node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode", node_options); } + void TearDown() { node_.reset(); } + +protected: rclcpp::Node::SharedPtr node_; }; -TEST_F(JointLimitsRosParamTest, GetJointLimits) +TEST_F(JointLimitsRosParamTest, parse_joint_limits) { // Invalid specification { - joint_limits_interface::JointLimits limits; - EXPECT_FALSE(getJointLimits("bad_joint", node_, limits)); - EXPECT_FALSE(getJointLimits("unknown_joint", node_, limits)); + joint_limits::JointLimits limits; + EXPECT_FALSE(get_joint_limits("bad_joint", node_, limits)); + EXPECT_FALSE(get_joint_limits("unknown_joint", node_, limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); @@ -54,8 +56,8 @@ TEST_F(JointLimitsRosParamTest, GetJointLimits) // Get full specification from parameter server { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("foo_joint", node_, limits)); + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits("foo_joint", node_, limits)); EXPECT_TRUE(limits.has_position_limits); EXPECT_EQ(0.0, limits.min_position); @@ -78,8 +80,8 @@ TEST_F(JointLimitsRosParamTest, GetJointLimits) // Specifying flags but not values should set nothing { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("yinfoo_joint", node_, limits)); + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits("yinfoo_joint", node_, limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); @@ -90,8 +92,8 @@ TEST_F(JointLimitsRosParamTest, GetJointLimits) // Specifying values but not flags should set nothing { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("yangfoo_joint", node_, limits)); + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits("yangfoo_joint", node_, limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); @@ -102,15 +104,15 @@ TEST_F(JointLimitsRosParamTest, GetJointLimits) // Disable already set values { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("foo_joint", node_, limits)); + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits("foo_joint", node_, limits)); EXPECT_TRUE(limits.has_position_limits); EXPECT_TRUE(limits.has_velocity_limits); EXPECT_TRUE(limits.has_acceleration_limits); EXPECT_TRUE(limits.has_jerk_limits); EXPECT_TRUE(limits.has_effort_limits); - EXPECT_TRUE(getJointLimits("antifoo_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits("antifoo_joint", node_, limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); EXPECT_FALSE(limits.has_acceleration_limits); @@ -121,16 +123,16 @@ TEST_F(JointLimitsRosParamTest, GetJointLimits) // Incomplete position limits specification does not get loaded { - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits("baz_joint", node_, limits)); + joint_limits::JointLimits limits; + EXPECT_TRUE(get_joint_limits("baz_joint", node_, limits)); EXPECT_FALSE(limits.has_position_limits); } // Override only one field, leave all others unchanged { - joint_limits_interface::JointLimits limits, limits_ref; - EXPECT_TRUE(getJointLimits("bar_joint", node_, limits)); + joint_limits::JointLimits limits, limits_ref; + EXPECT_TRUE(get_joint_limits("bar_joint", node_, limits)); EXPECT_EQ(limits_ref.has_position_limits, limits.has_position_limits); EXPECT_EQ(limits_ref.min_position, limits.min_position); @@ -153,19 +155,19 @@ TEST_F(JointLimitsRosParamTest, GetJointLimits) } } -TEST_F(JointLimitsRosParamTest, GetSoftJointLimits) +TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) { // Invalid specification { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_FALSE(getSoftJointLimits("bad_joint", node_, soft_limits)); - EXPECT_FALSE(getSoftJointLimits("unknown_joint", node_, soft_limits)); + joint_limits::SoftJointLimits soft_limits; + EXPECT_FALSE(get_joint_limits("bad_joint", node_, soft_limits)); + EXPECT_FALSE(get_joint_limits("unknown_joint", node_, soft_limits)); } // Get full specification from parameter server { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_TRUE(getSoftJointLimits("foo_joint", node_, soft_limits)); + joint_limits::SoftJointLimits soft_limits; + EXPECT_TRUE(get_joint_limits("foo_joint", node_, soft_limits)); EXPECT_EQ(10.0, soft_limits.k_position); EXPECT_EQ(20.0, soft_limits.k_velocity); @@ -175,14 +177,14 @@ TEST_F(JointLimitsRosParamTest, GetSoftJointLimits) // Skip parsing soft limits if has_soft_limits is false { - joint_limits_interface::SoftJointLimits soft_limits, soft_limits_ref; - EXPECT_FALSE(getSoftJointLimits("foobar_joint", node_, soft_limits)); + joint_limits::SoftJointLimits soft_limits, soft_limits_ref; + EXPECT_FALSE(get_joint_limits("foobar_joint", node_, soft_limits)); } // Incomplete soft limits specification does not get loaded { - joint_limits_interface::SoftJointLimits soft_limits, soft_limits_ref; - EXPECT_FALSE(getSoftJointLimits("barbaz_joint", node_, soft_limits)); + joint_limits::SoftJointLimits soft_limits, soft_limits_ref; + EXPECT_FALSE(get_joint_limits("barbaz_joint", node_, soft_limits)); EXPECT_EQ(soft_limits.k_position, soft_limits_ref.k_position); EXPECT_EQ(soft_limits.k_velocity, soft_limits_ref.k_velocity); EXPECT_EQ(soft_limits.min_position, soft_limits_ref.min_position); From b409f7f64a358c6aa9c87738ee80431201d56c2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 4 May 2022 21:57:14 +0200 Subject: [PATCH 07/13] Update ros2_control/package.xml Co-authored-by: AndyZe --- ros2_control/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 25585bc7db..57e0d85277 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -15,7 +15,6 @@ controller_manager_msgs hardware_interface joint_limits - ros2controlcli ros2_control_test_assets ros2controlcli transmission_interface From 9f00962e1fd2cadf457289dbe8468b09b140de77 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 19 May 2022 19:26:44 +0200 Subject: [PATCH 08/13] Update joint_limits/package.xml --- joint_limits/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 85f24d8415..d89af39c0b 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -16,6 +16,7 @@ rclcpp + launch_testing_ament_cmake ament_cmake_gtest From 6d5e658f8c1b7c4ceef1ea650024ad235a4862be Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 18 Jun 2022 08:30:59 +0100 Subject: [PATCH 09/13] Fix building test & package name in launch script --- joint_limits/CMakeLists.txt | 8 ++--- .../test/joint_limits_rosparam.launch.py | 31 ++++++++++--------- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index dd698b632e..88fc72c65f 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -22,10 +22,10 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake) find_package(rclcpp) - add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp ament_cmake_gtest) + ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) + target_include_directories(joint_limits_rosparam_test PRIVATE include) + ament_target_dependencies(joint_limits_rosparam_test rclcpp) + add_launch_test(test/joint_limits_rosparam.launch.py) install( TARGETS diff --git a/joint_limits/test/joint_limits_rosparam.launch.py b/joint_limits/test/joint_limits_rosparam.launch.py index 9d0f31e0a3..c808954207 100644 --- a/joint_limits/test/joint_limits_rosparam.launch.py +++ b/joint_limits/test/joint_limits_rosparam.launch.py @@ -27,32 +27,35 @@ def generate_test_description(): - joint_limits_interface_path = get_package_share_directory('joint_limits_interface') + joint_limits_path = get_package_share_directory("joint_limits") node_under_test = Node( - package='joint_limits_interface', - executable='joint_limits_rosparam_test', - output='screen', - parameters=[os.path.join(joint_limits_interface_path, - 'test', - 'joint_limits_rosparam.yaml')], + package="joint_limits", + executable="joint_limits_rosparam_test", + output="screen", + parameters=[ + os.path.join(joint_limits_path, "test", "joint_limits_rosparam.yaml") + ], + ) + return ( + LaunchDescription( + [ + node_under_test, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ] + ), + locals(), ) - return LaunchDescription([ - node_under_test, - launch_testing.util.KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ]), locals() class TestJointLimitInterface(unittest.TestCase): - def test_termination(self, node_under_test, proc_info): proc_info.assertWaitForShutdown(process=node_under_test, timeout=(10)) @launch_testing.post_shutdown_test() class TestJointLimitInterfaceTestAfterShutdown(unittest.TestCase): - def test_exit_code(self, proc_info): # Check that all processes in the launch (in this case, there's just one) exit # with code 0 From a0e8cc5b60574e6c102e835f0f4734b617dfdb22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 18 Jun 2022 11:01:13 +0200 Subject: [PATCH 10/13] Repair tests after code changes. --- joint_limits/test/joint_limits_rosparam.yaml | 14 +++- .../test/joint_limits_rosparam_test.cpp | 79 ++++++++++++++----- 2 files changed, 72 insertions(+), 21 deletions(-) diff --git a/joint_limits/test/joint_limits_rosparam.yaml b/joint_limits/test/joint_limits_rosparam.yaml index 0ebb43af05..521fbf93f8 100644 --- a/joint_limits/test/joint_limits_rosparam.yaml +++ b/joint_limits/test/joint_limits_rosparam.yaml @@ -1,6 +1,7 @@ JointLimitsRosparamTestNode: ros__parameters: joint_limits: + # Get full specification from parameter server foo_joint: has_position_limits: true min_position: 0.0 @@ -13,13 +14,14 @@ JointLimitsRosparamTestNode: max_jerk: 100.0 has_effort_limits: true max_effort: 20.0 - angle_wraparound: true # should be ignored, has position limits + angle_wraparound: true # should be ignored, has position limits has_soft_limits: true k_position: 10.0 k_velocity: 20.0 soft_lower_limit: 0.1 soft_upper_limit: 0.9 + # Specifying flags but not values should set nothing yinfoo_joint: has_position_limits: true has_velocity_limits: true @@ -27,6 +29,7 @@ JointLimitsRosparamTestNode: has_jerk_limits: true has_effort_limits: true + # Specifying values but not flags should set nothing yangfoo_joint: min_position: 0.0 max_position: 1.0 @@ -35,23 +38,27 @@ JointLimitsRosparamTestNode: max_jerk: 100.0 max_effort: 20.0 + # Disable already set values antifoo_joint: has_position_limits: false has_velocity_limits: false has_acceleration_limits: false has_jerk_limits: false has_effort_limits: false - angle_wraparound: true # should be accepted, has no position limits + angle_wraparound: true # should be accepted, has no position limits + # Override only one field, leave all others unchanged bar_joint: has_velocity_limits: true max_velocity: 2.0 + # Incomplete position limits specification does not get loaded baz_joint: has_position_limits: true # Missing min_position max_position: 1.0 + # Skip parsing soft limits if has_soft_limits is false foobar_joint: has_soft_limits: false k_velocity: 20.0 @@ -59,8 +66,9 @@ JointLimitsRosparamTestNode: soft_lower_limit: 0.1 soft_upper_limit: 0.9 + # Incomplete soft limits specification does not get loaded barbaz_joint: - has_soft_limits: false + has_soft_limits: true k_position: 10.0 soft_lower_limit: 0.1 soft_upper_limit: 0.9 diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index 47f6488018..cfd1575b6e 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -44,14 +44,38 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Invalid specification { joint_limits::JointLimits limits; + + // test default values + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); + EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_TRUE(std::isnan(limits.max_velocity)); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_acceleration)); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_TRUE(std::isnan(limits.max_jerk)); + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(std::isnan(limits.max_effort)); + EXPECT_FALSE(limits.angle_wraparound); + + // try to read limits for not-existing joints EXPECT_FALSE(get_joint_limits("bad_joint", node_, limits)); EXPECT_FALSE(get_joint_limits("unknown_joint", node_, limits)); + // default values should not change EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); EXPECT_FALSE(limits.has_velocity_limits); + EXPECT_TRUE(std::isnan(limits.max_velocity)); EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_acceleration)); EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_TRUE(std::isnan(limits.max_jerk)); EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(std::isnan(limits.max_effort)); + EXPECT_FALSE(limits.angle_wraparound); } // Get full specification from parameter server @@ -75,6 +99,7 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_TRUE(limits.has_effort_limits); EXPECT_EQ(20.0, limits.max_effort); + // parameters is 'true', but because there are position limits it is ignored EXPECT_FALSE(limits.angle_wraparound); } @@ -127,31 +152,31 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_TRUE(get_joint_limits("baz_joint", node_, limits)); EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); } // Override only one field, leave all others unchanged { - joint_limits::JointLimits limits, limits_ref; + joint_limits::JointLimits limits; EXPECT_TRUE(get_joint_limits("bar_joint", node_, limits)); - EXPECT_EQ(limits_ref.has_position_limits, limits.has_position_limits); - EXPECT_EQ(limits_ref.min_position, limits.min_position); - EXPECT_EQ(limits_ref.max_position, limits.max_position); + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(std::isnan(limits.min_position)); + EXPECT_TRUE(std::isnan(limits.max_position)); // Max velocity is overridden - EXPECT_NE(limits_ref.has_velocity_limits, limits.has_velocity_limits); - EXPECT_NE(limits_ref.max_velocity, limits.max_velocity); EXPECT_TRUE(limits.has_velocity_limits); EXPECT_EQ(2.0, limits.max_velocity); - EXPECT_EQ(limits_ref.has_acceleration_limits, limits.has_acceleration_limits); - EXPECT_EQ(limits_ref.max_acceleration, limits.max_acceleration); + EXPECT_FALSE(limits.has_acceleration_limits); + EXPECT_TRUE(std::isnan(limits.max_acceleration)); - EXPECT_EQ(limits_ref.has_jerk_limits, limits.has_jerk_limits); - EXPECT_EQ(limits_ref.has_jerk_limits, limits.max_jerk); + EXPECT_FALSE(limits.has_jerk_limits); + EXPECT_TRUE(std::isnan(limits.max_jerk)); - EXPECT_EQ(limits_ref.has_effort_limits, limits.has_effort_limits); - EXPECT_EQ(limits_ref.max_effort, limits.max_effort); + EXPECT_FALSE(limits.has_effort_limits); + EXPECT_TRUE(std::isnan(limits.max_effort)); } } @@ -160,8 +185,22 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) // Invalid specification { joint_limits::SoftJointLimits soft_limits; + + // test default values + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); + + // try to read limits for not-existing joints EXPECT_FALSE(get_joint_limits("bad_joint", node_, soft_limits)); EXPECT_FALSE(get_joint_limits("unknown_joint", node_, soft_limits)); + + // default values should not change + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); } // Get full specification from parameter server @@ -177,18 +216,22 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) // Skip parsing soft limits if has_soft_limits is false { - joint_limits::SoftJointLimits soft_limits, soft_limits_ref; + joint_limits::SoftJointLimits soft_limits; EXPECT_FALSE(get_joint_limits("foobar_joint", node_, soft_limits)); + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); } // Incomplete soft limits specification does not get loaded { - joint_limits::SoftJointLimits soft_limits, soft_limits_ref; + joint_limits::SoftJointLimits soft_limits; EXPECT_FALSE(get_joint_limits("barbaz_joint", node_, soft_limits)); - EXPECT_EQ(soft_limits.k_position, soft_limits_ref.k_position); - EXPECT_EQ(soft_limits.k_velocity, soft_limits_ref.k_velocity); - EXPECT_EQ(soft_limits.min_position, soft_limits_ref.min_position); - EXPECT_EQ(soft_limits.max_position, soft_limits_ref.max_position); + EXPECT_TRUE(std::isnan(soft_limits.min_position)); + EXPECT_TRUE(std::isnan(soft_limits.max_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_position)); + EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); } } From 249d4c718654a6c4c0fe9325a6e85980b5d842bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 28 Jun 2022 15:37:46 +0200 Subject: [PATCH 11/13] Make better interfaces for joint limits. --- joint_limits/CMakeLists.txt | 4 +- .../joint_limits/joint_limits_rosparam.hpp | 497 ++++++++++++------ joint_limits/package.xml | 1 + .../test/joint_limits_rosparam_test.cpp | 203 ++++++- 4 files changed, 534 insertions(+), 171 deletions(-) diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 88fc72c65f..314ec67272 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -12,6 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) install(DIRECTORY include/ DESTINATION include @@ -21,10 +22,11 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(launch_testing_ament_cmake) find_package(rclcpp) + find_package(rclcpp_lifecycle) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_include_directories(joint_limits_rosparam_test PRIVATE include) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) + ament_target_dependencies(joint_limits_rosparam_test rclcpp rclcpp_lifecycle) add_launch_test(test/joint_limits_rosparam.launch.py) install( diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index cf291c4a45..6e0b66641e 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -22,6 +22,7 @@ #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace // utilities { @@ -34,298 +35,440 @@ namespace // utilities */ template auto auto_declare( - const rclcpp::Node::SharedPtr & node, const std::string & name, const ParameterT & default_value) + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const std::string & name, const ParameterT & default_value) { - if (!node->has_parameter(name)) + if (!param_itf->has_parameter(name)) { - return node->declare_parameter(name, default_value); - } - else - { - return node->get_parameter(name).get_value(); + auto param_default_value = rclcpp::ParameterValue(default_value); + param_itf->declare_parameter(name, param_default_value); } + return param_itf->get_parameter(name).get_value(); } } // namespace namespace joint_limits { -inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node) +/** + * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name using node + * parameters interface \p param_itf. + * + * The following parameter structure is declared with base name `joint_limits.joint_name`: + * \code + * has_position_limits: bool + * min_position: double + * max_position: double + * has_velocity_limits: bool + * max_velocity: double + * has_acceleration_limits: bool + * max_acceleration: double + * has_jerk_limits: bool + * max_jerk: double + * has_effort_limits: bool + * max_effort: double + * angle_wraparound: bool + * has_soft_limits: bool + * k_position: double + * k_velocity: double + * soft_lower_limit: double + * soft_upper_limit: double + * \endcode + * + * \param[in] joint_name name of the joint for which parameters will be declared. + * \param[in] param_itf node parameters interface object to access parameters. + * \param[in] logging_itf node logging interface to log if error happens. + * + * \returns True if parameters are successfully declared, false otherwise. + */ +inline bool declare_parameters( + const std::string & joint_name, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf) { const std::string param_base_name = "joint_limits." + joint_name; try { - auto_declare(node, param_base_name + ".has_position_limits", false); + auto_declare(param_itf, param_base_name + ".has_position_limits", false); auto_declare( - node, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); + param_itf, param_base_name + ".min_position", std::numeric_limits::quiet_NaN()); auto_declare( - node, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); - auto_declare(node, param_base_name + ".has_velocity_limits", false); + param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_velocity_limits", false); auto_declare( - node, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); + param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); auto_declare( - node, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); - auto_declare(node, param_base_name + ".has_acceleration_limits", false); + param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false); auto_declare( - node, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); - auto_declare(node, param_base_name + ".has_jerk_limits", false); + param_itf, param_base_name + ".max_acceleration", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_jerk_limits", false); auto_declare( - node, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); - auto_declare(node, param_base_name + ".has_effort_limits", false); + param_itf, param_base_name + ".max_jerk", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".has_effort_limits", false); auto_declare( - node, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); - auto_declare(node, param_base_name + ".angle_wraparound", false); - auto_declare(node, param_base_name + ".has_soft_limits", false); + param_itf, param_base_name + ".max_effort", std::numeric_limits::quiet_NaN()); + auto_declare(param_itf, param_base_name + ".angle_wraparound", false); + auto_declare(param_itf, param_base_name + ".has_soft_limits", false); auto_declare( - node, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); + param_itf, param_base_name + ".k_position", std::numeric_limits::quiet_NaN()); auto_declare( - node, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); + param_itf, param_base_name + ".k_velocity", std::numeric_limits::quiet_NaN()); auto_declare( - node, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); + param_itf, param_base_name + ".soft_lower_limit", std::numeric_limits::quiet_NaN()); auto_declare( - node, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); + param_itf, param_base_name + ".soft_upper_limit", std::numeric_limits::quiet_NaN()); } catch (const std::exception & ex) { - RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); + RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what()); return false; } return true; } -/// Populate a JointLimits instance from the ROS parameter server. /** - * It is assumed that the following parameter structure is followed on the provided NodeHandle. Unspecified parameters - * are simply not added to the joint limits specification. + * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name for the \p node + * object. + * This is a convenience function. + * For parameters structure see the underlying `declare_parameters` function. + * + * \param[in] joint_name name of the joint for which parameters will be declared. + * \param[in] node node for parameters should be declared. + * + * \returns True if parameters are successfully declared, false otherwise. + */ +inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node) +{ + return declare_parameters( + joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface()); +} + +/** + * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node + * object. + * This is a convenience function. + * For parameters structure see the underlying `declare_parameters` function. + * + * \param[in] joint_name name of the joint for which parameters will be declared. + * \param[in] lifecycle_node lifecycle node for parameters should be declared. + * + * \returns True if parameters are successfully declared, false otherwise. + */ +inline bool declare_parameters( + const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node) +{ + return declare_parameters( + joint_name, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface()); +} + +/// Populate a JointLimits instance from the node parameters. +/** + * It is assumed that parameter structure is the following: * \code - * joint_limits: - * foo_joint: - * has_position_limits: true - * min_position: 0.0 - * max_position: 1.0 - * has_velocity_limits: true - * max_velocity: 2.0 - * has_acceleration_limits: true - * max_acceleration: 5.0 - * has_jerk_limits: true - * max_jerk: 100.0 - * has_effort_limits: true - * max_effort: 20.0 - * bar_joint: - * has_position_limits: false # Continuous joint - * angle_wraparound: true - * has_velocity_limits: true - * max_velocity: 4.0 + * has_position_limits: bool + * min_position: double + * max_position: double + * has_velocity_limits: bool + * max_velocity: double + * has_acceleration_limits: bool + * max_acceleration: double + * has_jerk_limits: bool + * max_jerk: double + * has_effort_limits: bool + * max_effort: double + * angle_wraparound: bool # will be ignored if there are position limits * \endcode * - * This specification is similar to the one used by MoveIt2, - * but additionally supports effort limits. + * Unspecified parameters are not added to the joint limits specification. + * A specification in a yaml would look like this: + * \code + * + * ros__parameters: + * joint_limits: + * foo_joint: + * has_position_limits: true + * min_position: 0.0 + * max_position: 1.0 + * has_velocity_limits: true + * max_velocity: 2.0 + * has_acceleration_limits: true + * max_acceleration: 5.0 + * has_jerk_limits: true + * max_jerk: 100.0 + * has_effort_limits: true + * max_effort: 20.0 + * bar_joint: + * has_position_limits: false # Continuous joint + * angle_wraparound: true # available only for continuous joints + * has_velocity_limits: true + * max_velocity: 4.0 + * \endcode * - * \param[in] joint_name Name of joint whose limits are to be fetched. - * \param[in] node NodeHandle where the joint limits are specified. + * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". + * \param[in] param_itf node parameters interface of the node where parameters are specified. + * \param[in] logging_itf node logging interface to provide log errors. * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite * existing values. Values in \p limits not specified in the parameter server remain unchanged. - * \return True if a limits specification is found (ie. the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + * + * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter exists in \p node), false otherwise. */ inline bool get_joint_limits( - const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) + const std::string & joint_name, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + JointLimits & limits) { const std::string param_base_name = "joint_limits." + joint_name; try { if ( - !node->has_parameter(param_base_name + ".has_position_limits") && - !node->has_parameter(param_base_name + ".min_position") && - !node->has_parameter(param_base_name + ".max_position") && - !node->has_parameter(param_base_name + ".has_velocity_limits") && - !node->has_parameter(param_base_name + ".min_velocity") && - !node->has_parameter(param_base_name + ".max_velocity") && - !node->has_parameter(param_base_name + ".has_acceleration_limits") && - !node->has_parameter(param_base_name + ".max_acceleration") && - !node->has_parameter(param_base_name + ".has_jerk_limits") && - !node->has_parameter(param_base_name + ".max_jerk") && - !node->has_parameter(param_base_name + ".has_effort_limits") && - !node->has_parameter(param_base_name + ".max_effort") && - !node->has_parameter(param_base_name + ".angle_wraparound") && - !node->has_parameter(param_base_name + ".has_soft_limits") && - !node->has_parameter(param_base_name + ".k_position") && - !node->has_parameter(param_base_name + ".k_velocity") && - !node->has_parameter(param_base_name + ".soft_lower_limit") && - !node->has_parameter(param_base_name + ".soft_upper_limit")) + !param_itf->has_parameter(param_base_name + ".has_position_limits") && + !param_itf->has_parameter(param_base_name + ".min_position") && + !param_itf->has_parameter(param_base_name + ".max_position") && + !param_itf->has_parameter(param_base_name + ".has_velocity_limits") && + !param_itf->has_parameter(param_base_name + ".min_velocity") && + !param_itf->has_parameter(param_base_name + ".max_velocity") && + !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && + !param_itf->has_parameter(param_base_name + ".max_acceleration") && + !param_itf->has_parameter(param_base_name + ".has_jerk_limits") && + !param_itf->has_parameter(param_base_name + ".max_jerk") && + !param_itf->has_parameter(param_base_name + ".has_effort_limits") && + !param_itf->has_parameter(param_base_name + ".max_effort") && + !param_itf->has_parameter(param_base_name + ".angle_wraparound") && + !param_itf->has_parameter(param_base_name + ".has_soft_limits") && + !param_itf->has_parameter(param_base_name + ".k_position") && + !param_itf->has_parameter(param_base_name + ".k_velocity") && + !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && + !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) { RCLCPP_ERROR( - node->get_logger(), + logging_itf->get_logger(), "No joint limits specification found for joint '%s' in the parameter server " - "(node: %s param name: %s).", - joint_name.c_str(), node->get_name(), param_base_name.c_str()); + "(param name: %s).", + joint_name.c_str(), param_base_name.c_str()); return false; } } catch (const std::exception & ex) { - RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); + RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what()); return false; } // Position limits - bool has_position_limits = false; - if (node->get_parameter(param_base_name + ".has_position_limits", has_position_limits)) + if (param_itf->has_parameter(param_base_name + ".has_position_limits")) { - if (!has_position_limits) + limits.has_position_limits = + param_itf->get_parameter(param_base_name + ".has_position_limits").as_bool(); + if ( + limits.has_position_limits && param_itf->has_parameter(param_base_name + ".min_position") && + param_itf->has_parameter(param_base_name + ".max_position")) { - limits.has_position_limits = false; + limits.min_position = param_itf->get_parameter(param_base_name + ".min_position").as_double(); + limits.max_position = param_itf->get_parameter(param_base_name + ".max_position").as_double(); } - double min_pos, max_pos; - if ( - has_position_limits && node->get_parameter(param_base_name + ".min_position", min_pos) && - node->get_parameter(param_base_name + ".max_position", max_pos)) + else { - limits.has_position_limits = true; - limits.min_position = min_pos; - limits.max_position = max_pos; + limits.has_position_limits = false; } - bool angle_wraparound; if ( - !has_position_limits && - node->get_parameter(param_base_name + ".angle_wraparound", angle_wraparound)) + !limits.has_position_limits && + param_itf->has_parameter(param_base_name + ".angle_wraparound")) { - limits.angle_wraparound = angle_wraparound; + limits.angle_wraparound = + param_itf->get_parameter(param_base_name + ".angle_wraparound").as_bool(); } } // Velocity limits - bool has_velocity_limits = false; - if (node->get_parameter(param_base_name + ".has_velocity_limits", has_velocity_limits)) + if (param_itf->has_parameter(param_base_name + ".has_velocity_limits")) { - if (!has_velocity_limits) + limits.has_velocity_limits = + param_itf->get_parameter(param_base_name + ".has_velocity_limits").as_bool(); + if (limits.has_velocity_limits && param_itf->has_parameter(param_base_name + ".max_velocity")) { - limits.has_velocity_limits = false; + limits.max_velocity = param_itf->get_parameter(param_base_name + ".max_velocity").as_double(); } - double max_vel; - if (has_velocity_limits && node->get_parameter(param_base_name + ".max_velocity", max_vel)) + else { - limits.has_velocity_limits = true; - limits.max_velocity = max_vel; + limits.has_velocity_limits = false; } } // Acceleration limits - bool has_acceleration_limits = false; - if (node->get_parameter(param_base_name + ".has_acceleration_limits", has_acceleration_limits)) + if (param_itf->has_parameter(param_base_name + ".has_acceleration_limits")) { - if (!has_acceleration_limits) + limits.has_acceleration_limits = + param_itf->get_parameter(param_base_name + ".has_acceleration_limits").as_bool(); + if ( + limits.has_acceleration_limits && + param_itf->has_parameter(param_base_name + ".max_acceleration")) { - limits.has_acceleration_limits = false; + limits.max_acceleration = + param_itf->get_parameter(param_base_name + ".max_acceleration").as_double(); } - double max_acc; - if ( - has_acceleration_limits && - node->get_parameter(param_base_name + ".max_acceleration", max_acc)) + else { - limits.has_acceleration_limits = true; - limits.max_acceleration = max_acc; + limits.has_acceleration_limits = false; } } // Jerk limits - bool has_jerk_limits = false; - if (node->get_parameter(param_base_name + ".has_jerk_limits", has_jerk_limits)) + if (param_itf->has_parameter(param_base_name + ".has_jerk_limits")) { - if (!has_jerk_limits) + limits.has_jerk_limits = + param_itf->get_parameter(param_base_name + ".has_jerk_limits").as_bool(); + if (limits.has_jerk_limits && param_itf->has_parameter(param_base_name + ".max_jerk")) { - limits.has_jerk_limits = false; + limits.max_jerk = param_itf->get_parameter(param_base_name + ".max_jerk").as_double(); } - double max_jerk; - if (has_jerk_limits && node->get_parameter(param_base_name + ".max_jerk", max_jerk)) + else { - limits.has_jerk_limits = true; - limits.max_jerk = max_jerk; + limits.has_jerk_limits = false; } } // Effort limits - bool has_effort_limits = false; - if (node->get_parameter(param_base_name + ".has_effort_limits", has_effort_limits)) + if (param_itf->has_parameter(param_base_name + ".has_effort_limits")) { - if (!has_effort_limits) + limits.has_effort_limits = + param_itf->get_parameter(param_base_name + ".has_effort_limits").as_bool(); + if (limits.has_effort_limits && param_itf->has_parameter(param_base_name + ".max_effort")) { - limits.has_effort_limits = false; + limits.has_effort_limits = true; + limits.max_effort = param_itf->get_parameter(param_base_name + ".max_effort").as_double(); } - double max_effort; - if (has_effort_limits && node->get_parameter(param_base_name + ".max_effort", max_effort)) + else { - limits.has_effort_limits = true; - limits.max_effort = max_effort; + limits.has_effort_limits = false; } } return true; } +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node Node object for which parameters should be fetched. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * + * \returns True if a limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits) +{ + return get_joint_limits( + joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(), limits); +} + +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * + * \returns True if a limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + JointLimits & limits) +{ + return get_joint_limits( + joint_name, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), limits); +} + /// Populate a SoftJointLimits instance from the ROS parameter server. /** - * It is assumed that the following parameter structure is followed on the provided NodeHandle. Only completely specified soft - * joint limits specifications will be considered valid. + * It is assumed that the parameter structure is the following: * \code - * joint_limits: - * foo_joint: - * soft_lower_limit: 0.0 - * soft_upper_limit: 1.0 - * k_position: 10.0 - * k_velocity: 10.0 + * has_soft_limits: bool + * k_position: double + * k_velocity: double + * soft_lower_limit: double + * soft_upper_limit: double * \endcode * - * This specification is similar to the specification of the safety_controller tag in the URDF, adapted to the parameter server. + * Only completely specified soft joint limits specifications will be considered valid. + * For example a valid yaml configuration would look like: + * \code + * + * ros__parameters: + * joint_limits: + * foo_joint: + * soft_lower_limit: 0.0 + * soft_upper_limit: 1.0 + * k_position: 10.0 + * k_velocity: 10.0 + * \endcode * - * \param[in] joint_name Name of joint whose limits are to be fetched. - * \param[in] node NodeHandle where the joint limits are specified. + * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". + * \param[in] param_itf node parameters interface of the node where parameters are specified. + * \param[in] logging_itf node logging interface to provide log errors. * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite * existing values. * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ inline bool get_joint_limits( - const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + const std::string & joint_name, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, SoftJointLimits & soft_limits) { const std::string param_base_name = "joint_limits." + joint_name; try { if ( - !node->has_parameter(param_base_name + ".has_soft_limits") && - !node->has_parameter(param_base_name + ".k_velocity") && - !node->has_parameter(param_base_name + ".k_position") && - !node->has_parameter(param_base_name + ".soft_lower_limit") && - !node->has_parameter(param_base_name + ".soft_upper_limit")) + !param_itf->has_parameter(param_base_name + ".has_soft_limits") && + !param_itf->has_parameter(param_base_name + ".k_velocity") && + !param_itf->has_parameter(param_base_name + ".k_position") && + !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && + !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) { RCLCPP_DEBUG( - node->get_logger(), + logging_itf->get_logger(), "No soft joint limits specification found for joint '%s' in the parameter server " - "(node: %s param name: %s).", - joint_name.c_str(), node->get_name(), param_base_name.c_str()); + "(param name: %s).", + joint_name.c_str(), param_base_name.c_str()); return false; } } catch (const std::exception & ex) { - RCLCPP_ERROR(node->get_logger(), "%s", ex.what()); + RCLCPP_ERROR(logging_itf->get_logger(), "%s", ex.what()); return false; } // Override soft limits if complete specification is found - bool has_soft_limits; - if (node->get_parameter(param_base_name + ".has_soft_limits", has_soft_limits)) + if (param_itf->has_parameter(param_base_name + ".has_soft_limits")) { if ( - has_soft_limits && node->has_parameter(param_base_name + ".k_position") && - node->has_parameter(param_base_name + ".k_velocity") && - node->has_parameter(param_base_name + ".soft_lower_limit") && - node->has_parameter(param_base_name + ".soft_upper_limit")) + param_itf->get_parameter(param_base_name + ".has_soft_limits").as_bool() && + param_itf->has_parameter(param_base_name + ".k_position") && + param_itf->has_parameter(param_base_name + ".k_velocity") && + param_itf->has_parameter(param_base_name + ".soft_lower_limit") && + param_itf->has_parameter(param_base_name + ".soft_upper_limit")) { - node->get_parameter(param_base_name + ".k_position", soft_limits.k_position); - node->get_parameter(param_base_name + ".k_velocity", soft_limits.k_velocity); - node->get_parameter(param_base_name + ".soft_lower_limit", soft_limits.min_position); - node->get_parameter(param_base_name + ".soft_upper_limit", soft_limits.max_position); + soft_limits.k_position = + param_itf->get_parameter(param_base_name + ".k_position").as_double(); + soft_limits.k_velocity = + param_itf->get_parameter(param_base_name + ".k_velocity").as_double(); + soft_limits.min_position = + param_itf->get_parameter(param_base_name + ".soft_lower_limit").as_double(); + soft_limits.max_position = + param_itf->get_parameter(param_base_name + ".soft_upper_limit").as_double(); return true; } } @@ -333,6 +476,48 @@ inline bool get_joint_limits( return false; } +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] node Node object for which parameters should be fetched. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * + * \returns True if a soft limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp::Node::SharedPtr & node, + SoftJointLimits & soft_limits) +{ + return get_joint_limits( + joint_name, node->get_node_parameters_interface(), node->get_node_logging_interface(), + soft_limits); +} + +/** + * Populate a JointLimits instance from the node parameters. + * This is a convenience function. + * For parameters structure see the underlying `get_joint_limits` function. + * + * \param[in] joint_name Name of joint whose limits are to be fetched. + * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite + * existing values. + * + * \returns True if a soft limits specification is found, false otherwise. + */ +inline bool get_joint_limits( + const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + SoftJointLimits & soft_limits) +{ + return get_joint_limits( + joint_name, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), soft_limits); +} + } // namespace joint_limits #endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index d89af39c0b..12e11638d2 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -15,6 +15,7 @@ ament_cmake rclcpp + rclcpp_lifecycle launch_testing_ament_cmake ament_cmake_gtest diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index cfd1575b6e..c5ddb8f585 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -20,6 +20,7 @@ #include "joint_limits/joint_limits_rosparam.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" class JointLimitsRosParamTest : public ::testing::Test { @@ -60,8 +61,12 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) EXPECT_FALSE(limits.angle_wraparound); // try to read limits for not-existing joints - EXPECT_FALSE(get_joint_limits("bad_joint", node_, limits)); - EXPECT_FALSE(get_joint_limits("unknown_joint", node_, limits)); + EXPECT_FALSE(get_joint_limits( + "bad_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); + EXPECT_FALSE(get_joint_limits( + "unknown_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); // default values should not change EXPECT_FALSE(limits.has_position_limits); @@ -81,7 +86,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Get full specification from parameter server { joint_limits::JointLimits limits; - EXPECT_TRUE(get_joint_limits("foo_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "foo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_TRUE(limits.has_position_limits); EXPECT_EQ(0.0, limits.min_position); @@ -106,7 +113,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Specifying flags but not values should set nothing { joint_limits::JointLimits limits; - EXPECT_TRUE(get_joint_limits("yinfoo_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "yinfoo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); @@ -118,7 +127,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Specifying values but not flags should set nothing { joint_limits::JointLimits limits; - EXPECT_TRUE(get_joint_limits("yangfoo_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "yangfoo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); @@ -130,14 +141,18 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Disable already set values { joint_limits::JointLimits limits; - EXPECT_TRUE(get_joint_limits("foo_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "foo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_TRUE(limits.has_position_limits); EXPECT_TRUE(limits.has_velocity_limits); EXPECT_TRUE(limits.has_acceleration_limits); EXPECT_TRUE(limits.has_jerk_limits); EXPECT_TRUE(limits.has_effort_limits); - EXPECT_TRUE(get_joint_limits("antifoo_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "antifoo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_FALSE(limits.has_velocity_limits); EXPECT_FALSE(limits.has_acceleration_limits); @@ -149,7 +164,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Incomplete position limits specification does not get loaded { joint_limits::JointLimits limits; - EXPECT_TRUE(get_joint_limits("baz_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "baz_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_TRUE(std::isnan(limits.min_position)); @@ -159,7 +176,9 @@ TEST_F(JointLimitsRosParamTest, parse_joint_limits) // Override only one field, leave all others unchanged { joint_limits::JointLimits limits; - EXPECT_TRUE(get_joint_limits("bar_joint", node_, limits)); + EXPECT_TRUE(get_joint_limits( + "bar_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + limits)); EXPECT_FALSE(limits.has_position_limits); EXPECT_TRUE(std::isnan(limits.min_position)); @@ -193,8 +212,12 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) EXPECT_TRUE(std::isnan(soft_limits.k_velocity)); // try to read limits for not-existing joints - EXPECT_FALSE(get_joint_limits("bad_joint", node_, soft_limits)); - EXPECT_FALSE(get_joint_limits("unknown_joint", node_, soft_limits)); + EXPECT_FALSE(get_joint_limits( + "bad_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); + EXPECT_FALSE(get_joint_limits( + "unknown_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); // default values should not change EXPECT_TRUE(std::isnan(soft_limits.min_position)); @@ -206,7 +229,9 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) // Get full specification from parameter server { joint_limits::SoftJointLimits soft_limits; - EXPECT_TRUE(get_joint_limits("foo_joint", node_, soft_limits)); + EXPECT_TRUE(get_joint_limits( + "foo_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); EXPECT_EQ(10.0, soft_limits.k_position); EXPECT_EQ(20.0, soft_limits.k_velocity); @@ -217,7 +242,9 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) // Skip parsing soft limits if has_soft_limits is false { joint_limits::SoftJointLimits soft_limits; - EXPECT_FALSE(get_joint_limits("foobar_joint", node_, soft_limits)); + EXPECT_FALSE(get_joint_limits( + "foobar_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); EXPECT_TRUE(std::isnan(soft_limits.min_position)); EXPECT_TRUE(std::isnan(soft_limits.max_position)); EXPECT_TRUE(std::isnan(soft_limits.k_position)); @@ -227,7 +254,9 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) // Incomplete soft limits specification does not get loaded { joint_limits::SoftJointLimits soft_limits; - EXPECT_FALSE(get_joint_limits("barbaz_joint", node_, soft_limits)); + EXPECT_FALSE(get_joint_limits( + "barbaz_joint", node_->get_node_parameters_interface(), node_->get_node_logging_interface(), + soft_limits)); EXPECT_TRUE(std::isnan(soft_limits.min_position)); EXPECT_TRUE(std::isnan(soft_limits.max_position)); EXPECT_TRUE(std::isnan(soft_limits.k_position)); @@ -235,6 +264,152 @@ TEST_F(JointLimitsRosParamTest, parse_soft_joint_limits) } } +class JointLimitsUndeclaredRosParamTest : public ::testing::Test +{ +public: + void SetUp() { node_ = rclcpp::Node::make_shared("JointLimitsRosparamTestNode"); } + + void TearDown() { node_.reset(); } + +protected: + rclcpp::Node::SharedPtr node_; +}; + +class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test +{ +public: + void SetUp() + { + lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("JointLimitsRosparamTestNode"); + } + + void TearDown() { lifecycle_node_.reset(); } + +protected: + rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_; +}; + +TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_joint_limits_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::JointLimits limits; + // try to read limits for not-existing joints + EXPECT_FALSE(get_joint_limits("bad_joint", node_, limits)); + EXPECT_FALSE(get_joint_limits("unknown_joint", node_, limits)); + + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", node_, limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", node_, limits)); + + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(0.0, limits.min_position); + EXPECT_EQ(1.0, limits.max_position); + + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_EQ(5.0, limits.max_acceleration); + + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_EQ(100.0, limits.max_jerk); + + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_EQ(20.0, limits.max_effort); + + // parameters is 'true', but because there are position limits it is ignored + EXPECT_FALSE(limits.angle_wraparound); + } +} + +TEST_F(JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_joint_limits_lifecycle_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::JointLimits limits; + // try to read limits for not-existing joints + EXPECT_FALSE(get_joint_limits("bad_joint", lifecycle_node_, limits)); + EXPECT_FALSE(get_joint_limits("unknown_joint", lifecycle_node_, limits)); + + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", lifecycle_node_, limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", lifecycle_node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", lifecycle_node_, limits)); + + EXPECT_TRUE(limits.has_position_limits); + EXPECT_EQ(0.0, limits.min_position); + EXPECT_EQ(1.0, limits.max_position); + + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_EQ(2.0, limits.max_velocity); + + EXPECT_TRUE(limits.has_acceleration_limits); + EXPECT_EQ(5.0, limits.max_acceleration); + + EXPECT_TRUE(limits.has_jerk_limits); + EXPECT_EQ(100.0, limits.max_jerk); + + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_EQ(20.0, limits.max_effort); + + // parameters is 'true', but because there are position limits it is ignored + EXPECT_FALSE(limits.angle_wraparound); + } +} + +TEST_F(JointLimitsUndeclaredRosParamTest, parse_declared_soft_joint_limits_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::SoftJointLimits soft_limits; + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", node_, soft_limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", node_, soft_limits)); + + EXPECT_EQ(10.0, soft_limits.k_position); + EXPECT_EQ(20.0, soft_limits.k_velocity); + EXPECT_EQ(0.1, soft_limits.min_position); + EXPECT_EQ(0.9, soft_limits.max_position); + } +} + +TEST_F( + JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_soft_joint_limits_lifecycle_node) +{ + // Get full specification from parameter server - no need to test logic + { + joint_limits::SoftJointLimits soft_limits; + // try to read existing but undeclared joint + EXPECT_FALSE(get_joint_limits("foo_joint", lifecycle_node_, soft_limits)); + + // declare parameters + EXPECT_TRUE(joint_limits::declare_parameters("foo_joint", lifecycle_node_)); + + // now should be successful + EXPECT_TRUE(get_joint_limits("foo_joint", lifecycle_node_, soft_limits)); + + EXPECT_EQ(10.0, soft_limits.k_position); + EXPECT_EQ(20.0, soft_limits.k_velocity); + EXPECT_EQ(0.1, soft_limits.min_position); + EXPECT_EQ(0.9, soft_limits.max_position); + } +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From 950e1a616bc5e5faa8f6af0c646eab8896fbae5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 7 Jul 2022 22:38:35 +0200 Subject: [PATCH 12/13] Remove 'debug_to_string' methods and use only 'to_string'. --- .../include/joint_limits/joint_limits.hpp | 39 ------------------- 1 file changed, 39 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index a6bd1edf15..34362c1d7b 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -67,43 +67,6 @@ struct JointLimits { std::stringstream ss_output; - if (has_position_limits) - { - ss_output << " position limits: " - << "[" << min_position << ", " << max_position << "]\n"; - } - if (has_velocity_limits) - { - ss_output << " velocity limit: " - << "[" << max_velocity << "]\n"; - } - if (has_acceleration_limits) - { - ss_output << " acceleration limit: " - << "[" << max_acceleration << "]\n"; - } - if (has_jerk_limits) - { - ss_output << " jerk limit: " - << "[" << max_jerk << "]\n"; - } - if (has_effort_limits) - { - ss_output << " effort limit: " - << "[" << max_effort << "]\n"; - } - if (angle_wraparound) - { - ss_output << " angle wraparound is active."; - } - - return ss_output.str(); - } - - std::string debug_to_string() - { - std::stringstream ss_output; - ss_output << " has position limits: " << (has_position_limits ? "true" : "false") << "[" << min_position << ", " << max_position << "]\n"; ss_output << " has velocity limits: " << (has_velocity_limits ? "true" : "false") << "[" @@ -154,8 +117,6 @@ struct SoftJointLimits return ss_output.str(); } - - std::string debug_to_string() { return to_string(); } }; } // namespace joint_limits From 175509a2f23c785287f27a0f5fb8f217ce8e324d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 7 Jul 2022 22:42:11 +0200 Subject: [PATCH 13/13] Update documentation according to the comment. --- .../include/joint_limits/joint_limits.hpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index 34362c1d7b..ae1168d27d 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -85,7 +85,23 @@ struct JointLimits /** * SoftJointLimits stores values from the `` tag of URDF. - * For details about meaning of each variable check here: https://wiki.ros.org/urdf/XML/joint + * The meaning of the fields are: + * + * An element can contain the following attributes: + * + * **soft_lower_limit** (optional, defaults to 0) - An attribute specifying the lower joint boundary + * where the safety controller starts limiting the position of the joint. This limit needs to be + * larger than the lower joint limit (see above). See See safety limits for more details. + * + * **soft_upper_limit** (optional, defaults to 0) - An attribute specifying the upper joint boundary + * where the safety controller starts limiting the position of the joint. This limit needs to be + * smaller than the upper joint limit (see above). See See safety limits for more details. + * + * **k_position** (optional, defaults to 0) - An attribute specifying the relation between position + * and velocity limits. See See safety limits for more details. + * + * k_velocity (required) - An attribute specifying the relation between effort and velocity limits. + * See See safety limits for more details. */ struct SoftJointLimits {