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

CM: Check if a valid time is received #1901

Merged
merged 11 commits into from
Dec 4, 2024
Merged

CM: Check if a valid time is received #1901

Changes from 1 commit
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
0284fbb
Check if a valid time is received
christophfroehlich Nov 30, 2024
ba22541
Use time argument instead
christophfroehlich Dec 1, 2024
5ee083c
Apply suggestions from code review
christophfroehlich Dec 2, 2024
7dd5e90
Revert "Apply suggestions from code review"
christophfroehlich Dec 2, 2024
6b3587e
Improve log output
christophfroehlich Dec 2, 2024
5b697fa
Return proper warnings and use controller_period if necessary
christophfroehlich Dec 2, 2024
9fd84f5
Simplify logic
christophfroehlich Dec 2, 2024
ec46577
Use throttled output
christophfroehlich Dec 3, 2024
7b9dfed
Merge branch 'master' into no_sim_time
christophfroehlich Dec 3, 2024
999a93d
Merge branch 'master' into no_sim_time
christophfroehlich Dec 3, 2024
6b400b9
Merge latest diagnostics changes
christophfroehlich Dec 3, 2024
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
28 changes: 20 additions & 8 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2367,18 +2367,30 @@ controller_interface::return_type ControllerManager::update(
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));

const rclcpp::Time current_time = get_clock()->now();
rclcpp::Duration controller_actual_period(0, 0);
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
if (
*loaded_controller.last_update_cycle_time ==
current_time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
{
// it is zero after activation
*loaded_controller.last_update_cycle_time = current_time - controller_period;
RCLCPP_DEBUG(
get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
// this can happen with sim_time until the /clock is received
RCLCPP_INFO(get_logger(), "No clock received, use default controller_period");
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
controller_actual_period = controller_period;
}
else
{
if (
*loaded_controller.last_update_cycle_time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
{
// it is zero after activation
*loaded_controller.last_update_cycle_time = current_time - controller_period;
RCLCPP_DEBUG(
get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
loaded_controller.last_update_cycle_time->seconds(),
loaded_controller.info.name.c_str());
}
controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time);
}
const auto controller_actual_period =
(current_time - *loaded_controller.last_update_cycle_time);

/// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
/// jitter in the system sleep cycles.
Expand Down