Merge branch 'master' into integrate/pal_statistics

Author: saikishor

.pre-commit-config.yaml

@@ -37,7 +37,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.19.0
+    rev: v3.19.1
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.4
+    rev: v19.1.5
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -100,7 +100,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 public:
   ControllerInterfaceBase() = default;
 
-  virtual ~ControllerInterfaceBase() = default;
+  virtual ~ControllerInterfaceBase();
 
   /// Get configuration for controller's required command interfaces.
   /**

controller_interface/src/controller_interface_base.cpp

@@ -23,6 +23,22 @@
 
 namespace controller_interface
 {
+ControllerInterfaceBase::~ControllerInterfaceBase()
+{
+  // check if node is initialized and we still have a valid context
+  if (
+    node_.get() &&
+    get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED &&
+    rclcpp::ok())
+  {
+    RCLCPP_DEBUG(
+      get_node()->get_logger(),
+      "Calling shutdown transition of controller node '%s' due to destruction.",
+      get_node()->get_name());
+    node_->shutdown();
+  }
+}
+
 return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
@@ -53,6 +69,11 @@ return_type ControllerInterfaceBase::init(
       break;
     case LifecycleNodeInterface::CallbackReturn::ERROR:
     case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Calling shutdown transition of controller node '%s' due to init failure.",
+        get_node()->get_name());
+      node_->shutdown();
       return return_type::ERROR;
   }
 
@@ -169,6 +190,10 @@ void ControllerInterfaceBase::release_interfaces()
 
 const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
 {
+  if (!node_.get())
+  {
+    throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
+  }
   return node_->get_current_state();
 }
 

controller_interface/test/test_controller_interface.cpp

@@ -36,16 +36,21 @@ TEST(TestableControllerInterface, init)
   rclcpp::init(argc, argv);
 
   TestableControllerInterface controller;
+  const TestableControllerInterface & const_controller = controller;
 
   // try to get node when not initialized
   ASSERT_THROW(controller.get_node(), std::runtime_error);
+  ASSERT_THROW(const_controller.get_node(), std::runtime_error);
+  ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error);
 
   // initialize, create node
   const auto node_options = controller.define_custom_node_options();
   ASSERT_EQ(
     controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options),
     controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
+  ASSERT_NO_THROW(const_controller.get_node());
+  ASSERT_NO_THROW(controller.get_lifecycle_state());
 
   // update_rate is set to controller_manager's rate
   ASSERT_EQ(controller.get_update_rate(), 10u);
@@ -54,6 +59,7 @@ TEST(TestableControllerInterface, init)
   controller.configure();
   ASSERT_EQ(controller.get_update_rate(), 10u);
 
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -80,6 +86,8 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
   // The configure should fail and the update rate should stay the same
   ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
   ASSERT_EQ(controller.get_update_rate(), 1000u);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -149,6 +157,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   ASSERT_EQ(controller.get_update_rate(), 623u);
 
   executor->cancel();
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -166,6 +175,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
     controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options),
     controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(
+    controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 
@@ -183,6 +194,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
     controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
     controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(
+    controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 

controller_interface/test/test_controller_with_options.cpp

@@ -41,7 +41,7 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -54,6 +54,8 @@ TEST(ControllerWithOption, init_with_overrides)
   EXPECT_EQ(controller.params["parameter1"], 1.);
   EXPECT_EQ(controller.params["parameter2"], 2.);
   EXPECT_EQ(controller.params["parameter3"], 3.);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -68,7 +70,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
   controller_node_options.arguments(
     {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.",
      "-p", "parameter_list.parameter3:=3."});
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller_node_options),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -81,6 +83,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
   EXPECT_EQ(controller.params["parameter1"], 1.);
   EXPECT_EQ(controller.params["parameter2"], 2.);
   EXPECT_EQ(controller.params["parameter3"], 3.);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -96,7 +100,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
   std::cerr << params_file_path << std::endl;
   auto controller_node_options = controller.define_custom_node_options();
   controller_node_options.arguments({"--ros-args", "--params-file", params_file_path});
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller_node_options),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -112,6 +116,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
   bool use_sim_time(true);
   controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
   ASSERT_FALSE(use_sim_time);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -130,7 +136,7 @@ TEST(
   controller_node_options.arguments(
     {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785",
      "-p", "use_sim_time:=true"});
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller_node_options),
     controller_interface::return_type::OK);
   // checks that the node options have been updated
@@ -146,6 +152,8 @@ TEST(
   bool use_sim_time(false);
   controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
   ASSERT_TRUE(use_sim_time);
+
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -157,7 +165,7 @@ TEST(ControllerWithOption, init_without_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(
+  ASSERT_EQ(
     controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
     controller_interface::return_type::ERROR);
   // checks that the node options have been updated
@@ -166,5 +174,6 @@ TEST(ControllerWithOption, init_without_overrides)
   EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
   // checks that no parameter has been declared from overrides
   EXPECT_EQ(controller.params.size(), 0u);
+
   rclcpp::shutdown();
 }

controller_manager/src/controller_manager.cpp

@@ -256,7 +256,8 @@ ControllerManager::ControllerManager(
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
-  cm_node_options_(options)
+  cm_node_options_(options),
+  robot_description_(urdf)
 {
   initialize_parameters();
   resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(

hardware_interface/doc/writing_new_hardware_component.rst

@@ -22,14 +22,12 @@ The following is a step-by-step guide to create source files, basic tests, and c
    After creating the package, you should have at least ``CMakeLists.txt`` and ``package.xml`` files in it.
    Create also ``include/<PACKAGE_NAME>/`` and ``src`` folders if they do not already exist.
    In ``include/<PACKAGE_NAME>/`` folder add ``<robot_hardware_interface_name>.hpp`` and ``<robot_hardware_interface_name>.cpp`` in the ``src`` folder.
-   Optionally add ``visibility_control.h`` with the definition of export rules for Windows.
-   You can copy this file from an existing controller package and change the name prefix to the ``<PACKAGE_NAME>``.
 
 #. **Adding declarations into header file (.hpp)**
 
    1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ).
 
-   2. Include ``"hardware_interface/$interface_type$_interface.hpp"`` and ``visibility_control.h`` if you are using one.
+   2. Include ``"hardware_interface/$interface_type$_interface.hpp"``.
       ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check :ref:`Hardware Components description <overview_hardware_components>`.
 
    3. Define a unique namespace for your hardware_interface. This is usually the package name written in ``snake_case``.

hardware_interface/src/component_parser.cpp

@@ -20,7 +20,12 @@
 #include <unordered_map>
 #include <vector>
 
+#include "rclcpp/version.h"
+#if RCLCPP_VERSION_GTE(29, 0, 0)
+#include "urdf/model.hpp"
+#else
 #include "urdf/model.h"
+#endif
 
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/hardware_info.hpp"

joint_limits/CMakeLists.txt

@@ -44,14 +44,28 @@ target_include_directories(joint_limiter_interface PUBLIC
 )
 ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+add_library(joint_limits_helpers SHARED
+  src/joint_limits_helpers.cpp
+)
+target_compile_features(joint_limits_helpers PUBLIC cxx_std_17)
+target_include_directories(joint_limits_helpers PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/joint_limits>
+)
+ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 add_library(joint_saturation_limiter SHARED
   src/joint_saturation_limiter.cpp
+  src/joint_range_limiter.cpp
+  src/joint_soft_limiter.cpp
 )
 target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17)
 target_include_directories(joint_saturation_limiter PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/joint_limits>
 )
+target_link_libraries(joint_saturation_limiter PUBLIC joint_limits_helpers)
+
 ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml)
@@ -91,6 +105,24 @@ if(BUILD_TESTING)
     rclcpp
   )
 
+  ament_add_gmock(test_joint_range_limiter test/test_joint_range_limiter.cpp)
+  target_include_directories(test_joint_range_limiter PRIVATE include)
+  target_link_libraries(test_joint_range_limiter joint_limiter_interface)
+  ament_target_dependencies(
+    test_joint_range_limiter
+    pluginlib
+    rclcpp
+  )
+
+  ament_add_gmock(test_joint_soft_limiter test/test_joint_soft_limiter.cpp)
+  target_include_directories(test_joint_soft_limiter PRIVATE include)
+  target_link_libraries(test_joint_soft_limiter joint_limiter_interface)
+  ament_target_dependencies(
+    test_joint_soft_limiter
+    pluginlib
+    rclcpp
+  )
+
 endif()
 
 install(
@@ -101,6 +133,7 @@ install(TARGETS
     joint_limits
     joint_limiter_interface
     joint_saturation_limiter
+    joint_limits_helpers
   EXPORT export_joint_limits
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

joint_limits/include/joint_limits/data_structures.hpp

@@ -0,0 +1,66 @@
+// Copyright 2024 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 Sai Kishor Kothakota
+
+#ifndef JOINT_LIMITS__DATA_STRUCTURES_HPP_
+#define JOINT_LIMITS__DATA_STRUCTURES_HPP_
+
+#include <memory>
+#include <optional>
+#include <string>
+
+namespace joint_limits
+{
+
+struct JointControlInterfacesData
+{
+  std::string joint_name;
+  std::optional<double> position = std::nullopt;
+  std::optional<double> velocity = std::nullopt;
+  std::optional<double> effort = std::nullopt;
+  std::optional<double> acceleration = std::nullopt;
+  std::optional<double> jerk = std::nullopt;
+
+  bool has_data() const
+  {
+    return has_position() || has_velocity() || has_effort() || has_acceleration() || has_jerk();
+  }
+
+  bool has_position() const { return position.has_value(); }
+
+  bool has_velocity() const { return velocity.has_value(); }
+
+  bool has_effort() const { return effort.has_value(); }
+
+  bool has_acceleration() const { return acceleration.has_value(); }
+
+  bool has_jerk() const { return jerk.has_value(); }
+};
+
+struct JointInterfacesCommandLimiterData
+{
+  std::string joint_name;
+  JointControlInterfacesData actual;
+  JointControlInterfacesData command;
+  JointControlInterfacesData prev_command;
+  JointControlInterfacesData limited;
+
+  bool has_actual_data() const { return actual.has_data(); }
+
+  bool has_command_data() const { return command.has_data(); }
+};
+
+}  // namespace joint_limits
+#endif  // JOINT_LIMITS__DATA_STRUCTURES_HPP_