Correct the timestamp used by the camera (kinetic-devel)

gazebo_plugins/CMakeLists.txt

@@ -1,7 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(gazebo_plugins)
 
-find_package(catkin REQUIRED COMPONENTS
+option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF)
+
+find_package(catkin REQUIRED COMPONENTS 
   message_generation
   gazebo_msgs
   roscpp
@@ -361,11 +363,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()

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,

gazebo_plugins/src/gazebo_ros_camera.cpp

@@ -76,7 +76,11 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
     unsigned int _width, unsigned int _height, unsigned int _depth,
     const std::string &_format)
 {
-  this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+# if GAZEBO_MAJOR_VERSION >= 7
+  common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
+# else
+  common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
+# endif
 
   if (!this->parentSensor->IsActive())
   {
@@ -88,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;
       }
     }
   }

gazebo_plugins/src/gazebo_ros_depth_camera.cpp

@@ -191,7 +191,12 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
-  this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
+# if GAZEBO_MAJOR_VERSION >= 7
+  this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
+# else
+  this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
+# endif
+
   if (this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ <= 0 &&
@@ -227,7 +232,12 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
-  this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
+# if GAZEBO_MAJOR_VERSION >= 7
+  this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime();
+# else
+  this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
+# endif
+
   if (!this->parentSensor->IsActive())
   {
     if (this->point_cloud_connect_count_ > 0)
@@ -292,8 +302,12 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
   if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
     return;
 
-  //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->Name().c_str(),this->frame_name_.c_str());
-  this->sensor_update_time_ = this->parentSensor->LastUpdateTime();
+  //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->LastMeasurementTime();
+# else
+  this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime();
+# endif
 
   if (!this->parentSensor->IsActive())
   {
@@ -304,7 +318,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 +503,16 @@ void GazeboRosDepthCamera::PublishCameraInfo()
 
   if (this->depth_info_connect_count_ > 0)
   {
-    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
-    common::Time cur_time = this->world_->GetSimTime();
-    if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
+# if GAZEBO_MAJOR_VERSION >= 7
+    common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
+# else
+    common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
+# endif
+    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;
     }
   }
 }

gazebo_plugins/src/gazebo_ros_multicamera.cpp

@@ -94,44 +94,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];
-  util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
+# if GAZEBO_MAJOR_VERSION >= 7
+  common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
+# else
+  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];
-  util->sensor_update_time_ = util->parentSensor_->LastUpdateTime();
-
-  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]);
 }
 }

gazebo_plugins/test/camera/camera.cpp

@@ -0,0 +1,72 @@
+#include <gtest/gtest.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+
+class CameraTest : public testing::Test
+{
+protected:
+  virtual void SetUp()
+  {
+    has_new_image_ = false;
+  }
+
+  ros::NodeHandle nh_;
+  image_transport::Subscriber cam_sub_;
+  bool has_new_image_;
+  ros::Time image_stamp_;
+public:
+  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    image_stamp_ = msg->header.stamp;
+    has_new_image_ = true;
+  }
+};
+
+// Test if the camera image is published at all, and that the timestamp
+// is not too long in the past.
+TEST_F(CameraTest, cameraSubscribeTest)
+{
+  image_transport::ImageTransport it(nh_);
+  cam_sub_ = it.subscribe("camera1/image_raw", 1,
+                          &CameraTest::imageCallback,
+                          dynamic_cast<CameraTest*>(this));
+
+#if 0
+  // wait for gazebo to start publishing
+  // TODO(lucasw) this isn't really necessary since this test
+  // is purely passive
+  bool wait_for_topic = true;
+  while (wait_for_topic)
+  {
+    // @todo this fails without the additional 0.5 second sleep after the
+    // publisher comes online, which means on a slower or more heavily
+    // loaded system it may take longer than 0.5 seconds, and the test
+    // would hang until the timeout is reached and fail.
+    if (cam_sub_.getNumPublishers() > 0)
+       wait_for_topic = false;
+    ros::Duration(0.5).sleep();
+  }
+#endif
+
+  while (!has_new_image_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.1).sleep();
+  }
+
+  // This check depends on the update period being much longer
+  // than the expected difference between now and the received image time
+  // TODO(lucasw)
+  // this likely isn't that robust - what if the testing system is really slow?
+  double time_diff = (ros::Time::now() - image_stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, 1.0);
+  cam_sub_.shutdown();
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "gazebo_camera_test");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo_plugins/test/camera/camera.test

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="false" />
+
+  <param name="/use_sim_time" value="true" />
+
+  <node name="gazebo" pkg="gazebo_ros" type="gzserver"
+      respawn="false" output="screen"
+      args="-r $(find gazebo_plugins)/test/camera/camera.world" />
+
+  <group if="$(arg gui)">
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
+  </group>
+
+  <test test-name="camera" pkg="gazebo_plugins" type="camera-test"
+      clear_params="true" time-limit="30.0" />
+</launch>