Skip to content

Commit

Permalink
Fix the initial wrong periodicity reported by controller_manager (#2018)
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor authored Jan 29, 2025
1 parent d2dd762 commit f4e3f5a
Showing 1 changed file with 5 additions and 1 deletion.
6 changes: 5 additions & 1 deletion controller_manager/src/ros2_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,10 @@ int main(int argc, char ** argv)
}
}

// wait for the clock to be available
cm->get_clock()->wait_until_started();
cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate()));

RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
RCLCPP_INFO(
Expand Down Expand Up @@ -122,7 +126,7 @@ int main(int argc, char ** argv)
auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds());
std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>
next_iteration_time{cm_now};
next_iteration_time{cm_now - period};

// for calculating the measured period of the loop
rclcpp::Time previous_time = cm->now() - period;
Expand Down

0 comments on commit f4e3f5a

Please sign in to comment.