diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 285f36692..b4f1557a2 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -370,7 +370,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 + + + + + + +