diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
index 29e14bc16..23ac86647 100644
--- a/gazebo_plugins/CMakeLists.txt
+++ b/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,6 +363,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
@@ -368,4 +372,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 8367b7faa..9c71c69fa 100644
--- a/gazebo_plugins/src/gazebo_ros_camera.cpp
+++ b/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;
       }
     }
   }
diff --git a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp
index 3e7584fa3..b0624b2d0 100644
--- a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp
+++ b/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;
     }
   }
 }
diff --git a/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/gazebo_plugins/src/gazebo_ros_multicamera.cpp
index eda927968..2e5b62ae2 100644
--- a/gazebo_plugins/src/gazebo_ros_multicamera.cpp
+++ b/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]);
 }
 }
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..978a0a4b2
--- /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 -->
+          <depthImageTopicCameraInfoName>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>