Fix timestamp issues for rendering sensors (indigo-devel) (#551)

* Fix for issue #408. The last measurement time is the time that gazebo generated the sensor data, so ought to be used. updateRate doesn't seem that useful. The other cameras need similar fixes to have the proper timestamps. * #408 Making a test for multicamra that shows the timestamps are currently outdated, will fix them similar to how the regular camera was fixed. * #408 Make the multi camera timestamps current rather than outdated, also reuse the same update code * Adding depth camera world to use in test to make depth camera have right timestamp #408- appears to be working (though only looking at horizon) but getting these sdf errors: Error [SDF.cc:789] Missing element description for [pointCloudTopicName] Error [SDF.cc:789] Missing element description for [depthImageCameraInfoTopicName] Error [SDF.cc:789] Missing element description for [pointCloudCutoff] * Disabling this test because of #409 * #408 Created test for depth camera, which fails, so next make it pass * #408 also test points publication * #408 fix timestamps for depth image, test passes now * #408 Increasing max time because some systems are taking 0.6 seconds to receive the messages (still well less than 2.0 seconds). Also all the tests can be run with run_tests_gazebo_plugins_rostest but only with the -j1 flag #409 * Adding @j-rivero default disabling of tests that require a display * change LastUpdateTime to LastMeasurementTime
ros-simulation · Feb 28, 2017 · 9ff9864 · 9ff9864
1 parent 37e120d
commit 9ff9864
Show file tree

Hide file tree

Showing 17 changed files with 849 additions and 73 deletions.
diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(gazebo_plugins)
 
+option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF)
+
 find_package(catkin REQUIRED COMPONENTS 
   message_generation 
   gazebo_msgs 
@@ -357,11 +359,28 @@ install(DIRECTORY test
   )
 
 # Tests
+# These need to be run with -j1 flag because gazebo can't be run
+# in parallel.
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest_gtest(set_model_state-test
                     test/set_model_state_test/set_model_state_test.test 
                     test/set_model_state_test/set_model_state_test.cpp)
   add_rostest(test/range/range_plugin.test)
   target_link_libraries(set_model_state-test ${catkin_LIBRARIES})
+
+  if (ENABLE_DISPLAY_TESTS)
+    add_rostest_gtest(depth_camera-test
+                      test/camera/depth_camera.test
+                      test/camera/depth_camera.cpp)
+    target_link_libraries(depth_camera-test ${catkin_LIBRARIES})
+    add_rostest_gtest(multicamera-test
+                      test/camera/multicamera.test
+                      test/camera/multicamera.cpp)
+    target_link_libraries(multicamera-test ${catkin_LIBRARIES})
+    add_rostest_gtest(camera-test
+                      test/camera/camera.test
+                      test/camera/camera.cpp)
+    target_link_libraries(camera-test ${catkin_LIBRARIES})
+  endif()
 endif()
diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h
@@ -42,6 +42,8 @@ namespace gazebo
 
     std::vector<GazeboRosCameraUtils*> utils;
 
+    protected: void OnNewFrame(const unsigned char *_image,
+                   GazeboRosCameraUtils* util);
     /// \brief Update the controller
     /// FIXME: switch to function vectors
     protected: virtual void OnNewFrameLeft(const unsigned char *_image,

diff --git a/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp
@@ -77,9 +77,9 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
     const std::string &_format)
 {
 # if GAZEBO_MAJOR_VERSION >= 7
-  this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+  common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
 # else
-  this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
+  common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
 # endif
 
   if (!this->parentSensor->IsActive())
@@ -92,12 +92,18 @@ void GazeboRosCamera::OnNewFrame(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_)
+      // OnNewFrame is triggered at the gazebo sensor <update_rate>
+      // while there is also a plugin <updateRate> that can throttle the
+      // rate down further (but then why not reduce the sensor rate?
+      // what is the use case?).
+      // Setting the <updateRate> to zero will make this plugin
+      // update at the gazebo sensor <update_rate>, update_period_ will be
+      // zero and the conditional always will be true.
+      if (sensor_update_time - this->last_update_time_ >= this->update_period_)
       {
-        this->PutCameraData(_image);
-        this->PublishCameraInfo();
-        this->last_update_time_ = cur_time;
+        this->PutCameraData(_image, sensor_update_time);
+        this->PublishCameraInfo(sensor_update_time);
+        this->last_update_time_ = sensor_update_time;
       }
     }
   }

diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp
@@ -626,15 +626,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_depth_camera.cpp b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp
@@ -192,9 +192,9 @@ void GazeboRosDepthCamera::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())
   {
@@ -232,9 +232,9 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
     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())
   {
@@ -302,9 +302,9 @@ void GazeboRosDepthCamera::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())
@@ -316,7 +316,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);
+    }
   }
 }
 
@@ -502,15 +506,15 @@ void GazeboRosDepthCamera::PublishCameraInfo()
   if (this->depth_info_connect_count_ > 0)
   {
 # if GAZEBO_MAJOR_VERSION >= 7
-    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+    common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
 # else
-    this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
+    common::Time 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_)
+    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;
     }
   }
 }

diff --git a/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/gazebo_plugins/src/gazebo_ros_multicamera.cpp
@@ -102,52 +102,41 @@ void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-// Update the controller
-void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
-    unsigned int _width, unsigned int _height, unsigned int _depth,
-    const std::string &_format)
+
+void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image,
+    GazeboRosCameraUtils* util)
 {
-  GazeboRosCameraUtils* util = this->utils[0];
 # if GAZEBO_MAJOR_VERSION >= 7
-  util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
+  common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
 # else
-  util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
+  common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime();
 # endif
 
   if (util->parentSensor_->IsActive())
   {
-    common::Time cur_time = util->world_->GetSimTime();
-    if (cur_time - util->last_update_time_ >= util->update_period_)
+    if (sensor_update_time - util->last_update_time_ >= util->update_period_)
     {
-      util->PutCameraData(_image);
-      util->PublishCameraInfo();
-      util->last_update_time_ = cur_time;
+      util->PutCameraData(_image, sensor_update_time);
+      util->PublishCameraInfo(sensor_update_time);
+      util->last_update_time_ = sensor_update_time;
     }
   }
 }
 
+// Update the controller
+void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
+    unsigned int _width, unsigned int _height, unsigned int _depth,
+    const std::string &_format)
+{
+  OnNewFrame(_image, this->utils[0]);
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 // Update the controller
 void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
     unsigned int _width, unsigned int _height, unsigned int _depth,
     const std::string &_format)
 {
-  GazeboRosCameraUtils* util = this->utils[1];
-# if GAZEBO_MAJOR_VERSION >= 7
-  util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
-# else
-  util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
-# endif
-
-  if (util->parentSensor_->IsActive())
-  {
-    common::Time cur_time = util->world_->GetSimTime();
-    if (cur_time - util->last_update_time_ >= util->update_period_)
-    {
-      util->PutCameraData(_image);
-      util->PublishCameraInfo();
-      util->last_update_time_ = cur_time;
-    }
-  }
+  OnNewFrame(_image, this->utils[1]);
 }
 }
diff --git 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
@@ -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 = &image;
           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;