Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Check the update_rate set to the controllers to be a valid one #1788

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 20 additions & 2 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp_lifecycle::LifecycleNode>(
controller_name, node_namespace, node_options,
false); // disable LifecycleNode service interfaces

try
{
auto_declare<int>("update_rate", cm_update_rate);
auto_declare<int>("update_rate", update_rate_);
auto_declare<bool>("is_async", false);
}
catch (const std::exception & e)
Expand Down Expand Up @@ -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<unsigned int>(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<unsigned int>(update_rate);
}
is_async_ = get_node()->get_parameter("is_async").as_bool();
}

Expand Down
37 changes: 32 additions & 5 deletions controller_interface/test/test_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <string>
#include <vector>

#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/executor_options.hpp"
#include "rclcpp/executors/multi_threaded_executor.hpp"

Expand Down Expand Up @@ -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();
Expand All @@ -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<std::string> 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
Expand All @@ -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
Expand All @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion controller_manager/test/test_controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_,
Expand Down
4 changes: 2 additions & 2 deletions controller_manager/test/test_load_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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());
saikishor marked this conversation as resolved.
Show resolved Hide resolved
EXPECT_EQ(50u, controller_if->get_update_rate());
}

class TestLoadedController : public TestLoadController
Expand Down
Loading