diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 630cb9f27c..21065fa4c5 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/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; } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 424d54e882..2a7311fea0 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/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(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); diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 61a5e5b313..abfa8868e2 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/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; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp index eb2bbf24c7..e012d92407 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp +++ b/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_