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 <zelenak@picknik.ai>

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

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 <string>
+#include <vector>
+
+#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 <typename LimitsType>
+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<std::string> 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<LimitsType> joint_limits_;
+  rclcpp::Node::SharedPtr node_;
+};
+
+}  // namespace joint_limits
+
+#endif  // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_

hardware_interface/include/joint_limits/joint_limits.hpp → joint_limits/include/joint_limits/joint_limits.hpp

@@ -18,6 +18,8 @@
 #define JOINT_LIMITS__JOINT_LIMITS_HPP_
 
 #include <limits>
+#include <sstream>
+#include <string>
 
 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

hardware_interface/include/joint_limits/joint_limits_rosparam.hpp → 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)
 {

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 <string>
+
+#include "joint_limits/joint_limiter_interface.hpp"
+#include "joint_limits/joint_limits.hpp"
+
+namespace joint_limits
+{
+template <typename LimitsType>
+class SimpleJointLimiter : public JointLimiterInterface<JointLimits>
+{
+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_

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_

joint_limits/joint_limiters.xml

@@ -0,0 +1,9 @@
+<library path="simple_joint_limiter">
+  <class name="joint_limits/SimpleJointLimiter"
+         type="joint_limits::SimpleJointLimiter<joint_limits::JointLimits>"
+         base_class_type="joint_limits::JointLimiterInterface<joint_limits::JointLimits>">
+    <description>
+      Simple joint limiter using clamping approach.
+    </description>
+  </class>
+</library>