From d92a409cadf3384810c72ffb57df7e4d9f2235a5 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 17 Mar 2016 18:53:18 -0700 Subject: [PATCH 01/11] 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 d2c31c26d..3b3b0c254 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -364,4 +364,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 42ad4004b7e0e32df4c936754df5f78f8800b693 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 23 Mar 2016 05:41:11 -0700 Subject: [PATCH 02/11] #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 + + + + + + + From 5fa7cc85724a851c639d33b9172ecf4dfc2029af Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 23 Mar 2016 05:52:53 -0700 Subject: [PATCH 03/11] #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 d8cb64774e23cddf811bf666507b9c5c51d8a3a0 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 06:45:24 -0700 Subject: [PATCH 04/11] 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 61cb624198d14731676d0f900ba4854086c093e5 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 06:47:29 -0700 Subject: [PATCH 05/11] 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 151e874b4..646e67051 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -366,10 +366,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 2bf30a1c42208ec539bde6c1b993695a4dc5b464 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 21:52:48 -0700 Subject: [PATCH 06/11] #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 646e67051..b33da3a26 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -357,6 +357,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 @@ -366,12 +368,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 b3265d86ba703a358037f026f99da5d904ec3f63 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 22:12:51 -0700 Subject: [PATCH 07/11] #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 113bdfd53c2e72dcff117fef4b5fecc7a56bb5c6 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 29 Mar 2016 22:19:35 -0700 Subject: [PATCH 08/11] #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 e9163373247a3cdecc2c3f8eef02471af5aa6d02 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 28 Apr 2016 05:40:48 -0700 Subject: [PATCH 09/11] #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 b33da3a26..da7e09515 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -367,7 +367,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 0d23e1f5d728375007a2f44ab08e8ba966fe1d73 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 18 Oct 2016 10:16:51 -0700 Subject: [PATCH 10/11] 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 da7e09515..b3fefc62c 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 @@ -367,16 +369,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() From 87c4dbfde1758a67c9b6f3e7bbeae824965ab262 Mon Sep 17 00:00:00 2001 From: iche033 Date: Fri, 10 Feb 2017 11:07:39 -0800 Subject: [PATCH 11/11] change LastUpdateTime to LastMeasurementTime --- .../src/gazebo_ros_camera_utils.cpp | 9 ++++---- .../src/gazebo_ros_openni_kinect.cpp | 17 +++++++-------- gazebo_plugins/src/gazebo_ros_prosilica.cpp | 21 +++++++++---------- gazebo_plugins/test/camera/depth_camera.world | 2 +- 4 files changed, 23 insertions(+), 26 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 4776cae45..7d34042bd 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -626,15 +626,14 @@ void GazeboRosCameraUtils::PublishCameraInfo() if (this->camera_info_pub_.getNumSubscribers() > 0) { # 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 - common::Time cur_time = this->world_->GetSimTime(); - if (cur_time - this->last_info_update_time_ >= this->update_period_) + if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_) { this->PublishCameraInfo(this->camera_info_pub_); - this->last_info_update_time_ = cur_time; + this->last_info_update_time_ = this->sensor_update_time_; } } } diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index c852f6f94..869c3fa64 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -189,9 +189,9 @@ void GazeboRosOpenniKinect::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()) { @@ -231,9 +231,9 @@ void GazeboRosOpenniKinect::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()) @@ -447,15 +447,14 @@ void GazeboRosOpenniKinect::PublishCameraInfo() if (this->depth_info_connect_count_ > 0) { # 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 - common::Time cur_time = this->world_->GetSimTime(); - if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) + if (this->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->last_depth_image_camera_info_update_time_ = this->sensor_update_time_; } } } diff --git a/gazebo_plugins/src/gazebo_ros_prosilica.cpp b/gazebo_plugins/src/gazebo_ros_prosilica.cpp index dd0203290..2e529ca9b 100644 --- a/gazebo_plugins/src/gazebo_ros_prosilica.cpp +++ b/gazebo_plugins/src/gazebo_ros_prosilica.cpp @@ -132,9 +132,9 @@ void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image, // should do nothing except turning camera on/off, as we are using service. /// @todo: consider adding thumbnailing feature here if subscribed. # if GAZEBO_MAJOR_VERSION >= 7 - common::Time sensor_update_time = this->parentSensor_->LastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); # else - common::Time sensor_update_time = this->parentSensor_->GetLastUpdateTime(); + common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime(); # endif // as long as ros is connected, parent is active @@ -153,12 +153,11 @@ void GazeboRosProsilica::OnNewImageFrame(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_) + if (sensor_update_time - this->last_update_time_ >= this->update_period_) { this->PutCameraData(_image, sensor_update_time); this->PublishCameraInfo(sensor_update_time); - this->last_update_time_ = cur_time; + this->last_update_time_ = sensor_update_time; } } } @@ -227,9 +226,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re this->roiCameraInfoMsg->header.frame_id = this->frame_name_; # if GAZEBO_MAJOR_VERSION >= 7 - common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime(); # else - common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->GetLastMeasurementTime(); # endif this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec; this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec; @@ -285,9 +284,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re this->image_msg_.header.frame_id = this->frame_name_; # if GAZEBO_MAJOR_VERSION >= 7 - common::Time lastRenderTime = this->parentSensor_->LastUpdateTime(); + common::Time lastRenderTime = this->parentSensor_->LastMeasurementTime(); # else - common::Time lastRenderTime = this->parentSensor_->GetLastUpdateTime(); + common::Time lastRenderTime = this->parentSensor_->GetLastMeasurementTime(); # endif this->image_msg_.header.stamp.sec = lastRenderTime.sec; this->image_msg_.header.stamp.nsec = lastRenderTime.nsec; @@ -313,9 +312,9 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re this->roiImageMsg = ℑ this->roiImageMsg->header.frame_id = this->frame_name_; # if GAZEBO_MAJOR_VERSION >= 7 - common::Time roiLastRenderTime = this->parentSensor_->LastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime(); # else - common::Time roiLastRenderTime = this->parentSensor_->GetLastUpdateTime(); + common::Time roiLastRenderTime = this->parentSensor_->GetLastMeasurementTime(); # endif this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec; this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec; diff --git a/gazebo_plugins/test/camera/depth_camera.world b/gazebo_plugins/test/camera/depth_camera.world index 978a0a4b2..73132f908 100644 --- a/gazebo_plugins/test/camera/depth_camera.world +++ b/gazebo_plugins/test/camera/depth_camera.world @@ -124,7 +124,7 @@ depth/image_raw - depth/camera_info + depth/camera_info points camera_link