camera.cpp
Go to the documentation of this file.
00001 #include <gtest/gtest.h>
00002 #include <image_transport/image_transport.h>
00003 #include <ros/ros.h>
00004 
00005 class CameraTest : public testing::Test
00006 {
00007 protected:
00008   virtual void SetUp()
00009   {
00010     has_new_image_ = false;
00011   }
00012 
00013   ros::NodeHandle nh_;
00014   image_transport::Subscriber cam_sub_;
00015   bool has_new_image_;
00016   ros::Time image_stamp_;
00017 public:
00018   void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00019   {
00020     image_stamp_ = msg->header.stamp;
00021     has_new_image_ = true;
00022   }
00023 };
00024 
00025 // Test if the camera image is published at all, and that the timestamp
00026 // is not too long in the past.
00027 TEST_F(CameraTest, cameraSubscribeTest)
00028 {
00029   image_transport::ImageTransport it(nh_);
00030   cam_sub_ = it.subscribe("camera1/image_raw", 1,
00031                           &CameraTest::imageCallback,
00032                           dynamic_cast<CameraTest*>(this));
00033 
00034 #if 0
00035   // wait for gazebo to start publishing
00036   // TODO(lucasw) this isn't really necessary since this test
00037   // is purely passive
00038   bool wait_for_topic = true;
00039   while (wait_for_topic)
00040   {
00041     // @todo this fails without the additional 0.5 second sleep after the
00042     // publisher comes online, which means on a slower or more heavily
00043     // loaded system it may take longer than 0.5 seconds, and the test
00044     // would hang until the timeout is reached and fail.
00045     if (cam_sub_.getNumPublishers() > 0)
00046        wait_for_topic = false;
00047     ros::Duration(0.5).sleep();
00048   }
00049 #endif
00050 
00051   while (!has_new_image_)
00052   {
00053     ros::spinOnce();
00054     ros::Duration(0.1).sleep();
00055   }
00056 
00057   // This check depends on the update period being much longer
00058   // than the expected difference between now and the received image time
00059   // TODO(lucasw)
00060   // this likely isn't that robust - what if the testing system is really slow?
00061   double time_diff = (ros::Time::now() - image_stamp_).toSec();
00062   ROS_INFO_STREAM(time_diff);
00063   EXPECT_LT(time_diff, 1.0);
00064   cam_sub_.shutdown();
00065 }
00066 
00067 int main(int argc, char** argv)
00068 {
00069   ros::init(argc, argv, "gazebo_camera_test");
00070   testing::InitGoogleTest(&argc, argv);
00071   return RUN_ALL_TESTS();
00072 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09