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.

Sign up for GitHub

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

Jump to bottom

Fixes the issue with individual controller's update rate (backport #1082) #1097

Merged
merged 1 commit into from
Aug 7, 2023
Merged

Fixes the issue with individual controller's update rate (backport #1082) #1097

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
23 changes: 19 additions & 4 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -481,6 +481,19 @@ controller_interface::return_type ControllerManager::configure_controller(
"update rate.",
controller_name.c_str(), controller_update_rate, cm_update_rate);
}
else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0)
{
// NOTE: The following computation is done to compute the approx controller update that can be
// achieved w.r.t to the CM's update rate. This is done this way to take into account the
// unsigned integer division.
const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate);
RCLCPP_WARN(
get_logger(),
"The controller : %s update rate : %d Hz is not a perfect divisor of the controller "
"manager's update rate : %d Hz!. The controller will be updated with nearest divisor's "
"update rate which is : %d Hz.",
controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate);
}

// CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
if (controller->is_chainable())
Expand Down Expand Up @@ -1695,10 +1708,12 @@ controller_interface::return_type ControllerManager::update(
if (is_controller_active(*loaded_controller.c))
{
const auto controller_update_rate = loaded_controller.c->get_update_rate();
const auto controller_update_factor =
(controller_update_rate == 0) || (controller_update_rate >= update_rate_)
? 1u
: update_rate_ / controller_update_rate;

bool controller_go = controller_update_rate == 0 ||
((update_loop_counter_ % controller_update_rate) == 0) ||
(controller_update_rate >= update_rate_);
bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0);
RCLCPP_DEBUG(
get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
update_loop_counter_, controller_go ? "True" : "False",
Expand All @@ -1707,7 +1722,7 @@ controller_interface::return_type ControllerManager::update(
if (controller_go)
{
auto controller_ret = loaded_controller.c->update(
time, (controller_update_rate != update_rate_ && controller_update_rate != 0)
time, (controller_update_factor != 1u)
? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
: period);

Expand Down
89 changes: 86 additions & 3 deletions controller_manager/test/test_controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -246,9 +246,13 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
// As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle
for (size_t i = 0; i < 25; i++)
{
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
}
EXPECT_GE(test_controller->internal_counter, 1u);
EXPECT_EQ(test_controller->get_update_rate(), 4u);
}
Expand Down Expand Up @@ -364,3 +368,82 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
INSTANTIATE_TEST_SUITE_P(
per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates,
testing::Values(100, 232, 400));

class TestControllerUpdateRates
: public ControllerManagerFixture<controller_manager::ControllerManager>,
public testing::WithParamInterface<unsigned int>
{
};

TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
{
const unsigned int ctrl_update_rate = GetParam();
auto test_controller = std::make_shared<test_controller::TestController>();
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
EXPECT_EQ(2, test_controller.use_count());

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());

test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());

// Start controller, will take effect at the end of the update function
const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());

EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
const auto cm_update_rate = cm_->get_update_rate();
const auto controller_update_rate = test_controller->get_update_rate();
const auto controller_factor = (cm_update_rate / controller_update_rate);
const auto expected_controller_update_rate =
static_cast<unsigned int>(std::round(cm_update_rate / static_cast<double>(controller_factor)));

const auto initial_counter = test_controller->internal_counter;
for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter)
{
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));

if (update_counter % cm_update_rate == 0)
{
const auto no_of_secs_passed = update_counter / cm_update_rate;
EXPECT_EQ(
test_controller->internal_counter - initial_counter,
(expected_controller_update_rate * no_of_secs_passed));
}
}
}

INSTANTIATE_TEST_SUITE_P(
per_controller_update_rate_check, TestControllerUpdateRates,
testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));
Loading