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