diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 4d7d18f89..792bef9ff 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) { - this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->GetLastUpdateTime(); if (!this->parentSensor->IsActive()) { @@ -91,8 +91,8 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image, common::Time cur_time = this->world_->GetSimTime(); if (cur_time - this->last_update_time_ >= this->update_period_) { - this->PutCameraData(_image); - this->PublishCameraInfo(); + this->PutCameraData(_image, sensor_update_time); + this->PublishCameraInfo(sensor_update_time); this->last_update_time_ = cur_time; } }