Fix wrong publish timestamp initialization (#585) (#593)

Signed-off-by: Noel Jimenez <noel.jimenez@pal-robotics.com>

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -147,7 +147,7 @@ class DiffDriveController : public controller_interface::ControllerInterface
   // publish rate limiter
   double publish_rate_ = 50.0;
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
-  rclcpp::Time previous_publish_timestamp_{0};
+  rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
 
   bool is_halted = false;
   bool use_stamped_vel_ = true;

diff_drive_controller/src/diff_drive_controller.cpp

@@ -185,7 +185,23 @@ controller_interface::return_type DiffDriveController::update(
   tf2::Quaternion orientation;
   orientation.setRPY(0.0, 0.0, odometry_.getHeading());
 
-  if (previous_publish_timestamp_ + publish_period_ < time)
+  bool should_publish = false;
+  try
+  {
+    if (previous_publish_timestamp_ + publish_period_ < time)
+    {
+      previous_publish_timestamp_ += publish_period_;
+      should_publish = true;
+    }
+  }
+  catch (const std::runtime_error)
+  {
+    // Handle exceptions when the time source changes and initialize publish timestamp
+    previous_publish_timestamp_ = time;
+    should_publish = true;
+  }
+
+  if (should_publish)
   {
     previous_publish_timestamp_ += publish_period_;
 
@@ -400,7 +416,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   // limit the publication on the topics /odom and /tf
   publish_rate_ = get_node()->get_parameter("publish_rate").as_double();
   publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_);
-  previous_publish_timestamp_ = get_node()->get_clock()->now();
 
   // initialize odom values zeros
   odometry_message.twist =