depth_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 #include <sensor_msgs/PointCloud2.h>
00005 
00006 class DepthCameraTest : public testing::Test
00007 {
00008 protected:
00009   virtual void SetUp()
00010   {
00011     has_new_image_ = false;
00012     has_new_depth_ = false;
00013     has_new_points_ = false;
00014   }
00015 
00016   ros::NodeHandle nh_;
00017   image_transport::Subscriber cam_sub_;
00018   image_transport::Subscriber depth_sub_;
00019   ros::Subscriber points_sub_;
00020   bool has_new_image_;
00021   ros::Time image_stamp_;
00022   bool has_new_depth_;
00023   ros::Time depth_stamp_;
00024   bool has_new_points_;
00025   ros::Time points_stamp_;
00026 public:
00027   void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00028   {
00029     image_stamp_ = msg->header.stamp;
00030     has_new_image_ = true;
00031   }
00032   void depthCallback(const sensor_msgs::ImageConstPtr& msg)
00033   {
00034     depth_stamp_ = msg->header.stamp;
00035     has_new_depth_ = true;
00036   }
00037   void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
00038   {
00039     points_stamp_ = msg->header.stamp;
00040     has_new_points_ = true;
00041   }
00042 };
00043 
00044 // Test if the camera image is published at all, and that the timestamp
00045 // is not too long in the past.
00046 TEST_F(DepthCameraTest, cameraSubscribeTest)
00047 {
00048   image_transport::ImageTransport it(nh_);
00049   cam_sub_ = it.subscribe("camera1/image_raw", 1,
00050                           &DepthCameraTest::imageCallback,
00051                           dynamic_cast<DepthCameraTest*>(this));
00052   depth_sub_ = it.subscribe("camera1/depth/image_raw", 1,
00053                             &DepthCameraTest::depthCallback,
00054                             dynamic_cast<DepthCameraTest*>(this));
00055   points_sub_ = nh_.subscribe("camera1/points", 1,
00056                              &DepthCameraTest::pointsCallback,
00057                              dynamic_cast<DepthCameraTest*>(this));
00058 #if 0
00059   // wait for gazebo to start publishing
00060   // TODO(lucasw) this isn't really necessary since this test
00061   // is purely passive
00062   bool wait_for_topic = true;
00063   while (wait_for_topic)
00064   {
00065     // @todo this fails without the additional 0.5 second sleep after the
00066     // publisher comes online, which means on a slower or more heavily
00067     // loaded system it may take longer than 0.5 seconds, and the test
00068     // would hang until the timeout is reached and fail.
00069     if (cam_sub_.getNumPublishers() > 0)
00070        wait_for_topic = false;
00071     ros::Duration(0.5).sleep();
00072   }
00073 #endif
00074 
00075   while (!has_new_image_ || !has_new_depth_ || !has_new_points_)
00076   {
00077     ros::spinOnce();
00078     ros::Duration(0.1).sleep();
00079   }
00080 
00081   EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec());
00082   EXPECT_EQ(depth_stamp_.toSec(), points_stamp_.toSec());
00083   // This check depends on the update period (currently 1.0/update_rate = 2.0 seconds)
00084   // being much longer than the expected difference between now and the
00085   // received image time.
00086   const double max_time = 1.0;
00087   const ros::Time current_time = ros::Time::now();
00088   // TODO(lucasw)
00089   // this likely isn't that robust - what if the testing system is really slow?
00090   double time_diff;
00091   time_diff = (current_time - image_stamp_).toSec();
00092   ROS_INFO_STREAM(time_diff);
00093   EXPECT_LT(time_diff, max_time);
00094 
00095   time_diff = (current_time - depth_stamp_).toSec();
00096   ROS_INFO_STREAM(time_diff);
00097   EXPECT_LT(time_diff, max_time);
00098 
00099   time_diff = (current_time - points_stamp_).toSec();
00100   ROS_INFO_STREAM(time_diff);
00101   EXPECT_LT(time_diff, max_time);
00102 
00103   cam_sub_.shutdown();
00104   depth_sub_.shutdown();
00105   points_sub_.shutdown();
00106 }
00107 
00108 int main(int argc, char** argv)
00109 {
00110   ros::init(argc, argv, "gazebo_depth_camera_test");
00111   testing::InitGoogleTest(&argc, argv);
00112   return RUN_ALL_TESTS();
00113 }


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