diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 1380b27665..f514370b88 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -543,12 +543,11 @@ TEST (RealsenseTests, testTransforms) { // make sure all transforms are being broadcast as expected tf::TransformListener tf_listener; - ros::Duration(1).sleep(); // must listen for ~1 sec or tf won't be found - EXPECT_TRUE(tf_listener.canTransform (BASE_DEF_FRAME, DEPTH_DEF_FRAME, ros::Time::now())); - EXPECT_TRUE(tf_listener.canTransform (DEPTH_DEF_FRAME, DEPTH_OPTICAL_DEF_FRAME, ros::Time::now())); - EXPECT_TRUE(tf_listener.canTransform (BASE_DEF_FRAME, COLOR_DEF_FRAME, ros::Time::now())); - EXPECT_TRUE(tf_listener.canTransform (COLOR_DEF_FRAME,COLOR_OPTICAL_DEF_FRAME, ros::Time::now())); + EXPECT_TRUE(tf_listener.waitForTransform (DEPTH_DEF_FRAME, BASE_DEF_FRAME, ros::Time(0), ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform (DEPTH_OPTICAL_DEF_FRAME, DEPTH_DEF_FRAME, ros::Time(0), ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform (COLOR_DEF_FRAME, BASE_DEF_FRAME, ros::Time(0), ros::Duration(3.0))); + EXPECT_TRUE(tf_listener.waitForTransform (COLOR_OPTICAL_DEF_FRAME, COLOR_DEF_FRAME, ros::Time(0), ros::Duration(3.0))); } TEST (RealsenseTests, testCameraOptions)