From d00bd47f73a9aa05524cfc0936b9c2df760cc24d Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 17 Mar 2016 18:53:18 -0700 Subject: [PATCH 01/10] Fix for issue #408. The last measurement time is the time that gazebo generated the sensor data, so ought to be used. updateRate doesn't seem that useful. The other cameras need similar fixes to have the proper timestamps. --- gazebo_plugins/CMakeLists.txt | 6 ++ gazebo_plugins/src/gazebo_ros_camera.cpp | 20 ++-- gazebo_plugins/test/camera/camera.cpp | 72 +++++++++++++ gazebo_plugins/test/camera/camera.test | 11 ++ gazebo_plugins/test/camera/camera.world | 131 +++++++++++++++++++++++ 5 files changed, 233 insertions(+), 7 deletions(-) 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 2a6a14b96..285f36692 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -369,4 +369,10 @@ if (CATKIN_ENABLE_TESTING) test/set_model_state_test/set_model_state_test.cpp) 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 + # 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/src/gazebo_ros_camera.cpp b/gazebo_plugins/src/gazebo_ros_camera.cpp index 10f6d0c00..9c71c69fa 100644 --- a/gazebo_plugins/src/gazebo_ros_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -77,9 +77,9 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image, const std::string &_format) { # if GAZEBO_MAJOR_VERSION >= 7 - this->sensor_update_time_ = this->parentSensor_->LastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); # else - this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); # endif if (!this->parentSensor->IsActive()) @@ -92,12 +92,18 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image, { if ((*this->image_connect_count_) > 0) { - common::Time cur_time = this->world_->GetSimTime(); - if (cur_time - this->last_update_time_ >= this->update_period_) + // OnNewFrame is triggered at the gazebo sensor + // while there is also a plugin that can throttle the + // rate down further (but then why not reduce the sensor rate? + // what is the use case?). + // Setting the to zero will make this plugin + // update at the gazebo sensor , update_period_ will be + // zero and the conditional always will be true. + if (sensor_update_time - this->last_update_time_ >= this->update_period_) { - this->PutCameraData(_image); - this->PublishCameraInfo(); - this->last_update_time_ = cur_time; + this->PutCameraData(_image, sensor_update_time); + this->PublishCameraInfo(sensor_update_time); + this->last_update_time_ = sensor_update_time; } } } diff --git a/gazebo_plugins/test/camera/camera.cpp b/gazebo_plugins/test/camera/camera.cpp new file mode 100644 index 000000000..25a2932d3 --- /dev/null +++ b/gazebo_plugins/test/camera/camera.cpp @@ -0,0 +1,72 @@ +#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)); + +#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(); + } + + // 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); + cam_sub_.shutdown(); +} + +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 + + + + + + + From 3fb2feaa9182f60574e416adb95f3ed2a10af36d Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 23 Mar 2016 05:41:11 -0700 Subject: [PATCH 02/10] #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 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 + + + + + + + From 34b45d09c129b355a258c45d9bc58bc1daacc318 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 23 Mar 2016 05:52:53 -0700 Subject: [PATCH 03/10] #408 Make the multi camera timestamps current rather than outdated, also reuse the same update code --- .../gazebo_plugins/gazebo_ros_multicamera.h | 2 + gazebo_plugins/src/gazebo_ros_multicamera.cpp | 46 ++++++++----------- 2 files changed, 20 insertions(+), 28 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h index 233194551..aa9970541 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h @@ -42,6 +42,8 @@ namespace gazebo std::vector utils; + protected: void OnNewFrame(const unsigned char *_image, + GazeboRosCameraUtils* util); /// \brief Update the controller /// FIXME: switch to function vectors protected: virtual void OnNewFrameLeft(const unsigned char *_image, diff --git a/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/gazebo_plugins/src/gazebo_ros_multicamera.cpp index 6a9a1f941..248a252be 100644 --- a/gazebo_plugins/src/gazebo_ros_multicamera.cpp +++ b/gazebo_plugins/src/gazebo_ros_multicamera.cpp @@ -102,52 +102,42 @@ void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent, } //////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image, - unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string &_format) + +void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image, + GazeboRosCameraUtils* util) { GazeboRosCameraUtils* util = this->utils[0]; # if GAZEBO_MAJOR_VERSION >= 7 - util->sensor_update_time_ = util->parentSensor_->LastUpdateTime(); + common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime(); # else - util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime(); + common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime(); # endif if (util->parentSensor_->IsActive()) { - common::Time cur_time = util->world_->GetSimTime(); - if (cur_time - util->last_update_time_ >= util->update_period_) + if (sensor_update_time - util->last_update_time_ >= util->update_period_) { - util->PutCameraData(_image); - util->PublishCameraInfo(); - util->last_update_time_ = cur_time; + util->PutCameraData(_image, sensor_update_time); + util->PublishCameraInfo(sensor_update_time); + util->last_update_time_ = sensor_update_time; } } } +// Update the controller +void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + OnNewFrame(_image, this->utils[0]); +} + //////////////////////////////////////////////////////////////////////////////// // Update the controller void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) { - GazeboRosCameraUtils* util = this->utils[1]; -# if GAZEBO_MAJOR_VERSION >= 7 - util->sensor_update_time_ = util->parentSensor_->LastUpdateTime(); -# else - util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime(); -# endif - - if (util->parentSensor_->IsActive()) - { - common::Time cur_time = util->world_->GetSimTime(); - if (cur_time - util->last_update_time_ >= util->update_period_) - { - util->PutCameraData(_image); - util->PublishCameraInfo(); - util->last_update_time_ = cur_time; - } - } + OnNewFrame(_image, this->utils[1]); } } From d248877a8806533755c18118272d4b7e8e347be8 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 06:45:24 -0700 Subject: [PATCH 04/10] Adding depth camera world to use in test to make depth camera have right timestamp #408- appears to be working (though only looking at horizon) but getting these sdf errors: Error [SDF.cc:789] Missing element description for [pointCloudTopicName] Error [SDF.cc:789] Missing element description for [depthImageCameraInfoTopicName] Error [SDF.cc:789] Missing element description for [pointCloudCutoff] --- gazebo_plugins/test/camera/depth_camera.world | 136 ++++++++++++++++++ 1 file changed, 136 insertions(+) create mode 100644 gazebo_plugins/test/camera/depth_camera.world diff --git a/gazebo_plugins/test/camera/depth_camera.world b/gazebo_plugins/test/camera/depth_camera.world new file mode 100644 index 000000000..25ade9c52 --- /dev/null +++ b/gazebo_plugins/test/camera/depth_camera.world @@ -0,0 +1,136 @@ + + + + + + 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 + + + + true + + 0.0 + image_raw + depth/image_raw + depth/camera_info + points + camera_info + depth_cam + camera_link + + 0.07 + 0.001 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + true + + + + + + From 546f285dccf79a21177518145dde035aba534cd3 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 06:47:29 -0700 Subject: [PATCH 05/10] Disabling this test because of #409 --- gazebo_plugins/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index b4f1557a2..37399ea26 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -371,10 +371,10 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) # 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(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) From 58d47ae07cab4c21588781f4a2793875505f6797 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 21:52:48 -0700 Subject: [PATCH 06/10] #408 Created test for depth camera, which fails, so next make it pass --- gazebo_plugins/CMakeLists.txt | 22 +++-- gazebo_plugins/src/gazebo_ros_multicamera.cpp | 1 - gazebo_plugins/test/camera/camera.test | 6 ++ gazebo_plugins/test/camera/camera.world | 9 +- gazebo_plugins/test/camera/depth_camera.cpp | 92 +++++++++++++++++++ gazebo_plugins/test/camera/depth_camera.test | 17 ++++ gazebo_plugins/test/camera/depth_camera.world | 18 +++- 7 files changed, 150 insertions(+), 15 deletions(-) create mode 100644 gazebo_plugins/test/camera/depth_camera.cpp create mode 100644 gazebo_plugins/test/camera/depth_camera.test diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 37399ea26..a769e22a6 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -362,6 +362,8 @@ install(DIRECTORY test ) # Tests +# These need to be run with -j1 flag because gazebo can't be run +# in parallel. if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(set_model_state-test @@ -371,12 +373,16 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) # 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) - # target_link_libraries(camera-test ${catkin_LIBRARIES}) + add_rostest_gtest(depth_camera-test + test/camera/depth_camera.test + test/camera/depth_camera.cpp) + target_link_libraries(depth_camera-test ${catkin_LIBRARIES}) + 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) + target_link_libraries(camera-test ${catkin_LIBRARIES}) endif() diff --git a/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/gazebo_plugins/src/gazebo_ros_multicamera.cpp index 248a252be..e46649c9c 100644 --- a/gazebo_plugins/src/gazebo_ros_multicamera.cpp +++ b/gazebo_plugins/src/gazebo_ros_multicamera.cpp @@ -106,7 +106,6 @@ void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent, void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image, GazeboRosCameraUtils* util) { - GazeboRosCameraUtils* util = this->utils[0]; # if GAZEBO_MAJOR_VERSION >= 7 common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime(); # else diff --git a/gazebo_plugins/test/camera/camera.test b/gazebo_plugins/test/camera/camera.test index 06f58a4e6..3a91b824b 100644 --- a/gazebo_plugins/test/camera/camera.test +++ b/gazebo_plugins/test/camera/camera.test @@ -1,11 +1,17 @@ + + + + + + diff --git a/gazebo_plugins/test/camera/camera.world b/gazebo_plugins/test/camera/camera.world index f48a94c00..3154be354 100644 --- a/gazebo_plugins/test/camera/camera.world +++ b/gazebo_plugins/test/camera/camera.world @@ -21,7 +21,7 @@ false - 0.0 2.0 2.0 0.0 0.0 0.0 + 2.0 0.0 4.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 @@ -85,7 +85,14 @@ 100.0 + + + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 0.5 diff --git a/gazebo_plugins/test/camera/depth_camera.cpp b/gazebo_plugins/test/camera/depth_camera.cpp new file mode 100644 index 000000000..b7d4eae76 --- /dev/null +++ b/gazebo_plugins/test/camera/depth_camera.cpp @@ -0,0 +1,92 @@ +#include +#include +#include + +class DepthCameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + has_new_image_ = false; + has_new_depth_ = false; + } + + ros::NodeHandle nh_; + image_transport::Subscriber cam_sub_; + image_transport::Subscriber depth_sub_; + bool has_new_image_; + ros::Time image_stamp_; + bool has_new_depth_; + ros::Time depth_stamp_; +public: + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + image_stamp_ = msg->header.stamp; + has_new_image_ = true; + } + void depthCallback(const sensor_msgs::ImageConstPtr& msg) + { + depth_stamp_ = msg->header.stamp; + has_new_depth_ = true; + } +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(DepthCameraTest, cameraSubscribeTest) +{ + image_transport::ImageTransport it(nh_); + cam_sub_ = it.subscribe("camera1/image_raw", 1, + &DepthCameraTest::imageCallback, + dynamic_cast(this)); + depth_sub_ = it.subscribe("camera1/depth/image_raw", 1, + &DepthCameraTest::depthCallback, + dynamic_cast(this)); + +#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_ || !has_new_depth_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec()); + // 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; + time_diff = (ros::Time::now() - image_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 0.5); + + time_diff = (ros::Time::now() - depth_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 0.5); + + cam_sub_.shutdown(); + depth_sub_.shutdown(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_depth_camera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_plugins/test/camera/depth_camera.test b/gazebo_plugins/test/camera/depth_camera.test new file mode 100644 index 000000000..3f4dd72bc --- /dev/null +++ b/gazebo_plugins/test/camera/depth_camera.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/camera/depth_camera.world b/gazebo_plugins/test/camera/depth_camera.world index 25ade9c52..978a0a4b2 100644 --- a/gazebo_plugins/test/camera/depth_camera.world +++ b/gazebo_plugins/test/camera/depth_camera.world @@ -21,7 +21,7 @@ false - 0.0 2.0 2.0 0.0 0.0 0.0 + 2.0 0.0 4.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 @@ -85,8 +85,15 @@ 100.0 + + - + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + 0.5 0.0 + camera1 image_raw + camera_info depth/image_raw + depth/camera_info points - camera_info - depth_cam camera_link 0.07 @@ -127,7 +136,6 @@ 0.0 0.0 - true From b1dec2f27d6dc009d0f1b3ab81e167afc603e8d6 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 22:12:51 -0700 Subject: [PATCH 07/10] #408 also test points publication --- gazebo_plugins/test/camera/depth_camera.cpp | 22 +++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/test/camera/depth_camera.cpp b/gazebo_plugins/test/camera/depth_camera.cpp index b7d4eae76..b6809e87c 100644 --- a/gazebo_plugins/test/camera/depth_camera.cpp +++ b/gazebo_plugins/test/camera/depth_camera.cpp @@ -1,6 +1,7 @@ #include #include #include +#include class DepthCameraTest : public testing::Test { @@ -9,15 +10,19 @@ class DepthCameraTest : public testing::Test { has_new_image_ = false; has_new_depth_ = false; + has_new_points_ = false; } ros::NodeHandle nh_; image_transport::Subscriber cam_sub_; image_transport::Subscriber depth_sub_; + ros::Subscriber points_sub_; bool has_new_image_; ros::Time image_stamp_; bool has_new_depth_; ros::Time depth_stamp_; + bool has_new_points_; + ros::Time points_stamp_; public: void imageCallback(const sensor_msgs::ImageConstPtr& msg) { @@ -29,6 +34,11 @@ class DepthCameraTest : public testing::Test depth_stamp_ = msg->header.stamp; has_new_depth_ = true; } + void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& msg) + { + points_stamp_ = msg->header.stamp; + has_new_points_ = true; + } }; // Test if the camera image is published at all, and that the timestamp @@ -42,7 +52,9 @@ TEST_F(DepthCameraTest, cameraSubscribeTest) depth_sub_ = it.subscribe("camera1/depth/image_raw", 1, &DepthCameraTest::depthCallback, dynamic_cast(this)); - + points_sub_ = nh_.subscribe("camera1/points", 1, + &DepthCameraTest::pointsCallback, + dynamic_cast(this)); #if 0 // wait for gazebo to start publishing // TODO(lucasw) this isn't really necessary since this test @@ -60,13 +72,14 @@ TEST_F(DepthCameraTest, cameraSubscribeTest) } #endif - while (!has_new_image_ || !has_new_depth_) + while (!has_new_image_ || !has_new_depth_ || !has_new_points_) { ros::spinOnce(); ros::Duration(0.1).sleep(); } EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec()); + EXPECT_EQ(depth_stamp_.toSec(), points_stamp_.toSec()); // This check depends on the update period being much longer // than the expected difference between now and the received image time // TODO(lucasw) @@ -80,8 +93,13 @@ TEST_F(DepthCameraTest, cameraSubscribeTest) ROS_INFO_STREAM(time_diff); EXPECT_LT(time_diff, 0.5); + time_diff = (ros::Time::now() - points_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 0.5); + cam_sub_.shutdown(); depth_sub_.shutdown(); + points_sub_.shutdown(); } int main(int argc, char** argv) From 2be9a20b0eb66ec9ebd3add472387e6306367422 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 22:19:35 -0700 Subject: [PATCH 08/10] #408 fix timestamps for depth image, test passes now --- .../src/gazebo_ros_depth_camera.cpp | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp index b1438c38a..a9f027eed 100644 --- a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp @@ -192,9 +192,9 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image, return; # if GAZEBO_MAJOR_VERSION >= 7 - this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime(); + this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); # else - this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime(); + this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); # endif if (this->parentSensor->IsActive()) { @@ -232,9 +232,9 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, return; # if GAZEBO_MAJOR_VERSION >= 7 - this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime(); + this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); # else - this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime(); + this->depth_sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); # endif if (!this->parentSensor->IsActive()) { @@ -302,9 +302,9 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image, //ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); # if GAZEBO_MAJOR_VERSION >= 7 - this->sensor_update_time_ = this->parentSensor->LastUpdateTime(); + this->sensor_update_time_ = this->parentSensor->LastMeasurementTime(); # else - this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime(); + this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); # endif if (!this->parentSensor->IsActive()) @@ -316,7 +316,11 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image, else { if ((*this->image_connect_count_) > 0) + { this->PutCameraData(_image); + // TODO(lucasw) publish camera info with depth image + // this->PublishCameraInfo(sensor_update_time); + } } } @@ -502,15 +506,15 @@ void GazeboRosDepthCamera::PublishCameraInfo() if (this->depth_info_connect_count_ > 0) { # if GAZEBO_MAJOR_VERSION >= 7 - this->sensor_update_time_ = this->parentSensor_->LastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); # else - this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); # endif - common::Time cur_time = this->world_->GetSimTime(); - if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) + this->sensor_update_time_ = sensor_update_time; + if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) { - this->PublishCameraInfo(this->depth_image_camera_info_pub_); - this->last_depth_image_camera_info_update_time_ = cur_time; + this->PublishCameraInfo(this->depth_image_camera_info_pub_); // , sensor_update_time); + this->last_depth_image_camera_info_update_time_ = sensor_update_time; } } } From 9b56df9899e8283b8f6ec6d129ebb0777782885e Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 28 Apr 2016 05:40:48 -0700 Subject: [PATCH 09/10] #408 Increasing max time because some systems are taking 0.6 seconds to receive the messages (still well less than 2.0 seconds). Also all the tests can be run with run_tests_gazebo_plugins_rostest but only with the -j1 flag #409 --- gazebo_plugins/CMakeLists.txt | 1 - gazebo_plugins/test/camera/camera.cpp | 2 +- gazebo_plugins/test/camera/depth_camera.cpp | 19 +++++++++++-------- gazebo_plugins/test/camera/multicamera.cpp | 2 +- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index a769e22a6..5519e33ec 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -372,7 +372,6 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/range/range_plugin.test) target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) - # Can't run these and the above test together add_rostest_gtest(depth_camera-test test/camera/depth_camera.test test/camera/depth_camera.cpp) diff --git a/gazebo_plugins/test/camera/camera.cpp b/gazebo_plugins/test/camera/camera.cpp index 25a2932d3..b7805bbae 100644 --- a/gazebo_plugins/test/camera/camera.cpp +++ b/gazebo_plugins/test/camera/camera.cpp @@ -60,7 +60,7 @@ TEST_F(CameraTest, cameraSubscribeTest) // 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); + EXPECT_LT(time_diff, 1.0); cam_sub_.shutdown(); } diff --git a/gazebo_plugins/test/camera/depth_camera.cpp b/gazebo_plugins/test/camera/depth_camera.cpp index b6809e87c..596e6d50f 100644 --- a/gazebo_plugins/test/camera/depth_camera.cpp +++ b/gazebo_plugins/test/camera/depth_camera.cpp @@ -80,22 +80,25 @@ TEST_F(DepthCameraTest, cameraSubscribeTest) EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec()); EXPECT_EQ(depth_stamp_.toSec(), points_stamp_.toSec()); - // This check depends on the update period being much longer - // than the expected difference between now and the received image time + // This check depends on the update period (currently 1.0/update_rate = 2.0 seconds) + // being much longer than the expected difference between now and the + // received image time. + const double max_time = 1.0; + const ros::Time current_time = ros::Time::now(); // TODO(lucasw) // this likely isn't that robust - what if the testing system is really slow? double time_diff; - time_diff = (ros::Time::now() - image_stamp_).toSec(); + time_diff = (current_time - image_stamp_).toSec(); ROS_INFO_STREAM(time_diff); - EXPECT_LT(time_diff, 0.5); + EXPECT_LT(time_diff, max_time); - time_diff = (ros::Time::now() - depth_stamp_).toSec(); + time_diff = (current_time - depth_stamp_).toSec(); ROS_INFO_STREAM(time_diff); - EXPECT_LT(time_diff, 0.5); + EXPECT_LT(time_diff, max_time); - time_diff = (ros::Time::now() - points_stamp_).toSec(); + time_diff = (current_time - points_stamp_).toSec(); ROS_INFO_STREAM(time_diff); - EXPECT_LT(time_diff, 0.5); + EXPECT_LT(time_diff, max_time); cam_sub_.shutdown(); depth_sub_.shutdown(); diff --git a/gazebo_plugins/test/camera/multicamera.cpp b/gazebo_plugins/test/camera/multicamera.cpp index cd66a0778..0823c53da 100644 --- a/gazebo_plugins/test/camera/multicamera.cpp +++ b/gazebo_plugins/test/camera/multicamera.cpp @@ -89,7 +89,7 @@ TEST_F(MultiCameraTest, cameraSubscribeTest) // 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); + EXPECT_LT(time_diff, 1.0); // cam_sub_.shutdown(); } From 3dc0df1879d8f323ffb626af32f652c13f78b8b7 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 18 Oct 2016 10:16:51 -0700 Subject: [PATCH 10/10] Adding @j-rivero default disabling of tests that require a display --- gazebo_plugins/CMakeLists.txt | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 5519e33ec..0998ecffe 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(gazebo_plugins) +option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF) + find_package(catkin REQUIRED COMPONENTS message_generation gazebo_msgs @@ -372,16 +374,18 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/range/range_plugin.test) target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) - add_rostest_gtest(depth_camera-test - test/camera/depth_camera.test - test/camera/depth_camera.cpp) - target_link_libraries(depth_camera-test ${catkin_LIBRARIES}) - 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) - target_link_libraries(camera-test ${catkin_LIBRARIES}) + if (ENABLE_DISPLAY_TESTS) + add_rostest_gtest(depth_camera-test + test/camera/depth_camera.test + test/camera/depth_camera.cpp) + target_link_libraries(depth_camera-test ${catkin_LIBRARIES}) + 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) + target_link_libraries(camera-test ${catkin_LIBRARIES}) + endif() endif()