ros-simulation#408 Making very simple tests to show that running two …

…gazebo tests in sequence doesn't work.

gazebo_ros/CMakeLists.txt

@@ -97,3 +97,15 @@ install(DIRECTORY launch/
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
   )
 
+# Tests
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest_gtest(gazebo_ros-test
+                    test/gazebo_ros.test
+                    test/gazebo_ros_test.cpp)
+  target_link_libraries(gazebo_ros-test ${catkin_LIBRARIES})
+  add_rostest_gtest(gazebo_ros-test2
+                    test/gazebo_ros2.test
+                    test/gazebo_ros_test.cpp)
+  target_link_libraries(gazebo_ros-test2 ${catkin_LIBRARIES})
+endif()

gazebo_ros/test/gazebo_ros.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/empty.world" />
+
+  <test test-name="gazebo_ros" pkg="gazebo_ros" type="gazebo_ros-test"
+      clear_params="true" time-limit="30.0" />
+</launch>

gazebo_ros/test/gazebo_ros2.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/empty.world" />
+
+  <test test-name="gazebo_ros2" pkg="gazebo_ros" type="gazebo_ros-test"
+      clear_params="true" time-limit="30.0" />
+</launch>

gazebo_ros/test/gazebo_ros_test.cpp

@@ -0,0 +1,55 @@
+#include <gazebo_msgs/ModelStates.h>
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <rosgraph_msgs/Clock.h>
+
+class GazeboRosTest : public testing::Test
+{
+protected:
+  virtual void SetUp()
+  {
+    has_new_stamp_ = false;
+  }
+
+  ros::NodeHandle nh_;
+  ros::Subscriber model_states_sub_;
+  bool has_new_stamp_;
+  ros::Time stamp_;
+public:
+  void clockCallback(const rosgraph_msgs::ClockConstPtr& msg)
+  {
+    stamp_ = msg->clock;
+    has_new_stamp_ = true;
+  }
+};
+
+// Test if the camera image is published at all, and that the timestamp
+// is not too long in the past.
+TEST_F(GazeboRosTest, cameraSubscribeTest)
+{
+  model_states_sub_ = nh_.subscribe("clock", 1,
+                          &GazeboRosTest::clockCallback,
+                          dynamic_cast<GazeboRosTest*>(this));
+
+  while (!has_new_stamp_)
+  {
+    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() - stamp_).toSec();
+  ROS_INFO_STREAM(time_diff);
+  EXPECT_LT(time_diff, 0.5);
+  model_states_sub_.shutdown();
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "gazebo_ros_test");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}