Fix sensors after time reset (lunar-devel) (#705)

* 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
ros-simulation · Apr 9, 2018 · 7899edc · 7899edc
1 parent be2c750
commit 7899edc
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 0 deletions.
diff --git 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
@@ -92,6 +92,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
@@ -116,7 +116,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
@@ -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
@@ -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 =