[gazebo_plugins] Fix for issue #408 - correct the timestamp used by the camera

[lucasw]

Fix for #408

The last measurement time is the time that gazebo generated the sensor data, so ought to be used.

The other cameras need similar fixes to have the proper timestamps.

There is a test to show that the timestamp is good, but issue #409 has lead me to comment it out for now.

There probably ought to be a separate issue about what updateRate is supposed to be for, and if it isn't useful remove it (from all the sensors that have it) in another PR.

[scpeters]

There's a gzserver failure in the travis test:

gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:653: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::rendering::DepthCamera; typename boost::detail::sp_member_access<T>::type = gazebo::rendering::DepthCamera*]: Assertion `px != 0' failed.

Aborted (core dumped)

[lucasw]

Can the travis ci build be triggered for 596b302? That had no code changes to the depth camera, but ran it the depth camera in a test which might indicate some other issue that is independent of my changes. The test should still fail, but because of timestamp issues, not the px assertions.

The test that fails wouldn't actually be enabled when merged (the test set_model_state-test would be re-enabled until #409 is resolved).

[scpeters]

I don't have permissions to trigger builds for a specific commit. I'll try making a separate branch...

[scpeters]

I pushed 596b302 to my fork ( scpeters@596b302 ) and am awaiting the travis results:

https://travis-ci.org/scpeters/gazebo_ros_pkgs/builds/125919069

[scpeters]

It looks like 596b302 also fails. It could be because these buildfarm machines don't have GPU's. @j-rivero mentioned that he would figure out how to disable tests that require a GPU if one is not available.

[lucasw]

Yeah it is the same px != 0 failure (as opposed to my test failing because timestamps are wrong). Does it actually need a gpu, or would a software virtual framebuffer work?

I think this test out to be commented out and the set_model_state-test test restored. But can someone other than me try out the changes manually (both running the tests and loading the depth, multi, and regular camera world files in a regular session and viewing published topics).

[j-rivero]

Yeah it is the same px != 0 failure (as opposed to my test failing because timestamps are wrong).

I was able to reproduce the error when there is no valid DISPLAY available

Does it actually need a gpu, or would a software virtual framebuffer work?

I think that a virtual framebuffer would be enough, @iche033 what do you think?

I think this test out to be commented out and the set_model_state-test test restored.

I'm interested into enabling all the possible tests. To run the test one by one we can use the -j1 argument. Maybe we can this option:

-- 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" ON)
+
 find_package(catkin REQUIRED COMPONENTS 
   message_generation 
   gazebo_msgs 
@@ -339,22 +341,23 @@ install(DIRECTORY test
 # Tests
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
-  # add_rostest_gtest(set_model_state-test
-  #                   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})
-
-  # 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)
-  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})
+  add_rostest_gtest(set_model_state-test
+                    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})
+
+  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()

But can someone other than me try out the changes manually (both running the tests and loading the depth, multi, and regular camera world files in a regular session and viewing published topics).

Test are failing on my local system:

[ROSUNIT] Outputting test results to /tmp/tmp.tVYKeyj2OF/build/gazebo_plugins/test_results/gazebo_plugins/rostest-test_camera_depth_camera.xml
[ INFO] [1461707320.297311178]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1461707320.297679763]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
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]
[ INFO] [1461707321.006708696]: Camera Plugin (robotNamespace = ), Info: the 'robotNamespace' param did not exit
[ INFO] [1461707321.032785687, 0.023000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1461707321.033451535, 0.024000000]: Camera Plugin (ns = )  <tf_prefix_>, set to ""
[ INFO] [1461707321.086888269, 0.074000000]: Physics dynamic reconfigure ready.
testdepth_camera ... ok

[ROSTEST]-----------------------------------------------------------------------

[gazebo_plugins.rosunit-depth_camera/cameraSubscribeTest][FAILURE]--------------
/tmp/tmp.tVYKeyj2OF/src/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.cpp:90
Expected: (time_diff) < (0.5), actual: 0.523 vs 0.5
--------------------------------------------------------------------------------


[gazebo_plugins.rosunit-depth_camera/cameraSubscribeTest][FAILURE]--------------
/tmp/tmp.tVYKeyj2OF/src/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.cpp:94
Expected: (time_diff) < (0.5), actual: 0.523 vs 0.5
--------------------------------------------------------------------------------


[gazebo_plugins.rosunit-depth_camera/cameraSubscribeTest][FAILURE]--------------
/tmp/tmp.tVYKeyj2OF/src/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.cpp:98
Expected: (time_diff) < (0.5), actual: 0.523 vs 0.5
--------------------------------------------------------------------------------

[lucasw]

Tests are failing on my local system:

@j-rivero Are you on the latest c033556 ?

[j-rivero]

@j-rivero Are you on the latest c033556 ?

unless I have something very broken, I see that commit in the git log on my branch.

[lucasw]

Expected: (time_diff) < (0.5), actual: 0.523 vs 0.5

So if the new code wasn't working at all the time difference would be more than 2 seconds (the update period of the sensor in the test world file)- and looking now my computer is taking about 0.3 seconds. I just chose 0.5 thinking it was generous at the time, but it could be increased to say 1.0. If any more is required I'd like to decrease the sensor update rate to get a more generous margin on both sides.

[scpeters]

Sounds fine to me to relax the tolerances

[j-rivero]

Thanks for the fixes. Now I only have one error if I run the test locally on my Intel laptop:

[ INFO] [1464139734.627722818]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
Error [parser.cc:292] Error parsing XML in file [/home/jrivero/code/ros/ws/pr_410/src/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.world]: Error reading end tag.
gzserver: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed.
Aborted (core dumped)
testdepth_camera ... ERROR!
ERROR: max time [30.0s] allotted for test [depth_camera] of type [gazebo_plugins/depth_camera-test]

To able to keep our CI headless working, can we please try to merge my previous proposed patch (I've changed the default not to run DISPLAY tests):

-- 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 
@@ -339,22 +341,23 @@ install(DIRECTORY test
 # Tests
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
-  # add_rostest_gtest(set_model_state-test
-  #                   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})
-
-  # 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)
-  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})
+  add_rostest_gtest(set_model_state-test
+                    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})
+
+  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()

`

[lucasw]

I've made the tests disabled by default unless ENABLE_DISPLAY_TESTS is on, and rebased off the latest jade-devel.

[codebot]

I haven't tried the tests myself, but we just independently ran into the same bug and chased down the same underlying fix (using LastMeasurementTime instead of LastUpdateTime). It indeed fixes the timestamp problems on the ROS side.

[iche033]

see this related issue in the gazebo repo. I made similar changes to the openni_kinect plugin and the timestamp issue is fixed for me.

[iche033]

I ran into an xml parser error that turned out to be due to this line. There is a mismatch between open and end tags

[iche033]

had to make a minor fix for the depth camera test to run. After that, all tests passed for me

diff --git a/gazebo_plugins/test/camera/depth_camera.world b/gazebo_plugins/test/camera/depth_camera.world
index 978a0a4..73132f9 100644
--- a/gazebo_plugins/test/camera/depth_camera.world
+++ b/gazebo_plugins/test/camera/depth_camera.world
@@ -124,7 +124,7 @@
           <depthImageTopicName>depth/image_raw</depthImageTopicName>
           <!-- neither camera info is getting published, frame_id is empty 
             in points and both image headers -->
-          <depthImageTopicCameraInfoName>depth/camera_info</depthImageCameraInfoTopicName>
+          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
           <pointCloudTopicName>points</pointCloudTopicName>
           <frameName>camera_link</frameName>
           <!-- TODO(lucasw) is this used by depth camera at all? -->

[j-rivero]

had to make a minor fix for the depth camera test to run. After that, all tests passed for me

Thanks Ian for the testing. @lucasw if you please could apply the patch that Ian mentioned, it would be great so I can propagate the change to the different branches with the fix included.

[j-rivero]

Thanks Ian for the testing. @lucasw if you please could apply the patch that Ian mentioned, it would be great so I can propagate the change to the different branches with the fix included.

Don't worry, let's merge this PR and continue with the pending changes in pr #538