From 42ad4004b7e0e32df4c936754df5f78f8800b693 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 23 Mar 2016 05:41:11 -0700 Subject: [PATCH] #408 Making a test for multicamra that shows the timestamps are currently outdated, will fix them similar to how the regular camera was fixed. --- gazebo_plugins/CMakeLists.txt | 6 +- gazebo_plugins/test/camera/multicamera.cpp | 101 +++++++++++++ gazebo_plugins/test/camera/multicamera.test | 11 ++ gazebo_plugins/test/camera/multicamera.world | 146 +++++++++++++++++++ 4 files changed, 263 insertions(+), 1 deletion(-) create mode 100644 gazebo_plugins/test/camera/multicamera.cpp create mode 100644 gazebo_plugins/test/camera/multicamera.test create mode 100644 gazebo_plugins/test/camera/multicamera.world diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 3b3b0c254..151e874b4 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -365,7 +365,11 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/range/range_plugin.test) target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) - # Can't run this and the above test together + # Can't run these and the above test together + add_rostest_gtest(multicamera-test + test/camera/multicamera.test + test/camera/multicamera.cpp) + target_link_libraries(multicamera-test ${catkin_LIBRARIES}) # add_rostest_gtest(camera-test # test/camera/camera.test # test/camera/camera.cpp) diff --git a/gazebo_plugins/test/camera/multicamera.cpp b/gazebo_plugins/test/camera/multicamera.cpp new file mode 100644 index 000000000..cd66a0778 --- /dev/null +++ b/gazebo_plugins/test/camera/multicamera.cpp @@ -0,0 +1,101 @@ +#include +// #include +// #include +// #include +#include +#include +#include +#include + +class MultiCameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + has_new_image_ = false; + } + + ros::NodeHandle nh_; + // image_transport::SubscriberFilter cam_left_sub_; + // image_transport::SubscriberFilter cam_right_sub_; + message_filters::Subscriber cam_left_sub_; + message_filters::Subscriber cam_right_sub_; + + bool has_new_image_; + ros::Time image_left_stamp_; + ros::Time image_right_stamp_; + + // typedef message_filters::sync_policies::ApproximateTime< + // sensor_msgs::Image, sensor_msgs::Image + // > MySyncPolicy; + // message_filters::Synchronizer< MySyncPolicy > sync_; + + +public: + void imageCallback( + const sensor_msgs::ImageConstPtr& left_msg, + const sensor_msgs::ImageConstPtr& right_msg) + { + image_left_stamp_ = left_msg->header.stamp; + image_right_stamp_ = right_msg->header.stamp; + has_new_image_ = true; + } +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(MultiCameraTest, cameraSubscribeTest) +{ + // image_transport::ImageTransport it(nh_); + // cam_left_sub_.subscribe(it, "stereo/camera/left/image_raw", 1); + // cam_right_sub_.subscribe(it, "stereo/camera/right/image_raw", 1); + // sync_ = message_filters::Synchronizer( + // MySyncPolicy(4), cam_left_sub_, cam_right_sub_); + cam_left_sub_.subscribe(nh_, "stereo/camera/left/image_raw", 1); + cam_right_sub_.subscribe(nh_, "stereo/camera/right/image_raw", 1); + message_filters::TimeSynchronizer sync( + cam_left_sub_, cam_right_sub_, 4); + sync.registerCallback(boost::bind(&MultiCameraTest::imageCallback, + dynamic_cast(this), _1, _2)); +#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_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + double sync_diff = (image_left_stamp_ - image_right_stamp_).toSec(); + EXPECT_EQ(sync_diff, 0.0); + + // 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() - image_left_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 0.5); + // cam_sub_.shutdown(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_multicamera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/camera/multicamera.test b/gazebo_plugins/test/camera/multicamera.test new file mode 100644 index 000000000..7a96b28ef --- /dev/null +++ b/gazebo_plugins/test/camera/multicamera.test @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/gazebo_plugins/test/camera/multicamera.world b/gazebo_plugins/test/camera/multicamera.world new file mode 100644 index 000000000..4c0b176b4 --- /dev/null +++ b/gazebo_plugins/test/camera/multicamera.world @@ -0,0 +1,146 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + false + 0.0 2.0 2.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + 0.1 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + 0 -0.07 0 0 0 0 + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + stereo/camera + image_raw + camera_info + left_camera_optical_frame + + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + +