Fix the tests with the new changes of considering actual position for…

… limiting the desired

hardware_interface_testing/test/test_components/test_actuator.cpp

@@ -109,7 +109,7 @@ class TestActuator : public ActuatorInterface
     return hardware_interface::return_type::OK;
   }
 
-  return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
+  return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override
   {
     // simulate error on read
     if (velocity_command_ == test_constants::READ_FAIL_VALUE)
@@ -130,6 +130,13 @@ class TestActuator : public ActuatorInterface
     // is no "state = command" line or some other mixture of interfaces
     // somewhere in the test stack.
     velocity_state_ = velocity_command_ / 2;
+    position_state_ += velocity_state_ * period.seconds();
+
+    if (velocity_command_ == test_constants::RESET_STATE_INTERFACES_VALUE)
+    {
+      position_state_ = 0.0;
+      velocity_state_ = 0.0;
+    }
     return return_type::OK;
   }
 

hardware_interface_testing/test/test_resource_manager.cpp

@@ -2188,6 +2188,7 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
 
     state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1]));
     state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1]));
+    state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[0]));
 
     check_if_interface_available(true, true);
     // with default values read and write should run without any problems
@@ -2280,7 +2281,9 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       // enforcing limits
       rm->enforce_command_limits(duration);
 
-      EXPECT_EQ(claimed_itfs[0].get_value(), 0.0);
+      ASSERT_NEAR(state_itfs[2].get_value(), 5.05, 0.00001);
+      // it is limited to the M_PI as the actual position is outside the range
+      EXPECT_NEAR(claimed_itfs[0].get_value(), M_PI, 0.00001);
       EXPECT_EQ(claimed_itfs[1].get_value(), 0.0);
 
       auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
@@ -2292,19 +2295,34 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       auto [read_ok, failed_hardware_names_read] = rm->read(time, duration);
       EXPECT_TRUE(read_ok);
       EXPECT_TRUE(failed_hardware_names_read.empty());
+
+      ASSERT_NEAR(state_itfs[0].get_value(), M_PI_2, 0.00001);
+      ASSERT_EQ(state_itfs[1].get_value(), 0.0);
+    }
+
+    // Reset the position state interface of actuator to zero
+    {
+      ASSERT_GT(state_itfs[2].get_value(), 5.05);
+      claimed_itfs[0].set_value(test_constants::RESET_STATE_INTERFACES_VALUE);
+      auto [read_ok, failed_hardware_names_read] = rm->read(time, duration);
+      EXPECT_TRUE(read_ok);
+      EXPECT_TRUE(failed_hardware_names_read.empty());
+      ASSERT_EQ(state_itfs[2].get_value(), 0.0);
+      claimed_itfs[0].set_value(0.0);
+      claimed_itfs[1].set_value(0.0);
     }
 
     double new_state_value_1 = state_itfs[0].get_value();
     double new_state_value_2 = state_itfs[1].get_value();
     // Now loop and see that the joint limits are being enforced progressively
-    for (size_t i = 1; i < 300; i++)
+    for (size_t i = 1; i < 2000; i++)
     {
-      // let's amplifiy the limit enforce period, to test more rapidly. It would work with 0.01s as
+      // let's amplify the limit enforce period, to test more rapidly. It would work with 0.01s as
       // well
       const rclcpp::Duration enforce_period =
         rclcpp::Duration::from_seconds(duration.seconds() * 10.0);
 
-      auto [ok, failed_hardware_names] = rm->read(time, duration);
+      auto [ok, failed_hardware_names] = rm->read(time, enforce_period);
       EXPECT_TRUE(ok);
       EXPECT_TRUE(failed_hardware_names.empty());
 
@@ -2321,9 +2339,9 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
 
       // the joint limits value is same as in the parsed URDF
       const double velocity_joint_1 = 0.2;
-      EXPECT_NEAR(
+      ASSERT_NEAR(
         claimed_itfs[0].get_value(),
-        std::min((velocity_joint_1 * (enforce_period.seconds() * static_cast<double>(i))), M_PI),
+        std::min(state_itfs[2].get_value() + (velocity_joint_1 * enforce_period.seconds()), M_PI),
         1.0e-8)
         << "This should be progressively increasing as it is a position limit for iteration : "
         << i;
@@ -2334,7 +2352,7 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest
       new_state_value_1 = claimed_itfs[0].get_value() / 2.0;
       new_state_value_2 = claimed_itfs[1].get_value() / 2.0;
 
-      auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
+      auto [ok_write, failed_hardware_names_write] = rm->write(time, enforce_period);
       EXPECT_TRUE(ok_write);
       EXPECT_TRUE(failed_hardware_names_write.empty());
       node_.get_clock()->sleep_until(time + duration);

joint_limits/src/joint_limits_helpers.cpp

@@ -52,8 +52,10 @@ PositionLimits compute_position_limits(
     const double delta_pos = max_vel * dt;
     const double position_reference =
       act_pos.has_value() ? act_pos.value() : prev_command_pos.value();
-    pos_limits.lower_limit = std::max(position_reference - delta_pos, pos_limits.lower_limit);
-    pos_limits.upper_limit = std::min(position_reference + delta_pos, pos_limits.upper_limit);
+    pos_limits.lower_limit = std::max(
+      std::min(position_reference - delta_pos, pos_limits.upper_limit), pos_limits.lower_limit);
+    pos_limits.upper_limit = std::min(
+      std::max(position_reference + delta_pos, pos_limits.lower_limit), pos_limits.upper_limit);
   }
   internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit);
   return pos_limits;

ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp

@@ -24,5 +24,6 @@ constexpr double READ_FAIL_VALUE = 28282828.0;
 constexpr double WRITE_FAIL_VALUE = 23232323.0;
 constexpr double READ_DEACTIVATE_VALUE = 29292929.0;
 constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0;
+constexpr double RESET_STATE_INTERFACES_VALUE = 82937364.0;
 }  // namespace test_constants
 #endif  // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_