multicamera.cpp
Go to the documentation of this file.
00001 #include <gtest/gtest.h>
00002 // #include <image_transport/image_transport.h>
00003 // #include <image_transport/subscriber_filter.h>
00004 // #include <message_filters/sync_policies/approximate_time.h>
00005 #include <message_filters/subscriber.h>
00006 #include <message_filters/time_synchronizer.h>
00007 #include <ros/ros.h>
00008 #include <sensor_msgs/Image.h>
00009 
00010 class MultiCameraTest : public testing::Test
00011 {
00012 protected:
00013   virtual void SetUp()
00014   {
00015     has_new_image_ = false;
00016   }
00017 
00018   ros::NodeHandle nh_;
00019   // image_transport::SubscriberFilter cam_left_sub_;
00020   // image_transport::SubscriberFilter cam_right_sub_;
00021   message_filters::Subscriber<sensor_msgs::Image> cam_left_sub_;
00022   message_filters::Subscriber<sensor_msgs::Image> cam_right_sub_;
00023 
00024   bool has_new_image_;
00025   ros::Time image_left_stamp_;
00026   ros::Time image_right_stamp_;
00027 
00028   // typedef message_filters::sync_policies::ApproximateTime<
00029   //     sensor_msgs::Image, sensor_msgs::Image
00030   //     > MySyncPolicy;
00031   // message_filters::Synchronizer< MySyncPolicy > sync_;
00032 
00033 
00034 public:
00035   void imageCallback(
00036       const sensor_msgs::ImageConstPtr& left_msg,
00037       const sensor_msgs::ImageConstPtr& right_msg)
00038   {
00039     image_left_stamp_ = left_msg->header.stamp;
00040     image_right_stamp_ = right_msg->header.stamp;
00041     has_new_image_ = true;
00042   }
00043 };
00044 
00045 // Test if the camera image is published at all, and that the timestamp
00046 // is not too long in the past.
00047 TEST_F(MultiCameraTest, cameraSubscribeTest)
00048 {
00049   // image_transport::ImageTransport it(nh_);
00050   // cam_left_sub_.subscribe(it, "stereo/camera/left/image_raw", 1);
00051   // cam_right_sub_.subscribe(it, "stereo/camera/right/image_raw", 1);
00052   // sync_ = message_filters::Synchronizer<MySyncPolicy>(
00053   //     MySyncPolicy(4), cam_left_sub_, cam_right_sub_);
00054   cam_left_sub_.subscribe(nh_, "stereo/camera/left/image_raw", 1);
00055   cam_right_sub_.subscribe(nh_, "stereo/camera/right/image_raw", 1);
00056   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> sync(
00057       cam_left_sub_, cam_right_sub_, 4);
00058   sync.registerCallback(boost::bind(&MultiCameraTest::imageCallback,
00059       dynamic_cast<MultiCameraTest*>(this), _1, _2));
00060 #if 0
00061   // wait for gazebo to start publishing
00062   // TODO(lucasw) this isn't really necessary since this test
00063   // is purely passive
00064   bool wait_for_topic = true;
00065   while (wait_for_topic)
00066   {
00067     // @todo this fails without the additional 0.5 second sleep after the
00068     // publisher comes online, which means on a slower or more heavily
00069     // loaded system it may take longer than 0.5 seconds, and the test
00070     // would hang until the timeout is reached and fail.
00071     if (cam_sub_.getNumPublishers() > 0)
00072        wait_for_topic = false;
00073     ros::Duration(0.5).sleep();
00074   }
00075 #endif
00076 
00077   while (!has_new_image_)
00078   {
00079     ros::spinOnce();
00080     ros::Duration(0.1).sleep();
00081   }
00082 
00083   double sync_diff = (image_left_stamp_ - image_right_stamp_).toSec();
00084   EXPECT_EQ(sync_diff, 0.0);
00085 
00086   // This check depends on the update period being much longer
00087   // than the expected difference between now and the received image time
00088   // TODO(lucasw)
00089   // this likely isn't that robust - what if the testing system is really slow?
00090   double time_diff = (ros::Time::now() - image_left_stamp_).toSec();
00091   ROS_INFO_STREAM(time_diff);
00092   EXPECT_LT(time_diff, 1.0);
00093   // cam_sub_.shutdown();
00094 }
00095 
00096 int main(int argc, char** argv)
00097 {
00098   ros::init(argc, argv, "gazebo_multicamera_test");
00099   testing::InitGoogleTest(&argc, argv);
00100   return RUN_ALL_TESTS();
00101 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:23