From 6372af84890678c0dc3943203630fb6b36099994 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 19 Mar 2016 05:13:39 -0700 Subject: [PATCH] #408 GetLastMeasurementTime is the time of the image that 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 | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 792bef9ff..fd95e2bcb 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/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 + // while there is also a plugin that can throttle the + // rate down further (but then why not reduce the sensor rate? + // what is the use case?). + // Setting the to zero will make this plugin + // update at the gazebo sensor , 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; } } }