functional integration of joint limiter in resource manager

hardware_interface/src/resource_manager.cpp

@@ -673,10 +673,13 @@ class ResourceStorage
       limiters_data_[hw_info.name] = {};
       for (const auto & [joint_name, limits] : hw_info.limits)
       {
+        RCUTILS_LOG_INFO_NAMED(
+          "resource_manager", "%s with limits:%s", joint_name.c_str(), limits.to_string().c_str());
+
         std::unique_ptr<
           joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>
           limits_interface;
-        const joint_limits::JointLimits hard_limits{limits};
+        const joint_limits::JointLimits hard_limit{limits};
         joint_limits::JointInterfacesCommandLimiterData data;
         data.joint_name = joint_name;
         limiters_data_[hw_info.name].push_back(data);
@@ -685,16 +688,15 @@ class ResourceStorage
         {
           std::vector<joint_limits::SoftJointLimits> soft_limits = {
             hw_info.soft_limits.at(joint_name)};
-          limits_interface = std::make_unique<
-            joint_limits::SoftJointLimiter<joint_limits::JointControlInterfacesData>>();
+          limits_interface = std::make_unique<joint_limits::JointSoftLimiter>();
+          limits_interface->init({joint_name}, {hard_limit}, soft_limits, nullptr, nullptr);
         }
         // no soft limits given so we use the joint saturation limiter (joint_range_limiter.hpp)
         else
         {
           limits_interface = std::make_unique<
             joint_limits::JointSaturationLimiter<joint_limits::JointControlInterfacesData>>();
-
-          limits_interface->init({joint_name}, hard_limits, soft_limits, nullptr, nullptr);
+          limits_interface->init({joint_name}, {hard_limit}, {}, nullptr, nullptr);
         }
         joint_limiters_interface_[hw_info.name].push_back(std::move(limits_interface));
       }
@@ -713,28 +715,28 @@ class ResourceStorage
       interface_map.end())
     {
       state.position =
-        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_POSITION).get_value();
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_POSITION)->get_value();
     }
     if (
       interface_map.find(joint_name + "/" + hardware_interface::HW_IF_VELOCITY) !=
       interface_map.end())
     {
       state.velocity =
-        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_VELOCITY).get_value();
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_VELOCITY)->get_value();
     }
     if (
       interface_map.find(joint_name + "/" + hardware_interface::HW_IF_EFFORT) !=
       interface_map.end())
     {
       state.effort =
-        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_EFFORT).get_value();
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_EFFORT)->get_value();
     }
     if (
       interface_map.find(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) !=
       interface_map.end())
     {
       state.acceleration =
-        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION).get_value();
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)->get_value();
     }
   }
 
@@ -749,39 +751,39 @@ class ResourceStorage
       state.position.has_value())
     {
       interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_POSITION)
-        .set_value(state.position.value());
+        ->set_value(state.position.value());
     }
     if (
       interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY) !=
         interface_map.end() &&
       state.velocity.has_value())
     {
       interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY)
-        .set_value(state.velocity.value());
+        ->set_value(state.velocity.value());
     }
     if (
       interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT) !=
         interface_map.end() &&
       state.effort.has_value())
     {
       interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT)
-        .set_value(state.effort.value());
+        ->set_value(state.effort.value());
     }
     if (
       interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) !=
         interface_map.end() &&
       state.acceleration.has_value())
     {
       interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)
-        .set_value(state.acceleration.value());
+        ->set_value(state.acceleration.value());
     }
   }
 
   void update_joint_limiters_data()
   {
-    for (auto & joint_limiter_data : limiters_data_)
+    for (auto & [hw_info_name, joint_limiter_data] : limiters_data_)
     {
-      for (auto & data : joint_limiter_data.second)
+      for (auto & data : joint_limiter_data)
       {
         update_joint_limiters_state(data.joint_name, state_interface_map_, data.actual);
         update_joint_limiters_state(data.joint_name, command_interface_map_, data.command);

joint_limits/CMakeLists.txt

@@ -57,7 +57,7 @@ ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEP
 add_library(joint_saturation_limiter SHARED
   src/joint_saturation_limiter.cpp
   src/joint_range_limiter.cpp
-  src/soft_joint_limiter.cpp
+  src/joint_soft_limiter.cpp
 )
 target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17)
 target_include_directories(joint_saturation_limiter PUBLIC
@@ -117,11 +117,11 @@ if(BUILD_TESTING)
     rclcpp
   )
 
-  ament_add_gtest(test_soft_joint_limiter test/test_soft_joint_limiter.cpp)
-  target_include_directories(test_soft_joint_limiter PRIVATE include)
-  target_link_libraries(test_soft_joint_limiter joint_limiter_interface)
+  ament_add_gtest(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_soft_joint_limiter
+    test_joint_soft_limiter
     pluginlib
     rclcpp
   )

joint_limits/include/joint_limits/joint_limits.hpp

@@ -67,7 +67,7 @@ struct JointLimits
   bool has_effort_limits;
   bool angle_wraparound;
 
-  std::string to_string()
+  std::string to_string() const
   {
     std::stringstream ss_output;
 

joint_limits/include/joint_limits/joint_soft_limiter.hpp

@@ -27,7 +27,7 @@ constexpr double VALUE_CONSIDERED_ZERO = 1e-10;
 namespace joint_limits
 {
 
-class SoftJointLimiter : public JointSaturationLimiter<JointControlInterfacesData>
+class JointSoftLimiter : public JointSaturationLimiter<JointControlInterfacesData>
 {
 public:
   bool on_init() override

joint_limits/src/joint_limits_helpers.cpp

@@ -86,12 +86,12 @@ std::pair<double, double> compute_velocity_limits(
     vel_limits.first = std::max(prev_command_vel.value() - delta_vel, vel_limits.first);
     vel_limits.second = std::min(prev_command_vel.value() + delta_vel, vel_limits.second);
   }
-  RCLCPP_ERROR(
+  RCLCPP_DEBUG(
     rclcpp::get_logger("joint_limiter_interface"),
     "Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), vel_limits.first,
     vel_limits.second);
   internal::check_and_swap_limits(vel_limits);
-  RCLCPP_ERROR(
+  RCLCPP_DEBUG(
     rclcpp::get_logger("joint_limiter_interface"),
     "After swapping Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(),
     vel_limits.first, vel_limits.second);

joint_limits/src/soft_joint_limiter.cpp → joint_limits/src/joint_soft_limiter.cpp

@@ -18,7 +18,7 @@
 namespace joint_limits
 {
 
-bool SoftJointLimiter::on_enforce(
+bool JointSoftLimiter::on_enforce(
   JointControlInterfacesData & actual, JointControlInterfacesData & desired,
   const rclcpp::Duration & dt)
 {
@@ -260,7 +260,7 @@ bool SoftJointLimiter::on_enforce(
 
 #include "pluginlib/class_list_macros.hpp"
 
-typedef joint_limits::SoftJointLimiter JointInterfacesSoftLimiter;
+typedef joint_limits::JointSoftLimiter JointInterfacesSoftLimiter;
 typedef joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>
   JointInterfacesLimiterInterfaceBase;
 PLUGINLIB_EXPORT_CLASS(JointInterfacesSoftLimiter, JointInterfacesLimiterInterfaceBase)

joint_limits/test/test_joint_limiter.hpp

@@ -129,10 +129,10 @@ class JointSaturationLimiterTest : public JointLimiterTest
   }
 };
 
-class SoftJointLimiterTest : public JointLimiterTest
+class JointSoftLimiterTest : public JointLimiterTest
 {
 public:
-  SoftJointLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {}
+  JointSoftLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {}
 
   bool Init(
     const joint_limits::JointLimits & limits, const joint_limits::SoftJointLimits & soft_limits,

joint_limits/test/test_soft_joint_limiter.cpp → joint_limits/test/test_joint_soft_limiter.cpp

@@ -18,17 +18,17 @@
 #include <limits>
 #include "test_joint_limiter.hpp"
 
-TEST_F(SoftJointLimiterTest, when_loading_limiter_plugin_expect_loaded)
+TEST_F(JointSoftLimiterTest, when_loading_limiter_plugin_expect_loaded)
 {
-  // Test SoftJointLimiter loading
+  // Test JointSoftLimiter loading
   ASSERT_NO_THROW(
     joint_limiter_ = std::unique_ptr<JointLimiter>(
       joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)));
   ASSERT_NE(joint_limiter_, nullptr);
 }
 
 // NOTE: We accept also if there is no limit defined for a joint.
-TEST_F(SoftJointLimiterTest, when_joint_not_found_expect_init_fail)
+TEST_F(JointSoftLimiterTest, when_joint_not_found_expect_init_fail)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());
@@ -37,7 +37,7 @@ TEST_F(SoftJointLimiterTest, when_joint_not_found_expect_init_fail)
   ASSERT_TRUE(joint_limiter_->init(joint_names, node_));
 }
 
-TEST_F(SoftJointLimiterTest, when_invalid_dt_expect_enforce_fail)
+TEST_F(JointSoftLimiterTest, when_invalid_dt_expect_enforce_fail)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());
@@ -48,7 +48,7 @@ TEST_F(SoftJointLimiterTest, when_invalid_dt_expect_enforce_fail)
   ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period));
 }
 
-TEST_F(SoftJointLimiterTest, check_desired_position_only_cases)
+TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
 {
   SetupNode("soft_joint_limiter");
   ASSERT_TRUE(Load());
@@ -276,7 +276,7 @@ TEST_F(SoftJointLimiterTest, check_desired_position_only_cases)
   EXPECT_FALSE(desired_state_.has_jerk());
 }
 
-TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases)
+TEST_F(JointSoftLimiterTest, check_desired_velocity_only_cases)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());
@@ -500,7 +500,7 @@ TEST_F(SoftJointLimiterTest, check_desired_velocity_only_cases)
   test_limit_enforcing(6.0, -1.0, 0.0, true);
 }
 
-TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases)
+TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());
@@ -779,7 +779,7 @@ TEST_F(SoftJointLimiterTest, check_desired_effort_only_cases)
   }
 }
 
-TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases)
+TEST_F(JointSoftLimiterTest, check_desired_acceleration_only_cases)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());
@@ -869,7 +869,7 @@ TEST_F(SoftJointLimiterTest, check_desired_acceleration_only_cases)
   test_limit_enforcing(-0.4, 3.0, 0.25, true);
 }
 
-TEST_F(SoftJointLimiterTest, check_desired_jerk_only_cases)
+TEST_F(JointSoftLimiterTest, check_desired_jerk_only_cases)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());
@@ -911,7 +911,7 @@ TEST_F(SoftJointLimiterTest, check_desired_jerk_only_cases)
   test_limit_enforcing(-1.5, -0.5, true);
 }
 
-TEST_F(SoftJointLimiterTest, check_all_desired_references_limiting)
+TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting)
 {
   SetupNode("joint_saturation_limiter");
   ASSERT_TRUE(Load());