From 5a718dad98e3e6dda4a17e8ce7f9fcc960816de0 Mon Sep 17 00:00:00 2001
From: Julian Kooij <julian.kooij@gmail.com>
Date: Mon, 9 Apr 2018 13:43:54 +0200
Subject: [PATCH] Fix sensors after time reset (#683)

* camera plugin keeps publishing after negative sensor update interval

World resets result in a negative time differences between current world
time and the last recorded sensor update time, preventing the plugin
from publishing new frames. This commit detects such events and resets
the internal sensor update timestamp.

* block_laser, range, and joint_state_publisher keep publishing after clock reset

* p3d keeps publishing after clock reset
---
 gazebo_plugins/src/gazebo_ros_block_laser.cpp           | 6 ++++++
 gazebo_plugins/src/gazebo_ros_camera.cpp                | 6 ++++++
 gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp | 7 +++++++
 gazebo_plugins/src/gazebo_ros_p3d.cpp                   | 6 ++++++
 gazebo_plugins/src/gazebo_ros_range.cpp                 | 6 ++++++
 5 files changed, 31 insertions(+)

diff --git a/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/src/gazebo_ros_block_laser.cpp
index 250be0112..b330e0137 100644
--- a/gazebo_plugins/src/gazebo_ros_block_laser.cpp
+++ b/gazebo_plugins/src/gazebo_ros_block_laser.cpp
@@ -211,6 +211,12 @@ void GazeboRosBlockLaser::OnNewLaserScans()
   if (this->topic_name_ != "")
   {
     common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime();
+    if (sensor_update_time < last_update_time_)
+    {
+        ROS_WARN_NAMED("block_laser", "Negative sensor update time difference detected.");
+        last_update_time_ = sensor_update_time;
+    }
+
     if (last_update_time_ < sensor_update_time)
     {
       this->PutLaserData(sensor_update_time);
diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp
index edb90ff4d..8a59a129c 100644
--- a/gazebo_plugins/src/gazebo_ros_camera.cpp
+++ b/gazebo_plugins/src/gazebo_ros_camera.cpp
@@ -88,6 +88,12 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
   {
     if ((*this->image_connect_count_) > 0)
     {
+      if (sensor_update_time < this->last_update_time_)
+      {
+          ROS_WARN_NAMED("camera", "Negative sensor update time difference detected.");
+          this->last_update_time_ = sensor_update_time;
+      }
+
       // OnNewFrame is triggered at the gazebo sensor <update_rate>
       // while there is also a plugin <updateRate> that can throttle the
       // rate down further (but then why not reduce the sensor rate?
diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp
index 1911db8f9..c125d731e 100644
--- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp
+++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp
@@ -112,7 +112,14 @@ void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info )
 #else
     common::Time current_time = this->world_->GetSimTime();
 #endif
+    if (current_time < last_update_time_)
+    {
+        ROS_WARN_NAMED("joint_state_publisher", "Negative joint state update time difference detected.");
+        last_update_time_ = current_time;
+    }
+
     double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
+
     if ( seconds_since_last_update > update_period_ ) {
 
         publishJointStates();
diff --git a/gazebo_plugins/src/gazebo_ros_p3d.cpp b/gazebo_plugins/src/gazebo_ros_p3d.cpp
index 57a381ab8..a252d222a 100644
--- a/gazebo_plugins/src/gazebo_ros_p3d.cpp
+++ b/gazebo_plugins/src/gazebo_ros_p3d.cpp
@@ -221,6 +221,12 @@ void GazeboRosP3D::UpdateChild()
   common::Time cur_time = this->world_->GetSimTime();
 #endif
 
+  if (cur_time < this->last_time_)
+  {
+      ROS_WARN_NAMED("p3d", "Negative update time difference detected.");
+      this->last_time_ = cur_time;
+  }
+
   // rate control
   if (this->update_rate_ > 0 &&
       (cur_time-this->last_time_).Double() < (1.0/this->update_rate_))
diff --git a/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_plugins/src/gazebo_ros_range.cpp
index b9dc37d74..043955232 100644
--- a/gazebo_plugins/src/gazebo_ros_range.cpp
+++ b/gazebo_plugins/src/gazebo_ros_range.cpp
@@ -248,6 +248,12 @@ void GazeboRosRange::OnNewLaserScans()
 #else
     common::Time cur_time = this->world_->GetSimTime();
 #endif
+    if (cur_time < this->last_update_time_)
+    {
+        ROS_WARN_NAMED("range", "Negative sensor update time difference detected.");
+        this->last_update_time_ = cur_time;
+    }
+
     if (cur_time - this->last_update_time_ >= this->update_period_)
     {
       common::Time sensor_update_time =