diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index d5dfe8395..991244dfa 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -629,15 +629,14 @@ void GazeboRosCameraUtils::PublishCameraInfo() if (this->camera_info_pub_.getNumSubscribers() > 0) { # if GAZEBO_MAJOR_VERSION >= 7 - this->sensor_update_time_ = this->parentSensor_->LastUpdateTime(); + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); # else - this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime(); + this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime(); # endif - common::Time cur_time = this->world_->GetSimTime(); - if (cur_time - this->last_info_update_time_ >= this->update_period_) + if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_) { this->PublishCameraInfo(this->camera_info_pub_); - this->last_info_update_time_ = cur_time; + this->last_info_update_time_ = this->sensor_update_time_; } } } diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index c852f6f94..869c3fa64 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -189,9 +189,9 @@ void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image, return; # if GAZEBO_MAJOR_VERSION >= 7 - this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime(); + this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); # else - this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime(); + this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); # endif if (this->parentSensor->IsActive()) { @@ -231,9 +231,9 @@ void GazeboRosOpenniKinect::OnNewImageFrame(const unsigned char *_image, //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); # if GAZEBO_MAJOR_VERSION >= 7 - this->sensor_update_time_ = this->parentSensor_->LastUpdateTime(); + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); # else - this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime(); + this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime(); # endif if (this->parentSensor->IsActive()) @@ -447,15 +447,14 @@ void GazeboRosOpenniKinect::PublishCameraInfo() if (this->depth_info_connect_count_ > 0) { # if GAZEBO_MAJOR_VERSION >= 7 - this->sensor_update_time_ = this->parentSensor_->LastUpdateTime(); + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); # else - this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime(); + this->sensor_update_time_ = this->parentSensor_->GetLastMeasurementTime(); # endif - common::Time cur_time = this->world_->GetSimTime(); - if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) + if (this->sensor_update_time_ - this->last_depth_image_camera_info_update_time_ >= this->update_period_) { this->PublishCameraInfo(this->depth_image_camera_info_pub_); - this->last_depth_image_camera_info_update_time_ = cur_time; + this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_; } } } diff --git a/gazebo_plugins/src/gazebo_ros_prosilica.cpp b/gazebo_plugins/src/gazebo_ros_prosilica.cpp index dd0203290..2e529ca9b 100644 --- a/gazebo_plugins/src/gazebo_ros_prosilica.cpp +++ b/gazebo_plugins/src/gazebo_ros_prosilica.cpp @@ -132,9 +132,9 @@ void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image, // should do nothing except turning camera on/off, as we are using service. /// @todo: consider adding thumbnailing feature here if subscribed. # if GAZEBO_MAJOR_VERSION >= 7 - common::Time sensor_update_time = this->parentSensor_->LastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); # else - common::Time sensor_update_time = this->parentSensor_->GetLastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); # endif // as long as ros is connected, parent is active @@ -153,12 +153,11 @@ void GazeboRosProsilica::OnNewImageFrame(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_) + 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; } } } @@ -227,9 +226,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re this->roiCameraInfoMsg->header.frame_id = this->frame_name_; # if GAZEBO_MAJOR_VERSION >= 7 - common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime(); # else - common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->GetLastMeasurementTime(); # endif this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec; this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec; @@ -285,9 +284,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re this->image_msg_.header.frame_id = this->frame_name_; # if GAZEBO_MAJOR_VERSION >= 7 - common::Time lastRenderTime = this->parentSensor_->LastUpdateTime(); + common::Time lastRenderTime = this->parentSensor_->LastMeasurementTime(); # else - common::Time lastRenderTime = this->parentSensor_->GetLastUpdateTime(); + common::Time lastRenderTime = this->parentSensor_->GetLastMeasurementTime(); # endif this->image_msg_.header.stamp.sec = lastRenderTime.sec; this->image_msg_.header.stamp.nsec = lastRenderTime.nsec; @@ -313,9 +312,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re this->roiImageMsg = ℑ this->roiImageMsg->header.frame_id = this->frame_name_; # if GAZEBO_MAJOR_VERSION >= 7 - common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime(); # else - common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->GetLastMeasurementTime(); # endif this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec; this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec; diff --git a/gazebo_plugins/test/camera/depth_camera.world b/gazebo_plugins/test/camera/depth_camera.world index 978a0a4b2..73132f908 100644 --- a/gazebo_plugins/test/camera/depth_camera.world +++ b/gazebo_plugins/test/camera/depth_camera.world @@ -124,7 +124,7 @@ depth/image_raw - depth/camera_info + depth/camera_info points camera_link