ros-simulation#408 GetLastMeasurementTime is the time of the image th…

…at has triggered the OnNewFrame, therefore it should go into the timestamp. (the OnNewFrame ought to contain the timestamp) There is no purpose to cur_time since the OnNewFrame is never triggered at a higher rate than gazebo sensor- maybe the idea was that a higher rate could be synthesized by sending the same image at the plugin rate even when the sensor rate was lower? Why does updateRate exist at all, it seems likely to cause confusion and be set to a non-zero number (like the same number as update_rate) that will result in skipped frames.

gazebo_plugins/src/gazebo_ros_camera.cpp

@@ -76,7 +76,7 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
     unsigned int _width, unsigned int _height, unsigned int _depth,
     const std::string &_format)
 {
-  common::Time sensor_update_time = this->parentSensor_->GetLastUpdateTime();
+  common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
 
   if (!this->parentSensor->IsActive())
   {
@@ -88,12 +88,18 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
   {
     if ((*this->image_connect_count_) > 0)
     {
-      common::Time cur_time = this->world_->GetSimTime();
-      if (cur_time - this->last_update_time_ >= this->update_period_)
+      // 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?
+      // what is the use case?).
+      // Setting the <updateRate> to zero will make this plugin
+      // update at the gazebo sensor <update_rate>, update_period_ will be
+      // zero and the conditional always will be true.
+      if (sensor_update_time - this->last_update_time_ >= this->update_period_)
       {
         this->PutCameraData(_image, sensor_update_time);
         this->PublishCameraInfo(sensor_update_time);
-        this->last_update_time_ = cur_time;
+        this->last_update_time_ = sensor_update_time;
       }
     }
   }