#408 fix timestamps for depth image, test passes now

ros-simulation · Mar 30, 2016 · c033556 · c033556
1 parent 902ad87
commit c033556
Showing 1 changed file with 12 additions and 8 deletions.
diff --git a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp
@@ -191,7 +191,7 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
-  this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
+  this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
   if (this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ <= 0 &&
@@ -227,7 +227,7 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
-  this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
+  this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
   if (!this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ > 0)
@@ -293,7 +293,7 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
     return;
 
   //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
-  this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
+  this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
 
   if (!this->parentSensor->IsActive())
   {
@@ -304,7 +304,11 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
   else
   {
     if ((*this->image_connect_count_) > 0)
+    {
       this->PutCameraData(_image);
+      // TODO(lucasw) publish camera info with depth image
+      // this->PublishCameraInfo(sensor_update_time);
+    }
   }
 }
 
@@ -485,12 +489,12 @@ void GazeboRosDepthCamera::PublishCameraInfo()
 
   if (this->depth_info_connect_count_ > 0)
   {
-    this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
-    common::Time cur_time = this->world_->GetSimTime();
-    if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
+    common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
+    this->sensor_update_time_ = sensor_update_time;
+    if (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->PublishCameraInfo(this->depth_image_camera_info_pub_);  // , sensor_update_time);
+      this->last_depth_image_camera_info_update_time_ = sensor_update_time;
     }
   }
 }