From e2bfde325fa53ba30aad79bac96833edc0a50e4b Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 19 Mar 2016 06:31:14 -0700 Subject: [PATCH] Have a camera test that passes when launched from rostest or catkin_make run_tests_gazebo_plugins_rostest_test_camera_camera.test, but it fails when run from catkin_make run_tests_gazebo_plugins- the actual test succeeds, but then doesn't exit, and the time runs out. issue #408 --- gazebo_plugins/CMakeLists.txt | 5 + gazebo_plugins/test/camera/camera.cpp | 69 +++++++++++++ gazebo_plugins/test/camera/camera.test | 11 ++ gazebo_plugins/test/camera/camera.world | 131 ++++++++++++++++++++++++ 4 files changed, 216 insertions(+) create mode 100644 gazebo_plugins/test/camera/camera.cpp create mode 100644 gazebo_plugins/test/camera/camera.test create mode 100644 gazebo_plugins/test/camera/camera.world diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 32025fe27..6f6f91360 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -343,4 +343,9 @@ if (CATKIN_ENABLE_TESTING) test/set_model_state_test/set_model_state_test.test test/set_model_state_test/set_model_state_test.cpp) target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) + + add_rostest_gtest(camera-test + test/camera/camera.test + test/camera/camera.cpp) + target_link_libraries(camera-test ${catkin_LIBRARIES}) endif() diff --git a/gazebo_plugins/test/camera/camera.cpp b/gazebo_plugins/test/camera/camera.cpp new file mode 100644 index 000000000..83fa8b98b --- /dev/null +++ b/gazebo_plugins/test/camera/camera.cpp @@ -0,0 +1,69 @@ +#include +#include +#include + +class CameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + has_new_image_ = false; + } + + ros::NodeHandle nh_; + image_transport::Subscriber cam_sub_; + bool has_new_image_; + ros::Time image_stamp_; +public: + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + image_stamp_ = 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(CameraTest, cameraSubscribeTest) +{ + image_transport::ImageTransport it(nh_); + cam_sub_ = it.subscribe("camera1/image_raw", 1, + &CameraTest::imageCallback, + dynamic_cast(this)); + + // 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(); + } + + while (!has_new_image_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + // 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_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 0.5); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_camera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/camera/camera.test b/gazebo_plugins/test/camera/camera.test new file mode 100644 index 000000000..06f58a4e6 --- /dev/null +++ b/gazebo_plugins/test/camera/camera.test @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/gazebo_plugins/test/camera/camera.world b/gazebo_plugins/test/camera/camera.world new file mode 100644 index 000000000..f48a94c00 --- /dev/null +++ b/gazebo_plugins/test/camera/camera.world @@ -0,0 +1,131 @@ + + + + + + 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.5 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + + 0.0 + camera1 + image_raw + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + +