Skip to content

Commit

Permalink
[CM] Fixes the issue with individual controller's update rate (ros-co…
Browse files Browse the repository at this point in the history 
…ntrols#1082)

* added parameterized test for the controller's update rate check
* added warning for the update rate to be a perfect factor
* fix the issue with the period when the controller update rate is higher than the controller manager
  • Loading branch information
@saikishor
saikishor authored Aug 7, 2023
1 parent bed7c66 commit ad70c9a
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 7 deletions.
23 changes: 19 additions & 4 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -621,6 +621,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 @@ -1887,10 +1900,12 @@ controller_interface::return_type ControllerManager::update(
if (!loaded_controller.c->is_async() && 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 @@ -1899,7 +1914,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
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));

0 comments on commit ad70c9a

Please sign in to comment.