Fix the initial wrong periodicity reported by controller_manager (#2018)

Author: saikishor

controller_manager/src/ros2_control_node.cpp

@@ -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(
@@ -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;