diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 61d2c780b8..e2c1fa480a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -28,13 +28,14 @@ return_type ControllerInterfaceBase::init( const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { urdf_ = urdf; + update_rate_ = cm_update_rate; node_ = std::make_shared( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces try { - auto_declare("update_rate", cm_update_rate); + auto_declare("update_rate", update_rate_); auto_declare("is_async", false); } catch (const std::exception & e) @@ -85,7 +86,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Other solution is to add check into the LifecycleNode if a transition is valid to trigger if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); + const auto update_rate = get_node()->get_parameter("update_rate").as_int(); + if (update_rate < 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!"); + return get_lifecycle_state(); + } + if (update_rate_ != 0u && update_rate > update_rate_) + { + RCLCPP_WARN( + get_node()->get_logger(), + "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the " + "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.", + update_rate, update_rate_); + } + else + { + update_rate_ = static_cast(update_rate); + } is_async_ = get_node()->get_parameter("is_async").as_bool(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index f5c0a6b3de..4204d32f45 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -19,6 +19,7 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -46,8 +47,8 @@ TEST(TestableControllerInterface, init) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - // update_rate is set to default 0 - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 10u); // Even after configure is 10 controller.configure(); @@ -56,6 +57,32 @@ TEST(TestableControllerInterface, init) rclcpp::shutdown(); } +TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + rclcpp::init(0, nullptr); + + TestableControllerInterface controller; + // initialize, create node + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=-100"); + node_options = node_options.arguments(node_options_arguments); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options), + controller_interface::return_type::OK); + + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 1000u); + + // The configure should fail and the update rate should stay the same + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(controller.get_update_rate(), 1000u); + rclcpp::shutdown(); +} + TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file @@ -70,7 +97,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) node_options_arguments.push_back("update_rate:=2812"); node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), + controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options), controller_interface::return_type::OK); // initialize executor to be able to get parameter update @@ -85,8 +112,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) using namespace std::chrono_literals; std::this_thread::sleep_for(50ms); - // update_rate is set to default 0 because it is set on configure - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 5000u); // Even after configure is 0 controller.configure(); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 28e9f0477c..3bf56dd900 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -398,7 +398,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // if we do 2 times of the controller_manager update rate, the internal counter should be // similarly incremented EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); - EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + EXPECT_EQ(test_controller->get_update_rate(), cm_->get_update_rate()); auto deactivate_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 3645c06c24..ce7285fc49 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -66,13 +66,13 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_lifecycle_state().id()); - controller_if->get_node()->set_parameter({"update_rate", 1337}); + controller_if->get_node()->set_parameter({"update_rate", 50}); cm_->configure_controller("test_controller_01"); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - EXPECT_EQ(1337u, controller_if->get_update_rate()); + EXPECT_EQ(50u, controller_if->get_update_rate()); } class TestLoadedController : public TestLoadController