From 596b302667da6f68d5cdcb9b91afec8d32871dd3 Mon Sep 17 00:00:00 2001
From: Lucas Walter <wsacul@gmail.com>
Date: Tue, 29 Mar 2016 21:52:48 -0700
Subject: [PATCH] #408 Created test for depth camera, which fails, so next make
 it pass

---
 gazebo_plugins/CMakeLists.txt                 | 12 ++-
 gazebo_plugins/test/camera/camera.test        |  6 ++
 gazebo_plugins/test/camera/camera.world       |  9 +-
 gazebo_plugins/test/camera/depth_camera.cpp   | 92 +++++++++++++++++++
 gazebo_plugins/test/camera/depth_camera.test  | 17 ++++
 gazebo_plugins/test/camera/depth_camera.world | 18 +++-
 6 files changed, 144 insertions(+), 10 deletions(-)
 create mode 100644 gazebo_plugins/test/camera/depth_camera.cpp
 create mode 100644 gazebo_plugins/test/camera/depth_camera.test

diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
index feaa606ac..d4331fbc9 100644
--- a/gazebo_plugins/CMakeLists.txt
+++ b/gazebo_plugins/CMakeLists.txt
@@ -339,12 +339,16 @@ install(DIRECTORY test
 # Tests
 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)
-  target_link_libraries(set_model_state-test ${catkin_LIBRARIES})
+  # 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)
+  # target_link_libraries(set_model_state-test ${catkin_LIBRARIES})
 
   # Can't run these and the above test together
+  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)
diff --git a/gazebo_plugins/test/camera/camera.test b/gazebo_plugins/test/camera/camera.test
index 06f58a4e6..3a91b824b 100644
--- a/gazebo_plugins/test/camera/camera.test
+++ b/gazebo_plugins/test/camera/camera.test
@@ -1,11 +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
index f48a94c00..3154be354 100644
--- a/gazebo_plugins/test/camera/camera.world
+++ b/gazebo_plugins/test/camera/camera.world
@@ -21,7 +21,7 @@
 
   <model name="model_1">
     <static>false</static>
-    <pose>0.0 2.0 2.0 0.0 0.0 0.0</pose>
+    <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>
@@ -85,7 +85,14 @@
         </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">
diff --git a/gazebo_plugins/test/camera/depth_camera.cpp b/gazebo_plugins/test/camera/depth_camera.cpp
new file mode 100644
index 000000000..b7d4eae76
--- /dev/null
+++ b/gazebo_plugins/test/camera/depth_camera.cpp
@@ -0,0 +1,92 @@
+#include <gtest/gtest.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+
+class DepthCameraTest : public testing::Test
+{
+protected:
+  virtual void SetUp()
+  {
+    has_new_image_ = false;
+    has_new_depth_ = false;
+  }
+
+  ros::NodeHandle nh_;
+  image_transport::Subscriber cam_sub_;
+  image_transport::Subscriber depth_sub_;
+  bool has_new_image_;
+  ros::Time image_stamp_;
+  bool has_new_depth_;
+  ros::Time depth_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;
+  }
+};
+
+// 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));
+
+#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_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.1).sleep();
+  }
+
+  EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec());
+  // 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;
+  time_diff = (ros::Time::now() - image_stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, 0.5);
+
+  time_diff = (ros::Time::now() - depth_stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, 0.5);
+
+  cam_sub_.shutdown();
+  depth_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
index 25ade9c52..978a0a4b2 100644
--- a/gazebo_plugins/test/camera/depth_camera.world
+++ b/gazebo_plugins/test/camera/depth_camera.world
@@ -21,7 +21,7 @@
 
   <model name="model_1">
     <static>false</static>
-    <pose>0.0 2.0 2.0 0.0 0.0 0.0</pose>
+    <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>
@@ -85,8 +85,15 @@
         </surface>
         <laser_retro>100.0</laser_retro>
       </collision>
+    </link>
+  </model>
 
-      <sensor name="depth_camera" type="depth">
+  <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? 
@@ -111,12 +118,14 @@
           <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>
-          <cameraInfoTopicName>camera_info</cameraInfoTopicName>
-          <cameraName>depth_cam</cameraName>
           <frameName>camera_link</frameName>
           <!-- TODO(lucasw) is this used by depth camera at all? -->
           <hackBaseline>0.07</hackBaseline>
@@ -127,7 +136,6 @@
           <distortionT1>0.0</distortionT1>
           <distortionT2>0.0</distortionT2>
         </plugin>
-        <always_on>true</always_on>
       </sensor>
     </link>
   </model>