We read every piece of feedback, and take your input very seriously.
To see all available qualifiers, see our documentation.
There was an error while loading. Please reload this page.
2 parents 087a94a + 73551ef commit 34c4d71Copy full SHA for 34c4d71
webots_ros2_control/src/Ros2Control.cpp
@@ -90,7 +90,7 @@ namespace webots_ros2_control {
90
void Ros2Control::step() {
91
const int nowMs = wb_robot_get_time() * 1000.0;
92
const int periodMs = nowMs - mLastControlUpdateMs;
93
- if (periodMs >= mControlPeriodMs) {
+ if (periodMs >= mControlPeriodMs && mNode->get_clock()->now() != rclcpp::Time(0, 0, mNode->get_clock()->get_clock_type())) {
94
const rclcpp::Duration dt = rclcpp::Duration::from_seconds(mControlPeriodMs / 1000.0);
95
mControllerManager->read(mNode->get_clock()->now(), dt);
96
0 commit comments