From 063fbc30e3d1ca9a843b241869e11d2bb32a1f4c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 19:58:58 +0100 Subject: [PATCH 01/16] Add first integration of the joint_limits to the ResourceManager --- hardware_interface/src/resource_manager.cpp | 152 ++++++++++++++++++++ 1 file changed, 152 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7a616d890d..41554832c6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -32,6 +32,9 @@ #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" +#include "joint_limits/joint_soft_limiter.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/logging.hpp" @@ -657,6 +660,130 @@ class ResourceStorage } } + void import_joint_limiters(const std::vector & hardware_infos) + { + for (const auto & hw_info : hardware_infos) + { + limiters_data_[hw_info.name] = {}; + for (const auto & [joint_name, limits] : hw_info.limits) + { + std::vector soft_limits; + const std::vector hard_limits{limits}; + joint_limits::JointInterfacesCommandLimiterData data; + data.joint_name = joint_name; + limiters_data_[hw_info.name].push_back(data); + // If the joint limits is found in the softlimits, then extract it + if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end()) + { + soft_limits = {hw_info.soft_limits.at(joint_name)}; + } + std::unique_ptr< + joint_limits::JointLimiterInterface> + limits_interface; + if (soft_limits.empty()) + { + limits_interface = std::make_unique< + joint_limits::JointSaturationLimiter>(); + } + else + { + limits_interface = std::make_unique(); + } + limits_interface->init({joint_name}, hard_limits, soft_limits, nullptr, nullptr); + joint_limiters_interface_[hw_info.name].push_back(std::move(limits_interface)); + } + } + } + + template + void update_joint_limiters_state( + const std::string & joint_name, const std::map & interface_map, + joint_limits::JointControlInterfacesData & state) + { + state.joint_name = joint_name; + // update the actual state of the limiters + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_POSITION) != + interface_map.end()) + { + state.position = + 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(); + } + 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(); + } + 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(); + } + } + + template + void update_joint_limiters_commands( + const joint_limits::JointControlInterfacesData & state, + std::map & interface_map) + { + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) != + interface_map.end() && + state.position.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) + ->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()); + } + 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()); + } + 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()); + } + } + + void update_joint_limiters_data() + { + for (auto & joint_limiter_data : limiters_data_) + { + for (auto & data : joint_limiter_data.second) + { + update_joint_limiters_state(data.joint_name, state_interface_map_, data.actual); + update_joint_limiters_state(data.joint_name, command_interface_map_, data.command); + data.limited = data.command; + } + } + } + std::string add_state_interface(StateInterface::ConstSharedPtr interface) { auto interface_name = interface->get_name(); @@ -1007,6 +1134,14 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + std::unordered_map> + limiters_data_; + + std::unordered_map< + std::string, std::vector>>> + joint_limiters_interface_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1049,6 +1184,7 @@ bool ResourceManager::load_and_initialize_components( resource_storage_->cm_update_rate_ = update_rate; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + resource_storage_->import_joint_limiters(hardware_info); // Set the update rate for all hardware components for (auto & hw : hardware_info) { @@ -1819,6 +1955,22 @@ HardwareReadWriteStatus ResourceManager::write( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); + // Joint Limiters operations + { + resource_storage_->update_joint_limiters_data(); + for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_) + { + for (size_t i = 0; i < limiters.size(); i++) + { + joint_limits::JointInterfacesCommandLimiterData & data = + resource_storage_->limiters_data_[hw_name][i]; + limiters[i]->enforce(data.actual, data.limited, period); + resource_storage_->update_joint_limiters_commands( + data.limited, resource_storage_->command_interface_map_); + } + } + } + auto write_components = [&](auto & components) { for (auto & component : components) From c65d133ac6b391ad54f00103087513dbc8ea21b5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 20:38:31 +0100 Subject: [PATCH 02/16] Add enforce command limits method --- .../hardware_interface/resource_manager.hpp | 8 +++++ hardware_interface/src/resource_manager.cpp | 36 ++++++++++--------- 2 files changed, 28 insertions(+), 16 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b67aba088d..bcd53ad541 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -426,6 +426,14 @@ class ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); + /** + * Enforce the command limits for the position, velocity, effort, and acceleration interfaces. + * @note This method is RT-safe + * @return true if the command interfaces are out of limits and the limits are enforced + * @return false if the command interfaces values are within limits + */ + bool enforce_command_limits(const rclcpp::Duration & period); + /// Reads all loaded hardware components. /** * Reads from all active hardware components. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 41554832c6..8182b33861 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1862,6 +1862,26 @@ return_type ResourceManager::set_component_state( return result; } +// CM API: Called in "update"-thread +bool ResourceManager::enforce_command_limits(const rclcpp::Duration & period) +{ + bool enforce_result = false; + // Joint Limiters operations + resource_storage_->update_joint_limiters_data(); + for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_) + { + for (size_t i = 0; i < limiters.size(); i++) + { + joint_limits::JointInterfacesCommandLimiterData & data = + resource_storage_->limiters_data_[hw_name][i]; + enforce_result |= limiters[i]->enforce(data.actual, data.limited, period); + resource_storage_->update_joint_limiters_commands( + data.limited, resource_storage_->command_interface_map_); + } + } + return enforce_result; +} + // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) @@ -1955,22 +1975,6 @@ HardwareReadWriteStatus ResourceManager::write( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); - // Joint Limiters operations - { - resource_storage_->update_joint_limiters_data(); - for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_) - { - for (size_t i = 0; i < limiters.size(); i++) - { - joint_limits::JointInterfacesCommandLimiterData & data = - resource_storage_->limiters_data_[hw_name][i]; - limiters[i]->enforce(data.actual, data.limited, period); - resource_storage_->update_joint_limiters_commands( - data.limited, resource_storage_->command_interface_map_); - } - } - } - auto write_components = [&](auto & components) { for (auto & component : components) From caaab0eda764f6b1bd123f815a371034fefc3f6a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 22:58:38 +0100 Subject: [PATCH 03/16] Add first RM command limit enforcement test --- .../test/test_resource_manager.cpp | 210 ++++++++++++++++++ 1 file changed, 210 insertions(+) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index be9e672b3b..37065ccf01 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2158,6 +2158,216 @@ TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_ check_read_and_write_cycles(false); } +class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + node_, ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(*rm); + + cm_update_rate_ = 100u; // The default value inside + time = node_.get_clock()->now(); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + 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])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + void check_limit_enforcement() + { + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(-20.0); + + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + for (size_t i = 1; i < 100; i++) + { + // read now and check that without limit enforcement the values are half of command as this is + // the logic implemented in test components + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_EQ(state_itfs[0].get_value(), 5.0); + EXPECT_EQ(state_itfs[1].get_value(), -10.0); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + + // Let's enforce for one loop and then run the read and write again and reset interfaces to zero + // state + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_EQ(state_itfs[0].get_value(), 5.0); + EXPECT_EQ(state_itfs[1].get_value(), -10.0); + + claimed_itfs[0].set_value(0.0); + claimed_itfs[1].set_value(0.0); + EXPECT_EQ(claimed_itfs[0].get_value(), 0.0); + EXPECT_EQ(claimed_itfs[1].get_value(), 0.0); + + // enforcing limits + rm->enforce_command_limits(duration); + + EXPECT_EQ(claimed_itfs[0].get_value(), 0.0); + EXPECT_EQ(claimed_itfs[1].get_value(), 0.0); + + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + + auto [read_ok, failed_hardware_names_read] = rm->read(time, duration); + EXPECT_TRUE(read_ok); + EXPECT_TRUE(failed_hardware_names_read.empty()); + } + + 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++) + { + // let's amplifiy 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); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_EQ(state_itfs[0].get_value(), new_state_value_1); + EXPECT_EQ(state_itfs[1].get_value(), new_state_value_2); + + claimed_itfs[0].set_value(10.0); + claimed_itfs[1].set_value(-20.0); + EXPECT_EQ(claimed_itfs[0].get_value(), 10.0); + EXPECT_EQ(claimed_itfs[1].get_value(), -20.0); + + // enforcing limits + rm->enforce_command_limits(enforce_period); + + // the joint limits value is same as in the parsed URDF + const double velocity_joint_1 = 0.2; + EXPECT_NEAR( + claimed_itfs[0].get_value(), + std::min((velocity_joint_1 * (enforce_period.seconds() * static_cast(i))), M_PI), + 1.0e-8) + << "This should be progressively increasing as it is a position limit for iteration : " + << i; + EXPECT_NEAR(claimed_itfs[1].get_value(), -0.2, 1.0e-8) + << "This should be -0.2 as it is velocity limit"; + + // This is as per the logic of the test components internally + 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); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + + EXPECT_NEAR(state_itfs[0].get_value(), M_PI_2, 0.00001); + EXPECT_NEAR(state_itfs[1].get_value(), -0.1, 0.00001); + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); + + // values to set to hardware to simulate failure on read and write +}; + +TEST_F(ResourceManagerTestCommandLimitEnforcement, test_command_interfaces_limit_enforcement) +{ + setup_resource_manager_and_do_initial_checks(); + + check_limit_enforcement(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); From b696793f319d68227d48f6d7657a2b3d1061dec9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 23:33:04 +0100 Subject: [PATCH 04/16] enable the limit enforcement from rolling and newer distros by default --- .../include/controller_manager/controller_manager.hpp | 1 + controller_manager/src/controller_manager.cpp | 10 ++++++++++ 2 files changed, 11 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7acbe20166..4a55f1a93b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -563,6 +563,7 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + bool enforce_command_limits_; controller_manager::MovingAverageStatistics periodicity_stats_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fd59c89ea8..6ff42a9be2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -322,6 +322,11 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.add( "Controller Manager Activity", this, &ControllerManager::controller_manager_diagnostic_callback); + + // Declare the enforce_command_limits parameter such a way that it is enabled by default for + // rolling and newer alone + enforce_command_limits_ = + this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false); } void ControllerManager::initialize_parameters() @@ -2600,6 +2605,11 @@ controller_interface::return_type ControllerManager::update( } } + if (enforce_command_limits_) + { + resource_manager_->enforce_command_limits(period); + } + // there are controllers to (de)activate if (switch_params_.do_switch) { From 3d0a334bd42b671b11f0d57a5a67e3fcb31b23a3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 Jan 2025 23:42:54 +0100 Subject: [PATCH 05/16] rename methods --- hardware_interface/src/resource_manager.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 8182b33861..b709540593 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -696,38 +696,38 @@ class ResourceStorage } template - void update_joint_limiters_state( + void update_joint_limiters_data( const std::string & joint_name, const std::map & interface_map, - joint_limits::JointControlInterfacesData & state) + joint_limits::JointControlInterfacesData & data, bool is_command = false) { - state.joint_name = joint_name; - // update the actual state of the limiters + data.joint_name = joint_name; + // update the actual data of the limiters if ( interface_map.find(joint_name + "/" + hardware_interface::HW_IF_POSITION) != interface_map.end()) { - state.position = + data.position = 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 = + data.velocity = 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 = + data.effort = 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 = + data.acceleration = interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)->get_value(); } } @@ -777,8 +777,8 @@ class ResourceStorage { for (auto & data : joint_limiter_data.second) { - update_joint_limiters_state(data.joint_name, state_interface_map_, data.actual); - update_joint_limiters_state(data.joint_name, command_interface_map_, data.command); + update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual); + update_joint_limiters_data(data.joint_name, command_interface_map_, data.command); data.limited = data.command; } } From 2ed1b370bf1d1735bc09fe5d6f8e5e944ea9b01a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 00:12:53 +0100 Subject: [PATCH 06/16] Use lambda functions to simplify the methods --- hardware_interface/src/resource_manager.cpp | 98 ++++++++------------- 1 file changed, 37 insertions(+), 61 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b709540593..a076e90a62 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -698,38 +698,32 @@ class ResourceStorage template void update_joint_limiters_data( const std::string & joint_name, const std::map & interface_map, - joint_limits::JointControlInterfacesData & data, bool is_command = false) + joint_limits::JointControlInterfacesData & data, bool is_command_itf = false) { data.joint_name = joint_name; - // update the actual data of the limiters - if ( - interface_map.find(joint_name + "/" + hardware_interface::HW_IF_POSITION) != - interface_map.end()) - { - data.position = - 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()) - { - data.velocity = - 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()) - { - data.effort = - 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()) + + const auto fill_interface_data = + [&](const std::string & interface_type, std::optional & value) { - data.acceleration = - interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)->get_value(); - } + const std::string interface_name = joint_name + "/" + interface_type; + if (interface_map.find(interface_name) != interface_map.end()) + { + // If the command interface is not claimed, then the value is not set + if (is_command_itf && !claimed_command_interface_map_.at(interface_name)) + { + value = std::nullopt; + } + else + { + value = interface_map.at(interface_name)->get_value(); + } + } + }; + // update the actual data of the limiters + fill_interface_data(hardware_interface::HW_IF_POSITION, data.position); + fill_interface_data(hardware_interface::HW_IF_VELOCITY, data.velocity); + fill_interface_data(hardware_interface::HW_IF_EFFORT, data.effort); + fill_interface_data(hardware_interface::HW_IF_ACCELERATION, data.acceleration); } template @@ -737,38 +731,20 @@ class ResourceStorage const joint_limits::JointControlInterfacesData & state, std::map & interface_map) { - if ( - interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) != - interface_map.end() && - state.position.has_value()) - { - interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) - ->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()); - } - 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()); - } - if ( - interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != - interface_map.end() && - state.acceleration.has_value()) + const auto set_interface_command = + [&](const std::string & interface_type, const std::optional & data) { - interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) - ->set_value(state.acceleration.value()); - } + const std::string interface_name = state.joint_name + "/" + interface_type; + if (interface_map.find(interface_name) != interface_map.end() && data.has_value()) + { + interface_map.at(interface_name)->set_value(data.value()); + } + }; + // update the command data of the limiters + set_interface_command(hardware_interface::HW_IF_POSITION, state.position); + set_interface_command(hardware_interface::HW_IF_VELOCITY, state.velocity); + set_interface_command(hardware_interface::HW_IF_EFFORT, state.effort); + set_interface_command(hardware_interface::HW_IF_ACCELERATION, state.acceleration); } void update_joint_limiters_data() @@ -778,7 +754,7 @@ class ResourceStorage for (auto & data : joint_limiter_data.second) { update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual); - update_joint_limiters_data(data.joint_name, command_interface_map_, data.command); + update_joint_limiters_data(data.joint_name, command_interface_map_, data.command, true); data.limited = data.command; } } From 666560e0ca975c5886d4e7d02036c963bcae3489 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 00:17:34 +0100 Subject: [PATCH 07/16] Update docs on the enforce_command_limits parameter --- controller_manager/doc/userdoc.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index f6967930eb..1f92dd66be 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -57,6 +57,11 @@ robot_description [std_msgs::msg::String] Parameters ----------- +enforce_command_limits (optional; bool; default: ``false`` for Jazzy and earlier distro and ``true`` for Rolling and newer distros) + Enforces the joint limits to the joint command interfaces. + If the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits in the URDF. + If the command is within the limits, the command is passed through without any changes. + .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. From f3e8444c296a38cee7bc48136e8d4fd303fb7e23 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 00:19:54 +0100 Subject: [PATCH 08/16] update release notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 58930ec6c4..22d86e050f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -86,6 +86,7 @@ controller_manager * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). +* A new parameter ``enforce_command_limits```is introduced to be able to enable and disable the enforcement of the command limits (`#1989 `_). hardware_interface ****************** From e815465b0f575d49b9dccb1e600c91e63519a358 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 00:27:34 +0100 Subject: [PATCH 09/16] Fix release_notes --- doc/release_notes.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 22d86e050f..cd5b06e560 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -86,7 +86,7 @@ controller_manager * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 `_). -* A new parameter ``enforce_command_limits```is introduced to be able to enable and disable the enforcement of the command limits (`#1989 `_). +* A new parameter ``enforce_command_limits`` is introduced to be able to enable and disable the enforcement of the command limits (`#1989 `_). hardware_interface ****************** From f86c6c7fd8bb6296c4a570f5877b85935c56a0e7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 19:10:34 +0100 Subject: [PATCH 10/16] Fix error handling tests --- ...roller_manager_hardware_error_handling.cpp | 6 ++ .../ros2_control_test_assets/descriptions.hpp | 75 +++++++++++++++++++ 2 files changed, 81 insertions(+) diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 936a2dd4c4..47a4d3a66f 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -66,6 +66,12 @@ class TestControllerManagerWithTestableCM public testing::WithParamInterface { public: + TestControllerManagerWithTestableCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf_continuous_no_limits) + { + } + void SetupAndConfigureControllers(int strictness) { test_controller_actuator = std::make_shared(); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 1744a806bc..16e6e703ea 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -324,6 +324,41 @@ const auto urdf_head_continuous_missing_limits = + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; const auto urdf_head_continuous_with_limits = @@ -415,6 +450,41 @@ const auto urdf_head_continuous_with_limits = + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; const auto urdf_head_prismatic_missing_limits = @@ -2030,6 +2100,11 @@ const auto diff_drive_robot_sdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_urdf_continuous = std::string(urdf_head_continuous_with_limits) + + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_urdf_continuous_no_limits = + std::string(urdf_head_continuous_missing_limits) + std::string(hardware_resources) + + std::string(urdf_tail); const auto minimal_async_robot_urdf = std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_urdf_with_different_hw_rw_rate = From 5b96666c8c84ea72bb4e91a72c516d0d559c9d24 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 19:10:56 +0100 Subject: [PATCH 11/16] Add logging information on the limit enforcement --- controller_manager/src/controller_manager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6ff42a9be2..295f5df4d4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -327,6 +327,8 @@ void ControllerManager::init_controller_manager() // rolling and newer alone enforce_command_limits_ = this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false); + RCLCPP_INFO_EXPRESSION( + get_logger(), enforce_command_limits_, "Enforcing command limits is enabled..."); } void ControllerManager::initialize_parameters() From c4d023cb8b9c1ef8618567d3dc5f3093de53a8d3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 21:40:51 +0100 Subject: [PATCH 12/16] Fix the controller chaining tests for the large commands --- ...roller_manager_hardware_error_handling.cpp | 2 +- ...llers_chaining_with_controller_manager.cpp | 6 +++ .../ros2_control_test_assets/descriptions.hpp | 42 +------------------ 3 files changed, 9 insertions(+), 41 deletions(-) diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 47a4d3a66f..c1fb31d54d 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -68,7 +68,7 @@ class TestControllerManagerWithTestableCM public: TestControllerManagerWithTestableCM() : ControllerManagerFixture( - ros2_control_test_assets::minimal_robot_urdf_continuous_no_limits) + ros2_control_test_assets::minimal_robot_urdf_no_limits) { } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index c143ab4862..870eec7835 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -103,6 +103,12 @@ class TestControllerChainingWithControllerManager public testing::WithParamInterface { public: + TestControllerChainingWithControllerManager() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf_no_limits) + { + } + void SetUp() { executor_ = std::make_shared(); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 16e6e703ea..09a360104d 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -450,41 +450,6 @@ const auto urdf_head_continuous_with_limits = - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - )"; const auto urdf_head_prismatic_missing_limits = @@ -2100,11 +2065,8 @@ const auto diff_drive_robot_sdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); -const auto minimal_robot_urdf_continuous = std::string(urdf_head_continuous_with_limits) + - std::string(hardware_resources) + std::string(urdf_tail); -const auto minimal_robot_urdf_continuous_no_limits = - std::string(urdf_head_continuous_missing_limits) + std::string(hardware_resources) + - std::string(urdf_tail); +const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) + + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf = std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_urdf_with_different_hw_rw_rate = From c78338f6c97d71a9626cc5b4b09071cd4a6f7701 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 8 Jan 2025 22:31:43 +0100 Subject: [PATCH 13/16] Increase the URDF velocity limits of the diffbot_urdf for the chaining test --- ...controllers_chaining_with_controller_manager.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 870eec7835..7b94ce254f 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -103,18 +104,16 @@ class TestControllerChainingWithControllerManager public testing::WithParamInterface { public: - TestControllerChainingWithControllerManager() - : ControllerManagerFixture( - ros2_control_test_assets::minimal_robot_urdf_no_limits) - { - } - void SetUp() { executor_ = std::make_shared(); + const std::regex velocity_pattern(R"(velocity\s*=\s*"-?[0-9]+(\.[0-9]+)?")"); + const std::string velocity_replacement = R"(velocity="10000.0")"; + const std::string diffbot_urdf_large_limits = std::regex_replace( + ros2_control_test_assets::diffbot_urdf, velocity_pattern, velocity_replacement); cm_ = std::make_shared( std::make_unique( - ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(), + diffbot_urdf_large_limits, rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface(), true), executor_, TEST_CM_NAME); run_updater_ = false; From efa76b3b33769b051d6221c3aba9f25fb3aacfb9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 9 Jan 2025 23:22:56 +0100 Subject: [PATCH 14/16] Add `import_joint_limiters` method to also work for Gazebo and other simulators --- controller_manager/src/controller_manager.cpp | 2 ++ .../include/hardware_interface/resource_manager.hpp | 6 ++++++ hardware_interface/src/resource_manager.cpp | 7 ++++++- hardware_interface_testing/test/test_resource_manager.cpp | 1 + 4 files changed, 15 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 295f5df4d4..08cebdad09 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -289,6 +289,7 @@ void ControllerManager::init_controller_manager() // Get parameters needed for RT "update" loop to work if (is_resource_manager_initialized()) { + resource_manager_->import_joint_limiters(robot_description_); init_services(); } else @@ -370,6 +371,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & get_logger(), "Resource Manager has been successfully initialized. Starting Controller Manager " "services..."); + resource_manager_->import_joint_limiters(robot_description_); init_services(); } } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index bcd53ad541..cd759a96e4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -89,6 +89,12 @@ class ResourceManager virtual bool load_and_initialize_components( const std::string & urdf, const unsigned int update_rate = 100); + /** + * @brief Import joint limiters from the URDF. + * @param urdf string containing the URDF. + */ + void import_joint_limiters(const std::string & urdf); + /** * @brief if the resource manager load_and_initialize_components(...) function has been called * this returns true. We want to permit to loading the urdf later on, but we currently don't want diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a076e90a62..e2f1110206 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1160,7 +1160,6 @@ bool ResourceManager::load_and_initialize_components( resource_storage_->cm_update_rate_ = update_rate; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - resource_storage_->import_joint_limiters(hardware_info); // Set the update rate for all hardware components for (auto & hw : hardware_info) { @@ -1235,6 +1234,12 @@ bool ResourceManager::load_and_initialize_components( return components_are_loaded_and_initialized_; } +void ResourceManager::import_joint_limiters(const std::string & urdf) +{ + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + resource_storage_->import_joint_limiters(hardware_info); +} + bool ResourceManager::are_components_initialized() const { return components_are_loaded_and_initialized_; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 37065ccf01..424d54e882 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2165,6 +2165,7 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest { rm = std::make_shared( node_, ros2_control_test_assets::minimal_robot_urdf, false); + rm->import_joint_limiters(ros2_control_test_assets::minimal_robot_urdf); activate_components(*rm); cm_update_rate_ = 100u; // The default value inside From 5b00b44bc9bc76a6fa8c6d44795e80b84ea1f821 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 10 Jan 2025 00:43:25 +0100 Subject: [PATCH 15/16] Add `get_urdf` method for the ResourceManager --- controller_manager/src/controller_manager.cpp | 3 ++- .../include/hardware_interface/resource_manager.hpp | 2 ++ hardware_interface/src/resource_manager.cpp | 9 ++++++++- 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 08cebdad09..4e14eb209c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -278,7 +278,8 @@ ControllerManager::ControllerManager( chainable_loader_( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), - cm_node_options_(options) + cm_node_options_(options), + robot_description_(resource_manager_->get_urdf()) { initialize_parameters(); init_controller_manager(); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index cd759a96e4..efadf53528 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -471,6 +471,8 @@ class ResourceManager */ bool state_interface_exists(const std::string & key) const; + const std::string & get_urdf() const; + protected: /// Gets the logger for the resource manager /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e2f1110206..f5bf36a4f0 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1118,6 +1118,8 @@ class ResourceStorage joint_limits::JointLimiterInterface>>> joint_limiters_interface_; + std::string robot_description_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1138,6 +1140,7 @@ ResourceManager::ResourceManager( const unsigned int update_rate) : resource_storage_(std::make_unique(clock_interface, logger_interface)) { + resource_storage_->robot_description_ = urdf; load_and_initialize_components(urdf, update_rate); if (activate_all) @@ -1157,6 +1160,7 @@ bool ResourceManager::load_and_initialize_components( { components_are_loaded_and_initialized_ = true; + resource_storage_->robot_description_ = urdf; resource_storage_->cm_update_rate_ = update_rate; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); @@ -2056,7 +2060,10 @@ bool ResourceManager::state_interface_exists(const std::string & key) const return resource_storage_->state_interface_map_.find(key) != resource_storage_->state_interface_map_.end(); } - +const std::string & ResourceManager::get_urdf() const +{ + return resource_storage_->robot_description_; +} // END: "used only in tests and locally" rclcpp::Logger ResourceManager::get_logger() const { return resource_storage_->get_logger(); } From 672f22729cacb55e9b81e097bdb5160acde69364 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Feb 2025 23:45:44 +0100 Subject: [PATCH 16/16] Fix the tests with the new changes of considering actual position for limiting the desired --- .../test/test_components/test_actuator.cpp | 9 +++++- .../test/test_resource_manager.cpp | 32 +++++++++++++++---- joint_limits/src/joint_limits_helpers.cpp | 6 ++-- .../test_hardware_interface_constants.hpp | 1 + 4 files changed, 38 insertions(+), 10 deletions(-) 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_