From 9ff986430eec3c984dd49186833c86f6e57724a6 Mon Sep 17 00:00:00 2001
From: Jose Luis Rivero <jrivero@osrfoundation.org>
Date: Tue, 28 Feb 2017 17:35:53 +0100
Subject: [PATCH] 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
---
 gazebo_plugins/CMakeLists.txt                 |  19 +++
 .../gazebo_plugins/gazebo_ros_multicamera.h   |   2 +
 gazebo_plugins/src/gazebo_ros_camera.cpp      |  20 ++-
 .../src/gazebo_ros_camera_utils.cpp           |   9 +-
 .../src/gazebo_ros_depth_camera.cpp           |  28 ++--
 gazebo_plugins/src/gazebo_ros_multicamera.cpp |  47 +++---
 .../src/gazebo_ros_openni_kinect.cpp          |  17 +-
 gazebo_plugins/src/gazebo_ros_prosilica.cpp   |  21 ++-
 gazebo_plugins/test/camera/camera.cpp         |  72 +++++++++
 gazebo_plugins/test/camera/camera.test        |  17 ++
 gazebo_plugins/test/camera/camera.world       | 138 +++++++++++++++++
 gazebo_plugins/test/camera/depth_camera.cpp   | 113 ++++++++++++++
 gazebo_plugins/test/camera/depth_camera.test  |  17 ++
 gazebo_plugins/test/camera/depth_camera.world | 144 +++++++++++++++++
 gazebo_plugins/test/camera/multicamera.cpp    | 101 ++++++++++++
 gazebo_plugins/test/camera/multicamera.test   |  11 ++
 gazebo_plugins/test/camera/multicamera.world  | 146 ++++++++++++++++++
 17 files changed, 849 insertions(+), 73 deletions(-)
 create mode 100644 gazebo_plugins/test/camera/camera.cpp
 create mode 100644 gazebo_plugins/test/camera/camera.test
 create mode 100644 gazebo_plugins/test/camera/camera.world
 create mode 100644 gazebo_plugins/test/camera/depth_camera.cpp
 create mode 100644 gazebo_plugins/test/camera/depth_camera.test
 create mode 100644 gazebo_plugins/test/camera/depth_camera.world
 create mode 100644 gazebo_plugins/test/camera/multicamera.cpp
 create mode 100644 gazebo_plugins/test/camera/multicamera.test
 create mode 100644 gazebo_plugins/test/camera/multicamera.world

diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
index d2c31c26d..b3fefc62c 100644
--- 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,6 +359,8 @@ 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
@@ -364,4 +368,19 @@ if (CATKIN_ENABLE_TESTING)
                     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
index 233194551..aa9970541 100644
--- 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
index 10f6d0c00..9c71c69fa 100644
--- 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
index 4776cae45..7d34042bd 100644
--- 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
index b1438c38a..a9f027eed 100644
--- 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
index 6a9a1f941..e46649c9c 100644
--- 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
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 = &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;
diff --git a/gazebo_plugins/test/camera/camera.cpp b/gazebo_plugins/test/camera/camera.cpp
new file mode 100644
index 000000000..b7805bbae
--- /dev/null
+++ b/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();
+}
diff --git a/gazebo_plugins/test/camera/camera.test b/gazebo_plugins/test/camera/camera.test
new file mode 100644
index 000000000..3a91b824b
--- /dev/null
+++ b/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>
diff --git a/gazebo_plugins/test/camera/camera.world b/gazebo_plugins/test/camera/camera.world
new file mode 100644
index 000000000..3154be354
--- /dev/null
+++ b/gazebo_plugins/test/camera/camera.world
@@ -0,0 +1,138 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Focus camera on tall pendulum -->
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
+        <view_controller>orbit</view_controller>
+      </camera>
+    </gui>
+
+  <model name="model_1">
+    <static>false</static>
+    <pose>2.0 0.0 4.0 0.0 0.0 0.0</pose>
+    <link name="link_1">
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <inertia>
+          <ixx>1.0</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>1.0</iyy>
+          <iyz>0.0</iyz>
+          <izz>1.0</izz>
+        </inertia>
+        <mass>10.0</mass>
+      </inertial>
+      <visual name="visual_sphere">
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <ambient>0.03 0.5 0.5 1.0</ambient>
+          <script>Gazebo/Green</script>
+        </material>
+        <cast_shadows>true</cast_shadows>
+        <laser_retro>100.0</laser_retro>
+      </visual>
+      <collision name="collision_sphere">
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <max_contacts>250</max_contacts>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.5</mu>
+              <mu2>0.2</mu2>
+              <fdir1>1.0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1000000.0</threshold>
+          </bounce>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e15</kp>
+              <kd>1e13</kd>
+              <max_vel>100.0</max_vel>
+              <min_depth>0.0001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+        <laser_retro>100.0</laser_retro>
+      </collision>
+    </link>
+  </model>
+
+  <model name="camera_model">
+    <static>true</static>
+    <pose>0.0 0.0 0.5 0.0 0.0 0.0</pose>
+    <link name="cmaera_link">
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+    <sensor type="camera" name="camera1">
+      <update_rate>0.5</update_rate>
+      <camera name="head">
+        <horizontal_fov>1.3962634</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <!-- Noise is sampled independently per pixel on each frame.  
+               That pixel's noise value is added to each of its color
+               channels, which at that point lie in the range [0,1]. -->
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <!-- Keep this zero, update_rate will control the frame rate -->
+        <updateRate>0.0</updateRate>
+        <cameraName>camera1</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>camera_link</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+    </link>
+  </model>
+
+  </world>
+</sdf>
diff --git a/gazebo_plugins/test/camera/depth_camera.cpp b/gazebo_plugins/test/camera/depth_camera.cpp
new file mode 100644
index 000000000..596e6d50f
--- /dev/null
+++ b/gazebo_plugins/test/camera/depth_camera.cpp
@@ -0,0 +1,113 @@
+#include <gtest/gtest.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+
+class DepthCameraTest : public testing::Test
+{
+protected:
+  virtual void SetUp()
+  {
+    has_new_image_ = false;
+    has_new_depth_ = false;
+    has_new_points_ = false;
+  }
+
+  ros::NodeHandle nh_;
+  image_transport::Subscriber cam_sub_;
+  image_transport::Subscriber depth_sub_;
+  ros::Subscriber points_sub_;
+  bool has_new_image_;
+  ros::Time image_stamp_;
+  bool has_new_depth_;
+  ros::Time depth_stamp_;
+  bool has_new_points_;
+  ros::Time points_stamp_;
+public:
+  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    image_stamp_ = msg->header.stamp;
+    has_new_image_ = true;
+  }
+  void depthCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    depth_stamp_ = msg->header.stamp;
+    has_new_depth_ = true;
+  }
+  void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
+  {
+    points_stamp_ = msg->header.stamp;
+    has_new_points_ = true;
+  }
+};
+
+// Test if the camera image is published at all, and that the timestamp
+// is not too long in the past.
+TEST_F(DepthCameraTest, cameraSubscribeTest)
+{
+  image_transport::ImageTransport it(nh_);
+  cam_sub_ = it.subscribe("camera1/image_raw", 1,
+                          &DepthCameraTest::imageCallback,
+                          dynamic_cast<DepthCameraTest*>(this));
+  depth_sub_ = it.subscribe("camera1/depth/image_raw", 1,
+                            &DepthCameraTest::depthCallback,
+                            dynamic_cast<DepthCameraTest*>(this));
+  points_sub_ = nh_.subscribe("camera1/points", 1,
+                             &DepthCameraTest::pointsCallback,
+                             dynamic_cast<DepthCameraTest*>(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_ || !has_new_depth_ || !has_new_points_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.1).sleep();
+  }
+
+  EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec());
+  EXPECT_EQ(depth_stamp_.toSec(), points_stamp_.toSec());
+  // This check depends on the update period (currently 1.0/update_rate = 2.0 seconds)
+  // being much longer than the expected difference between now and the
+  // received image time.
+  const double max_time = 1.0;
+  const ros::Time current_time = ros::Time::now();
+  // TODO(lucasw)
+  // this likely isn't that robust - what if the testing system is really slow?
+  double time_diff;
+  time_diff = (current_time - image_stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, max_time);
+
+  time_diff = (current_time - depth_stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, max_time);
+
+  time_diff = (current_time - points_stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, max_time);
+
+  cam_sub_.shutdown();
+  depth_sub_.shutdown();
+  points_sub_.shutdown();
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "gazebo_depth_camera_test");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/gazebo_plugins/test/camera/depth_camera.test b/gazebo_plugins/test/camera/depth_camera.test
new file mode 100644
index 000000000..3f4dd72bc
--- /dev/null
+++ b/gazebo_plugins/test/camera/depth_camera.test
@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" value="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/depth_camera.world" />
+
+  <group if="$(arg gui)">
+    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
+  </group>
+
+  <test test-name="depth_camera" pkg="gazebo_plugins" type="depth_camera-test"
+      clear_params="true" time-limit="30.0" />
+</launch>
diff --git a/gazebo_plugins/test/camera/depth_camera.world b/gazebo_plugins/test/camera/depth_camera.world
new file mode 100644
index 000000000..73132f908
--- /dev/null
+++ b/gazebo_plugins/test/camera/depth_camera.world
@@ -0,0 +1,144 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Focus camera on tall pendulum -->
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
+        <view_controller>orbit</view_controller>
+      </camera>
+    </gui>
+
+  <model name="model_1">
+    <static>false</static>
+    <pose>2.0 0.0 4.0 0.0 0.0 0.0</pose>
+    <link name="link_1">
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <inertia>
+          <ixx>1.0</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>1.0</iyy>
+          <iyz>0.0</iyz>
+          <izz>1.0</izz>
+        </inertia>
+        <mass>10.0</mass>
+      </inertial>
+      <visual name="visual_sphere">
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <ambient>0.03 0.5 0.5 1.0</ambient>
+          <script>Gazebo/Green</script>
+        </material>
+        <cast_shadows>true</cast_shadows>
+        <laser_retro>100.0</laser_retro>
+      </visual>
+      <collision name="collision_sphere">
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <max_contacts>250</max_contacts>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.5</mu>
+              <mu2>0.2</mu2>
+              <fdir1>1.0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1000000.0</threshold>
+          </bounce>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e15</kp>
+              <kd>1e13</kd>
+              <max_vel>100.0</max_vel>
+              <min_depth>0.0001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+        <laser_retro>100.0</laser_retro>
+      </collision>
+    </link>
+  </model>
+
+  <model name="camera_model">
+    <static>true</static>
+    <pose>0.0 0.0 0.5 0.0 0.0 0.0</pose>
+    <link name="camera_link">
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <sensor type="depth" name="camera1">
+        <update_rate>0.5</update_rate>
+        <camera name="head">
+          <!-- TODO(lucasw) is noise used? 
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+          -->
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+        </camera>
+        <plugin name="camera_controller" filename="libgazebo_ros_depth_camera.so">
+          <alwaysOn>true</alwaysOn>
+          <!-- Keep this zero, update_rate will control the frame rate -->
+          <updateRate>0.0</updateRate>
+          <cameraName>camera1</cameraName>
+          <imageTopicName>image_raw</imageTopicName>
+          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+          <depthImageTopicName>depth/image_raw</depthImageTopicName>
+          <!-- neither camera info is getting published, frame_id is empty 
+            in points and both image headers -->
+          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
+          <pointCloudTopicName>points</pointCloudTopicName>
+          <frameName>camera_link</frameName>
+          <!-- TODO(lucasw) is this used by depth camera at all? -->
+          <hackBaseline>0.07</hackBaseline>
+          <pointCloudCutoff>0.001</pointCloudCutoff>
+          <distortionK1>0.0</distortionK1>
+          <distortionK2>0.0</distortionK2>
+          <distortionK3>0.0</distortionK3>
+          <distortionT1>0.0</distortionT1>
+          <distortionT2>0.0</distortionT2>
+        </plugin>
+      </sensor>
+    </link>
+  </model>
+
+  </world>
+</sdf>
diff --git a/gazebo_plugins/test/camera/multicamera.cpp b/gazebo_plugins/test/camera/multicamera.cpp
new file mode 100644
index 000000000..0823c53da
--- /dev/null
+++ b/gazebo_plugins/test/camera/multicamera.cpp
@@ -0,0 +1,101 @@
+#include <gtest/gtest.h>
+// #include <image_transport/image_transport.h>
+// #include <image_transport/subscriber_filter.h>
+// #include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+
+class MultiCameraTest : public testing::Test
+{
+protected:
+  virtual void SetUp()
+  {
+    has_new_image_ = false;
+  }
+
+  ros::NodeHandle nh_;
+  // image_transport::SubscriberFilter cam_left_sub_;
+  // image_transport::SubscriberFilter cam_right_sub_;
+  message_filters::Subscriber<sensor_msgs::Image> cam_left_sub_;
+  message_filters::Subscriber<sensor_msgs::Image> cam_right_sub_;
+
+  bool has_new_image_;
+  ros::Time image_left_stamp_;
+  ros::Time image_right_stamp_;
+
+  // typedef message_filters::sync_policies::ApproximateTime<
+  //     sensor_msgs::Image, sensor_msgs::Image
+  //     > MySyncPolicy;
+  // message_filters::Synchronizer< MySyncPolicy > sync_;
+
+
+public:
+  void imageCallback(
+      const sensor_msgs::ImageConstPtr& left_msg,
+      const sensor_msgs::ImageConstPtr& right_msg)
+  {
+    image_left_stamp_ = left_msg->header.stamp;
+    image_right_stamp_ = right_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(MultiCameraTest, cameraSubscribeTest)
+{
+  // image_transport::ImageTransport it(nh_);
+  // cam_left_sub_.subscribe(it, "stereo/camera/left/image_raw", 1);
+  // cam_right_sub_.subscribe(it, "stereo/camera/right/image_raw", 1);
+  // sync_ = message_filters::Synchronizer<MySyncPolicy>(
+  //     MySyncPolicy(4), cam_left_sub_, cam_right_sub_);
+  cam_left_sub_.subscribe(nh_, "stereo/camera/left/image_raw", 1);
+  cam_right_sub_.subscribe(nh_, "stereo/camera/right/image_raw", 1);
+  message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(
+      cam_left_sub_, cam_right_sub_, 4);
+  sync.registerCallback(boost::bind(&MultiCameraTest::imageCallback,
+      dynamic_cast<MultiCameraTest*>(this), _1, _2));
+#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();
+  }
+
+  double sync_diff = (image_left_stamp_ - image_right_stamp_).toSec();
+  EXPECT_EQ(sync_diff, 0.0);
+
+  // 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_left_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_multicamera_test");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/gazebo_plugins/test/camera/multicamera.test b/gazebo_plugins/test/camera/multicamera.test
new file mode 100644
index 000000000..7a96b28ef
--- /dev/null
+++ b/gazebo_plugins/test/camera/multicamera.test
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <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/multicamera.world" />
+
+  <test test-name="multicamera" pkg="gazebo_plugins" type="multicamera-test"
+      clear_params="true" time-limit="15.0" />
+</launch>
diff --git a/gazebo_plugins/test/camera/multicamera.world b/gazebo_plugins/test/camera/multicamera.world
new file mode 100644
index 000000000..4c0b176b4
--- /dev/null
+++ b/gazebo_plugins/test/camera/multicamera.world
@@ -0,0 +1,146 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Focus camera on tall pendulum -->
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
+        <view_controller>orbit</view_controller>
+      </camera>
+    </gui>
+
+  <model name="model_1">
+    <static>false</static>
+    <pose>0.0 2.0 2.0 0.0 0.0 0.0</pose>
+    <link name="link_1">
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <inertia>
+          <ixx>1.0</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>1.0</iyy>
+          <iyz>0.0</iyz>
+          <izz>1.0</izz>
+        </inertia>
+        <mass>10.0</mass>
+      </inertial>
+      <visual name="visual_sphere">
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <ambient>0.03 0.5 0.5 1.0</ambient>
+          <script>Gazebo/Green</script>
+        </material>
+        <cast_shadows>true</cast_shadows>
+        <laser_retro>100.0</laser_retro>
+      </visual>
+      <collision name="collision_sphere">
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <max_contacts>250</max_contacts>
+        <geometry>
+          <sphere>
+            <radius>0.5</radius>
+          </sphere>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>0.5</mu>
+              <mu2>0.2</mu2>
+              <fdir1>1.0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1000000.0</threshold>
+          </bounce>
+          <contact>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e15</kp>
+              <kd>1e13</kd>
+              <max_vel>100.0</max_vel>
+              <min_depth>0.0001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+        <laser_retro>100.0</laser_retro>
+      </collision>
+
+      <sensor type="multicamera" name="stereo_camera">
+        <update_rate>0.1</update_rate>
+        <camera name="left">
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <camera name="right">
+          <pose>0 -0.07 0 0 0 0</pose>
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
+          <alwaysOn>true</alwaysOn>
+          <updateRate>0.0</updateRate>
+          <cameraName>stereo/camera</cameraName>
+          <imageTopicName>image_raw</imageTopicName>
+          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+          <frameName>left_camera_optical_frame</frameName>
+          <!--<rightFrameName>right_camera_optical_frame</rightFrameName>-->
+          <hackBaseline>0.07</hackBaseline>
+          <distortionK1>0.0</distortionK1>
+          <distortionK2>0.0</distortionK2>
+          <distortionK3>0.0</distortionK3>
+          <distortionT1>0.0</distortionT1>
+          <distortionT2>0.0</distortionT2>
+        </plugin>
+      </sensor>
+    </link>
+  </model>
+
+  </world>
+</sdf>