diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 7e8ffb52fa..b672316c33 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. diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 49eed135be..4e49110d4e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -577,6 +577,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 bef2985c0b..33225e5f4e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -285,7 +285,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(); @@ -336,6 +337,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 +372,13 @@ void ControllerManager::init_controller_manager() "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); + RCLCPP_INFO_EXPRESSION( + get_logger(), enforce_command_limits_, "Enforcing command limits is enabled..."); + // Add on_shutdown callback to stop the controller manager rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context(); preshutdown_cb_handle_ = @@ -433,6 +442,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(); } } @@ -2675,6 +2685,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) { 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..c1fb31d54d 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_no_limits) + { + } + void SetupAndConfigureControllers(int strictness) { test_controller_actuator = std::make_shared(); 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..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 @@ -106,9 +107,13 @@ class TestControllerChainingWithControllerManager 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; diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 3dec704f23..ba289dc2f1 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -88,6 +88,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 ****************** diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 06b8f51952..e36a9783eb 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -96,6 +96,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 @@ -433,6 +439,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. @@ -464,6 +478,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 a99ae3db3b..f82fac4dd1 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,106 @@ 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_data( + const std::string & joint_name, const std::map & interface_map, + joint_limits::JointControlInterfacesData & data, bool is_command_itf = false) + { + data.joint_name = joint_name; + + const auto fill_interface_data = + [&](const std::string & interface_type, std::optional & 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 + void update_joint_limiters_commands( + const joint_limits::JointControlInterfacesData & state, + std::map & interface_map) + { + const auto set_interface_command = + [&](const std::string & interface_type, const std::optional & data) + { + 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() + { + for (auto & joint_limiter_data : limiters_data_) + { + 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, true); + data.limited = data.command; + } + } + } + std::string add_state_interface(StateInterface::ConstSharedPtr interface) { auto interface_name = interface->get_name(); @@ -1007,6 +1110,16 @@ 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_; + + 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; @@ -1027,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) @@ -1062,6 +1176,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); @@ -1139,6 +1254,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_; @@ -1742,6 +1863,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) @@ -1935,7 +2076,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(); } 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 be9e672b3b..2a7311fea0 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2158,6 +2158,235 @@ 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); + rm->import_joint_limiters(ros2_control_test_assets::minimal_robot_urdf); + 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])); + 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 + { + 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); + + 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); + 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()); + + 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 < 2000; i++) + { + // 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, enforce_period); + 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; + ASSERT_NEAR( + claimed_itfs[0].get_value(), + 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; + 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, enforce_period); + 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); 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/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 1744a806bc..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 @@ -324,6 +324,41 @@ const auto urdf_head_continuous_missing_limits = + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; const auto urdf_head_continuous_with_limits = @@ -2030,6 +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_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 = 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_