Fix timestamp issues for rendering sensors (kinetic-devel)

This PR builds on top of pull request #410 and applies the timestamp fix
to kinect_openni and prosilica sensors

gazebo_plugins/src/gazebo_ros_camera_utils.cpp

@@ -606,12 +606,11 @@ void GazeboRosCameraUtils::PublishCameraInfo()
 
   if (this->camera_info_pub_.getNumSubscribers() > 0)
   {
-    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
-    common::Time cur_time = this->world_->GetSimTime();
-    if (cur_time - this->last_info_update_time_ >= this->update_period_)
+    this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
+    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_;
     }
   }
 }

gazebo_plugins/src/gazebo_ros_openni_kinect.cpp

@@ -188,7 +188,7 @@ void GazeboRosOpenniKinect::OnNewDepthFrame(const float *_image,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
-  this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
+  this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
   if (this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ <= 0 &&
@@ -226,7 +226,7 @@ void GazeboRosOpenniKinect::OnNewImageFrame(const unsigned char *_image,
     return;
 
   //ROS_ERROR_NAMED("openni_kinect", "camera_ new frame %s %s",this->parentSensor_->Name().c_str(),this->frame_name_.c_str());
-  this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+  this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
 
   if (this->parentSensor->IsActive())
   {
@@ -434,12 +434,12 @@ void GazeboRosOpenniKinect::PublishCameraInfo()
 
   if (this->depth_info_connect_count_ > 0)
   {
-    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+    this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
     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_;
     }
   }
 }

gazebo_plugins/src/gazebo_ros_prosilica.cpp

@@ -131,7 +131,7 @@ 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.
-  common::Time sensor_update_time = this->parentSensor_->LastUpdateTime();
+  common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
 
   // as long as ros is connected, parent is active
   //ROS_ERROR_NAMED("prosilica", "debug image count %d",this->image_connect_count_);
@@ -149,12 +149,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;
         }
       }
     }
@@ -218,7 +217,7 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
         this->roiCameraInfoMsg = &info;
         this->roiCameraInfoMsg->header.frame_id = this->frame_name_;
 
-        common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime();
+        common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime();
         this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec;
         this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec;
 
@@ -272,7 +271,7 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
         // copy data into image_msg_, then convert to roiImageMsg(image)
         this->image_msg_.header.frame_id    = this->frame_name_;
 
-        common::Time lastRenderTime = this->parentSensor_->LastUpdateTime();
+        common::Time lastRenderTime = this->parentSensor_->LastMeasurementTime();
         this->image_msg_.header.stamp.sec = lastRenderTime.sec;
         this->image_msg_.header.stamp.nsec = lastRenderTime.nsec;
 
@@ -296,7 +295,7 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
           // copy data into ROI image
           this->roiImageMsg = ℑ
           this->roiImageMsg->header.frame_id = this->frame_name_;
-          common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime();
+          common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime();
           this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec;
           this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec;
 

gazebo_plugins/test/camera/depth_camera.world

@@ -96,7 +96,7 @@
       <sensor type="depth" name="camera1">
         <update_rate>0.5</update_rate>
         <camera name="head">
-          <!-- TODO(lucasw) is noise used? 
+          <!-- TODO(lucasw) is noise used?
           <noise>
             <type>gaussian</type>
             <mean>0.0</mean>
@@ -122,9 +122,9 @@
           <imageTopicName>image_raw</imageTopicName>
           <cameraInfoTopicName>camera_info</cameraInfoTopicName>
           <depthImageTopicName>depth/image_raw</depthImageTopicName>
-          <!-- neither camera info is getting published, frame_id is empty 
+          <!-- neither camera info is getting published, frame_id is empty
             in points and both image headers -->
-          <depthImageTopicCameraInfoName>depth/camera_info</depthImageCameraInfoTopicName>
+          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
           <pointCloudTopicName>points</pointCloudTopicName>
           <frameName>camera_link</frameName>
           <!-- TODO(lucasw) is this used by depth camera at all? -->