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

Compute the actual update period for each controller and parse it #1140

Merged
merged 4 commits into from
Nov 21, 2023
Merged

Compute the actual update period for each controller and parse it #1140

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
bc6c158
check that the controller update rate is real
saikishor Oct 22, 2023
95fc622
added the actual controller update period computation
saikishor Oct 22, 2023
5fc79f8
Merge branch 'master' into pass_actual_controller_period
saikishor Nov 8, 2023
fd0b617
Merge branch 'master' into pass_actual_controller_period
bmagyar Nov 21, 2023
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
4 changes: 3 additions & 1 deletion controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2057,7 +2057,9 @@ controller_interface::return_type ControllerManager::update(

if (controller_go)
{
auto controller_ret = loaded_controller.c->update(time, controller_period);
const auto controller_actual_period =
(time - *loaded_controller.next_update_cycle_time) + controller_period;
Comment on lines +2066 to +2067
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shouldn't this be something like time-time_old_?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The next_update_cycle_time is computed such that there is a condition that checks if the time is greater than or equal to the current time and if the condition passes, then it triggers the update. Usually, when it is equal this difference is zero, but that means you have a control period in between.

If not, I can check if I can move some logic around to see if I can avoid adding this controller period at this stage. I will check this evening.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, maybe this is fine as it is.

auto controller_ret = loaded_controller.c->update(time, controller_actual_period);

if (
*loaded_controller.next_update_cycle_time ==
Expand Down
3 changes: 2 additions & 1 deletion controller_manager/test/test_controller/test_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
}

controller_interface::return_type TestController::update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
update_period_ = period.seconds();
++internal_counter;

// set value to hardware to produce and test different behaviors there
Expand Down
1 change: 1 addition & 0 deletions controller_manager/test/test_controller/test_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,7 @@ class TestController : public controller_interface::ControllerInterface
// enables external setting of values to command interfaces - used for simulation of hardware
// errors
double set_first_command_interface_value_to;
double update_period_ = 0;
};

} // namespace test_controller
Expand Down
12 changes: 12 additions & 0 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 @@ -314,6 +314,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";

time_ += rclcpp::Duration::from_seconds(0.01);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
Expand All @@ -328,11 +329,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd

const auto pre_internal_counter = test_controller->internal_counter;
rclcpp::Rate loop_rate(cm_->get_update_rate());
const auto cm_update_rate = cm_->get_update_rate();
for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++)
{
time_ += rclcpp::Duration::from_seconds(0.01);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
// In case of a non perfect divisor, the update period should respect the rule
// [controller_update_rate, 2*controller_update_rate)
ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate);
ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate);
loop_rate.sleep();
}
// if we do 2 times of the controller_manager update rate, the internal counter should be
Expand Down Expand Up @@ -408,6 +415,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";

time_ += rclcpp::Duration::from_seconds(0.01);
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
Expand All @@ -430,6 +438,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
// In case of a non perfect divisor, the update period should respect the rule
// [controller_update_rate, 2*controller_update_rate)
ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate);
ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate);

time += rclcpp::Duration::from_seconds(0.01);
if (update_counter % cm_update_rate == 0)
Expand Down
Loading