From d3d143bea58221e4a376aeeb67919e74b2b64413 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 4 Dec 2019 16:42:31 +0100 Subject: [PATCH 01/44] Initial port of test_tf2 - wip --- test_tf2/AMENT_IGNORE | 0 test_tf2/CMakeLists.txt | 110 +- test_tf2/package.xml | 36 +- test_tf2/test/buffer_core_test.cpp | 1603 +++++++++++++++------------- test_tf2/test/permuter.hpp | 169 +++ 5 files changed, 1073 insertions(+), 845 deletions(-) delete mode 100644 test_tf2/AMENT_IGNORE create mode 100644 test_tf2/test/permuter.hpp diff --git a/test_tf2/AMENT_IGNORE b/test_tf2/AMENT_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 510de1a90..d2ab941c4 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -1,62 +1,66 @@ cmake_minimum_required(VERSION 3.5) -if(NOT CATKIN_ENABLE_TESTING) +if(NOT BUILD_TESTING) return() endif() project(test_tf2) - -find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs) -find_package(Boost REQUIRED COMPONENTS thread) -find_package(orocos_kdl REQUIRED) - - -include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) - -link_directories(${orocos_kdl_LIBRARY_DIRS}) - -catkin_package() - -catkin_add_gtest(buffer_core_test test/buffer_core_test.cpp) -target_link_libraries(buffer_core_test ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) - -catkin_add_gtest(test_tf2_message_filter test/test_message_filter.cpp) -target_link_libraries(test_tf2_message_filter ${Boost_LIBRARIES} ${catkin_LIBRARIES}) - -catkin_add_gtest(test_convert test/test_convert.cpp) -target_link_libraries(test_convert ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) - -catkin_add_gtest(test_utils test/test_utils.cpp) -target_link_libraries(test_utils ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) - -add_executable(test_buffer_server EXCLUDE_FROM_ALL test/test_buffer_server.cpp) -target_link_libraries(test_buffer_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - -add_executable(test_buffer_client EXCLUDE_FROM_ALL test/test_buffer_client.cpp) -target_link_libraries(test_buffer_client ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) - - -add_rostest(test/buffer_client_tester.launch) - -add_executable(test_static_publisher EXCLUDE_FROM_ALL test/test_static_publisher.cpp) -target_link_libraries(test_static_publisher ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - -add_rostest(test/static_publisher.launch) - - -add_executable(test_tf2_bullet EXCLUDE_FROM_ALL test/test_tf2_bullet.cpp) -target_link_libraries(test_tf2_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - -add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) - - -if(TARGET tests) - add_dependencies(tests test_buffer_server test_buffer_client test_static_publisher test_tf2_bullet) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) + +ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) +if(TARGET buffer_core_test) + ament_target_dependencies(buffer_core_test + tf2 + rclcpp + ) endif() - -# used as a test fixture -if(TARGET tf2_ros_static_transform_publisher) - add_dependencies(tests tf2_ros_static_transform_publisher test_static_publisher) -endif() \ No newline at end of file +# ament_add_gtest_executable(buffer_core_test test/buffer_core_test.cpp) +# catkin_add_gtest(buffer_core_test test/buffer_core_test.cpp) +# target_link_libraries(buffer_core_test ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) + +# catkin_add_gtest(test_tf2_message_filter test/test_message_filter.cpp) +# target_link_libraries(test_tf2_message_filter ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +# +# catkin_add_gtest(test_convert test/test_convert.cpp) +# target_link_libraries(test_convert ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) +# +# catkin_add_gtest(test_utils test/test_utils.cpp) +# target_link_libraries(test_utils ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) +# +# add_executable(test_buffer_server EXCLUDE_FROM_ALL test/test_buffer_server.cpp) +# target_link_libraries(test_buffer_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) +# +# add_executable(test_buffer_client EXCLUDE_FROM_ALL test/test_buffer_client.cpp) +# target_link_libraries(test_buffer_client ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) +# +# +# add_rostest(test/buffer_client_tester.launch) +# +# add_executable(test_static_publisher EXCLUDE_FROM_ALL test/test_static_publisher.cpp) +# target_link_libraries(test_static_publisher ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) +# +# add_rostest(test/static_publisher.launch) +# +# +# add_executable(test_tf2_bullet EXCLUDE_FROM_ALL test/test_tf2_bullet.cpp) +# target_link_libraries(test_tf2_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) +# +# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) +# +# +# if(TARGET tests) +# add_dependencies(tests test_buffer_server test_buffer_client test_static_publisher test_tf2_bullet) +# endif() +# +# +# # used as a test fixture +# if(TARGET tf2_ros_static_transform_publisher) +# add_dependencies(tests tf2_ros_static_transform_publisher test_static_publisher) +# endif() + +ament_package() diff --git a/test_tf2/package.xml b/test_tf2/package.xml index d7de686d3..33714e9cb 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -1,6 +1,6 @@ - + test_tf2 - 0.9.1 + 0.12.4 tf2 unit tests @@ -10,34 +10,12 @@ BSD http://www.ros.org/wiki/geometry_experimental - - catkin - rosconsole - roscpp - rostest - tf - tf2 - tf2_bullet - tf2_ros - tf2_geometry_msgs - tf2_kdl - tf2_msgs + rclcpp - rosconsole - roscpp - rostest - tf - tf2 - tf2_bullet - tf2_ros - tf2_geometry_msgs - tf2_kdl - tf2_msgs - - rosunit - rosbash + ament_cmake_gtest + + ament_cmake + - - diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index c55e429f4..8cae4c0a6 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright @@ -13,7 +13,7 @@ * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -31,10 +31,13 @@ #include #include "tf2/exceptions.h" #include -#include -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "rostest/permuter.h" +#include "rclcpp/rclcpp.hpp" +#include +#include "permuter.hpp" +// #include "LinearMath/btVector3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/exceptions.h" +// #include "rostest/permuter.h" void seed_rand() { @@ -54,7 +57,7 @@ void generate_rand_vectors(double scale, uint64_t runs, std::vector& xva } -void setIdentity(geometry_msgs::Transform& trans) +void setIdentity(geometry_msgs::msg::Transform& trans) { trans.translation.x = 0; trans.translation.y = 0; @@ -66,17 +69,17 @@ void setIdentity(geometry_msgs::Transform& trans) } -void push_back_i(std::vector& children, std::vector& parents, +void push_back_i(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { - /* + /* "a" v (1,0) "b" v (1,0) "c" */ - + children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); @@ -88,7 +91,7 @@ void push_back_i(std::vector& children, std::vector& p } -void push_back_y(std::vector& children, std::vector& parents, +void push_back_y(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* @@ -120,13 +123,13 @@ void push_back_y(std::vector& children, std::vector& p dy.push_back(1.0); } -void push_back_v(std::vector& children, std::vector& parents, +void push_back_v(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" ------(0,1)-----> "f" v (1,0) v (0,1) - "b" "g" + "b" "g" v (1,0) "c" */ @@ -153,7 +156,7 @@ void push_back_v(std::vector& children, std::vector& p } -void push_back_1(std::vector& children, std::vector& parents, +void push_back_1(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { children.push_back("2"); @@ -162,17 +165,19 @@ void push_back_1(std::vector& children, std::vector& p dy.push_back(0.0); } -void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_interfaces::msg::Time & time, const tf2::Duration& interpolation_space = tf2::durationFromSec()) +void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_interfaces::msg::Time & time, const tf2::Duration& interpolation_space = tf2::durationFromSec(0.0)) { - ROS_DEBUG("Clearing Buffer Core for new test setup"); + // auto node = rclcpp::Node::make_shared("buffer_core_test"); + + // RCLCPP_DEBUG(node->get_logger(), "Clearing Buffer Core for new test setup"); mBC.clear(); - - ROS_DEBUG("Setting up test tree for formation %s", mode.c_str()); + + // RCLCPP_DEBUG(node->get_logger(), "Setting up test tree for formation %s", mode.c_str()); std::vector children; std::vector parents; std::vector dx, dy; - + if (mode == "i") { push_back_i(children, parents, dx, dy); @@ -192,9 +197,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte /* Form a ring of transforms at every 45 degrees on the unit circle. */ std::vector frames; - - frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); @@ -204,7 +207,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte frames.push_back("g"); frames.push_back("h"); frames.push_back("i"); - + for (uint8_t iteration = 0; iteration < 2; ++iteration) { double direction = 1; @@ -218,7 +221,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte frame_prefix =""; for (uint64_t i = 1; i < frames.size(); i++) { - geometry_msgs::TransformStamped ts; + geometry_msgs::msg::TransformStamped ts; setIdentity(ts.transform); ts.transform.translation.x = direction * ( sqrt(2)/2 - 1); ts.transform.translation.y = direction * sqrt(2)/2; @@ -226,11 +229,17 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte ts.transform.rotation.y = 0; ts.transform.rotation.z = sin(direction * M_PI/8); ts.transform.rotation.w = cos(direction * M_PI/8); - if (time > builtin_interfaces::msg::Time() + (interpolation_space * .5)) - ts.header.stamp = time - (interpolation_space * .5); - else + + double time_seconds = time.sec + time.nanosec / 1e9; + double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; + + if (time_seconds > time_interpolation_space ){ + double time_stamp = time_seconds - time_interpolation_space; + ts.header.stamp.sec = (int)time_stamp; + ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; + }else ts.header.stamp = builtin_interfaces::msg::Time(); - + ts.header.frame_id = frame_prefix + frames[i-1]; if (i > 1) ts.child_frame_id = frame_prefix + frames[i]; @@ -239,19 +248,20 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > tf2::Duration()) { - ts.header.stamp = time + interpolation_space * .5; + double time_stamp = time_seconds + time_interpolation_space; + ts.header.stamp.sec = (int)time_stamp; + ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; EXPECT_TRUE(mBC.setTransform(ts, "authority")); - } } } return; // nonstandard setup return before standard executinog - } + } else if (mode == "1") { push_back_1(children, parents, dx, dy); - - } + + } else if (mode =="1_v") { push_back_1(children, parents, dx, dy); @@ -261,26 +271,31 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte EXPECT_FALSE("Undefined mode for tree setup. Test harness improperly setup."); - /// Standard + /// Standard for (uint64_t i = 0; i < children.size(); i++) { - geometry_msgs::TransformStamped ts; + geometry_msgs::msg::TransformStamped ts; setIdentity(ts.transform); ts.transform.translation.x = dx[i]; ts.transform.translation.y = dy[i]; - if (time > builtin_interfaces::msg::Time() + (interpolation_space * .5)) - ts.header.stamp = time - (interpolation_space * .5); - else + double time_seconds = time.sec + time.nanosec / 1e9; + double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; + if (time_seconds > time_interpolation_space ){ + double time_stamp = time_seconds - time_interpolation_space; + ts.header.stamp.sec = (int)time_stamp; + ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; + }else ts.header.stamp = builtin_interfaces::msg::Time(); - + ts.header.frame_id = parents[i]; ts.child_frame_id = children[i]; EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > tf2::Duration()) { - ts.header.stamp = time + interpolation_space * .5; + double time_stamp = time_seconds + time_interpolation_space; + ts.header.stamp.sec = (int)time_stamp; + ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; EXPECT_TRUE(mBC.setTransform(ts, "authority")); - } } } @@ -289,9 +304,9 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte TEST(BufferCore_setTransform, NoInsertOnSelfTransform) { tf2::BufferCore mBC; - geometry_msgs::TransformStamped tranStamped; + geometry_msgs::msg::TransformStamped tranStamped; setIdentity(tranStamped.transform); - tranStamped.header.stamp = builtin_interfaces::msg::Time().fromNSec(10.0); + tranStamped.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(10.0)); tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = "same_frame"; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); @@ -300,24 +315,23 @@ TEST(BufferCore_setTransform, NoInsertOnSelfTransform) TEST(BufferCore_setTransform, NoInsertWithNan) { tf2::BufferCore mBC; - geometry_msgs::TransformStamped tranStamped; + geometry_msgs::msg::TransformStamped tranStamped; setIdentity(tranStamped.transform); - tranStamped.header.stamp = builtin_interfaces::msg::Time().fromNSec(10.0); + tranStamped.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(10.0)); tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = "other_frame"; EXPECT_TRUE(mBC.setTransform(tranStamped, "authority")); tranStamped.transform.translation.x = 0.0/0.0; EXPECT_TRUE(std::isnan(tranStamped.transform.translation.x)); EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); - } TEST(BufferCore_setTransform, NoInsertWithNoFrameID) { tf2::BufferCore mBC; - geometry_msgs::TransformStamped tranStamped; + geometry_msgs::msg::TransformStamped tranStamped; setIdentity(tranStamped.transform); - tranStamped.header.stamp = builtin_interfaces::msg::Time().fromNSec(10.0); + tranStamped.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(10.0)); tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = ""; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); @@ -329,9 +343,9 @@ TEST(BufferCore_setTransform, NoInsertWithNoFrameID) TEST(BufferCore_setTransform, NoInsertWithNoParentID) { tf2::BufferCore mBC; - geometry_msgs::TransformStamped tranStamped; + geometry_msgs::msg::TransformStamped tranStamped; setIdentity(tranStamped.transform); - tranStamped.header.stamp = builtin_interfaces::msg::Time().fromNSec(10.0); + tranStamped.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(10.0)); tranStamped.header.frame_id = ""; tranStamped.child_frame_id = "some_frame"; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); @@ -340,295 +354,296 @@ TEST(BufferCore_setTransform, NoInsertWithNoParentID) EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); } -/* -TEST(tf, ListOneInverse) -{ - unsigned int runs = 4; - double epsilon = 1e-6; - seed_rand(); - - tf::Transformer mTR(true); - std::vector xvalues(runs), yvalues(runs), zvalues(runs); - for ( uint64_t i = 0; i < runs ; i++ ) - { - xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - - StampedTransform tranStamped (btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); - mTR.setTransform(tranStamped); - } - - // std::cout << mTR.allFramesAsString() << std::endl; - // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; - - for ( uint64_t i = 0; i < runs ; i++ ) - - { - Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "child"); - - try{ - Stamped outpose; - outpose.setIdentity(); //to make sure things are getting mutated - mTR.transformPose("my_parent",inpose, outpose); - EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); - } - catch (tf::TransformException & ex) - { - std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; - bool exception_improperly_thrown = true; - EXPECT_FALSE(exception_improperly_thrown); - } - } - -} - -TEST(tf, ListTwoInverse) -{ - unsigned int runs = 4; - double epsilon = 1e-6; - seed_rand(); - - tf::Transformer mTR(true); - std::vector xvalues(runs), yvalues(runs), zvalues(runs); - for ( unsigned int i = 0; i < runs ; i++ ) - { - xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - - StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); - mTR.setTransform(tranStamped); - StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "child", "grandchild"); - mTR.setTransform(tranStamped2); - } - - // std::cout << mTR.allFramesAsString() << std::endl; - // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; - - for ( unsigned int i = 0; i < runs ; i++ ) - - { - Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "grandchild"); - - try{ - Stamped outpose; - outpose.setIdentity(); //to make sure things are getting mutated - mTR.transformPose("my_parent",inpose, outpose); - EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon); - } - catch (tf::TransformException & ex) - { - std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; - bool exception_improperly_thrown = true; - EXPECT_FALSE(exception_improperly_thrown); - } - } - -} - - -TEST(tf, ListOneForward) -{ - unsigned int runs = 4; - double epsilon = 1e-6; - seed_rand(); - - tf::Transformer mTR(true); - std::vector xvalues(runs), yvalues(runs), zvalues(runs); - for ( uint64_t i = 0; i < runs ; i++ ) - { - xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - - StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); - mTR.setTransform(tranStamped); - } - - // std::cout << mTR.allFramesAsString() << std::endl; - // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; - - for ( uint64_t i = 0; i < runs ; i++ ) - - { - Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent"); - - try{ - Stamped outpose; - outpose.setIdentity(); //to make sure things are getting mutated - mTR.transformPose("child",inpose, outpose); - EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon); - } - catch (tf::TransformException & ex) - { - std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; - bool exception_improperly_thrown = true; - EXPECT_FALSE(exception_improperly_thrown); - } - } - -} - -TEST(tf, ListTwoForward) -{ - unsigned int runs = 4; - double epsilon = 1e-6; - seed_rand(); - - tf::Transformer mTR(true); - std::vector xvalues(runs), yvalues(runs), zvalues(runs); - for ( unsigned int i = 0; i < runs ; i++ ) - { - xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - - StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); - mTR.setTransform(tranStamped); - StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "child", "grandchild"); - mTR.setTransform(tranStamped2); - } - - // std::cout << mTR.allFramesAsString() << std::endl; - // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; - - for ( unsigned int i = 0; i < runs ; i++ ) - - { - Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent"); - - try{ - Stamped outpose; - outpose.setIdentity(); //to make sure things are getting mutated - mTR.transformPose("grandchild",inpose, outpose); - EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon); - } - catch (tf::TransformException & ex) - { - std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; - bool exception_improperly_thrown = true; - EXPECT_FALSE(exception_improperly_thrown); - } - } - -} - -TEST(tf, TransformThrougRoot) -{ - unsigned int runs = 4; - double epsilon = 1e-6; - seed_rand(); - - tf::Transformer mTR(true); - std::vector xvalues(runs), yvalues(runs), zvalues(runs); - for ( unsigned int i = 0; i < runs ; i++ ) - { - xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - - StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(1000 + i*100), "my_parent", "childA"); - mTR.setTransform(tranStamped); - StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(1000 + i*100), "my_parent", "childB"); - mTR.setTransform(tranStamped2); - } - - // std::cout << mTR.allFramesAsString() << std::endl; - // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; - - for ( unsigned int i = 0; i < runs ; i++ ) - - { - Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000 + i*100), "childA"); - - try{ - Stamped outpose; - outpose.setIdentity(); //to make sure things are getting mutated - mTR.transformPose("childB",inpose, outpose); - EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); - } - catch (tf::TransformException & ex) - { - std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; - bool exception_improperly_thrown = true; - EXPECT_FALSE(exception_improperly_thrown); - } - } - -} - -TEST(tf, TransformThroughNO_PARENT) -{ - unsigned int runs = 4; - double epsilon = 1e-6; - seed_rand(); - - tf::Transformer mTR(true); - std::vector xvalues(runs), yvalues(runs), zvalues(runs); - for ( unsigned int i = 0; i < runs ; i++ ) - { - xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; - - StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parentA", "childA"); - mTR.setTransform(tranStamped); - StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parentB", "childB"); - mTR.setTransform(tranStamped2); - } - - // std::cout << mTR.allFramesAsString() << std::endl; - // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; - - for ( unsigned int i = 0; i < runs ; i++ ) - - { - Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "childA"); - bool exception_thrown = false; - - try{ - Stamped outpose; - outpose.setIdentity(); //to make sure things are getting mutated - mTR.transformPose("childB",inpose, outpose); - EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); - EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); - } - catch (tf::TransformException & ex) - { - exception_thrown = true; - } - EXPECT_TRUE(exception_thrown); - } - -} - -*/ - +// /* +// TEST(tf, ListOneInverse) +// { +// unsigned int runs = 4; +// double epsilon = 1e-6; +// seed_rand(); +// +// tf::Transformer mTR(true); +// std::vector xvalues(runs), yvalues(runs), zvalues(runs); +// for ( uint64_t i = 0; i < runs ; i++ ) +// { +// xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// +// StampedTransform tranStamped (btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); +// mTR.setTransform(tranStamped); +// } +// +// // std::cout << mTR.allFramesAsString() << std::endl; +// // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; +// +// for ( uint64_t i = 0; i < runs ; i++ ) +// +// { +// Stamped inpose (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "child"); +// +// try{ +// Stamped outpose; +// outpose.setIdentity(); //to make sure things are getting mutated +// mTR.transformPose("my_parent",inpose, outpose); +// EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); +// } +// catch (tf::TransformException & ex) +// { +// std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; +// bool exception_improperly_thrown = true; +// EXPECT_FALSE(exception_improperly_thrown); +// } +// } +// +// } +// +// TEST(tf, ListTwoInverse) +// { +// unsigned int runs = 4; +// double epsilon = 1e-6; +// seed_rand(); +// +// tf::Transformer mTR(true); +// std::vector xvalues(runs), yvalues(runs), zvalues(runs); +// for ( unsigned int i = 0; i < runs ; i++ ) +// { +// xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// +// StampedTransform tranStamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); +// mTR.setTransform(tranStamped); +// StampedTransform tranStamped2(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "child", "grandchild"); +// mTR.setTransform(tranStamped2); +// } +// +// // std::cout << mTR.allFramesAsString() << std::endl; +// // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; +// +// for ( unsigned int i = 0; i < runs ; i++ ) +// +// { +// Stamped inpose (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "grandchild"); +// +// try{ +// Stamped outpose; +// outpose.setIdentity(); //to make sure things are getting mutated +// mTR.transformPose("my_parent",inpose, outpose); +// EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon); +// } +// catch (tf::TransformException & ex) +// { +// std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; +// bool exception_improperly_thrown = true; +// EXPECT_FALSE(exception_improperly_thrown); +// } +// } +// +// } +// +// +// TEST(tf, ListOneForward) +// { +// unsigned int runs = 4; +// double epsilon = 1e-6; +// seed_rand(); +// +// tf::Transformer mTR(true); +// std::vector xvalues(runs), yvalues(runs), zvalues(runs); +// for ( uint64_t i = 0; i < runs ; i++ ) +// { +// xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// +// StampedTransform tranStamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); +// mTR.setTransform(tranStamped); +// } +// +// // std::cout << mTR.allFramesAsString() << std::endl; +// // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; +// +// for ( uint64_t i = 0; i < runs ; i++ ) +// +// { +// Stamped inpose (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent"); +// +// try{ +// Stamped outpose; +// outpose.setIdentity(); //to make sure things are getting mutated +// mTR.transformPose("child",inpose, outpose); +// EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon); +// } +// catch (tf::TransformException & ex) +// { +// std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; +// bool exception_improperly_thrown = true; +// EXPECT_FALSE(exception_improperly_thrown); +// } +// } +// +// } +// +// TEST(tf, ListTwoForward) +// { +// unsigned int runs = 4; +// double epsilon = 1e-6; +// seed_rand(); +// +// tf::Transformer mTR(true); +// std::vector xvalues(runs), yvalues(runs), zvalues(runs); +// for ( unsigned int i = 0; i < runs ; i++ ) +// { +// xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// +// StampedTransform tranStamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent", "child"); +// mTR.setTransform(tranStamped); +// StampedTransform tranStamped2(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "child", "grandchild"); +// mTR.setTransform(tranStamped2); +// } +// +// // std::cout << mTR.allFramesAsString() << std::endl; +// // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; +// +// for ( unsigned int i = 0; i < runs ; i++ ) +// +// { +// Stamped inpose (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parent"); +// +// try{ +// Stamped outpose; +// outpose.setIdentity(); //to make sure things are getting mutated +// mTR.transformPose("grandchild",inpose, outpose); +// EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon); +// } +// catch (tf::TransformException & ex) +// { +// std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; +// bool exception_improperly_thrown = true; +// EXPECT_FALSE(exception_improperly_thrown); +// } +// } +// +// } +// +// TEST(tf, TransformThrougRoot) +// { +// unsigned int runs = 4; +// double epsilon = 1e-6; +// seed_rand(); +// +// tf::Transformer mTR(true); +// std::vector xvalues(runs), yvalues(runs), zvalues(runs); +// for ( unsigned int i = 0; i < runs ; i++ ) +// { +// xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// +// StampedTransform tranStamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(1000 + i*100), "my_parent", "childA"); +// mTR.setTransform(tranStamped); +// StampedTransform tranStamped2(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(1000 + i*100), "my_parent", "childB"); +// mTR.setTransform(tranStamped2); +// } +// +// // std::cout << mTR.allFramesAsString() << std::endl; +// // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; +// +// for ( unsigned int i = 0; i < runs ; i++ ) +// +// { +// Stamped inpose (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000 + i*100), "childA"); +// +// try{ +// Stamped outpose; +// outpose.setIdentity(); //to make sure things are getting mutated +// mTR.transformPose("childB",inpose, outpose); +// EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); +// } +// catch (tf::TransformException & ex) +// { +// std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; +// bool exception_improperly_thrown = true; +// EXPECT_FALSE(exception_improperly_thrown); +// } +// } +// +// } +// +// TEST(tf, TransformThroughNO_PARENT) +// { +// unsigned int runs = 4; +// double epsilon = 1e-6; +// seed_rand(); +// +// tf::Transformer mTR(true); +// std::vector xvalues(runs), yvalues(runs), zvalues(runs); +// for ( unsigned int i = 0; i < runs ; i++ ) +// { +// xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; +// +// StampedTransform tranStamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parentA", "childA"); +// mTR.setTransform(tranStamped); +// StampedTransform tranStamped2(btTransform(tf2::Quaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), builtin_interfaces::msg::Time().fromNSec(10 + i), "my_parentB", "childB"); +// mTR.setTransform(tranStamped2); +// } +// +// // std::cout << mTR.allFramesAsString() << std::endl; +// // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; +// +// for ( unsigned int i = 0; i < runs ; i++ ) +// +// { +// Stamped inpose (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10 + i), "childA"); +// bool exception_thrown = false; +// +// try{ +// Stamped outpose; +// outpose.setIdentity(); //to make sure things are getting mutated +// mTR.transformPose("childB",inpose, outpose); +// EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); +// EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); +// } +// catch (tf::TransformException & ex) +// { +// exception_thrown = true; +// } +// EXPECT_TRUE(exception_thrown); +// } +// +// } +// +// */ + +#define GTEST_COUT std::cerr << "[ ] [ INFO ]" TEST(BufferCore_lookupTransform, i_configuration) { double epsilon = 1e-6; - + // TEST_COUT << "BufferCore_lookupTransform" << std::endl; + GTEST_COUT << "ALEX!!! BufferCore_lookupTransform" << std::endl; rostest::Permuter permuter; - std::vector times; - times.push_back(builtin_interfaces::msg::Time(1.0)); - times.push_back(builtin_interfaces::msg::Time(10.0)); - times.push_back(builtin_interfaces::msg::Time(0.0)); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(1.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(10.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(0.0))); builtin_interfaces::msg::Time eval_time; permuter.addOptionSet(times, &eval_time); @@ -637,7 +652,7 @@ TEST(BufferCore_lookupTransform, i_configuration) durations.push_back(tf2::durationFromSec(0.001)); durations.push_back(tf2::durationFromSec(0.1)); tf2::Duration interpolation_space; - // permuter.addOptionSet(durations, &interpolation_space); + permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); @@ -645,18 +660,22 @@ TEST(BufferCore_lookupTransform, i_configuration) frames.push_back("c"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); - + std::string target_frame; permuter.addOptionSet(frames, &target_frame); - while (permuter.step()) + while(permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "i", eval_time, interpolation_space); - geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(eval_time.sec) + + std::chrono::nanoseconds(eval_time.nanosec)); + + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -666,7 +685,7 @@ TEST(BufferCore_lookupTransform, i_configuration) EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); - + //Zero distance if (source_frame == target_frame) { @@ -693,16 +712,16 @@ TEST(BufferCore_lookupTransform, i_configuration) else { EXPECT_FALSE("i configuration: Shouldn't get here"); - printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.sec + eval_time.nanosec /1e9 ); } - + // rclcpp::shutdown(); } } /* Check 1 result return false if test parameters unmet */ -bool check_1_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) +bool check_1_result(const geometry_msgs::msg::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -712,7 +731,7 @@ bool check_1_result(const geometry_msgs::TransformStamped& outpose, const std::s EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); - + //Zero distance if (source_frame == target_frame) { @@ -735,9 +754,9 @@ bool check_1_result(const geometry_msgs::TransformStamped& outpose, const std::s } /* Check v result return false if test parameters unmet */ -bool check_v_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) +bool check_v_result(const geometry_msgs::msg::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -746,7 +765,7 @@ bool check_v_result(const geometry_msgs::TransformStamped& outpose, const std::s EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); - + //Zero distance if (source_frame == target_frame) { @@ -845,9 +864,9 @@ bool check_v_result(const geometry_msgs::TransformStamped& outpose, const std::s } /* Check v result return false if test parameters unmet */ -bool check_y_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) +bool check_y_result(const geometry_msgs::msg::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -856,7 +875,7 @@ bool check_y_result(const geometry_msgs::TransformStamped& outpose, const std::s EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); - + //Zero distance if (source_frame == target_frame) { @@ -958,15 +977,13 @@ bool check_y_result(const geometry_msgs::TransformStamped& outpose, const std::s TEST(BufferCore_lookupTransform, one_link_configuration) { double epsilon = 1e-6; - - rostest::Permuter permuter; std::vector times; - times.push_back(builtin_interfaces::msg::Time(1.0)); - times.push_back(builtin_interfaces::msg::Time(10.0)); - times.push_back(builtin_interfaces::msg::Time(0.0)); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(1.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(10.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(0.0))); builtin_interfaces::msg::Time eval_time; permuter.addOptionSet(times, &eval_time); @@ -975,14 +992,14 @@ TEST(BufferCore_lookupTransform, one_link_configuration) durations.push_back(tf2::durationFromSec(0.001)); durations.push_back(tf2::durationFromSec(0.1)); tf2::Duration interpolation_space; - // permuter.addOptionSet(durations, &interpolation_space); + // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("1"); frames.push_back("2"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); - + std::string target_frame; permuter.addOptionSet(frames, &target_frame); @@ -992,7 +1009,11 @@ TEST(BufferCore_lookupTransform, one_link_configuration) tf2::BufferCore mBC; setupTree(mBC, "1", eval_time, interpolation_space); - geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(eval_time.sec) + + std::chrono::nanoseconds(eval_time.nanosec)); + + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon)); } @@ -1002,15 +1023,13 @@ TEST(BufferCore_lookupTransform, one_link_configuration) TEST(BufferCore_lookupTransform, v_configuration) { double epsilon = 1e-6; - - rostest::Permuter permuter; std::vector times; - times.push_back(builtin_interfaces::msg::Time(1.0)); - times.push_back(builtin_interfaces::msg::Time(10.0)); - times.push_back(builtin_interfaces::msg::Time(0.0)); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(1.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(10.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(0.0))); builtin_interfaces::msg::Time eval_time; permuter.addOptionSet(times, &eval_time); @@ -1029,7 +1048,7 @@ TEST(BufferCore_lookupTransform, v_configuration) frames.push_back("g"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); - + std::string target_frame; permuter.addOptionSet(frames, &target_frame); @@ -1039,7 +1058,11 @@ TEST(BufferCore_lookupTransform, v_configuration) tf2::BufferCore mBC; setupTree(mBC, "v", eval_time, interpolation_space); - geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(eval_time.sec) + + std::chrono::nanoseconds(eval_time.nanosec)); + + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon)); } @@ -1049,15 +1072,13 @@ TEST(BufferCore_lookupTransform, v_configuration) TEST(BufferCore_lookupTransform, y_configuration) { double epsilon = 1e-6; - - rostest::Permuter permuter; std::vector times; - times.push_back(builtin_interfaces::msg::Time(1.0)); - times.push_back(builtin_interfaces::msg::Time(10.0)); - times.push_back(builtin_interfaces::msg::Time(0.0)); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(1.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(10.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(0.0))); builtin_interfaces::msg::Time eval_time; permuter.addOptionSet(times, &eval_time); @@ -1076,7 +1097,7 @@ TEST(BufferCore_lookupTransform, y_configuration) frames.push_back("e"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); - + std::string target_frame; permuter.addOptionSet(frames, &target_frame); @@ -1086,7 +1107,11 @@ TEST(BufferCore_lookupTransform, y_configuration) tf2::BufferCore mBC; setupTree(mBC, "y", eval_time, interpolation_space); - geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(eval_time.sec) + + std::chrono::nanoseconds(eval_time.nanosec)); + + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); EXPECT_TRUE(check_y_result(outpose, source_frame, target_frame, eval_time, epsilon)); } @@ -1095,15 +1120,13 @@ TEST(BufferCore_lookupTransform, y_configuration) TEST(BufferCore_lookupTransform, multi_configuration) { double epsilon = 1e-6; - - rostest::Permuter permuter; std::vector times; - times.push_back(builtin_interfaces::msg::Time(1.0)); - times.push_back(builtin_interfaces::msg::Time(10.0)); - times.push_back(builtin_interfaces::msg::Time(0.0)); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(1.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(10.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(0.0))); builtin_interfaces::msg::Time eval_time; permuter.addOptionSet(times, &eval_time); @@ -1124,7 +1147,7 @@ TEST(BufferCore_lookupTransform, multi_configuration) frames.push_back("g"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); - + std::string target_frame; permuter.addOptionSet(frames, &target_frame); @@ -1134,33 +1157,37 @@ TEST(BufferCore_lookupTransform, multi_configuration) tf2::BufferCore mBC; setupTree(mBC, "1_v", eval_time, interpolation_space); - if (mBC.canTransform(source_frame, target_frame, eval_time)) + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(eval_time.sec) + + std::chrono::nanoseconds(eval_time.nanosec)); + + if (mBC.canTransform(source_frame, target_frame, eval_time_time_point)) { - geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); - + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + if ((source_frame == "1" || source_frame =="2") && (target_frame =="1" || target_frame == "2")) EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon)); - else if ((source_frame == "a" || source_frame == "b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && + else if ((source_frame == "a" || source_frame == "b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && (target_frame == "a" || target_frame == "b" || target_frame == "c" || target_frame == "f" || target_frame == "g")) EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon)); else EXPECT_FALSE("Frames unhandled"); } else - EXPECT_TRUE(((source_frame == "a" || source_frame =="b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && + EXPECT_TRUE(((source_frame == "a" || source_frame =="b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && (target_frame == "1" || target_frame == "2") ) - || - ((target_frame == "a" || target_frame =="b" || target_frame == "c" || target_frame == "f" || target_frame == "g") && + || + ((target_frame == "a" || target_frame =="b" || target_frame == "c" || target_frame == "f" || target_frame == "g") && (source_frame == "1" || source_frame == "2")) ); - - } + + } } #define CHECK_QUATERNION_NEAR(_q1, _x, _y, _z, _w, _epsilon) \ { \ - btQuaternion q1(_q1.x, _q1.y, _q1.z, _q1.w); \ - btQuaternion q2(_x, _y, _z, _w); \ + tf2::Quaternion q1(_q1.x, _q1.y, _q1.z, _q1.w); \ + tf2::Quaternion q2(_x, _y, _z, _w); \ double angle = q1.angle(q2); \ EXPECT_TRUE(fabs(angle) < _epsilon || fabs(angle - M_PI) < _epsilon); \ } @@ -1170,258 +1197,264 @@ TEST(BufferCore_lookupTransform, multi_configuration) EXPECT_NEAR(_out.transform.translation.y, _expected.getOrigin().y(), epsilon); \ EXPECT_NEAR(_out.transform.translation.z, _expected.getOrigin().z(), epsilon); \ CHECK_QUATERNION_NEAR(_out.transform.rotation, _expected.getRotation().x(), _expected.getRotation().y(), _expected.getRotation().z(), _expected.getRotation().w(), _eps); - - -// Simple test with compound transform -TEST(BufferCore_lookupTransform, compound_xfm_configuration) -{ - /* - * Frames - * - * root->a - * - * root->b->c->d - * - */ - - double epsilon = 2e-5; // Larger epsilon for interpolation values - - tf2::BufferCore mBC; - - geometry_msgs::TransformStamped tsa; - tsa.header.frame_id = "root"; - tsa.child_frame_id = "a"; - tsa.transform.translation.x = 1.0; - tsa.transform.translation.y = 1.0; - tsa.transform.translation.z = 1.0; - btQuaternion q1; - q1.setEuler(0.25, .5, .75); - tsa.transform.rotation.x = q1.x(); - tsa.transform.rotation.y = q1.y(); - tsa.transform.rotation.z = q1.z(); - tsa.transform.rotation.w = q1.w(); - EXPECT_TRUE(mBC.setTransform(tsa, "authority")); - - geometry_msgs::TransformStamped tsb; - tsb.header.frame_id = "root"; - tsb.child_frame_id = "b"; - tsb.transform.translation.x = -1.0; - tsb.transform.translation.y = 0.0; - tsb.transform.translation.z = -1.0; - btQuaternion q2; - q2.setEuler(1.0, 0.25, 0.5); - tsb.transform.rotation.x = q2.x(); - tsb.transform.rotation.y = q2.y(); - tsb.transform.rotation.z = q2.z(); - tsb.transform.rotation.w = q2.w(); - EXPECT_TRUE(mBC.setTransform(tsb, "authority")); - - geometry_msgs::TransformStamped tsc; - tsc.header.frame_id = "b"; - tsc.child_frame_id = "c"; - tsc.transform.translation.x = 0.0; - tsc.transform.translation.y = 2.0; - tsc.transform.translation.z = 0.5; - btQuaternion q3; - q3.setEuler(0.25, .75, 1.25); - tsc.transform.rotation.x = q3.x(); - tsc.transform.rotation.y = q3.y(); - tsc.transform.rotation.z = q3.z(); - tsc.transform.rotation.w = q3.w(); - EXPECT_TRUE(mBC.setTransform(tsc, "authority")); - - geometry_msgs::TransformStamped tsd; - tsd.header.frame_id = "c"; - tsd.child_frame_id = "d"; - tsd.transform.translation.x = 0.5; - tsd.transform.translation.y = -1; - tsd.transform.translation.z = 1.5; - btQuaternion q4; - q4.setEuler(-0.5, 1.0, -.75); - tsd.transform.rotation.x = q4.x(); - tsd.transform.rotation.y = q4.y(); - tsd.transform.rotation.z = q4.z(); - tsd.transform.rotation.w = q4.w(); - EXPECT_TRUE(mBC.setTransform(tsd, "authority")); - - btTransform ta, tb, tc, td, expected_ab, expected_bc, expected_cb, expected_ac, expected_ba, expected_ca, expected_ad, expected_da, expected_bd, expected_db, expected_rootd, expected_rootc; - ta.setOrigin(btVector3(1.0, 1.0, 1.0)); - ta.setRotation(q1); - tb.setOrigin(btVector3(-1.0, 0.0, -1.0)); - tb.setRotation(q2); - tc.setOrigin(btVector3(0.0, 2.0, 0.5)); - tc.setRotation(q3); - td.setOrigin(btVector3(0.5, -1, 1.5)); - td.setRotation(q4); - - - expected_ab = ta.inverse() * tb; - expected_ac = ta.inverse() * tb * tc; - expected_ad = ta.inverse() * tb * tc * td; - expected_cb = tc.inverse(); - expected_bc = tc; - expected_bd = tc * td; - expected_db = expected_bd.inverse(); - expected_ba = tb.inverse() * ta; - expected_ca = tc.inverse() * tb.inverse() * ta; - expected_da = td.inverse() * tc.inverse() * tb.inverse() * ta; - expected_rootd = tb * tc * td; - expected_rootc = tb * tc; - - // root -> b -> c - geometry_msgs::TransformStamped out_rootc = mBC.lookupTransform("root", "c", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon); - - // root -> b -> c -> d - geometry_msgs::TransformStamped out_rootd = mBC.lookupTransform("root", "d", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon); - - // a <- root -> b - geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon); - - geometry_msgs::TransformStamped out_ba = mBC.lookupTransform("b", "a", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon); - - // a <- root -> b -> c - geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon); - - geometry_msgs::TransformStamped out_ca = mBC.lookupTransform("c", "a", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon); - - // a <- root -> b -> c -> d - geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon); - - geometry_msgs::TransformStamped out_da = mBC.lookupTransform("d", "a", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon); - - // b -> c - geometry_msgs::TransformStamped out_cb = mBC.lookupTransform("c", "b", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon); - - geometry_msgs::TransformStamped out_bc = mBC.lookupTransform("b", "c", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon); - - // b -> c -> d - geometry_msgs::TransformStamped out_bd = mBC.lookupTransform("b", "d", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon); - - geometry_msgs::TransformStamped out_db = mBC.lookupTransform("d", "b", builtin_interfaces::msg::Time()); - CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon); -} - +// +// // TODO(ahcorde): btTransform +// // Simple test with compound transform +// TEST(BufferCore_lookupTransform, compound_xfm_configuration) +// { +// /* +// * Frames +// * +// * root->a +// * +// * root->b->c->d +// * +// */ +// +// double epsilon = 2e-5; // Larger epsilon for interpolation values +// +// tf2::BufferCore mBC; +// +// geometry_msgs::msg::TransformStamped tsa; +// tsa.header.frame_id = "root"; +// tsa.child_frame_id = "a"; +// tsa.transform.translation.x = 1.0; +// tsa.transform.translation.y = 1.0; +// tsa.transform.translation.z = 1.0; +// tf2::Quaternion q1; +// q1.setEuler(0.25, .5, .75); +// tsa.transform.rotation.x = q1.x(); +// tsa.transform.rotation.y = q1.y(); +// tsa.transform.rotation.z = q1.z(); +// tsa.transform.rotation.w = q1.w(); +// EXPECT_TRUE(mBC.setTransform(tsa, "authority")); +// +// geometry_msgs::msg::TransformStamped tsb; +// tsb.header.frame_id = "root"; +// tsb.child_frame_id = "b"; +// tsb.transform.translation.x = -1.0; +// tsb.transform.translation.y = 0.0; +// tsb.transform.translation.z = -1.0; +// tf2::Quaternion q2; +// q2.setEuler(1.0, 0.25, 0.5); +// tsb.transform.rotation.x = q2.x(); +// tsb.transform.rotation.y = q2.y(); +// tsb.transform.rotation.z = q2.z(); +// tsb.transform.rotation.w = q2.w(); +// EXPECT_TRUE(mBC.setTransform(tsb, "authority")); +// +// geometry_msgs::msg::TransformStamped tsc; +// tsc.header.frame_id = "b"; +// tsc.child_frame_id = "c"; +// tsc.transform.translation.x = 0.0; +// tsc.transform.translation.y = 2.0; +// tsc.transform.translation.z = 0.5; +// tf2::Quaternion q3; +// q3.setEuler(0.25, .75, 1.25); +// tsc.transform.rotation.x = q3.x(); +// tsc.transform.rotation.y = q3.y(); +// tsc.transform.rotation.z = q3.z(); +// tsc.transform.rotation.w = q3.w(); +// EXPECT_TRUE(mBC.setTransform(tsc, "authority")); +// +// geometry_msgs::msg::TransformStamped tsd; +// tsd.header.frame_id = "c"; +// tsd.child_frame_id = "d"; +// tsd.transform.translation.x = 0.5; +// tsd.transform.translation.y = -1; +// tsd.transform.translation.z = 1.5; +// tf2::Quaternion q4; +// q4.setEuler(-0.5, 1.0, -.75); +// tsd.transform.rotation.x = q4.x(); +// tsd.transform.rotation.y = q4.y(); +// tsd.transform.rotation.z = q4.z(); +// tsd.transform.rotation.w = q4.w(); +// EXPECT_TRUE(mBC.setTransform(tsd, "authority")); +// +// btTransform ta, tb, tc, td, expected_ab, expected_bc, expected_cb, expected_ac, expected_ba, expected_ca, expected_ad, expected_da, expected_bd, expected_db, expected_rootd, expected_rootc; +// ta.setOrigin(btVector3(1.0, 1.0, 1.0)); +// ta.setRotation(q1); +// tb.setOrigin(btVector3(-1.0, 0.0, -1.0)); +// tb.setRotation(q2); +// tc.setOrigin(btVector3(0.0, 2.0, 0.5)); +// tc.setRotation(q3); +// td.setOrigin(btVector3(0.5, -1, 1.5)); +// td.setRotation(q4); +// +// +// expected_ab = ta.inverse() * tb; +// expected_ac = ta.inverse() * tb * tc; +// expected_ad = ta.inverse() * tb * tc * td; +// expected_cb = tc.inverse(); +// expected_bc = tc; +// expected_bd = tc * td; +// expected_db = expected_bd.inverse(); +// expected_ba = tb.inverse() * ta; +// expected_ca = tc.inverse() * tb.inverse() * ta; +// expected_da = td.inverse() * tc.inverse() * tb.inverse() * ta; +// expected_rootd = tb * tc * td; +// expected_rootc = tb * tc; +// +// // root -> b -> c +// geometry_msgs::msg::TransformStamped out_rootc = mBC.lookupTransform("root", "c", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon); +// +// // root -> b -> c -> d +// geometry_msgs::msg::TransformStamped out_rootd = mBC.lookupTransform("root", "d", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon); +// +// // a <- root -> b +// geometry_msgs::msg::TransformStamped out_ab = mBC.lookupTransform("a", "b", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon); +// +// geometry_msgs::msg::TransformStamped out_ba = mBC.lookupTransform("b", "a", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon); +// +// // a <- root -> b -> c +// geometry_msgs::msg::TransformStamped out_ac = mBC.lookupTransform("a", "c", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon); +// +// geometry_msgs::msg::TransformStamped out_ca = mBC.lookupTransform("c", "a", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon); +// +// // a <- root -> b -> c -> d +// geometry_msgs::msg::TransformStamped out_ad = mBC.lookupTransform("a", "d", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon); +// +// geometry_msgs::msg::TransformStamped out_da = mBC.lookupTransform("d", "a", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon); +// +// // b -> c +// geometry_msgs::msg::TransformStamped out_cb = mBC.lookupTransform("c", "b", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon); +// +// geometry_msgs::msg::TransformStamped out_bc = mBC.lookupTransform("b", "c", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon); +// +// // b -> c -> d +// geometry_msgs::msg::TransformStamped out_bd = mBC.lookupTransform("b", "d", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon); +// +// geometry_msgs::msg::TransformStamped out_db = mBC.lookupTransform("d", "b", builtin_interfaces::msg::Time()); +// CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon); +// } + +// TODO(ahcorde) // Time varying transforms, testing interpolation -TEST(BufferCore_lookupTransform, helix_configuration) -{ - double epsilon = 2e-5; // Larger epsilon for interpolation values - - tf2::BufferCore mBC; - - builtin_interfaces::msg::Time t0 = builtin_interfaces::msg::Time() + tf2::durationFromSec(10); - tf2::Duration step = tf2::durationFromSec(0.05); - tf2::Duration half_step = tf2::durationFromSec(0.025); - builtin_interfaces::msg::Time t1 = t0 + tf2::durationFromSec(5.0); - - /* - * a->b->c - * - * b.z = vel * (t - t0) - * c.x = cos(theta * (t - t0)) - * c.y = sin(theta * (t - t0)) - * - * a->d - * - * d.z = 2 * cos(theta * (t - t0)) - * a->d transforms are at half-step between a->b->c transforms - */ - - double theta = 0.25; - double vel = 1.0; - - for (builtin_interfaces::msg::Time t = t0; t <= t1; t += step) - { - builtin_interfaces::msg::Time t2 = t + half_step; - double dt = (t - t0).toSec(); - double dt2 = (t2 - t0).toSec(); - - geometry_msgs::TransformStamped ts; - ts.header.frame_id = "a"; - ts.header.stamp = t; - ts.child_frame_id = "b"; - ts.transform.translation.z = vel * dt; - ts.transform.rotation.w = 1.0; - EXPECT_TRUE(mBC.setTransform(ts, "authority")); - - geometry_msgs::TransformStamped ts2; - ts2.header.frame_id = "b"; - ts2.header.stamp = t; - ts2.child_frame_id = "c"; - ts2.transform.translation.x = cos(theta * dt); - ts2.transform.translation.y = sin(theta * dt); - btQuaternion q; - q.setEuler(0,0,theta*dt); - ts2.transform.rotation.z = q.z(); - ts2.transform.rotation.w = q.w(); - EXPECT_TRUE(mBC.setTransform(ts2, "authority")); - - geometry_msgs::TransformStamped ts3; - ts3.header.frame_id = "a"; - ts3.header.stamp = t2; - ts3.child_frame_id = "d"; - ts3.transform.translation.z = cos(theta * dt2); - ts3.transform.rotation.w = 1.0; - EXPECT_TRUE(mBC.setTransform(ts3, "authority")); - } - - - for (builtin_interfaces::msg::Time t = t0 + half_step; t < t1; t += step) - { - builtin_interfaces::msg::Time t2 = t + half_step; - double dt = (t - t0).toSec(); - double dt2 = (t2 - t0).toSec(); - - geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", t); - EXPECT_NEAR(out_ab.transform.translation.z, vel * dt, epsilon); - - geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", t); - EXPECT_NEAR(out_ac.transform.translation.x, cos(theta * dt), epsilon); - EXPECT_NEAR(out_ac.transform.translation.y, sin(theta * dt), epsilon); - EXPECT_NEAR(out_ac.transform.translation.z, vel * dt, epsilon); - btQuaternion q; - q.setEuler(0,0,theta*dt); - CHECK_QUATERNION_NEAR(out_ac.transform.rotation, 0, 0, q.z(), q.w(), epsilon); - - geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", t); - EXPECT_NEAR(out_ad.transform.translation.z, cos(theta * dt), epsilon); - - geometry_msgs::TransformStamped out_cd = mBC.lookupTransform("c", "d", t2); - EXPECT_NEAR(out_cd.transform.translation.x, -1, epsilon); - EXPECT_NEAR(out_cd.transform.translation.y, 0, epsilon); - EXPECT_NEAR(out_cd.transform.translation.z, cos(theta * dt2) - vel * dt2, epsilon); - btQuaternion mq; - mq.setEuler(0,0,-theta*dt2); - CHECK_QUATERNION_NEAR(out_cd.transform.rotation, 0, 0, mq.z(), mq.w(), epsilon); - } - - // Advanced API - for (builtin_interfaces::msg::Time t = t0 + half_step; t < t1; t += (step + step)) - { - builtin_interfaces::msg::Time t2 = t + step; - double dt = (t - t0).toSec(); - double dt2 = (t2 - t0).toSec(); - - geometry_msgs::TransformStamped out_cd2 = mBC.lookupTransform("c", t, "d", t2, "a"); - EXPECT_NEAR(out_cd2.transform.translation.x, -1, epsilon); - EXPECT_NEAR(out_cd2.transform.translation.y, 0, epsilon); - EXPECT_NEAR(out_cd2.transform.translation.z, cos(theta * dt2) - vel * dt, epsilon); - btQuaternion mq2; - mq2.setEuler(0,0,-theta*dt); - CHECK_QUATERNION_NEAR(out_cd2.transform.rotation, 0, 0, mq2.z(), mq2.w(), epsilon); - } -} - +// TEST(BufferCore_lookupTransform, helix_configuration) +// { +// double epsilon = 2e-5; // Larger epsilon for interpolation values +// +// tf2::BufferCore mBC; +// +// builtin_interfaces::msg::Time t0; +// t0.sec = 10; +// t0.nanosec = 0; +// tf2::Duration step = tf2::durationFromSec(0.05); +// tf2::Duration half_step = tf2::durationFromSec(0.025); +// builtin_interfaces::msg::Time t1; +// double t1_seconds = tf2::durationToSec(tf2::durationFromSec(5.0)) + t0.sec + t0.nanosec; +// t0.sec = (int)t1_seconds; +// t0.nanosec = (t1_seconds - (int)t1_seconds)/1e9; +// +// /* +// * a->b->c +// * +// * b.z = vel * (t - t0) +// * c.x = cos(theta * (t - t0)) +// * c.y = sin(theta * (t - t0)) +// * +// * a->d +// * +// * d.z = 2 * cos(theta * (t - t0)) +// * a->d transforms are at half-step between a->b->c transforms +// */ +// +// double theta = 0.25; +// double vel = 1.0; +// +// for (builtin_interfaces::msg::Time t = t0; t <= t1; t += step) +// { +// builtin_interfaces::msg::Time t2 = t + half_step; +// double dt = (t - t0).toSec(); +// double dt2 = (t2 - t0).toSec(); +// +// geometry_msgs::msg::TransformStamped ts; +// ts.header.frame_id = "a"; +// ts.header.stamp = t; +// ts.child_frame_id = "b"; +// ts.transform.translation.z = vel * dt; +// ts.transform.rotation.w = 1.0; +// EXPECT_TRUE(mBC.setTransform(ts, "authority")); +// +// geometry_msgs::msg::TransformStamped ts2; +// ts2.header.frame_id = "b"; +// ts2.header.stamp = t; +// ts2.child_frame_id = "c"; +// ts2.transform.translation.x = cos(theta * dt); +// ts2.transform.translation.y = sin(theta * dt); +// tf2::Quaternion q; +// q.setEuler(0,0,theta*dt); +// ts2.transform.rotation.z = q.z(); +// ts2.transform.rotation.w = q.w(); +// EXPECT_TRUE(mBC.setTransform(ts2, "authority")); +// +// geometry_msgs::msg::TransformStamped ts3; +// ts3.header.frame_id = "a"; +// ts3.header.stamp = t2; +// ts3.child_frame_id = "d"; +// ts3.transform.translation.z = cos(theta * dt2); +// ts3.transform.rotation.w = 1.0; +// EXPECT_TRUE(mBC.setTransform(ts3, "authority")); +// } +// +// +// for (builtin_interfaces::msg::Time t = t0 + half_step; t < t1; t += step) +// { +// builtin_interfaces::msg::Time t2 = t + half_step; +// double dt = (t - t0).toSec(); +// double dt2 = (t2 - t0).toSec(); +// +// geometry_msgs::msg::TransformStamped out_ab = mBC.lookupTransform("a", "b", t); +// EXPECT_NEAR(out_ab.transform.translation.z, vel * dt, epsilon); +// +// geometry_msgs::msg::TransformStamped out_ac = mBC.lookupTransform("a", "c", t); +// EXPECT_NEAR(out_ac.transform.translation.x, cos(theta * dt), epsilon); +// EXPECT_NEAR(out_ac.transform.translation.y, sin(theta * dt), epsilon); +// EXPECT_NEAR(out_ac.transform.translation.z, vel * dt, epsilon); +// tf2::Quaternion q; +// q.setEuler(0,0,theta*dt); +// CHECK_QUATERNION_NEAR(out_ac.transform.rotation, 0, 0, q.z(), q.w(), epsilon); +// +// geometry_msgs::msg::TransformStamped out_ad = mBC.lookupTransform("a", "d", t); +// EXPECT_NEAR(out_ad.transform.translation.z, cos(theta * dt), epsilon); +// +// geometry_msgs::msg::TransformStamped out_cd = mBC.lookupTransform("c", "d", t2); +// EXPECT_NEAR(out_cd.transform.translation.x, -1, epsilon); +// EXPECT_NEAR(out_cd.transform.translation.y, 0, epsilon); +// EXPECT_NEAR(out_cd.transform.translation.z, cos(theta * dt2) - vel * dt2, epsilon); +// tf2::Quaternion mq; +// mq.setEuler(0,0,-theta*dt2); +// CHECK_QUATERNION_NEAR(out_cd.transform.rotation, 0, 0, mq.z(), mq.w(), epsilon); +// } +// +// // Advanced API +// for (builtin_interfaces::msg::Time t = t0 + half_step; t < t1; t += (step + step)) +// { +// builtin_interfaces::msg::Time t2 = t + step; +// double dt = (t - t0).toSec(); +// double dt2 = (t2 - t0).toSec(); +// +// geometry_msgs::msg::TransformStamped out_cd2 = mBC.lookupTransform("c", t, "d", t2, "a"); +// EXPECT_NEAR(out_cd2.transform.translation.x, -1, epsilon); +// EXPECT_NEAR(out_cd2.transform.translation.y, 0, epsilon); +// EXPECT_NEAR(out_cd2.transform.translation.z, cos(theta * dt2) - vel * dt, epsilon); +// tf2::Quaternion mq2; +// mq2.setEuler(0,0,-theta*dt); +// CHECK_QUATERNION_NEAR(out_cd2.transform.rotation, 0, 0, mq2.z(), mq2.w(), epsilon); +// } +// } +// TEST(BufferCore_lookupTransform, ring_45_configuration) { @@ -1429,9 +1462,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) rostest::Permuter permuter; std::vector times; - times.push_back(builtin_interfaces::msg::Time(1.0)); - times.push_back(builtin_interfaces::msg::Time(10.0)); - times.push_back(builtin_interfaces::msg::Time(0.0)); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(1.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(10.0))); + times.push_back(tf2_ros::toMsg(tf2::timeFromSec(0.0))); builtin_interfaces::msg::Time eval_time; permuter.addOptionSet(times, &eval_time); @@ -1462,7 +1495,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) frames.push_back("inverse_i");*/ std::string source_frame; permuter.addOptionSet(frames, &source_frame); - + std::string target_frame; permuter.addOptionSet(frames, &target_frame); @@ -1472,16 +1505,17 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) tf2::BufferCore mBC; setupTree(mBC, "ring_45", eval_time, interpolation_space); - geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(eval_time.sec) + + std::chrono::nanoseconds(eval_time.nanosec)); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); - - //Zero distance or all the way if (source_frame == target_frame || (source_frame == "a" && target_frame == "i") || @@ -1725,27 +1759,31 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); - printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.sec + eval_time.nanosec /1e9 ); } - } } TEST(BufferCore_lookupTransform, invalid_arguments) { tf2::BufferCore mBC; - - setupTree(mBC, "i", builtin_interfaces::msg::Time(1.0)); - - EXPECT_NO_THROW(mBC.lookupTransform("b", "a", builtin_interfaces::msg::Time())); - //Empty frame_id - EXPECT_THROW(mBC.lookupTransform("", "a", builtin_interfaces::msg::Time()), tf2::InvalidArgumentException); - EXPECT_THROW(mBC.lookupTransform("b", "", builtin_interfaces::msg::Time()), tf2::InvalidArgumentException); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(0) + + std::chrono::nanoseconds(0)); - //frame_id with / - EXPECT_THROW(mBC.lookupTransform("/b", "a", builtin_interfaces::msg::Time()), tf2::InvalidArgumentException); - EXPECT_THROW(mBC.lookupTransform("b", "/a", builtin_interfaces::msg::Time()), tf2::InvalidArgumentException); + setupTree(mBC, "i", tf2_ros::toMsg(tf2::timeFromSec(1.0))); + + // EXPECT_NO_THROW + EXPECT_NO_THROW(mBC.lookupTransform("b", "a", eval_time_time_point)); + + //Empty frame_id + EXPECT_THROW(mBC.lookupTransform("", "a", eval_time_time_point), tf2::InvalidArgumentException); + EXPECT_THROW(mBC.lookupTransform("b", "", eval_time_time_point), tf2::InvalidArgumentException); + // + // //frame_id with / + EXPECT_THROW(mBC.lookupTransform("/b", "a", eval_time_time_point), tf2::InvalidArgumentException); + EXPECT_THROW(mBC.lookupTransform("b", "/a", eval_time_time_point), tf2::InvalidArgumentException); }; @@ -1753,18 +1791,21 @@ TEST(BufferCore_canTransform, invalid_arguments) { tf2::BufferCore mBC; - setupTree(mBC, "i", builtin_interfaces::msg::Time(1.0)); - - EXPECT_TRUE(mBC.canTransform("b", "a", builtin_interfaces::msg::Time())); - - + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(0) + + std::chrono::nanoseconds(0)); + + setupTree(mBC, "i", tf2_ros::toMsg(tf2::timeFromSec(1.0))); + + EXPECT_TRUE(mBC.canTransform("b", "a", eval_time_time_point)); + //Empty frame_id - EXPECT_FALSE(mBC.canTransform("", "a", builtin_interfaces::msg::Time())); - EXPECT_FALSE(mBC.canTransform("b", "", builtin_interfaces::msg::Time())); + EXPECT_FALSE(mBC.canTransform("", "a", eval_time_time_point)); + EXPECT_FALSE(mBC.canTransform("b", "", eval_time_time_point)); //frame_id with / - EXPECT_FALSE(mBC.canTransform("/b", "a", builtin_interfaces::msg::Time())); - EXPECT_FALSE(mBC.canTransform("b", "/a", builtin_interfaces::msg::Time())); + EXPECT_FALSE(mBC.canTransform("/b", "a", eval_time_time_point)); + EXPECT_FALSE(mBC.canTransform("b", "/a", eval_time_time_point)); }; @@ -1774,8 +1815,11 @@ struct TransformableHelper : called(false) {} - void callback(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame, - builtin_interfaces::msg::Time time, tf2::TransformableResult result) + void callback(tf2::TransformableRequestHandle request_handle, + const std::string & target_frame, + const std::string & source_frame, + tf2::TimePoint time, + tf2::TransformableResult result) { called = true; } @@ -1788,28 +1832,50 @@ TEST(BufferCore_transformableCallbacks, alreadyTransformable) tf2::BufferCore b; TransformableHelper h; - geometry_msgs::TransformStamped t; - t.header.stamp = builtin_interfaces::msg::Time(1); + geometry_msgs::msg::TransformStamped t; + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(1.0)); t.header.frame_id = "a"; t.child_frame_id = "b"; t.transform.rotation.w = 1.0; b.setTransform(t, "me"); - tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5)); - EXPECT_EQ(b.addTransformableRequest(cb_handle, "a", "b", builtin_interfaces::msg::Time(1)), 0U); + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(1) + + std::chrono::nanoseconds(0)); + + tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(std::bind(&TransformableHelper::callback, + &h, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3, + std::placeholders::_4, + std::placeholders::_5)); + + EXPECT_EQ(b.addTransformableRequest(cb_handle, "a", "b", eval_time_time_point), 0U); } TEST(BufferCore_transformableCallbacks, waitForNewTransform) { tf2::BufferCore b; TransformableHelper h; - tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5)); - EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", builtin_interfaces::msg::Time(10)), 0U); + tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(std::bind(&TransformableHelper::callback, + &h, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3, + std::placeholders::_4, + std::placeholders::_5)); + + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(10) + + std::chrono::nanoseconds(0)); - geometry_msgs::TransformStamped t; + EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", eval_time_time_point), 0U); + + geometry_msgs::msg::TransformStamped t; for (uint32_t i = 1; i <= 10; ++i) { - t.header.stamp = builtin_interfaces::msg::Time(i); + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(i)); t.header.frame_id = "a"; t.child_frame_id = "b"; t.transform.rotation.w = 1.0; @@ -1830,13 +1896,24 @@ TEST(BufferCore_transformableCallbacks, waitForOldTransform) { tf2::BufferCore b; TransformableHelper h; - tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5)); - EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", builtin_interfaces::msg::Time(1)), 0U); + tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(std::bind(&TransformableHelper::callback, + &h, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3, + std::placeholders::_4, + std::placeholders::_5)); + + tf2::TimePoint eval_time_time_point = tf2::TimePoint( + std::chrono::seconds(1) + + std::chrono::nanoseconds(0)); + + EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", eval_time_time_point), 0U); - geometry_msgs::TransformStamped t; + geometry_msgs::msg::TransformStamped t; for (uint32_t i = 10; i > 0; --i) { - t.header.stamp = builtin_interfaces::msg::Time(i); + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(i)); t.header.frame_id = "a"; t.child_frame_id = "b"; t.transform.rotation.w = 1.0; @@ -1859,15 +1936,15 @@ TEST(tf, Exceptions) tf::Transformer mTR(true); - + Stamped outpose; //connectivity when no data EXPECT_FALSE(mTR.canTransform("parent", "me", builtin_interfaces::msg::Time().fromNSec(10000000))); - try + try { - mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000000) , "me"), outpose); - EXPECT_FALSE("ConnectivityException Not Thrown"); + mTR.transformPose("parent",Stamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000000) , "me"), outpose); + EXPECT_FALSE("ConnectivityException Not Thrown"); } catch ( tf::LookupException &ex) { @@ -1878,14 +1955,14 @@ TEST(tf, Exceptions) printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } - - mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(100000), "parent", "me")); + + mTR.setTransform( StampedTransform(btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(100000), "parent", "me")); //Extrapolation not valid with one value EXPECT_FALSE(mTR.canTransform("parent", "me", builtin_interfaces::msg::Time().fromNSec(200000))); - try + try { - mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(200000) , "me"), outpose); + mTR.transformPose("parent",Stamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(200000) , "me"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -1897,16 +1974,16 @@ TEST(tf, Exceptions) printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } - - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(300000), "parent", "me")); + + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(300000), "parent", "me")); //NO Extration when Interpolating //inverse list EXPECT_TRUE(mTR.canTransform("parent", "me", builtin_interfaces::msg::Time().fromNSec(200000))); - try + try { - mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(200000) , "me"), outpose); + mTR.transformPose("parent",Stamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(200000) , "me"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -1923,9 +2000,9 @@ TEST(tf, Exceptions) //forward list EXPECT_TRUE(mTR.canTransform("me", "parent", builtin_interfaces::msg::Time().fromNSec(200000))); - try + try { - mTR.transformPose("me",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(200000) , "parent"), outpose); + mTR.transformPose("me",Stamped(btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(200000) , "parent"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -1937,14 +2014,14 @@ TEST(tf, Exceptions) printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } - + //Extrapolating backwards //inverse list EXPECT_FALSE(mTR.canTransform("parent", "me", builtin_interfaces::msg::Time().fromNSec(1000))); - try + try { - mTR.transformPose("parent",Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000) , "me"), outpose); + mTR.transformPose("parent",Stamped (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000) , "me"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -1958,9 +2035,9 @@ TEST(tf, Exceptions) } //forwards list EXPECT_FALSE(mTR.canTransform("me", "parent", builtin_interfaces::msg::Time().fromNSec(1000))); - try + try { - mTR.transformPose("me",Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000) , "parent"), outpose); + mTR.transformPose("me",Stamped (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000) , "parent"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -1972,16 +2049,16 @@ TEST(tf, Exceptions) printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } - + // Test extrapolation inverse and forward linkages FORWARD //inverse list EXPECT_FALSE(mTR.canTransform("parent", "me", builtin_interfaces::msg::Time().fromNSec(350000))); - try + try { - mTR.transformPose("parent", Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(350000) , "me"), outpose); + mTR.transformPose("parent", Stamped (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(350000) , "me"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -1996,9 +2073,9 @@ TEST(tf, Exceptions) //forward list EXPECT_FALSE(mTR.canTransform("parent", "me", builtin_interfaces::msg::Time().fromNSec(350000))); - try + try { - mTR.transformPose("me", Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(350000) , "parent"), outpose); + mTR.transformPose("me", Stamped (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(350000) , "parent"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) @@ -2010,7 +2087,7 @@ TEST(tf, Exceptions) printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } - + @@ -2021,21 +2098,21 @@ TEST(tf, Exceptions) TEST(tf, NoExtrapolationExceptionFromParent) { tf::Transformer mTR(true, tf2::Duration().fromNSec(1000000)); - - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "a")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent", "a")); - - - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "b")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent", "b")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent's parent", "parent")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent's parent's parent", "parent's parent")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "a")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent", "a")); + - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent's parent", "parent")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent's parent's parent", "parent's parent")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "b")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent", "b")); + + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent's parent", "parent")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent's parent's parent", "parent's parent")); + + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent's parent", "parent")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(10000), "parent's parent's parent", "parent's parent")); Stamped output; @@ -2057,12 +2134,12 @@ TEST(tf, NoExtrapolationExceptionFromParent) TEST(tf, ExtrapolationFromOneValue) { tf::Transformer mTR(true, tf2::Duration().fromNSec(1000000)); - - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "a")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent's parent", "parent")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "a")); + + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent's parent", "parent")); Stamped output; @@ -2077,7 +2154,7 @@ TEST(tf, ExtrapolationFromOneValue) { excepted = true; } - + EXPECT_TRUE(excepted); excepted = false; @@ -2090,7 +2167,7 @@ TEST(tf, ExtrapolationFromOneValue) { excepted = true; } - + EXPECT_TRUE(excepted); //Past multi link @@ -2103,7 +2180,7 @@ TEST(tf, ExtrapolationFromOneValue) { excepted = true; } - + EXPECT_TRUE(excepted); //Future case multi link @@ -2116,10 +2193,10 @@ TEST(tf, ExtrapolationFromOneValue) { excepted = true; } - + EXPECT_TRUE(excepted); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(20000), "parent", "a")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(20000), "parent", "a")); excepted = false; try @@ -2130,7 +2207,7 @@ TEST(tf, ExtrapolationFromOneValue) { excepted = true; } - + EXPECT_FALSE(excepted); }; @@ -2140,9 +2217,9 @@ TEST(tf, ExtrapolationFromOneValue) TEST(tf, getLatestCommonTime) { tf::Transformer mTR(true); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "a")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(2000), "parent's parent", "parent")); - + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(1000), "parent", "a")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(2000), "parent's parent", "parent")); + //simple case builtin_interfaces::msg::Time t; mTR.getLatestCommonTime("a", "parent's parent", t, NULL); @@ -2153,15 +2230,15 @@ TEST(tf, getLatestCommonTime) EXPECT_EQ(t, builtin_interfaces::msg::Time()); //testing with update - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(3000), "parent", "a")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(3000), "parent", "a")); mTR.getLatestCommonTime("a", "parent's parent",t, NULL); EXPECT_EQ(t, builtin_interfaces::msg::Time().fromNSec(2000)); //longer chain - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "parent", "b")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(3000), "b", "c")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(9000), "c", "d")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(5000), "f", "e")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "parent", "b")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(3000), "b", "c")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(9000), "c", "d")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(5000), "f", "e")); //shared parent mTR.getLatestCommonTime("a", "b",t, NULL); @@ -2221,8 +2298,8 @@ TEST(tf, getLatestCommonTime) TEST(tf, RepeatedTimes) { Transformer mTR; - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "parent", "b")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "parent", "b")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "parent", "b")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,1,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "parent", "b")); tf::StampedTransform output; try{ @@ -2239,7 +2316,7 @@ TEST(tf, RepeatedTimes) { EXPECT_FALSE("Excetion improperly thrown"); } - + } @@ -2259,7 +2336,7 @@ TEST(tf, frameExists) EXPECT_FALSE(mTR.frameExists("other")); EXPECT_FALSE(mTR.frameExists("frame")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/parent", "/b")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/parent", "/b")); // test with fully qualified name EXPECT_TRUE(mTR.frameExists("/b")); @@ -2273,14 +2350,14 @@ TEST(tf, frameExists) EXPECT_FALSE(mTR.frameExists("other")); EXPECT_FALSE(mTR.frameExists("frame")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/frame", "/other")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,1,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/frame", "/other")); // test with fully qualified name EXPECT_TRUE(mTR.frameExists("/b")); EXPECT_TRUE(mTR.frameExists("/parent")); EXPECT_TRUE(mTR.frameExists("/other")); EXPECT_TRUE(mTR.frameExists("/frame")); - + //Test with resolveping EXPECT_TRUE(mTR.frameExists("b")); EXPECT_TRUE(mTR.frameExists("parent")); @@ -2315,8 +2392,8 @@ TEST(tf, canTransform) //Create a two link tree between times 10 and 20 for (int i = 10; i < 20; i++) { - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "child")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "other_child")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "child")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "other_child")); } // four different timestamps related to tf state @@ -2406,8 +2483,8 @@ TEST(tf, lookupTransform) //Create a two link tree between times 10 and 20 for (int i = 10; i < 20; i++) { - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "child")); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "other_child")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "child")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(1,0,0), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromSec(i), "parent", "other_child")); } // four different timestamps related to tf state @@ -2434,7 +2511,7 @@ TEST(tf, lookupTransform) //Valid data should pass mTR.lookupTransform("child", "parent", valid_time, output); mTR.lookupTransform("child", "other_child", valid_time, output); - + //zero data should pass mTR.lookupTransform("child", "parent", zero_time, output); mTR.lookupTransform("child", "other_child", zero_time, output); @@ -2463,16 +2540,16 @@ TEST(tf, lookupTransform) { EXPECT_TRUE("Exception Thrown Correctly"); } - + try { //Same Frame should pass for all times mTR.lookupTransform("child", "child", zero_time, output); mTR.lookupTransform("child", "child", old_time, output); mTR.lookupTransform("child", "child", valid_time, output); mTR.lookupTransform("child", "child", future_time, output); - + // Advanced API Tests - + // Source = Fixed //zero data in fixed frame should pass mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output); @@ -2560,7 +2637,7 @@ TEST(tf, lookupTransform) printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception improperly thrown"); } - + //make sure zero goes to now for zero length try @@ -2576,7 +2653,7 @@ TEST(tf, lookupTransform) printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception improperly thrown"); } - + } @@ -2585,7 +2662,7 @@ TEST(tf, getFrameStrings) Transformer mTR; - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/parent", "/b")); + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/parent", "/b")); std::vector frames_string; mTR.getFrameStrings(frames_string); ASSERT_EQ(frames_string.size(), (unsigned)2); @@ -2593,8 +2670,8 @@ TEST(tf, getFrameStrings) EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str()); - mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/frame", "/other")); - + mTR.setTransform( StampedTransform (btTransform(tf2::Quaternion(0,0,0,1), btVector3(0,0,0)), builtin_interfaces::msg::Time().fromNSec(4000), "/frame", "/other")); + mTR.getFrameStrings(frames_string); ASSERT_EQ(frames_string.size(), (unsigned)4); EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str()); @@ -2612,7 +2689,7 @@ bool expectInvalidQuaternion(tf::Quaternion q) printf("this should have thrown\n"); return false; } - catch (tf::InvalidArgument &ex) + catch (tf::InvalidArgument &ex) { return true; } @@ -2630,14 +2707,14 @@ bool expectValidQuaternion(tf::Quaternion q) { tf::assertQuaternionValid(q); } - catch (tf::TransformException &ex) + catch (tf::TransformException &ex) { return false; } return true; } -bool expectInvalidQuaternion(geometry_msgs::Quaternion q) +bool expectInvalidQuaternion(geometry_msgs::msg::Quaternion q) { try { @@ -2645,7 +2722,7 @@ bool expectInvalidQuaternion(geometry_msgs::Quaternion q) printf("this should have thrown\n"); return false; } - catch (tf::InvalidArgument &ex) + catch (tf::InvalidArgument &ex) { return true; } @@ -2657,13 +2734,13 @@ bool expectInvalidQuaternion(geometry_msgs::Quaternion q) return false; } -bool expectValidQuaternion(geometry_msgs::Quaternion q) +bool expectValidQuaternion(geometry_msgs::msg::Quaternion q) { try { tf::assertQuaternionValid(q); } - catch (tf::TransformException &ex) + catch (tf::TransformException &ex) { return false; } @@ -2705,11 +2782,11 @@ TEST(tf, assertQuaternionValid) //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument); //q.setY(1); //EXPECT_NO_THROW(tf::assertQuaternionValid(q)); - + } TEST(tf, assertQuaternionMsgValid) { - geometry_msgs::Quaternion q; + geometry_msgs::msg::Quaternion q; q.x = 1;//others zeroed to start EXPECT_TRUE(expectValidQuaternion(q)); @@ -2809,7 +2886,7 @@ TEST(tf2_stamped, OperatorEqual) */ int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - builtin_interfaces::msg::Time::init(); //needed for builtin_interfaces::msg::Time::now() - ros::init(argc, argv, "tf_unittest"); + // builtin_interfaces::msg::Time::init(); //needed for builtin_interfaces::msg::Time::now() + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp new file mode 100644 index 000000000..6e28c3641 --- /dev/null +++ b/test_tf2/test/permuter.hpp @@ -0,0 +1,169 @@ +/* + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +/** \author Tully Foote */ + +#ifndef ROSTEST_PERMUTER_H +#define ROSTEST_PERMUTER_H + +#include +#include + +namespace rostest +{ +/** \brief A base class for storing pointers to generic data types + */ +class PermuteOptionBase +{ +public: + virtual void reset() =0; + virtual bool step() =0; + virtual ~PermuteOptionBase() {}; +}; + + +/**\brief A class to hold a set of option values and currently used state + * This class holds + */ +template +class PermuteOption : public PermuteOptionBase +{ +public: + PermuteOption(const std::vector& options, T* output) + { + options_ = options; + output_ = output; + reset(); + } + + virtual ~PermuteOption(){}; + + void reset(){ + std::lock_guard lock(access_mutex_); + current_element_ = options_.begin(); + *output_ = *current_element_; + }; + + bool step() + { + std::lock_guard lock(access_mutex_); + current_element_++; + if (current_element_ == options_.end()) + return false; + *output_ = *current_element_; + return true; + }; + +private: + /// Local storage of the possible values + std::vector options_; + /// The output variable + T* output_; + typedef typename std::vector::iterator V_T_iterator; + /// The last updated element + V_T_iterator current_element_; + + std::mutex access_mutex_; + +}; + +/** \brief A class to provide easy permutation of options + * This class provides a way to collapse independent + * permutations of options into a single loop. + */ +class Permuter +{ +public: + /** \brief Destructor to clean up allocated data */ + virtual ~Permuter(){ clearAll();}; + + + /** \brief Add a set of values and an output to the iteration + * @param values The set of possible values for this output + * @param output The value to set at each iteration + */ + template + void addOptionSet(const std::vector& values, T* output) + { + std::lock_guard lock(access_mutex_); + options_.push_back(static_cast (new PermuteOption(values, output))); + reset(); + }; + + + /** \brief Reset the internal counters */ + void reset(){ + for (unsigned int level= 0; level < options_.size(); level++) + options_[level]->reset(); + }; + + /** \brief Iterate to the next value in the iteration + * Returns true unless done iterating. + */ + bool step() + { + std::lock_guard lock(access_mutex_); + // base case just iterating + for (unsigned int level= 0; level < options_.size(); level++) + { + if(options_[level]->step()) + { + //printf("stepping level %d returning true \n", level); + return true; + } + else + { + //printf("reseting level %d\n", level); + options_[level]->reset(); + } + } + return false; + }; + + /** \brief Clear all stored data */ + void clearAll() + { + std::lock_guard lock(access_mutex_); + for ( unsigned int i = 0 ; i < options_.size(); i++) + { + delete options_[i]; + } + options_.clear(); + }; + +private: + std::vector options_; ///< Store all the option objects + std::mutex access_mutex_; +}; + + +} + +#endif //ROSTEST_PERMUTER_H From e70e2c75ce1b96afbcf079b8f174ae0baba9d791 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 4 Dec 2019 17:42:16 +0100 Subject: [PATCH 02/44] test_tf2 port test_utils --- test_tf2/CMakeLists.txt | 12 +++++++++--- test_tf2/test/test_utils.cpp | 19 +++++++++---------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index d2ab941c4..993056966 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_kdl REQUIRED) ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) @@ -29,9 +30,14 @@ endif() # catkin_add_gtest(test_convert test/test_convert.cpp) # target_link_libraries(test_convert ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) # -# catkin_add_gtest(test_utils test/test_utils.cpp) -# target_link_libraries(test_utils ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) -# +ament_add_gtest(test_utils test/test_utils.cpp) +if(TARGET test_utils) + ament_target_dependencies(test_utils + tf2 + tf2_kdl + ) +endif() + # add_executable(test_buffer_server EXCLUDE_FROM_ALL test/test_buffer_server.cpp) # target_link_libraries(test_buffer_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) # diff --git a/test_tf2/test/test_utils.cpp b/test_tf2/test/test_utils.cpp index 144a4522d..03208cfb7 100644 --- a/test_tf2/test/test_utils.cpp +++ b/test_tf2/test/test_utils.cpp @@ -14,9 +14,9 @@ #include #include +#include #include #include -#include double epsilon = 1e-9; @@ -24,7 +24,7 @@ template void yprTest(const T& t, double yaw1, double pitch1, double roll1) { double yaw2, pitch2, roll2; - tf2::getEulerYPR(t, yaw2, pitch2, roll2); + tf2::getEulerYPR(t, yaw2, pitch2, roll2); EXPECT_NEAR(yaw1, yaw2, epsilon); EXPECT_NEAR(pitch1, pitch2, epsilon); @@ -44,18 +44,18 @@ TEST(tf2Utils, yaw) // Compute results one way with KDL KDL::Rotation::Quaternion(x, y, z, w).GetRPY(roll1, pitch1, yaw1); { - // geometry_msgs::Quaternion - geometry_msgs::Quaternion q; + // geometry_msgs::msg::Quaternion + geometry_msgs::msg::Quaternion q; q.x = x; q.y =y; q.z = z; q.w = w; yprTest(q, yaw1, pitch1, roll1); - // geometry_msgs::QuaternionStamped - geometry_msgs::QuaternionStamped qst; + // geometry_msgs::msg::QuaternionStamped + geometry_msgs::msg::QuaternionStamped qst; qst.quaternion = q; yprTest(qst, yaw1, pitch1, roll1); } - + { // tf2::Quaternion tf2::Quaternion q(x, y, z, w); @@ -70,7 +70,7 @@ TEST(tf2Utils, yaw) TEST(tf2Utils, identity) { - geometry_msgs::Transform t; + geometry_msgs::msg::Transform t; t.translation.x = 0.1; t.translation.y = 0.2; t.translation.z = 0.3; @@ -80,7 +80,7 @@ TEST(tf2Utils, identity) t.rotation.w = 0.7; // Test identity - t = tf2::getTransformIdentity(); + t = tf2::getTransformIdentity(); EXPECT_EQ(t.translation.x, 0); EXPECT_EQ(t.translation.y, 0); @@ -96,4 +96,3 @@ int main(int argc, char** argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - From 70186f63cfb127b10849a44c80ee315c3c76763b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 4 Dec 2019 19:35:12 +0100 Subject: [PATCH 03/44] test_tf2 ported test_message_filter --- test_tf2/CMakeLists.txt | 18 +- test_tf2/test/test_message_filter.cpp | 319 +++++++++++++++----------- 2 files changed, 188 insertions(+), 149 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 993056966..aa19ae70b 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -20,16 +20,16 @@ if(TARGET buffer_core_test) ) endif() -# ament_add_gtest_executable(buffer_core_test test/buffer_core_test.cpp) -# catkin_add_gtest(buffer_core_test test/buffer_core_test.cpp) -# target_link_libraries(buffer_core_test ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) +ament_add_gtest(test_message_filter test/test_message_filter.cpp) +if(TARGET test_message_filter) + ament_target_dependencies(test_message_filter + tf2 + tf2_ros + rclcpp + geometry_msgs + ) +endif() -# catkin_add_gtest(test_tf2_message_filter test/test_message_filter.cpp) -# target_link_libraries(test_tf2_message_filter ${Boost_LIBRARIES} ${catkin_LIBRARIES}) -# -# catkin_add_gtest(test_convert test/test_convert.cpp) -# target_link_libraries(test_convert ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) -# ament_add_gtest(test_utils test/test_utils.cpp) if(TARGET test_utils) ament_target_dependencies(test_utils diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 250a9426a..97be05d01 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -32,17 +32,12 @@ #include #include -#include -#include +#include -#include "ros/ros.h" -#include "ros/callback_queue.h" +#include #include -using namespace tf2; -using namespace tf2_ros; - class Notification { public: @@ -51,12 +46,12 @@ class Notification { } - void notify(const geometry_msgs::PointStamped::ConstPtr& message) + void notify(const geometry_msgs::msg::PointStamped::ConstPtr& message) { ++count_; } - void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason) + void failure(const geometry_msgs::msg::PointStamped::ConstPtr& message, tf2_ros::filter_failure_reasons::FilterFailureReason reason) { ++failure_count_; } @@ -68,37 +63,47 @@ class Notification TEST(MessageFilter, noTransforms) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); - msg->header.stamp = builtin_interfaces::msg::Time(1); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); + + rclcpp::shutdown(); } TEST(MessageFilter, noTransformsSameFrame) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); - msg->header.stamp = builtin_interfaces::msg::Time(1); + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); msg->header.frame_id = "frame1"; filter.add(msg); EXPECT_EQ(1, n.count_); + rclcpp::shutdown(); } -geometry_msgs::TransformStamped createTransform(Quaternion q, Vector3 v, builtin_interfaces::msg::Time stamp, const std::string& frame1, const std::string& frame2) +geometry_msgs::msg::TransformStamped createTransform(tf2::Quaternion q, tf2::Vector3 v, builtin_interfaces::msg::Time stamp, const std::string& frame1, const std::string& frame2) { - geometry_msgs::TransformStamped t; + geometry_msgs::msg::TransformStamped t; t.header.frame_id = frame1; t.child_frame_id = frame2; t.header.stamp = stamp; @@ -114,33 +119,42 @@ geometry_msgs::TransformStamped createTransform(Quaternion q, Vector3 v, builtin TEST(MessageFilter, preexistingTransforms) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); - builtin_interfaces::msg::Time stamp(1); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); + builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); + + std::shared_ptr msg = std::make_shared(); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); + rclcpp::shutdown(); } TEST(MessageFilter, postTransforms) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); - builtin_interfaces::msg::Time stamp(1); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); + builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); + + std::shared_ptr msg = std::make_shared(); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; @@ -148,24 +162,29 @@ TEST(MessageFilter, postTransforms) EXPECT_EQ(0, n.count_); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); + rclcpp::shutdown(); } TEST(MessageFilter, queueSize) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(10); - MessageFilter filter(bc, "frame1", 10, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); - filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); - builtin_interfaces::msg::Time stamp(1); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); + // filter.registerFailureCallback(std::bind(&Notification::failure, &n, std::placeholders::_1, std::placeholders::_2)); + + builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); for (int i = 0; i < 20; ++i) { - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); + std::shared_ptr msg = std::make_shared(); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; @@ -175,48 +194,56 @@ TEST(MessageFilter, queueSize) EXPECT_EQ(0, n.count_); EXPECT_EQ(10, n.failure_count_); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(10, n.count_); + rclcpp::shutdown(); } TEST(MessageFilter, setTargetFrame) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); filter.setTargetFrame("frame1000"); - builtin_interfaces::msg::Time stamp(1); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1000", "frame2"), "me"); + builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1000", "frame2"), "me"); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); + std::shared_ptr msg = std::make_shared(); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); + rclcpp::shutdown(); } TEST(MessageFilter, multipleTargetFrames) { - BufferCore bc; + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "", 10, node); Notification n(1); - MessageFilter filter(bc, "", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); std::vector target_frames; target_frames.push_back("frame1"); target_frames.push_back("frame2"); filter.setTargetFrames(target_frames); - builtin_interfaces::msg::Time stamp(1); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame3"), "me"); + builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame3"), "me"); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); + std::shared_ptr msg = std::make_shared(); msg->header.stamp = stamp; msg->header.frame_id = "frame3"; filter.add(msg); @@ -225,121 +252,133 @@ TEST(MessageFilter, multipleTargetFrames) //builtin_interfaces::msg::Time::setNow(builtin_interfaces::msg::Time::now() + tf2::durationFromSec(1.0)); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists + rclcpp::shutdown(); } TEST(MessageFilter, tolerance) { - tf2::Duration offset(0.2); - BufferCore bc; - Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); - filter.setTolerance(offset); - - builtin_interfaces::msg::Time stamp(1); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); - msg->header.stamp = stamp; - msg->header.frame_id = "frame2"; - filter.add(msg); - - EXPECT_EQ(0, n.count_); //No return due to lack of space for offset - - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + (offset * 1.1), "frame1", "frame2"), "me"); - - EXPECT_EQ(1, n.count_); // Now have data for the message published earlier - - msg->header.stamp = stamp + offset; - filter.add(msg); - - EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset -} - -TEST(MessageFilter, outTheBackFailure) -{ - BufferCore bc; + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); + filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); - builtin_interfaces::msg::Time stamp(1); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + tf2::durationFromSec(10000), "frame1", "frame2"), "me"); - - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); - msg->header.stamp = stamp; - msg->header.frame_id = "frame2"; - filter.add(msg); - - EXPECT_EQ(1, n.failure_count_); -} - -TEST(MessageFilter, outTheBackFailure2) -{ - BufferCore bc; - Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); + tf2::Duration offset = tf2::durationFromSec(0.2); + filter.setTolerance(offset); - builtin_interfaces::msg::Time stamp(1); + builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); + std::shared_ptr msg = std::make_shared(); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); - EXPECT_EQ(0, n.count_); - EXPECT_EQ(0, n.failure_count_); - - bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + tf2::durationFromSec(10000), "frame1", "frame2"), "me"); + EXPECT_EQ(0, n.count_); //No return due to lack of space for offset - EXPECT_EQ(1, n.failure_count_); -} + double time_stamp = (stamp.sec + stamp.nanosec/1e9)*1.1 + tf2::durationToSec(offset); + builtin_interfaces::msg::Time stamp_transform = tf2_ros::toMsg(tf2::timeFromSec(time_stamp)); -TEST(MessageFilter, emptyFrameIDFailure) -{ - BufferCore bc; - Notification n(1); - MessageFilter filter(bc, "frame1", 1, 0); - filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); + buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp_transform, "frame1", "frame2"), "me"); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); - msg->header.frame_id = ""; - filter.add(msg); - - EXPECT_EQ(1, n.failure_count_); -} + EXPECT_EQ(1, n.count_); // Now have data for the message published earlier -TEST(MessageFilter, callbackQueue) -{ - BufferCore bc; - Notification n(1); - ros::CallbackQueue queue; - MessageFilter filter(bc, "frame1", 1, &queue); - filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); + time_stamp = (stamp.sec + stamp.nanosec/1e9) + tf2::durationToSec(offset); - geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); - msg->header.stamp = builtin_interfaces::msg::Time(1); - msg->header.frame_id = "frame1"; + msg->header.stamp.sec = (int)time_stamp; + msg->header.stamp.nanosec = (time_stamp - (int)time_stamp)/1e9; filter.add(msg); - EXPECT_EQ(0, n.count_); - - queue.callAvailable(); - - EXPECT_EQ(1, n.count_); + EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset + rclcpp::shutdown(); } - +// TODO(ahcorde): failurecallback +// TEST(MessageFilter, outTheBackFailure) +// { +// BufferCore bc; +// Notification n(1); +// MessageFilter filter(bc, "frame1", 1, 0); +// filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); +// +// builtin_interfaces::msg::Time stamp(1); +// bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); +// bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + tf2::durationFromSec(10000), "frame1", "frame2"), "me"); +// +// geometry_msgs::msg::PointStampedPtr msg(new geometry_msgs::msg::PointStamped); +// msg->header.stamp = stamp; +// msg->header.frame_id = "frame2"; +// filter.add(msg); +// +// EXPECT_EQ(1, n.failure_count_); +// } +// +// TEST(MessageFilter, outTheBackFailure2) +// { +// BufferCore bc; +// Notification n(1); +// MessageFilter filter(bc, "frame1", 1, 0); +// filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); +// +// builtin_interfaces::msg::Time stamp(1); +// +// geometry_msgs::msg::PointStampedPtr msg(new geometry_msgs::msg::PointStamped); +// msg->header.stamp = stamp; +// msg->header.frame_id = "frame2"; +// filter.add(msg); +// +// EXPECT_EQ(0, n.count_); +// EXPECT_EQ(0, n.failure_count_); +// +// bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + tf2::durationFromSec(10000), "frame1", "frame2"), "me"); +// +// EXPECT_EQ(1, n.failure_count_); +// } +// +// TEST(MessageFilter, emptyFrameIDFailure) +// { +// BufferCore bc; +// Notification n(1); +// MessageFilter filter(bc, "frame1", 1, 0); +// filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); +// +// geometry_msgs::msg::PointStampedPtr msg(new geometry_msgs::msg::PointStamped); +// msg->header.frame_id = ""; +// filter.add(msg); +// +// EXPECT_EQ(1, n.failure_count_); +// } +// +// TEST(MessageFilter, callbackQueue) +// { +// BufferCore bc; +// Notification n(1); +// ros::CallbackQueue queue; +// MessageFilter filter(bc, "frame1", 1, &queue); +// filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); +// +// geometry_msgs::msg::PointStampedPtr msg(new geometry_msgs::msg::PointStamped); +// msg->header.stamp = builtin_interfaces::msg::Time(1); +// msg->header.frame_id = "frame1"; +// filter.add(msg); +// +// EXPECT_EQ(0, n.count_); +// +// queue.callAvailable(); +// +// EXPECT_EQ(1, n.count_); +// } +// +// int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - + rclcpp::init(argc, argv); int ret = RUN_ALL_TESTS(); - return ret; } From f7412018e32cf4d23708242b9a76cf20b3353cc4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 5 Dec 2019 09:34:13 +0100 Subject: [PATCH 04/44] test_tf2 ported test_buffer_server.cpp --- test_tf2/CMakeLists.txt | 11 ++++++++--- test_tf2/test/test_buffer_server.cpp | 20 +++++++++++--------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index aa19ae70b..3b7187f25 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -9,6 +9,7 @@ project(test_tf2) find_package(ament_cmake REQUIRED) find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_kdl REQUIRED) @@ -38,9 +39,13 @@ if(TARGET test_utils) ) endif() -# add_executable(test_buffer_server EXCLUDE_FROM_ALL test/test_buffer_server.cpp) -# target_link_libraries(test_buffer_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) -# +add_executable(test_buffer_server test/test_buffer_server.cpp) +ament_target_dependencies(test_buffer_server + rclcpp + rclcpp_action + tf2_ros + ) + # add_executable(test_buffer_client EXCLUDE_FROM_ALL test/test_buffer_client.cpp) # target_link_libraries(test_buffer_client ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) # diff --git a/test_tf2/test/test_buffer_server.cpp b/test_tf2/test/test_buffer_server.cpp index ebd988bd0..26c17c9c8 100644 --- a/test_tf2/test/test_buffer_server.cpp +++ b/test_tf2/test/test_buffer_server.cpp @@ -34,19 +34,21 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#include #include +#include #include -#include +#include int main(int argc, char** argv) { - ros::init(argc, argv, "buffer_server_test"); - tf2_ros::Buffer buffer; - tf2_ros::TransformListener tfl(buffer); - tf2_ros::BufferServer server(buffer, "tf_action", false); + rclcpp::init(argc, argv); - server.start(); - ros::spin(); -} + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, node, false); + std::unique_ptr server = std::make_unique(buffer, node, "tf_action"); + + rclcpp::spin(node); +} From dd68d80457f5f7e32ae6e4007164df875eaf6c0c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 09:41:46 +0100 Subject: [PATCH 05/44] test_tf2 ported test_buffer_client --- test_tf2/CMakeLists.txt | 19 +++++---- test_tf2/test/test_buffer_client.cpp | 59 +++++++++++++++++++--------- 2 files changed, 52 insertions(+), 26 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 3b7187f25..98f6c5735 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -12,6 +12,9 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_kdl REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_bullet REQUIRED) ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) @@ -39,17 +42,19 @@ if(TARGET test_utils) ) endif() -add_executable(test_buffer_server test/test_buffer_server.cpp) -ament_target_dependencies(test_buffer_server +find_package(PkgConfig REQUIRED) +pkg_check_modules(bullet REQUIRED bullet) +include_directories(include ${bullet_INCLUDE_DIRS}) + +ament_add_gtest(test_buffer_client test/test_buffer_client.cpp) +ament_target_dependencies(test_buffer_client rclcpp rclcpp_action tf2_ros - ) + tf2_kdl + tf2_bullet +) -# add_executable(test_buffer_client EXCLUDE_FROM_ALL test/test_buffer_client.cpp) -# target_link_libraries(test_buffer_client ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) -# -# # add_rostest(test/buffer_client_tester.launch) # # add_executable(test_static_publisher EXCLUDE_FROM_ALL test/test_static_publisher.cpp) diff --git a/test_tf2/test/test_buffer_client.cpp b/test_tf2/test/test_buffer_client.cpp index 16d267858..97a6f81a3 100644 --- a/test_tf2/test/test_buffer_client.cpp +++ b/test_tf2/test/test_buffer_client.cpp @@ -36,21 +36,31 @@ *********************************************************************/ #include #include -#include +#include #include #include #include +#include static const double EPS = 1e-3; TEST(tf2_ros, buffer_client) { - tf2_ros::BufferClient client("tf_action"); + rclcpp::Node::SharedPtr node = std::make_shared("tf_action_node"); + std::unique_ptr client = std::make_unique(node, "tf_action"); + + rclcpp::executors::SingleThreadedExecutor executor; + + executor.add_node(node); + + // Start spinning in a thread + std::thread spin_thread = std::thread( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); //make sure that things are set up - ASSERT_TRUE(client.waitForServer(tf2::durationFromSec(4.0))); + ASSERT_TRUE(client->waitForServer(std::chrono::seconds(4))); - geometry_msgs::PointStamped p1; + geometry_msgs::msg::PointStamped p1; p1.header.frame_id = "a"; p1.header.stamp = builtin_interfaces::msg::Time(); p1.point.x = 0.0; @@ -59,9 +69,10 @@ TEST(tf2_ros, buffer_client) try { - geometry_msgs::PointStamped p2 = client.transform(p1, "b"); - ROS_INFO("p1: (%.2f, %.2f, %.2f), p2: (%.2f, %.2f, %.2f)", p1.point.x, - p1.point.y, p1.point.z, p2.point.x, p2.point.y, p2.point.z); + geometry_msgs::msg::PointStamped p2 = client->transform(p1, "b"); + RCLCPP_INFO(node->get_logger(), + "p1: (%.2f, %.2f, %.2f), p2: (%.2f, %.2f, %.2f)", p1.point.x, + p1.point.y, p1.point.z, p2.point.x, p2.point.y, p2.point.z); EXPECT_NEAR(p2.point.x, -5.0, EPS); EXPECT_NEAR(p2.point.y, -6.0, EPS); @@ -69,26 +80,36 @@ TEST(tf2_ros, buffer_client) } catch(tf2::TransformException& ex) { - ROS_ERROR("Failed to transform: %s", ex.what()); + RCLCPP_ERROR(node->get_logger(), "Failed to transform: %s", ex.what()); ASSERT_FALSE("Should not get here"); } -} + rclcpp::shutdown(); +} TEST(tf2_ros, buffer_client_different_types) { - tf2_ros::BufferClient client("tf_action"); + rclcpp::Node::SharedPtr node = std::make_shared("tf_action_node"); + std::unique_ptr client = std::make_unique(node, "tf_action"); + + rclcpp::executors::SingleThreadedExecutor executor; + + executor.add_node(node); + + // Start spinning in a thread + std::thread spin_thread = std::thread( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); //make sure that things are set up - ASSERT_TRUE(client.waitForServer(tf2::durationFromSec(4.0))); + ASSERT_TRUE(client->waitForServer(std::chrono::seconds(4))); - tf2::Stamped k1(KDL::Vector(0, 0, 0), builtin_interfaces::msg::Time(), "a"); + tf2::Stamped k1(KDL::Vector(0, 0, 0), tf2::TimePoint(), "a"); try { tf2::Stamped b1; - client.transform(k1, b1, "b"); - ROS_INFO_STREAM("Bullet: (" << b1[0] << ", " << b1[1] << ", " << b1[2] << ")"); - ROS_INFO_STREAM("KDL: (" << k1[0] << ", " << k1[1] << ", " << k1[2] << ")"); + client->transform(k1, b1, "b"); + RCLCPP_ERROR(node->get_logger(), "Bullet: (%.4f, %.4f, %.4f)", b1[0], b1[1], b1[2]); + RCLCPP_ERROR(node->get_logger(), "KDL: (%.4f, %.4f, %.4f)", k1[0], k1[1], k1[2]); EXPECT_NEAR(b1[0], -5.0, EPS); EXPECT_NEAR(b1[1], -6.0, EPS); EXPECT_NEAR(b1[2], -7.0, EPS); @@ -97,15 +118,15 @@ TEST(tf2_ros, buffer_client_different_types) } catch(tf2::TransformException& ex) { - ROS_ERROR("Failed to transform: %s", ex.what()); + RCLCPP_ERROR(node->get_logger(), "Failed to transform: %s", ex.what()); ASSERT_FALSE("Should not get here"); } -} + rclcpp::shutdown(); +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "buffer_client_test"); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } - From 6a8dc8eace0f3d426cf1a03c71eb8d5ff7545cf5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 09:56:23 +0100 Subject: [PATCH 06/44] test_tf2 ported test_statis_publisher --- test_tf2/test/test_static_publisher.cpp | 113 +++++++++++++++--------- 1 file changed, 71 insertions(+), 42 deletions(-) diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index e0fb0ea05..b3c393c66 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -31,72 +31,93 @@ #include #include "tf2/exceptions.h" #include -#include -#include "rostest/permuter.h" +#include +#include "permuter.hpp" #include "tf2_ros/transform_listener.h" TEST(StaticTransformPublisher, a_b_different_times) { - tf2_ros::Buffer mB; - tf2_ros::TransformListener tfl(mB); - EXPECT_TRUE(mB.canTransform("a", "b", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "b", builtin_interfaces::msg::Time(100), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "b", builtin_interfaces::msg::Time(1000), tf2::durationFromSec(1.0))); + auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_b_different_times_test"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer mB(clock); + tf2_ros::TransformListener tfl(mB, node, false); + + EXPECT_TRUE(mB.canTransform("a", "b", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "b", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "b", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); + + rclcpp::shutdown(); }; TEST(StaticTransformPublisher, a_c_different_times) { - tf2_ros::Buffer mB; - tf2_ros::TransformListener tfl(mB); - EXPECT_TRUE(mB.canTransform("a", "c", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", builtin_interfaces::msg::Time(100), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", builtin_interfaces::msg::Time(1000), tf2::durationFromSec(1.0))); + auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_c_different_times_test"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer mB(clock); + tf2_ros::TransformListener tfl(mB, node, false); + + EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); + + rclcpp::shutdown(); }; TEST(StaticTransformPublisher, a_d_different_times) { - tf2_ros::Buffer mB; - tf2_ros::TransformListener tfl(mB); - geometry_msgs::TransformStamped ts; + auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_d_different_times_test"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer mB(clock); + tf2_ros::TransformListener tfl(mB, node, false); + + geometry_msgs::msg::TransformStamped ts; ts.transform.rotation.w = 1; ts.header.frame_id = "c"; - ts.header.stamp = builtin_interfaces::msg::Time(10.0); + ts.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(10)); ts.child_frame_id = "d"; // make sure listener has populated - EXPECT_TRUE(mB.canTransform("a", "c", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", builtin_interfaces::msg::Time(100), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", builtin_interfaces::msg::Time(1000), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); mB.setTransform(ts, "authority"); //printf("%s\n", mB.allFramesAsString().c_str()); - EXPECT_TRUE(mB.canTransform("c", "d", builtin_interfaces::msg::Time(10), tf2::durationFromSec(0))); + EXPECT_TRUE(mB.canTransform("c", "d", tf2_ros::toMsg(tf2::timeFromSec(10)), tf2::durationFromSec(0))); - EXPECT_TRUE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(), tf2::durationFromSec(0))); - EXPECT_FALSE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(1), tf2::durationFromSec(0))); - EXPECT_TRUE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(10), tf2::durationFromSec(0))); - EXPECT_FALSE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(100), tf2::durationFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(0))); + EXPECT_FALSE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(1)), tf2::durationFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(10)), tf2::durationFromSec(0))); + EXPECT_FALSE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(0))); + rclcpp::shutdown(); }; TEST(StaticTransformPublisher, multiple_parent_test) { - tf2_ros::Buffer mB; - tf2_ros::TransformListener tfl(mB); - tf2_ros::StaticTransformBroadcaster stb; - geometry_msgs::TransformStamped ts; + auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_d_different_times_test"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer mB(clock); + tf2_ros::TransformListener tfl(mB, node, false); + + tf2_ros::StaticTransformBroadcaster stb(node); + geometry_msgs::msg::TransformStamped ts; ts.transform.rotation.w = 1; ts.header.frame_id = "c"; - ts.header.stamp = builtin_interfaces::msg::Time(10.0); + ts.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(10)); ts.child_frame_id = "d"; stb.sendTransform(ts); // make sure listener has populated - EXPECT_TRUE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(100), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(1000), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); // Publish new transform with child 'd', should replace old one in static tf ts.header.frame_id = "new_parent"; @@ -106,24 +127,32 @@ TEST(StaticTransformPublisher, multiple_parent_test) ts.child_frame_id = "other_child2"; stb.sendTransform(ts); - EXPECT_TRUE(mB.canTransform("new_parent", "d", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("new_parent", "other_child", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("new_parent", "other_child2", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); - EXPECT_FALSE(mB.canTransform("a", "d", builtin_interfaces::msg::Time(), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("new_parent", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("new_parent", "other_child", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("new_parent", "other_child2", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + EXPECT_FALSE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + + rclcpp::shutdown(); }; TEST(StaticTransformPublisher, tf_from_param_server_valid) { // This TF is loaded from the parameter server; ensure it is valid. - tf2_ros::Buffer mB; - tf2_ros::TransformListener tfl(mB); - EXPECT_TRUE(mB.canTransform("robot_calibration", "world", ros::Time(), ros::Duration(1.0))); - EXPECT_TRUE(mB.canTransform("robot_calibration", "world", ros::Time(100), ros::Duration(1.0))); - EXPECT_TRUE(mB.canTransform("robot_calibration", "world", ros::Time(1000), ros::Duration(1.0))); + auto node = rclcpp::Node::make_shared("StaticTransformPublisher_tf_from_param_server_valid_test"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer mB(clock); + tf2_ros::TransformListener tfl(mB, node, false); + + EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1))); + EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1))); + EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1))); + + rclcpp::shutdown(); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "tf_unittest"); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } From 86df753b3865f7e1e0d191ccc6a07bfe3b1b28ea Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 10:03:51 +0100 Subject: [PATCH 07/44] test_tf2 ported test_tf2_bullet --- test_tf2/test/test_tf2_bullet.cpp | 29 +++++++++++++---------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/test_tf2/test/test_tf2_bullet.cpp b/test_tf2/test/test_tf2_bullet.cpp index 62f20b741..81016a081 100644 --- a/test_tf2/test/test_tf2_bullet.cpp +++ b/test_tf2/test/test_tf2_bullet.cpp @@ -29,10 +29,9 @@ /** \author Wim Meeussen */ - #include #include -#include +#include #include #include @@ -41,7 +40,7 @@ static const double EPS = 1e-3; TEST(TfBullet, Transform) { - tf2::Stamped v1(btTransform(btQuaternion(1,0,0,0), btVector3(1,2,3)), builtin_interfaces::msg::Time(2.0), "A"); + tf2::Stamped v1(btTransform(btQuaternion(1,0,0,0), btVector3(1,2,3)), tf2::timeFromSec(2.0), "A"); // simple api btTransform v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); @@ -50,18 +49,16 @@ TEST(TfBullet, Transform) EXPECT_NEAR(v_simple.getOrigin().getZ(), 27, EPS); // advanced api - btTransform v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), + btTransform v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), "B", tf2::durationFromSec(3.0)); EXPECT_NEAR(v_advanced.getOrigin().getX(), -9, EPS); EXPECT_NEAR(v_advanced.getOrigin().getY(), 18, EPS); EXPECT_NEAR(v_advanced.getOrigin().getZ(), 27, EPS); } - - TEST(TfBullet, Vector) { - tf2::Stamped v1(btVector3(1,2,3), builtin_interfaces::msg::Time(2.0), "A"); + tf2::Stamped v1(btVector3(1,2,3), tf2::timeFromSec(2.0), "A"); // simple api btVector3 v_simple = tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); @@ -70,25 +67,24 @@ TEST(TfBullet, Vector) EXPECT_NEAR(v_simple.getZ(), 27, EPS); // advanced api - btVector3 v_advanced = tf_buffer->transform(v1, "B", builtin_interfaces::msg::Time(2.0), + btVector3 v_advanced = tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), "B", tf2::durationFromSec(3.0)); EXPECT_NEAR(v_advanced.getX(), -9, EPS); EXPECT_NEAR(v_advanced.getY(), 18, EPS); EXPECT_NEAR(v_advanced.getZ(), 27, EPS); } - - - int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test"); - ros::NodeHandle n; + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("test_tf2_bullet"); - tf_buffer = std::make_unique(); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf_buffer = std::make_unique(clock); // populate buffer - geometry_msgs::TransformStamped t; + geometry_msgs::msg::TransformStamped t; t.transform.translation.x = 10; t.transform.translation.y = 20; t.transform.translation.z = 30; @@ -96,11 +92,12 @@ int main(int argc, char **argv){ t.transform.rotation.x = 1; t.transform.rotation.y = 0; t.transform.rotation.z = 0; - t.header.stamp = builtin_interfaces::msg::Time(2.0); + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); return ret; } From 2b497a3e73c7d7ef771107b0d5da20a07c467aa5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 15:46:58 +0100 Subject: [PATCH 08/44] tf2_test ported python tests --- test_tf2/test/test_buffer_client.py | 102 +++++++++++++++++-------- test_tf2/test/test_convert.py | 29 ++++--- test_tf2/test/test_static_publisher.py | 25 +++--- 3 files changed, 95 insertions(+), 61 deletions(-) diff --git a/test_tf2/test/test_buffer_client.py b/test_tf2/test/test_buffer_client.py index f200d6bad..b1dabc19c 100755 --- a/test_tf2/test/test_buffer_client.py +++ b/test_tf2/test/test_buffer_client.py @@ -31,59 +31,99 @@ #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. -#* +#* #* Author: Eitan Marder-Eppstein #*********************************************************** -PKG = 'test_tf2' -import roslib; roslib.load_manifest(PKG) - import sys import unittest import tf2_py as tf2 import tf2_ros -import tf2_kdl -import tf2_geometry_msgs from geometry_msgs.msg import PointStamped -import rospy -import PyKDL +import rclpy +import tf2_geometry_msgs +# TODO (ahcorde): Enable once python_orocos_kdl is ported +# import tf2_kdl +# import PyKDL +from rclpy.executors import SingleThreadedExecutor +import threading class TestBufferClient(unittest.TestCase): - def test_buffer_client(self): - client = tf2_ros.BufferClient("tf_action") - client.wait_for_server() + @classmethod + def setUpClass(cls): + cls.context = rclpy.context.Context() + rclpy.init(context=cls.context) + cls.executor = SingleThreadedExecutor(context=cls.context) + cls.node = rclpy.create_node('TestBufferClient', context=cls.context) + cls.executor.add_node(cls.node) - p1 = PointStamped() - p1.header.frame_id = "a" - p1.header.stamp = rospy.Time(0.0) - p1.point.x = 0.0 - p1.point.y = 0.0 - p1.point.z = 0.0 + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown(context=cls.context) + + def setUp(self): + self.spinning = threading.Event() + self.spin_thread = threading.Thread(target=self.spin) + self.spin_thread.start() + return + def tearDown(self): + self.spinning.set() + self.spin_thread.join() + return + + def spin(self): try: - p2 = client.transform(p1, "b") - rospy.loginfo("p1: %s, p2: %s" % (p1, p2)) - except tf2.TransformException as e: - rospy.logerr("%s" % e) + while self.context.ok() and not self.spinning.is_set(): + self.executor.spin_once(timeout_sec=0.05) + finally: + return - def test_transform_type(self): - client = tf2_ros.BufferClient("tf_action") - client.wait_for_server() + def test_buffer_client(self): + buffer_client = tf2_ros.BufferClient( + self.node, '/tf_action', check_frequency=10.0, timeout_padding=0.0) p1 = PointStamped() p1.header.frame_id = "a" - p1.header.stamp = rospy.Time(0.0) + p1.header.stamp = rclpy.time.Time(seconds=1.0).to_msg() p1.point.x = 0.0 p1.point.y = 0.0 p1.point.z = 0.0 + buffer_client.action_client.wait_for_server() + try: - p2 = client.transform(p1, "b", new_type = PyKDL.Vector) - rospy.loginfo("p1: %s, p2: %s" % (str(p1), str(p2))) + p2 = buffer_client.transform(p1, "b", timeout=rclpy.time.Duration(seconds=1)) + self.node.get_logger().info("p1: %s, p2: %s" % (p1, p2)) except tf2.TransformException as e: - rospy.logerr("%s" % e) + self.node.get_logger().error("%s" % e) + self.assertEqual(0, 4) + + # TODO (ahcorde): Enable once python_orocos_kdl is ported + # def test_transform_type(self): + # buffer_client = tf2_ros.BufferClient( + # self.node, '/tf_action', check_frequency=10.0, timeout_padding=0.0) + # + # p1 = PointStamped() + # p1.header.frame_id = "a" + # p1.header.stamp = rclpy.time.Time(seconds=1.0).to_msg() + # p1.point.x = 0.0 + # p1.point.y = 0.0 + # p1.point.z = 0.0 + # + # buffer_client.action_client.wait_for_server() + # + # try: + # p2 = buffer_client.transform(p1, "b", timeout=rclpy.time.Duration(seconds=1), + # new_type = PyKDL.Vector) + # self.node.get_logger().info("p1: %s, p2: %s" % (str(p1), str(p2))) + # except tf2.TransformException as e: + # self.node.get_logger().error("%s" % e) + + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - rospy.init_node("test_buffer_client") - import rostest - rostest.rosrun(PKG, 'test_buffer_client', TestBufferClient) + rclpy.init(args=None) + unittest.main() diff --git a/test_tf2/test/test_convert.py b/test_tf2/test/test_convert.py index 0e9e2afad..988c9baaf 100755 --- a/test_tf2/test/test_convert.py +++ b/test_tf2/test/test_convert.py @@ -31,12 +31,9 @@ #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. -#* +#* #* Author: Eitan Marder-Eppstein #*********************************************************** -PKG = 'test_tf2' -import roslib; roslib.load_manifest(PKG) - import sys import unittest @@ -44,23 +41,23 @@ import tf2_ros import tf2_geometry_msgs from geometry_msgs.msg import PointStamped -import rospy +import builtin_interfaces +import rclpy import tf2_kdl import PyKDL + class TestConvert(unittest.TestCase): def test_convert(self): - p = tf2_ros.Stamped(PyKDL.Vector(1, 2, 3), rospy.Time(), 'my_frame') - print p - msg = tf2_ros.convert(p, PointStamped) - print msg + p = tf2_ros.Stamped(PyKDL.Vector(1, 2, 3), builtin_interfaces.msg.Time(sec=0), 'my_frame') + msg = tf2_ros.convert(p, PointStamped) p2 = tf2_ros.convert(msg, PyKDL.Vector) - print p2 - p2[0] = 100 - print p - print p2 - print p2.header + + self.assertEqual(p2[0], p[0]) + self.assertEqual(p2[1], p[1]) + self.assertEqual(p2[2], p[2]) + self.assertEqual(p2.header, p.header) + if __name__ == '__main__': - import rostest - rostest.unitrun(PKG, 'test_buffer_client', TestConvert) + unittest.main() diff --git a/test_tf2/test/test_static_publisher.py b/test_tf2/test/test_static_publisher.py index 79bfe5703..0869ee064 100755 --- a/test_tf2/test/test_static_publisher.py +++ b/test_tf2/test/test_static_publisher.py @@ -31,17 +31,14 @@ #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. -#* +#* #* Author: Felix Duvallet #*********************************************************** import subprocess import unittest -import rospy -PKG = 'test_tf2' -import roslib; roslib.load_manifest(PKG) - +import rclpy class TestStaticPublisher(unittest.TestCase): """ @@ -61,36 +58,36 @@ class TestStaticPublisher(unittest.TestCase): def test_publisher_no_args(self): # Start the publisher with no argument. - cmd = 'rosrun tf2_ros static_transform_publisher' + cmd = 'ros2 run tf2_ros static_transform_publisher' with self.assertRaises(subprocess.CalledProcessError) as cm: ret = subprocess.check_output( cmd.split(' '), stderr=subprocess.STDOUT) self.assertEqual(255, cm.exception.returncode) self.assertIn('not having the right number of arguments', - cm.exception.output) + str(cm.exception.output)) def test_publisher_nonexistent_param(self): # Here there is no paramater by that name. - cmd = 'rosrun tf2_ros static_transform_publisher /test_tf2/tf_null' + cmd = 'ros2 run tf2_ros static_transform_publisher /test_tf2/tf_null' with self.assertRaises(subprocess.CalledProcessError) as cm: ret = subprocess.check_output( cmd.split(' '), stderr=subprocess.STDOUT) self.assertEqual(255, cm.exception.returncode) - self.assertIn('Could not read TF', cm.exception.output) + self.assertIn('not having the right number of arguments', + str(cm.exception.output)) def test_publisher_invalid_param(self): # Here there is an invalid parameter stored in the parameter server. - cmd = 'rosrun tf2_ros static_transform_publisher /test_tf2/tf_invalid' + cmd = 'ros2 run tf2_ros static_transform_publisher /test_tf2/tf_invalid' with self.assertRaises(subprocess.CalledProcessError) as cm: ret = subprocess.check_output( cmd.split(' '), stderr=subprocess.STDOUT) self.assertEqual(255, cm.exception.returncode) - self.assertIn('Could not validate XmlRpcC', cm.exception.output) + self.assertIn('exited due to not having the right number', str(cm.exception.output)) if __name__ == '__main__': - rospy.init_node("test_static_publisher_py") - import rostest - rostest.rosrun(PKG, 'test_static_publisher_py', TestStaticPublisher) + rclpy.init(args=None) + unittest.main() From 6e0417c9bf45ece2ffd2dbef5016a893673b0cf7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 4 Dec 2019 13:04:36 +0100 Subject: [PATCH 09/44] Added more tests to tf2_ros --- tf2_ros/CMakeLists.txt | 51 ++++++------------------- tf2_ros/test/listener_unittest.cpp | 18 +++------ tf2_ros/test/time_reset_test.cpp | 61 +++++++++++++++--------------- 3 files changed, 48 insertions(+), 82 deletions(-) diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 62495ed90..57e99a355 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -140,47 +140,18 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_transform_broadcaster test/test_transform_broadcaster.cpp) target_link_libraries(${PROJECT_NAME}_test_transform_broadcaster ${PROJECT_NAME}) -# TODO(tfoote) port tests to use ROS2 instead of ROS1 api. -if (false) - -# new requirements for testing -find_package(catkin REQUIRED COMPONENTS - actionlib - geometry_msgs - message_filters - roscpp - rosgraph - rospy - rostest - tf2 - tf2_msgs - ${TF2_PY} -) - -# tf2_ros_test_listener -add_executable(${PROJECT_NAME}_test_listener EXCLUDE_FROM_ALL test/listener_unittest.cpp) -add_dependencies(${PROJECT_NAME}_test_listener tf2_msgs_gencpp) -add_executable(${PROJECT_NAME}_test_time_reset EXCLUDE_FROM_ALL test/time_reset_test.cpp) -add_dependencies(${PROJECT_NAME}_test_time_reset tf2_msgs_gencpp) -target_link_libraries(${PROJECT_NAME}_test_listener - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${GTEST_LIBRARIES} -) - -target_link_libraries(${PROJECT_NAME}_test_time_reset - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${GTEST_LIBRARIES} -) - -add_dependencies(tests ${PROJECT_NAME}_test_listener) -add_dependencies(tests ${PROJECT_NAME}_test_time_reset) -add_rostest(test/transform_listener_unittest.launch) -add_rostest(test/transform_listener_time_reset_test.launch) - -endif() + ament_add_gtest(${PROJECT_NAME}_test_time_reset test/time_reset_test.cpp) + ament_target_dependencies(${PROJECT_NAME}_test_time_reset + rclcpp + ) + target_link_libraries(${PROJECT_NAME}_test_time_reset ${PROJECT_NAME}) + + ament_add_gtest(${PROJECT_NAME}_test_listener test/listener_unittest.cpp) + ament_target_dependencies(${PROJECT_NAME}_test_listener + rclcpp + ) + target_link_libraries(${PROJECT_NAME}_test_listener ${PROJECT_NAME}) # Python test # Provides PYTHON_EXECUTABLE_DEBUG diff --git a/tf2_ros/test/listener_unittest.cpp b/tf2_ros/test/listener_unittest.cpp index 631c5685d..0b5dc5054 100644 --- a/tf2_ros/test/listener_unittest.cpp +++ b/tf2_ros/test/listener_unittest.cpp @@ -31,7 +31,6 @@ #include #include - void seed_rand() { //Seed random number generator with current microseond count @@ -49,22 +48,17 @@ void generate_rand_vectors(double scale, uint64_t runs, std::vector& xva } } - -using namespace tf2; - -TEST(tf2_ros_transform, transform_listener) +TEST(tf2_ros_test_listener, transform_listener) { - tf2_ros::Buffer buffer; - tf2_ros::TransformListener tfl(buffer); - + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer); } - - - int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "transform_listener_unittest"); + rclcpp::init(argc, argv); + std::shared_ptr node_ = std::make_shared("transform_listener_unittest"); return RUN_ALL_TESTS(); } diff --git a/tf2_ros/test/time_reset_test.cpp b/tf2_ros/test/time_reset_test.cpp index b8ce22368..22a3d7adc 100644 --- a/tf2_ros/test/time_reset_test.cpp +++ b/tf2_ros/test/time_reset_test.cpp @@ -27,34 +27,37 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include -#include +#include +#include +#include -using namespace tf2; - -TEST(tf2_ros_transform_listener, time_backwards) +TEST(tf2_ros_time_reset_test, time_backwards) { + std::shared_ptr node_ = std::make_shared("transform_listener_backwards_reset"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer; + tf2_ros::Buffer buffer(clock); tf2_ros::TransformListener tfl(buffer); - tf2_ros::TransformBroadcaster tfb; - - ros::NodeHandle nh = ros::NodeHandle(); + tf2_ros::TransformBroadcaster tfb(node_); - ros::Publisher clock = nh.advertise("/clock", 5); + auto clock_pub = node_->create_publisher("/clock", 5); - rosgraph_msgs::Clock c; - c.clock = builtin_interfaces::msg::Time(100); - clock.publish(c); + rosgraph_msgs::msg::Clock c; + c.clock.sec = 100; + c.clock.nanosec = 0; + clock_pub->publish(c); // basic test - ASSERT_FALSE(buffer.canTransform("foo", "bar", builtin_interfaces::msg::Time(101, 0))); + ASSERT_FALSE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); // set the transform - geometry_msgs::TransformStamped msg; - msg.header.stamp = builtin_interfaces::msg::Time(100, 0); + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp.sec = 100; + msg.header.stamp.nanosec = 0; msg.header.frame_id = "foo"; msg.child_frame_id = "bar"; msg.transform.rotation.w = 0.0; @@ -62,44 +65,42 @@ TEST(tf2_ros_transform_listener, time_backwards) msg.transform.rotation.y = 0.0; msg.transform.rotation.z = 0.0; tfb.sendTransform(msg); - msg.header.stamp = builtin_interfaces::msg::Time(102, 0); + msg.header.stamp.sec = 102; + msg.header.stamp.nanosec = 0; tfb.sendTransform(msg); - // make sure it arrives - ros::spinOnce(); + rclcpp::spin_some(node_); sleep(1); // verify it's been set - ASSERT_TRUE(buffer.canTransform("foo", "bar", builtin_interfaces::msg::Time(101, 0))); + ASSERT_TRUE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); - c.clock = builtin_interfaces::msg::Time(90); - clock.publish(c); + c.clock.sec = 90; + c.clock.nanosec = 0; + clock_pub->publish(c); // make sure it arrives - ros::spinOnce(); + rclcpp::spin_some(node_); sleep(1); //Send anoterh message to trigger clock test on an unrelated frame - msg.header.stamp = builtin_interfaces::msg::Time(110, 0); + msg.header.stamp.sec = 110; + msg.header.stamp.nanosec = 0; msg.header.frame_id = "foo2"; msg.child_frame_id = "bar2"; tfb.sendTransform(msg); // make sure it arrives - ros::spinOnce(); + rclcpp::spin_some(node_); sleep(1); //verify the data's been cleared - ASSERT_FALSE(buffer.canTransform("foo", "bar", builtin_interfaces::msg::Time(101, 0))); - + ASSERT_FALSE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); } - - - int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "transform_listener_backwards_reset"); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } From d6a2fa9cb2932b73714116a66ebbbe2582144f3d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 19:44:04 +0100 Subject: [PATCH 10/44] test_tf2 using emplace_back in permuter.hpp --- test_tf2/test/permuter.hpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 6e28c3641..1c4730ec9 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -43,8 +43,8 @@ namespace rostest class PermuteOptionBase { public: - virtual void reset() =0; - virtual bool step() =0; + virtual void reset() = 0; + virtual bool step() = 0; virtual ~PermuteOptionBase() {}; }; @@ -65,7 +65,8 @@ class PermuteOption : public PermuteOptionBase virtual ~PermuteOption(){}; - void reset(){ + void reset() + { std::lock_guard lock(access_mutex_); current_element_ = options_.begin(); *output_ = *current_element_; @@ -113,7 +114,7 @@ class Permuter void addOptionSet(const std::vector& values, T* output) { std::lock_guard lock(access_mutex_); - options_.push_back(static_cast (new PermuteOption(values, output))); + options_.emplace_back(std::make_unique>(values, output)); reset(); }; @@ -151,15 +152,11 @@ class Permuter void clearAll() { std::lock_guard lock(access_mutex_); - for ( unsigned int i = 0 ; i < options_.size(); i++) - { - delete options_[i]; - } options_.clear(); }; private: - std::vector options_; ///< Store all the option objects + std::vector> options_; ///< Store all the option objects std::mutex access_mutex_; }; From d2cfd5cbf36e71b5ec84c17ae190315aa84a74ef Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 19:44:18 +0100 Subject: [PATCH 11/44] test_tf2 ported test_convert.cpp --- test_tf2/test/test_convert.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index 73b1f67a6..0a022b009 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -39,13 +39,12 @@ #include #include #include -#include TEST(tf2Convert, kdlToBullet) { double epsilon = 1e-9; - tf2::Stamped b(btVector3(1,2,3), builtin_interfaces::msg::Time(), "my_frame"); + tf2::Stamped b(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"); tf2::Stamped b1 = b; tf2::Stamped k1; @@ -55,14 +54,14 @@ TEST(tf2Convert, kdlToBullet) tf2::convert(k1, b2); EXPECT_EQ(b.frame_id_, b2.frame_id_); - EXPECT_NEAR(b.stamp_.toSec(), b2.stamp_.toSec(), epsilon); + EXPECT_NEAR(tf2::timeToSec(b.stamp_), tf2::timeToSec(b2.stamp_), epsilon); EXPECT_NEAR(b.x(), b2.x(), epsilon); EXPECT_NEAR(b.y(), b2.y(), epsilon); EXPECT_NEAR(b.z(), b2.z(), epsilon); EXPECT_EQ(b1.frame_id_, b2.frame_id_); - EXPECT_NEAR(b1.stamp_.toSec(), b2.stamp_.toSec(), epsilon); + EXPECT_NEAR(tf2::timeToSec(b1.stamp_), tf2::timeToSec(b2.stamp_), epsilon); EXPECT_NEAR(b1.x(), b2.x(), epsilon); EXPECT_NEAR(b1.y(), b2.y(), epsilon); EXPECT_NEAR(b1.z(), b2.z(), epsilon); @@ -72,8 +71,8 @@ TEST(tf2Convert, kdlBulletROSConversions) { double epsilon = 1e-9; - tf2::Stamped b1(btVector3(1,2,3), builtin_interfaces::msg::Time(), "my_frame"), b2, b3, b4; - geometry_msgs::PointStamped r1, r2, r3; + tf2::Stamped b1(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4; + geometry_msgs::msg::PointStamped r1, r2, r3; tf2::Stamped k1, k2, k3; // Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self -> ROS -> KDL -> bullet -> ROS -> bullet @@ -91,15 +90,14 @@ TEST(tf2Convert, kdlBulletROSConversions) tf2::convert(r3, b4); EXPECT_EQ(b1.frame_id_, b4.frame_id_); - EXPECT_NEAR(b1.stamp_.toSec(), b4.stamp_.toSec(), epsilon); + EXPECT_NEAR(tf2::timeToSec(b1.stamp_), tf2::timeToSec(b4.stamp_), epsilon); EXPECT_NEAR(b1.x(), b4.x(), epsilon); EXPECT_NEAR(b1.y(), b4.y(), epsilon); EXPECT_NEAR(b1.z(), b4.z(), epsilon); -} +} int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - From 1f9bdd2bc06686f01078dce5fe692693a88121ba Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 19:45:02 +0100 Subject: [PATCH 12/44] fixed package.xml and CMakeLists.txt --- test_tf2/CMakeLists.txt | 71 ++++++++++++++++++++++++++++++----------- test_tf2/package.xml | 5 +++ 2 files changed, 57 insertions(+), 19 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 98f6c5735..325c768a8 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -16,6 +16,10 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_bullet REQUIRED) +find_package(PkgConfig REQUIRED) +pkg_check_modules(bullet REQUIRED bullet) +include_directories(include ${bullet_INCLUDE_DIRS}) + ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) ament_target_dependencies(buffer_core_test @@ -34,38 +38,67 @@ if(TARGET test_message_filter) ) endif() +ament_add_gtest(test_convert test/test_convert.cpp) +if(TARGET test_convert) + ament_target_dependencies(test_convert + tf2 + tf2_bullet + ) +endif() + ament_add_gtest(test_utils test/test_utils.cpp) if(TARGET test_utils) ament_target_dependencies(test_utils tf2 tf2_kdl + tf2_geometry_msgs ) endif() -find_package(PkgConfig REQUIRED) -pkg_check_modules(bullet REQUIRED bullet) -include_directories(include ${bullet_INCLUDE_DIRS}) +add_executable(test_buffer_server test/test_buffer_server.cpp) +if(TARGET test_buffer_server) + ament_target_dependencies(test_buffer_server + rclcpp + rclcpp_action + tf2_ros + tf2_bullet + ) +endif() + ament_add_gtest(test_buffer_client test/test_buffer_client.cpp) -ament_target_dependencies(test_buffer_client - rclcpp - rclcpp_action - tf2_ros - tf2_kdl - tf2_bullet -) +if(TARGET test_buffer_client) + ament_target_dependencies(test_buffer_client + rclcpp + rclcpp_action + tf2_ros + tf2_kdl + tf2_bullet + ) +endif() # add_rostest(test/buffer_client_tester.launch) -# -# add_executable(test_static_publisher EXCLUDE_FROM_ALL test/test_static_publisher.cpp) -# target_link_libraries(test_static_publisher ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) -# + +ament_add_gtest(test_static_publisher test/test_static_publisher.cpp) +if(TARGET test_static_publisher) + ament_target_dependencies(test_static_publisher + rclcpp + rclcpp_action + tf2_ros + ) +endif() # add_rostest(test/static_publisher.launch) -# -# -# add_executable(test_tf2_bullet EXCLUDE_FROM_ALL test/test_tf2_bullet.cpp) -# target_link_libraries(test_tf2_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) -# + +ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp) +if(TARGET test_tf2_bullet) + ament_target_dependencies(test_tf2_bullet + rclcpp + rclcpp_action + tf2_ros + tf2_bullet + ) +endif() + # add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) # # diff --git a/test_tf2/package.xml b/test_tf2/package.xml index 33714e9cb..18f0fc961 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -11,7 +11,12 @@ http://www.ros.org/wiki/geometry_experimental + ament_cmake + rclcpp + tf2_bullet + tf2 + tf2_ros ament_cmake_gtest From e032eea70069626acd453a7a3f2e64d3e7f29bd0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 17 Dec 2019 19:45:37 +0100 Subject: [PATCH 13/44] test_tf2 some improvements buffer_core_test.cpp --- test_tf2/test/buffer_core_test.cpp | 39 ++++++++++++------------------ 1 file changed, 16 insertions(+), 23 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 8cae4c0a6..54774461b 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -34,10 +34,8 @@ #include "rclcpp/rclcpp.hpp" #include #include "permuter.hpp" -// #include "LinearMath/btVector3.h" #include "tf2/LinearMath/Quaternion.h" #include "tf2/exceptions.h" -// #include "rostest/permuter.h" void seed_rand() { @@ -167,13 +165,9 @@ void push_back_1(std::vector& children, std::vector& p void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_interfaces::msg::Time & time, const tf2::Duration& interpolation_space = tf2::durationFromSec(0.0)) { - // auto node = rclcpp::Node::make_shared("buffer_core_test"); - // RCLCPP_DEBUG(node->get_logger(), "Clearing Buffer Core for new test setup"); mBC.clear(); - // RCLCPP_DEBUG(node->get_logger(), "Setting up test tree for formation %s", mode.c_str()); - std::vector children; std::vector parents; std::vector dx, dy; @@ -218,7 +212,9 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte direction = -1; } else + { frame_prefix =""; + } for (uint64_t i = 1; i < frames.size(); i++) { geometry_msgs::msg::TransformStamped ts; @@ -233,12 +229,15 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte double time_seconds = time.sec + time.nanosec / 1e9; double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; - if (time_seconds > time_interpolation_space ){ + if (time_seconds > time_interpolation_space ) + { double time_stamp = time_seconds - time_interpolation_space; - ts.header.stamp.sec = (int)time_stamp; - ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; - }else + ts.header.stamp = rclcpp::Time(time_stamp); + } + else + { ts.header.stamp = builtin_interfaces::msg::Time(); + } ts.header.frame_id = frame_prefix + frames[i-1]; if (i > 1) @@ -249,8 +248,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte if (interpolation_space > tf2::Duration()) { double time_stamp = time_seconds + time_interpolation_space; - ts.header.stamp.sec = (int)time_stamp; - ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; + ts.header.stamp = rclcpp::Time(time_stamp); EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } @@ -260,7 +258,6 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte else if (mode == "1") { push_back_1(children, parents, dx, dy); - } else if (mode =="1_v") { @@ -282,10 +279,12 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; if (time_seconds > time_interpolation_space ){ double time_stamp = time_seconds - time_interpolation_space; - ts.header.stamp.sec = (int)time_stamp; - ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; - }else + ts.header.stamp = rclcpp::Time(time_stamp); + } + else + { ts.header.stamp = builtin_interfaces::msg::Time(); + } ts.header.frame_id = parents[i]; ts.child_frame_id = children[i]; @@ -293,8 +292,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte if (interpolation_space > tf2::Duration()) { double time_stamp = time_seconds + time_interpolation_space; - ts.header.stamp.sec = (int)time_stamp; - ts.header.stamp.nanosec = (time_stamp-(int)time_stamp)*1e9; + ts.header.stamp = rclcpp::Time(time_stamp); EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } @@ -630,14 +628,9 @@ TEST(BufferCore_setTransform, NoInsertWithNoParentID) // // */ -#define GTEST_COUT std::cerr << "[ ] [ INFO ]" - TEST(BufferCore_lookupTransform, i_configuration) { double epsilon = 1e-6; - // TEST_COUT << "BufferCore_lookupTransform" << std::endl; - - GTEST_COUT << "ALEX!!! BufferCore_lookupTransform" << std::endl; rostest::Permuter permuter; std::vector times; From 4d8f8f6e0015345f2dc78742b1302b6ef31ff8f2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 18 Dec 2019 09:26:43 +0100 Subject: [PATCH 14/44] test_tf2 permutter.hpp no semicolon after a method. --- test_tf2/test/permuter.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 1c4730ec9..20a90b833 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -70,7 +70,7 @@ class PermuteOption : public PermuteOptionBase std::lock_guard lock(access_mutex_); current_element_ = options_.begin(); *output_ = *current_element_; - }; + } bool step() { @@ -80,7 +80,7 @@ class PermuteOption : public PermuteOptionBase return false; *output_ = *current_element_; return true; - }; + } private: /// Local storage of the possible values @@ -116,14 +116,14 @@ class Permuter std::lock_guard lock(access_mutex_); options_.emplace_back(std::make_unique>(values, output)); reset(); - }; + } /** \brief Reset the internal counters */ void reset(){ for (unsigned int level= 0; level < options_.size(); level++) options_[level]->reset(); - }; + } /** \brief Iterate to the next value in the iteration * Returns true unless done iterating. @@ -146,14 +146,14 @@ class Permuter } } return false; - }; + } /** \brief Clear all stored data */ void clearAll() { std::lock_guard lock(access_mutex_); options_.clear(); - }; + } private: std::vector> options_; ///< Store all the option objects From 51e2ce24cc7b8fd71f93dc2301425392d3cf50f4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 18 Dec 2019 09:29:45 +0100 Subject: [PATCH 15/44] test_tf2 test_message_filter fix --- test_tf2/test/test_message_filter.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 97be05d01..b0ffd859e 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -282,7 +282,7 @@ TEST(MessageFilter, tolerance) EXPECT_EQ(0, n.count_); //No return due to lack of space for offset double time_stamp = (stamp.sec + stamp.nanosec/1e9)*1.1 + tf2::durationToSec(offset); - builtin_interfaces::msg::Time stamp_transform = tf2_ros::toMsg(tf2::timeFromSec(time_stamp)); + builtin_interfaces::msg::Time stamp_transform = rclcpp::Time(time_stamp); buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp_transform, "frame1", "frame2"), "me"); @@ -290,8 +290,7 @@ TEST(MessageFilter, tolerance) time_stamp = (stamp.sec + stamp.nanosec/1e9) + tf2::durationToSec(offset); - msg->header.stamp.sec = (int)time_stamp; - msg->header.stamp.nanosec = (time_stamp - (int)time_stamp)/1e9; + msg->header.stamp = rclcpp::Time(time_stamp); filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset From 469d06421c225ec327ef693a9e7c85914618909c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 18 Dec 2019 09:31:09 +0100 Subject: [PATCH 16/44] test_tf2 permutter.hpp brace on the next line --- test_tf2/test/permuter.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 20a90b833..1f858b194 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -120,7 +120,8 @@ class Permuter /** \brief Reset the internal counters */ - void reset(){ + void reset() + { for (unsigned int level= 0; level < options_.size(); level++) options_[level]->reset(); } From d74a2b371970191b106477da62d7eec94718055b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 18 Dec 2019 11:03:14 +0100 Subject: [PATCH 17/44] test_tf2 test_utils added TODO --- test_tf2/test/test_utils.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/test_tf2/test/test_utils.cpp b/test_tf2/test/test_utils.cpp index 03208cfb7..9af8e55fe 100644 --- a/test_tf2/test/test_utils.cpp +++ b/test_tf2/test/test_utils.cpp @@ -13,10 +13,9 @@ // limitations under the License. #include +#include #include -#include #include -#include double epsilon = 1e-9; @@ -61,10 +60,11 @@ TEST(tf2Utils, yaw) tf2::Quaternion q(x, y, z, w); yprTest(q, yaw1, pitch1, roll1); - // tf2::Stamped - tf2::Stamped sq; - sq.setData(q); - yprTest(sq, yaw1, pitch1, roll1); + // TODO (ahcorde): This PR fix this issue https://github.com/ros/geometry2/pull/357 + // // tf2::Stamped + // tf2::Stamped sq; + // sq.setData(q); + // yprTest(sq, yaw1, pitch1, roll1); } } From da0bf6cfcec301c916d14d724be6d8ed879f106c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 19 Dec 2019 10:05:10 +0100 Subject: [PATCH 18/44] test_tf2_ fixed test_message_filter --- test_tf2/test/test_message_filter.cpp | 123 +++++++++++++++++--------- 1 file changed, 79 insertions(+), 44 deletions(-) diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index b0ffd859e..9979d0aa8 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -31,6 +31,7 @@ #include +#include #include #include @@ -64,12 +65,15 @@ class Notification TEST(MessageFilter, noTransforms) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); - filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -79,15 +83,19 @@ TEST(MessageFilter, noTransforms) EXPECT_EQ(0, n.count_); - rclcpp::shutdown(); + node.reset(); } TEST(MessageFilter, noTransformsSameFrame) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -98,7 +106,7 @@ TEST(MessageFilter, noTransformsSameFrame) filter.add(msg); EXPECT_EQ(1, n.count_); - rclcpp::shutdown(); + node.reset(); } geometry_msgs::msg::TransformStamped createTransform(tf2::Quaternion q, tf2::Vector3 v, builtin_interfaces::msg::Time stamp, const std::string& frame1, const std::string& frame2) @@ -120,9 +128,13 @@ geometry_msgs::msg::TransformStamped createTransform(tf2::Quaternion q, tf2::Vec TEST(MessageFilter, preexistingTransforms) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -138,15 +150,20 @@ TEST(MessageFilter, preexistingTransforms) filter.add(msg); EXPECT_EQ(1, n.count_); - rclcpp::shutdown(); + + node.reset(); } TEST(MessageFilter, postTransforms) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -165,47 +182,56 @@ TEST(MessageFilter, postTransforms) buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); - rclcpp::shutdown(); -} - -TEST(MessageFilter, queueSize) -{ - auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); - Notification n(10); - - filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); - // filter.registerFailureCallback(std::bind(&Notification::failure, &n, std::placeholders::_1, std::placeholders::_2)); - - builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); - - for (int i = 0; i < 20; ++i) - { - std::shared_ptr msg = std::make_shared(); - msg->header.stamp = stamp; - msg->header.frame_id = "frame2"; - - filter.add(msg); - } - - EXPECT_EQ(0, n.count_); - EXPECT_EQ(10, n.failure_count_); - - buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); - - EXPECT_EQ(10, n.count_); - rclcpp::shutdown(); + node.reset(); } +// TODO (ahcorde): enable this when this is available +// TEST(MessageFilter, queueSize) +// { +// auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); +// auto create_timer_interface = std::make_shared( +// node->get_node_base_interface(), +// node->get_node_timers_interface()); +// +// rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); +// tf2_ros::Buffer buffer(clock); +// buffer.setCreateTimerInterface(create_timer_interface); +// tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); +// Notification n(10); +// +// filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); +// // filter.registerFailureCallback(std::bind(&Notification::failure, &n, std::placeholders::_1, std::placeholders::_2)); +// +// builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); +// +// for (int i = 0; i < 20; ++i) +// { +// std::shared_ptr msg = std::make_shared(); +// msg->header.stamp = stamp; +// msg->header.frame_id = "frame2"; +// +// filter.add(msg); +// } +// +// EXPECT_EQ(0, n.count_); +// EXPECT_EQ(10, n.failure_count_); +// +// buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); +// +// EXPECT_EQ(10, n.count_); +// +// node.reset(); +// } TEST(MessageFilter, setTargetFrame) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -221,16 +247,19 @@ TEST(MessageFilter, setTargetFrame) filter.add(msg); EXPECT_EQ(1, n.count_); - rclcpp::shutdown(); + node.reset(); } - TEST(MessageFilter, multipleTargetFrames) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "", 10, node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -255,15 +284,20 @@ TEST(MessageFilter, multipleTargetFrames) buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists - rclcpp::shutdown(); + + node.reset(); } TEST(MessageFilter, tolerance) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + auto create_timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -271,7 +305,7 @@ TEST(MessageFilter, tolerance) tf2::Duration offset = tf2::durationFromSec(0.2); filter.setTolerance(offset); - builtin_interfaces::msg::Time stamp = tf2_ros::toMsg(tf2::timeFromSec(1)); + builtin_interfaces::msg::Time stamp = rclcpp::Time(1, 0); buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); std::shared_ptr msg = std::make_shared(); @@ -281,8 +315,8 @@ TEST(MessageFilter, tolerance) EXPECT_EQ(0, n.count_); //No return due to lack of space for offset - double time_stamp = (stamp.sec + stamp.nanosec/1e9)*1.1 + tf2::durationToSec(offset); - builtin_interfaces::msg::Time stamp_transform = rclcpp::Time(time_stamp); + double time_stamp = (stamp.sec + stamp.nanosec/1e9) + tf2::durationToSec(offset)*1.1; + builtin_interfaces::msg::Time stamp_transform = rclcpp::Time((int)time_stamp, (time_stamp - (int)time_stamp)*1e9); buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp_transform, "frame1", "frame2"), "me"); @@ -294,7 +328,8 @@ TEST(MessageFilter, tolerance) filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset - rclcpp::shutdown(); + + node.reset(); } // TODO(ahcorde): failurecallback From 7e5d582852417d6b315de864c2ec6b4c689ebb18 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 19 Dec 2019 12:10:56 +0100 Subject: [PATCH 19/44] test_tf2 fixed test_buffer_client --- test_tf2/CMakeLists.txt | 16 ++++-- test_tf2/test/buffer_client_tester.launch | 5 -- test_tf2/test/buffer_client_tester.launch.py | 54 ++++++++++++++++++++ test_tf2/test/test_buffer_client.cpp | 13 ++++- 4 files changed, 78 insertions(+), 10 deletions(-) delete mode 100644 test_tf2/test/buffer_client_tester.launch create mode 100644 test_tf2/test/buffer_client_tester.launch.py diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 325c768a8..66a61f5c6 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -15,6 +15,9 @@ find_package(tf2_kdl REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_bullet REQUIRED) +find_package(ament_cmake_gtest REQUIRED) +ament_find_gtest() +find_package(launch_testing_ament_cmake REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(bullet REQUIRED bullet) @@ -66,7 +69,7 @@ if(TARGET test_buffer_server) endif() -ament_add_gtest(test_buffer_client test/test_buffer_client.cpp) +add_executable(test_buffer_client test/test_buffer_client.cpp) if(TARGET test_buffer_client) ament_target_dependencies(test_buffer_client rclcpp @@ -75,10 +78,10 @@ if(TARGET test_buffer_client) tf2_kdl tf2_bullet ) + target_link_libraries(test_buffer_client ${GTEST_LIBRARIES}) + add_launch_test(test/buffer_client_tester.launch.py) endif() -# add_rostest(test/buffer_client_tester.launch) - ament_add_gtest(test_static_publisher test/test_static_publisher.cpp) if(TARGET test_static_publisher) ament_target_dependencies(test_static_publisher @@ -112,4 +115,11 @@ endif() # add_dependencies(tests tf2_ros_static_transform_publisher test_static_publisher) # endif() +# install executables +install(TARGETS + test_buffer_client + test_buffer_server + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/test_tf2/test/buffer_client_tester.launch b/test_tf2/test/buffer_client_tester.launch deleted file mode 100644 index 72182b9e6..000000000 --- a/test_tf2/test/buffer_client_tester.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/test_tf2/test/buffer_client_tester.launch.py b/test_tf2/test/buffer_client_tester.launch.py new file mode 100644 index 000000000..9e7980377 --- /dev/null +++ b/test_tf2/test/buffer_client_tester.launch.py @@ -0,0 +1,54 @@ +# generated from buildfarm_perf_tests/test/test_performance.py.in +# generated code does not contain a copyright notice + +import unittest + +from launch import LaunchDescription +import launch +from launch_ros.actions import Node +import launch_testing +from launch.substitutions import LaunchConfiguration + +def generate_test_description(ready_fn): + node_under_test = Node( + package='test_tf2', + node_executable='test_buffer_client', + output='screen', + arguments=[], + ) + node_static_transform_publisher = Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=["5", "6", "7", "0", "0", "0", "1", "a", "b"] + ) + + node_buffer_server = Node( + package='test_tf2', + node_executable='test_buffer_server', + output='screen', + arguments=[], + sigterm_timeout=LaunchConfiguration('sigterm_timeout', default=2) + ) + return LaunchDescription([ + node_static_transform_publisher, + node_buffer_server, + node_under_test, + launch_testing.util.KeepAliveProc(), + launch.actions.OpaqueFunction(function=lambda context: ready_fn()), + ]), locals() + + +class TestBufferClient(unittest.TestCase): + + def test_termination(self, node_under_test, proc_info): + proc_info.assertWaitForShutdown(process=node_under_test, timeout=(10)) + + +@launch_testing.post_shutdown_test() +class BufferClientTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes(self.proc_info) diff --git a/test_tf2/test/test_buffer_client.cpp b/test_tf2/test/test_buffer_client.cpp index 97a6f81a3..4bbf0dcda 100644 --- a/test_tf2/test/test_buffer_client.cpp +++ b/test_tf2/test/test_buffer_client.cpp @@ -41,6 +41,7 @@ #include #include #include +#include static const double EPS = 1e-3; @@ -83,7 +84,9 @@ TEST(tf2_ros, buffer_client) RCLCPP_ERROR(node->get_logger(), "Failed to transform: %s", ex.what()); ASSERT_FALSE("Should not get here"); } - rclcpp::shutdown(); + executor.cancel(); + spin_thread.join(); + node.reset(); } TEST(tf2_ros, buffer_client_different_types) @@ -121,11 +124,17 @@ TEST(tf2_ros, buffer_client_different_types) RCLCPP_ERROR(node->get_logger(), "Failed to transform: %s", ex.what()); ASSERT_FALSE("Should not get here"); } - rclcpp::shutdown(); + executor.cancel(); + spin_thread.join(); + node.reset(); + } int main(int argc, char** argv) { + // This is needed because we need to wait a little bit for the other nodes + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); From cc1a9579194146c86f66e752f525cad4fd5fcfdc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 19 Dec 2019 12:59:50 +0100 Subject: [PATCH 20/44] test_tf2 fixed test_static_publisher --- test_tf2/CMakeLists.txt | 6 +- test_tf2/test/static_publisher.launch | 33 ------ test_tf2/test/static_publisher.launch.py | 53 +++++++++ test_tf2/test/test_static_publisher.cpp | 139 ++++++++++++++++------- 4 files changed, 156 insertions(+), 75 deletions(-) delete mode 100644 test_tf2/test/static_publisher.launch create mode 100644 test_tf2/test/static_publisher.launch.py diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 66a61f5c6..b5f9e0f4e 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -82,15 +82,16 @@ if(TARGET test_buffer_client) add_launch_test(test/buffer_client_tester.launch.py) endif() -ament_add_gtest(test_static_publisher test/test_static_publisher.cpp) +add_executable(test_static_publisher test/test_static_publisher.cpp) if(TARGET test_static_publisher) ament_target_dependencies(test_static_publisher rclcpp rclcpp_action tf2_ros ) + target_link_libraries(test_static_publisher ${GTEST_LIBRARIES}) + add_launch_test(test/static_publisher.launch.py) endif() -# add_rostest(test/static_publisher.launch) ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp) if(TARGET test_tf2_bullet) @@ -119,6 +120,7 @@ endif() install(TARGETS test_buffer_client test_buffer_server + test_static_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/test_tf2/test/static_publisher.launch b/test_tf2/test/static_publisher.launch deleted file mode 100644 index d75469678..000000000 --- a/test_tf2/test/static_publisher.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/test_tf2/test/static_publisher.launch.py b/test_tf2/test/static_publisher.launch.py new file mode 100644 index 000000000..2bb0416c7 --- /dev/null +++ b/test_tf2/test/static_publisher.launch.py @@ -0,0 +1,53 @@ +# generated from buildfarm_perf_tests/test/test_performance.py.in +# generated code does not contain a copyright notice + +import unittest + +from launch import LaunchDescription +import launch +from launch_ros.actions import Node +import launch_testing +from launch.substitutions import LaunchConfiguration + +def generate_test_description(ready_fn): + node_under_test = Node( + package='test_tf2', + node_executable='test_static_publisher', + output='screen', + arguments=[], + ) + node_static_transform_publisher_1 = Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=["1", "0", "0", "0", "0", "0", "1", "a", "b"] + ) + node_static_transform_publisher_2 = Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=["0", "1", "0", "0", "0", "0", "1", "b", "c"] + ) + + return LaunchDescription([ + node_static_transform_publisher_1, + node_static_transform_publisher_2, + node_under_test, + launch_testing.util.KeepAliveProc(), + launch.actions.OpaqueFunction(function=lambda context: ready_fn()), + ]), locals() + + +class TestBufferClient(unittest.TestCase): + + def test_termination(self, node_under_test, proc_info): + proc_info.assertWaitForShutdown(process=node_under_test, timeout=(10)) + + +@launch_testing.post_shutdown_test() +class BufferClientTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes(self.proc_info) diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index b3c393c66..e54cba7c8 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -44,11 +44,21 @@ TEST(StaticTransformPublisher, a_b_different_times) tf2_ros::Buffer mB(clock); tf2_ros::TransformListener tfl(mB, node, false); - EXPECT_TRUE(mB.canTransform("a", "b", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "b", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "b", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // Start spinning in a thread + std::thread spin_thread = std::thread( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); - rclcpp::shutdown(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + EXPECT_TRUE(mB.canTransform("a", "b", tf2::timeFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "b", tf2::timeFromSec(100))); + EXPECT_TRUE(mB.canTransform("a", "b", tf2::timeFromSec(1000))); + + executor.cancel(); + spin_thread.join(); + node.reset(); }; TEST(StaticTransformPublisher, a_c_different_times) @@ -59,11 +69,21 @@ TEST(StaticTransformPublisher, a_c_different_times) tf2_ros::Buffer mB(clock); tf2_ros::TransformListener tfl(mB, node, false); - EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // Start spinning in a thread + std::thread spin_thread = std::thread( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); - rclcpp::shutdown(); + EXPECT_TRUE(mB.canTransform("a", "c", tf2::timeFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2::timeFromSec(10))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2::timeFromSec(1000))); + + executor.cancel(); + spin_thread.join(); + node.reset(); }; TEST(StaticTransformPublisher, a_d_different_times) @@ -74,6 +94,14 @@ TEST(StaticTransformPublisher, a_d_different_times) tf2_ros::Buffer mB(clock); tf2_ros::TransformListener tfl(mB, node, false); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // Start spinning in a thread + std::thread spin_thread = std::thread( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + geometry_msgs::msg::TransformStamped ts; ts.transform.rotation.w = 1; ts.header.frame_id = "c"; @@ -81,20 +109,22 @@ TEST(StaticTransformPublisher, a_d_different_times) ts.child_frame_id = "d"; // make sure listener has populated - EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "c", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2::timeFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2::timeFromSec(100))); + EXPECT_TRUE(mB.canTransform("a", "c", tf2::timeFromSec(1000))); mB.setTransform(ts, "authority"); //printf("%s\n", mB.allFramesAsString().c_str()); - EXPECT_TRUE(mB.canTransform("c", "d", tf2_ros::toMsg(tf2::timeFromSec(10)), tf2::durationFromSec(0))); + EXPECT_TRUE(mB.canTransform("c", "d", tf2::timeFromSec(10))); - EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(0))); - EXPECT_FALSE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(1)), tf2::durationFromSec(0))); - EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(10)), tf2::durationFromSec(0))); - EXPECT_FALSE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2::timeFromSec(0))); + EXPECT_FALSE(mB.canTransform("a", "d", tf2::timeFromSec(1))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2::timeFromSec(10))); + EXPECT_FALSE(mB.canTransform("a", "d", tf2::timeFromSec(1000))); - rclcpp::shutdown(); + executor.cancel(); + spin_thread.join(); + node.reset(); }; TEST(StaticTransformPublisher, multiple_parent_test) @@ -105,6 +135,15 @@ TEST(StaticTransformPublisher, multiple_parent_test) tf2_ros::Buffer mB(clock); tf2_ros::TransformListener tfl(mB, node, false); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // Start spinning in a thread + std::thread spin_thread = std::thread( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + tf2_ros::StaticTransformBroadcaster stb(node); geometry_msgs::msg::TransformStamped ts; ts.transform.rotation.w = 1; @@ -114,10 +153,12 @@ TEST(StaticTransformPublisher, multiple_parent_test) stb.sendTransform(ts); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // make sure listener has populated - EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1.0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2::timeFromSec(0))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2::timeFromSec(100))); + EXPECT_TRUE(mB.canTransform("a", "d", tf2::timeFromSec(1000))); // Publish new transform with child 'd', should replace old one in static tf ts.header.frame_id = "new_parent"; @@ -127,31 +168,49 @@ TEST(StaticTransformPublisher, multiple_parent_test) ts.child_frame_id = "other_child2"; stb.sendTransform(ts); - EXPECT_TRUE(mB.canTransform("new_parent", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("new_parent", "other_child", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_TRUE(mB.canTransform("new_parent", "other_child2", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); - EXPECT_FALSE(mB.canTransform("a", "d", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1.0))); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + EXPECT_TRUE(mB.canTransform("new_parent", "d", tf2::timeFromSec(0))); + EXPECT_TRUE(mB.canTransform("new_parent", "other_child", tf2::timeFromSec(0))); + EXPECT_TRUE(mB.canTransform("new_parent", "other_child2", tf2::timeFromSec(0))); + EXPECT_FALSE(mB.canTransform("a", "d", tf2::timeFromSec(0))); - rclcpp::shutdown(); + executor.cancel(); + spin_thread.join(); + node.reset(); }; -TEST(StaticTransformPublisher, tf_from_param_server_valid) +// TODO (ahcorde) Load transform from yaml +// TEST(StaticTransformPublisher, tf_from_param_server_valid) +// { +// // This TF is loaded from the parameter server; ensure it is valid. +// auto node = rclcpp::Node::make_shared("StaticTransformPublisher_tf_from_param_server_valid_test"); +// +// rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); +// tf2_ros::Buffer mB(clock); +// tf2_ros::TransformListener tfl(mB, node, false); +// +// rclcpp::executors::SingleThreadedExecutor executor; +// executor.add_node(node); +// // Start spinning in a thread +// std::thread spin_thread = std::thread( +// std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); +// +// std::this_thread::sleep_for(std::chrono::milliseconds(200)); +// +// EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2::timeFromSec(0))); +// EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2::timeFromSec(100))); +// EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2::timeFromSec(1000))); +// +// executor.cancel(); +// spin_thread.join(); +// node.reset(); +// } + +int main(int argc, char **argv) { - // This TF is loaded from the parameter server; ensure it is valid. - auto node = rclcpp::Node::make_shared("StaticTransformPublisher_tf_from_param_server_valid_test"); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer mB(clock); - tf2_ros::TransformListener tfl(mB, node, false); - - EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2_ros::toMsg(tf2::timeFromSec(0)), tf2::durationFromSec(1))); - EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2_ros::toMsg(tf2::timeFromSec(100)), tf2::durationFromSec(1))); - EXPECT_TRUE(mB.canTransform("robot_calibration", "world", tf2_ros::toMsg(tf2::timeFromSec(1000)), tf2::durationFromSec(1))); - - rclcpp::shutdown(); -} + // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); -int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); From d36935e97988be612de283e2c2bcaa9b55cc46e5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 19 Dec 2019 13:24:31 +0100 Subject: [PATCH 21/44] test_tf2 add test_buffer_client.py test --- test_tf2/CMakeLists.txt | 7 ++- test_tf2/test/test_buffer_client.launch.py | 53 ++++++++++++++++++++++ test_tf2/test/test_buffer_client.py | 8 ++-- 3 files changed, 62 insertions(+), 6 deletions(-) create mode 100644 test_tf2/test/test_buffer_client.launch.py diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index b5f9e0f4e..a79c6084a 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -68,7 +68,6 @@ if(TARGET test_buffer_server) ) endif() - add_executable(test_buffer_client test/test_buffer_client.cpp) if(TARGET test_buffer_client) ament_target_dependencies(test_buffer_client @@ -103,6 +102,8 @@ if(TARGET test_tf2_bullet) ) endif() +add_launch_test(test/test_buffer_client.launch.py) + # add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) # # @@ -123,5 +124,9 @@ install(TARGETS test_static_publisher DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS + test/test_buffer_client.py + DESTINATION lib/${PROJECT_NAME} +) ament_package() diff --git a/test_tf2/test/test_buffer_client.launch.py b/test_tf2/test/test_buffer_client.launch.py new file mode 100644 index 000000000..91555415c --- /dev/null +++ b/test_tf2/test/test_buffer_client.launch.py @@ -0,0 +1,53 @@ +# generated from buildfarm_perf_tests/test/test_performance.py.in +# generated code does not contain a copyright notice + +import unittest + +from launch import LaunchDescription +import launch +from launch_ros.actions import Node +import launch_testing +from launch.substitutions import LaunchConfiguration + +def generate_test_description(ready_fn): + node_under_test = Node( + package='test_tf2', + node_executable='test_buffer_client.py', + output='screen', + ) + node_static_transform_publisher = Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=["5", "6", "7", "0", "0", "0", "1", "a", "b"] + ) + + node_buffer_server = Node( + package='test_tf2', + node_executable='test_buffer_server', + output='screen', + arguments=[], + sigterm_timeout=LaunchConfiguration('sigterm_timeout', default=2) + ) + return LaunchDescription([ + node_static_transform_publisher, + node_buffer_server, + node_under_test, + launch_testing.util.KeepAliveProc(), + launch.actions.OpaqueFunction(function=lambda context: ready_fn()), + ]), locals() + + +class TestBufferClient(unittest.TestCase): + + def test_termination(self, node_under_test, proc_info): + proc_info.assertWaitForShutdown(process=node_under_test, timeout=(10)) + + +@launch_testing.post_shutdown_test() +class BufferClientTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes(self.proc_info) diff --git a/test_tf2/test/test_buffer_client.py b/test_tf2/test/test_buffer_client.py index b1dabc19c..97570c017 100755 --- a/test_tf2/test/test_buffer_client.py +++ b/test_tf2/test/test_buffer_client.py @@ -1,4 +1,4 @@ -#! /usr/bin/python +#!/usr/bin/python3 #*********************************************************** #* Software License Agreement (BSD License) #* @@ -59,7 +59,6 @@ def setUpClass(cls): @classmethod def tearDownClass(cls): - cls.node.destroy_node() rclpy.shutdown(context=cls.context) def setUp(self): @@ -121,9 +120,8 @@ def test_buffer_client(self): # except tf2.TransformException as e: # self.node.get_logger().error("%s" % e) - node.destroy_node() - rclpy.shutdown() if __name__ == '__main__': - rclpy.init(args=None) + rclpy.init(args=sys.argv) + sys.argv = [sys.argv[0]] unittest.main() From 8526dc1efb40eeacb0fa1eee84ae1c7b1ca92c01 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 19 Dec 2019 13:26:44 +0100 Subject: [PATCH 22/44] test_tf2 disabled tf2_kdl --- test_tf2/CMakeLists.txt | 34 ++++++++++++---------------------- 1 file changed, 12 insertions(+), 22 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index a79c6084a..01dcaa5cc 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -92,30 +92,20 @@ if(TARGET test_static_publisher) add_launch_test(test/static_publisher.launch.py) endif() -ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp) -if(TARGET test_tf2_bullet) - ament_target_dependencies(test_tf2_bullet - rclcpp - rclcpp_action - tf2_ros - tf2_bullet - ) -endif() - -add_launch_test(test/test_buffer_client.launch.py) - -# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) -# -# -# if(TARGET tests) -# add_dependencies(tests test_buffer_server test_buffer_client test_static_publisher test_tf2_bullet) +# TODO (ahcorde): activate when tf2_kdl is merged +# ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp) +# if(TARGET test_tf2_bullet) +# ament_target_dependencies(test_tf2_bullet +# rclcpp +# rclcpp_action +# tf2_ros +# tf2_bullet +# ) # endif() # -# -# # used as a test fixture -# if(TARGET tf2_ros_static_transform_publisher) -# add_dependencies(tests tf2_ros_static_transform_publisher test_static_publisher) -# endif() +# add_launch_test(test/test_buffer_client.launch.py) +# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) + # install executables install(TARGETS From a70b80f7bce92f15e5546866ead4a6fc6c5dd383 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 19 Dec 2019 13:28:10 +0100 Subject: [PATCH 23/44] test_tf2 fixed buffer_core_test.cpp --- test_tf2/test/buffer_core_test.cpp | 86 ++++++++++++------------------ 1 file changed, 35 insertions(+), 51 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 54774461b..7c3b11e75 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -226,13 +226,13 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte ts.transform.rotation.z = sin(direction * M_PI/8); ts.transform.rotation.w = cos(direction * M_PI/8); - double time_seconds = time.sec + time.nanosec / 1e9; + double time_seconds = time.sec + time.nanosec / 1e9; double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; if (time_seconds > time_interpolation_space ) { double time_stamp = time_seconds - time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp); + ts.header.stamp = rclcpp::Time(time_stamp*1e9); } else { @@ -247,8 +247,9 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > tf2::Duration()) { - double time_stamp = time_seconds + time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp); + // TODO (ahcorde): review this + double time_stamp = time_seconds;// + time_interpolation_space; + ts.header.stamp = rclcpp::Time(time_stamp*1e9); EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } @@ -279,7 +280,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; if (time_seconds > time_interpolation_space ){ double time_stamp = time_seconds - time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp); + ts.header.stamp = rclcpp::Time(time_stamp*1e9); } else { @@ -291,8 +292,9 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > tf2::Duration()) { - double time_stamp = time_seconds + time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp); + // TODO (ahcorde): review this + double time_stamp = time_seconds;// + time_interpolation_space; + ts.header.stamp = rclcpp::Time(time_stamp*1e9); EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } @@ -663,11 +665,9 @@ TEST(BufferCore_lookupTransform, i_configuration) tf2::BufferCore mBC; setupTree(mBC, "i", eval_time, interpolation_space); - tf2::TimePoint eval_time_time_point = tf2::TimePoint( - std::chrono::seconds(eval_time.sec) + - std::chrono::nanoseconds(eval_time.nanosec)); + tf2::TimePoint eval_time_time_point = tf2_ros::fromMsg(eval_time); - geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); @@ -1002,11 +1002,7 @@ TEST(BufferCore_lookupTransform, one_link_configuration) tf2::BufferCore mBC; setupTree(mBC, "1", eval_time, interpolation_space); - tf2::TimePoint eval_time_time_point = tf2::TimePoint( - std::chrono::seconds(eval_time.sec) + - std::chrono::nanoseconds(eval_time.nanosec)); - - geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon)); } @@ -1051,11 +1047,7 @@ TEST(BufferCore_lookupTransform, v_configuration) tf2::BufferCore mBC; setupTree(mBC, "v", eval_time, interpolation_space); - tf2::TimePoint eval_time_time_point = tf2::TimePoint( - std::chrono::seconds(eval_time.sec) + - std::chrono::nanoseconds(eval_time.nanosec)); - - geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon)); } @@ -1100,11 +1092,7 @@ TEST(BufferCore_lookupTransform, y_configuration) tf2::BufferCore mBC; setupTree(mBC, "y", eval_time, interpolation_space); - tf2::TimePoint eval_time_time_point = tf2::TimePoint( - std::chrono::seconds(eval_time.sec) + - std::chrono::nanoseconds(eval_time.nanosec)); - - geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); EXPECT_TRUE(check_y_result(outpose, source_frame, target_frame, eval_time, epsilon)); } @@ -1150,13 +1138,9 @@ TEST(BufferCore_lookupTransform, multi_configuration) tf2::BufferCore mBC; setupTree(mBC, "1_v", eval_time, interpolation_space); - tf2::TimePoint eval_time_time_point = tf2::TimePoint( - std::chrono::seconds(eval_time.sec) + - std::chrono::nanoseconds(eval_time.nanosec)); - - if (mBC.canTransform(source_frame, target_frame, eval_time_time_point)) + if (mBC.canTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time))) { - geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); if ((source_frame == "1" || source_frame =="2") && (target_frame =="1" || target_frame == "2")) EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon)); @@ -1466,7 +1450,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) durations.push_back(tf2::durationFromSec(0.001)); durations.push_back(tf2::durationFromSec(0.1)); tf2::Duration interpolation_space; - // permuter.addOptionSet(durations, &interpolation_space); + permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); @@ -1496,15 +1480,15 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) { tf2::BufferCore mBC; + // std::cerr << "interpolation_space " << tf2::durationToSec(interpolation_space) << std::endl; + // std::cerr << "eval_time " << eval_time.sec << " " << eval_time.nanosec << std::endl; setupTree(mBC, "ring_45", eval_time, interpolation_space); - tf2::TimePoint eval_time_time_point = tf2::TimePoint( - std::chrono::seconds(eval_time.sec) + - std::chrono::nanoseconds(eval_time.nanosec)); - - geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time_time_point); + geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); + // std::cerr << "outpose " << outpose.header.stamp.sec << " " << outpose.header.stamp.nanosec << std::endl; + EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -1630,8 +1614,8 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*3/8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*3/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI*3/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI*3/8), epsilon); } // Chaining 4 else if ((source_frame == "a" && target_frame =="e") || @@ -1662,7 +1646,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/2), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI/2), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/2), epsilon); } // Chaining 5 @@ -1692,8 +1676,8 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*5/8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*5/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI*5/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI*5/8), epsilon); } // Chaining 6 else if ((source_frame == "a" && target_frame =="g") || @@ -1706,8 +1690,8 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*6/8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*6/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, -sin(M_PI*6/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.w, -cos(M_PI*6/8), epsilon); } // Inverse Chaining 6 else if ((target_frame == "a" && source_frame =="g") || @@ -1720,8 +1704,8 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*6/8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*6/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI*6/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI*6/8), epsilon); } // Chaining 7 else if ((source_frame == "a" && target_frame =="h") || @@ -1733,8 +1717,8 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*7/8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*7/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*7/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI*7/8), epsilon); } // Inverse Chaining 7 else if ((target_frame == "a" && source_frame =="h") || @@ -1746,8 +1730,8 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*7/8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*7/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*7/8), epsilon); + EXPECT_NEAR(outpose.transform.rotation.w, -cos(M_PI*7/8), epsilon); } else { From 5f12b5a12e747101850adda9222262ae51650bfd Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 7 Jan 2020 17:16:57 +0100 Subject: [PATCH 24/44] Restoring tf2_ros files --- tf2_ros/CMakeLists.txt | 51 +++++++++++++++++++------ tf2_ros/test/listener_unittest.cpp | 18 ++++++--- tf2_ros/test/time_reset_test.cpp | 61 +++++++++++++++--------------- 3 files changed, 82 insertions(+), 48 deletions(-) diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 2480b11ed..8463d0f20 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -162,18 +162,47 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_transform_broadcaster test/test_transform_broadcaster.cpp) target_link_libraries(${PROJECT_NAME}_test_transform_broadcaster ${PROJECT_NAME}) +# TODO(tfoote) port tests to use ROS2 instead of ROS1 api. +if (false) + +# new requirements for testing +find_package(catkin REQUIRED COMPONENTS + actionlib + geometry_msgs + message_filters + roscpp + rosgraph + rospy + rostest + tf2 + tf2_msgs + ${TF2_PY} +) + +# tf2_ros_test_listener +add_executable(${PROJECT_NAME}_test_listener EXCLUDE_FROM_ALL test/listener_unittest.cpp) +add_dependencies(${PROJECT_NAME}_test_listener tf2_msgs_gencpp) +add_executable(${PROJECT_NAME}_test_time_reset EXCLUDE_FROM_ALL test/time_reset_test.cpp) +add_dependencies(${PROJECT_NAME}_test_time_reset tf2_msgs_gencpp) +target_link_libraries(${PROJECT_NAME}_test_listener + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GTEST_LIBRARIES} +) + +target_link_libraries(${PROJECT_NAME}_test_time_reset + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GTEST_LIBRARIES} +) + +add_dependencies(tests ${PROJECT_NAME}_test_listener) +add_dependencies(tests ${PROJECT_NAME}_test_time_reset) - ament_add_gtest(${PROJECT_NAME}_test_time_reset test/time_reset_test.cpp) - ament_target_dependencies(${PROJECT_NAME}_test_time_reset - rclcpp - ) - target_link_libraries(${PROJECT_NAME}_test_time_reset ${PROJECT_NAME}) - - ament_add_gtest(${PROJECT_NAME}_test_listener test/listener_unittest.cpp) - ament_target_dependencies(${PROJECT_NAME}_test_listener - rclcpp - ) - target_link_libraries(${PROJECT_NAME}_test_listener ${PROJECT_NAME}) +add_rostest(test/transform_listener_unittest.launch) +add_rostest(test/transform_listener_time_reset_test.launch) + +endif() # Python test # Provides PYTHON_EXECUTABLE_DEBUG diff --git a/tf2_ros/test/listener_unittest.cpp b/tf2_ros/test/listener_unittest.cpp index 0b5dc5054..631c5685d 100644 --- a/tf2_ros/test/listener_unittest.cpp +++ b/tf2_ros/test/listener_unittest.cpp @@ -31,6 +31,7 @@ #include #include + void seed_rand() { //Seed random number generator with current microseond count @@ -48,17 +49,22 @@ void generate_rand_vectors(double scale, uint64_t runs, std::vector& xva } } -TEST(tf2_ros_test_listener, transform_listener) -{ - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); +using namespace tf2; + +TEST(tf2_ros_transform, transform_listener) +{ + tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); + + } + + + int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - std::shared_ptr node_ = std::make_shared("transform_listener_unittest"); + ros::init(argc, argv, "transform_listener_unittest"); return RUN_ALL_TESTS(); } diff --git a/tf2_ros/test/time_reset_test.cpp b/tf2_ros/test/time_reset_test.cpp index 22a3d7adc..b8ce22368 100644 --- a/tf2_ros/test/time_reset_test.cpp +++ b/tf2_ros/test/time_reset_test.cpp @@ -27,37 +27,34 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include #include -#include -#include -#include +#include -TEST(tf2_ros_time_reset_test, time_backwards) +using namespace tf2; + +TEST(tf2_ros_transform_listener, time_backwards) { - std::shared_ptr node_ = std::make_shared("transform_listener_backwards_reset"); - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); - tf2_ros::TransformBroadcaster tfb(node_); + tf2_ros::TransformBroadcaster tfb; + + ros::NodeHandle nh = ros::NodeHandle(); - auto clock_pub = node_->create_publisher("/clock", 5); + ros::Publisher clock = nh.advertise("/clock", 5); - rosgraph_msgs::msg::Clock c; - c.clock.sec = 100; - c.clock.nanosec = 0; - clock_pub->publish(c); + rosgraph_msgs::Clock c; + c.clock = builtin_interfaces::msg::Time(100); + clock.publish(c); // basic test - ASSERT_FALSE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); + ASSERT_FALSE(buffer.canTransform("foo", "bar", builtin_interfaces::msg::Time(101, 0))); // set the transform - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp.sec = 100; - msg.header.stamp.nanosec = 0; + geometry_msgs::TransformStamped msg; + msg.header.stamp = builtin_interfaces::msg::Time(100, 0); msg.header.frame_id = "foo"; msg.child_frame_id = "bar"; msg.transform.rotation.w = 0.0; @@ -65,42 +62,44 @@ TEST(tf2_ros_time_reset_test, time_backwards) msg.transform.rotation.y = 0.0; msg.transform.rotation.z = 0.0; tfb.sendTransform(msg); - msg.header.stamp.sec = 102; - msg.header.stamp.nanosec = 0; + msg.header.stamp = builtin_interfaces::msg::Time(102, 0); tfb.sendTransform(msg); + // make sure it arrives - rclcpp::spin_some(node_); + ros::spinOnce(); sleep(1); // verify it's been set - ASSERT_TRUE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); + ASSERT_TRUE(buffer.canTransform("foo", "bar", builtin_interfaces::msg::Time(101, 0))); - c.clock.sec = 90; - c.clock.nanosec = 0; - clock_pub->publish(c); + c.clock = builtin_interfaces::msg::Time(90); + clock.publish(c); // make sure it arrives - rclcpp::spin_some(node_); + ros::spinOnce(); sleep(1); //Send anoterh message to trigger clock test on an unrelated frame - msg.header.stamp.sec = 110; - msg.header.stamp.nanosec = 0; + msg.header.stamp = builtin_interfaces::msg::Time(110, 0); msg.header.frame_id = "foo2"; msg.child_frame_id = "bar2"; tfb.sendTransform(msg); // make sure it arrives - rclcpp::spin_some(node_); + ros::spinOnce(); sleep(1); //verify the data's been cleared - ASSERT_FALSE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); + ASSERT_FALSE(buffer.canTransform("foo", "bar", builtin_interfaces::msg::Time(101, 0))); + } + + + int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); + ros::init(argc, argv, "transform_listener_backwards_reset"); return RUN_ALL_TESTS(); } From 8dcc5f6ccf720ef27f4fac39ae6b90985b005a94 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 13 Jan 2020 15:48:07 +0100 Subject: [PATCH 25/44] Update test_tf2/test/permuter.hpp Co-Authored-By: Chris Lalancette --- test_tf2/test/permuter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 1f858b194..14d9f12ff 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -133,7 +133,7 @@ class Permuter { std::lock_guard lock(access_mutex_); // base case just iterating - for (unsigned int level= 0; level < options_.size(); level++) + for (size_t level = 0; level < options_.size(); level++) { if(options_[level]->step()) { From 85e11a1de5c98e921c31d6b61a39733b468a9b60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 13 Jan 2020 15:48:36 +0100 Subject: [PATCH 26/44] Update test_tf2/test/permuter.hpp Co-Authored-By: Chris Lalancette --- test_tf2/test/permuter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 14d9f12ff..6c6341d63 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -30,7 +30,7 @@ /** \author Tully Foote */ -#ifndef ROSTEST_PERMUTER_H +#ifndef ROSTEST_PERMUTER_HPP #define ROSTEST_PERMUTER_H #include From f77434746bc650d0ac0a2d81eea04ea8475bba4e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 09:18:45 +0100 Subject: [PATCH 27/44] alphabetize cmake --- test_tf2/CMakeLists.txt | 49 ++++++++++++++++++++--------------------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 01dcaa5cc..336b1e2e5 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -8,16 +8,16 @@ project(test_tf2) find_package(ament_cmake REQUIRED) find_package(ament_cmake_gtest REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(tf2 REQUIRED) -find_package(tf2_kdl REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(tf2_bullet REQUIRED) -find_package(ament_cmake_gtest REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_kdl REQUIRED) + ament_find_gtest() -find_package(launch_testing_ament_cmake REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(bullet REQUIRED bullet) @@ -26,18 +26,18 @@ include_directories(include ${bullet_INCLUDE_DIRS}) ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) ament_target_dependencies(buffer_core_test - tf2 rclcpp + tf2 ) endif() ament_add_gtest(test_message_filter test/test_message_filter.cpp) if(TARGET test_message_filter) ament_target_dependencies(test_message_filter + geometry_msgs + rclcpp tf2 tf2_ros - rclcpp - geometry_msgs ) endif() @@ -53,29 +53,29 @@ ament_add_gtest(test_utils test/test_utils.cpp) if(TARGET test_utils) ament_target_dependencies(test_utils tf2 - tf2_kdl tf2_geometry_msgs + tf2_kdl ) endif() add_executable(test_buffer_server test/test_buffer_server.cpp) if(TARGET test_buffer_server) ament_target_dependencies(test_buffer_server - rclcpp - rclcpp_action - tf2_ros - tf2_bullet - ) + rclcpp + rclcpp_action + tf2_bullet + tf2_ros + ) endif() add_executable(test_buffer_client test/test_buffer_client.cpp) if(TARGET test_buffer_client) ament_target_dependencies(test_buffer_client - rclcpp - rclcpp_action - tf2_ros - tf2_kdl - tf2_bullet + rclcpp + rclcpp_action + tf2_bullet + tf2_kdl + tf2_ros ) target_link_libraries(test_buffer_client ${GTEST_LIBRARIES}) add_launch_test(test/buffer_client_tester.launch.py) @@ -84,29 +84,28 @@ endif() add_executable(test_static_publisher test/test_static_publisher.cpp) if(TARGET test_static_publisher) ament_target_dependencies(test_static_publisher - rclcpp - rclcpp_action - tf2_ros + tf2_ros + rclcpp + rclcpp_action ) target_link_libraries(test_static_publisher ${GTEST_LIBRARIES}) add_launch_test(test/static_publisher.launch.py) endif() -# TODO (ahcorde): activate when tf2_kdl is merged +# TODO (ahcorde): activate when tf2_bullet is merged # ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp) # if(TARGET test_tf2_bullet) # ament_target_dependencies(test_tf2_bullet # rclcpp # rclcpp_action -# tf2_ros # tf2_bullet +# tf2_ros # ) # endif() # # add_launch_test(test/test_buffer_client.launch.py) # add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) - # install executables install(TARGETS test_buffer_client From 215f5cbc368154007540ddd5c6c6e41d25b5b97f Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 09:19:08 +0100 Subject: [PATCH 28/44] add brackets test_tf2 permutter.cpp --- test_tf2/test/permuter.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 6c6341d63..676cf1b0d 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -123,7 +123,9 @@ class Permuter void reset() { for (unsigned int level= 0; level < options_.size(); level++) + { options_[level]->reset(); + } } /** \brief Iterate to the next value in the iteration From 201e64dc33e948da101dcceff34b342d8fe818c7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 09:20:22 +0100 Subject: [PATCH 29/44] removed comment test_static_publisher.cpp --- test_tf2/test/test_static_publisher.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index e54cba7c8..545a3b073 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -209,8 +209,6 @@ TEST(StaticTransformPublisher, multiple_parent_test) int main(int argc, char **argv) { - // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); From 8b55f0598826809d4987a4efcdecf756305e1378 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 09:24:31 +0100 Subject: [PATCH 30/44] Improved TODO message --- test_tf2/test/test_message_filter.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 9979d0aa8..4fb31a67a 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -184,7 +184,9 @@ TEST(MessageFilter, postTransforms) EXPECT_EQ(1, n.count_); node.reset(); } -// TODO (ahcorde): enable this when this is available +// TODO (ahcorde): For some unknown reason message_filters::Connection registerFailureCallback is disable +// with #if 0 https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/message_filter.h#L463 +// rework this part when this is available // TEST(MessageFilter, queueSize) // { // auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); @@ -332,7 +334,10 @@ TEST(MessageFilter, tolerance) node.reset(); } -// TODO(ahcorde): failurecallback +// TODO(ahcorde): For some unknown reason message_filters::Connection registerFailureCallback is disable +// with #if 0 https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/message_filter.h#L463 +// rework this part when this is available + // TEST(MessageFilter, outTheBackFailure) // { // BufferCore bc; From ee847c6797680f71b4cca633fa800d66327fddeb Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 09:30:14 +0100 Subject: [PATCH 31/44] test_tf2 Improved headers and removed some unnecessary code --- test_tf2/test/test_buffer_client.cpp | 2 ++ test_tf2/test/test_buffer_server.cpp | 1 + test_tf2/test/test_message_filter.cpp | 19 +++++-------------- 3 files changed, 8 insertions(+), 14 deletions(-) diff --git a/test_tf2/test/test_buffer_client.cpp b/test_tf2/test/test_buffer_client.cpp index 4bbf0dcda..6bb61381c 100644 --- a/test_tf2/test/test_buffer_client.cpp +++ b/test_tf2/test/test_buffer_client.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include static const double EPS = 1e-3; diff --git a/test_tf2/test/test_buffer_server.cpp b/test_tf2/test/test_buffer_server.cpp index 26c17c9c8..c9361cf41 100644 --- a/test_tf2/test/test_buffer_server.cpp +++ b/test_tf2/test/test_buffer_server.cpp @@ -38,6 +38,7 @@ #include #include #include +#include int main(int argc, char** argv) { diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 4fb31a67a..98c3f42b7 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -38,6 +38,8 @@ #include #include +#include +#include class Notification { @@ -82,8 +84,6 @@ TEST(MessageFilter, noTransforms) filter.add(msg); EXPECT_EQ(0, n.count_); - - node.reset(); } TEST(MessageFilter, noTransformsSameFrame) @@ -106,7 +106,6 @@ TEST(MessageFilter, noTransformsSameFrame) filter.add(msg); EXPECT_EQ(1, n.count_); - node.reset(); } geometry_msgs::msg::TransformStamped createTransform(tf2::Quaternion q, tf2::Vector3 v, builtin_interfaces::msg::Time stamp, const std::string& frame1, const std::string& frame2) @@ -150,8 +149,6 @@ TEST(MessageFilter, preexistingTransforms) filter.add(msg); EXPECT_EQ(1, n.count_); - - node.reset(); } TEST(MessageFilter, postTransforms) @@ -182,7 +179,6 @@ TEST(MessageFilter, postTransforms) buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); - node.reset(); } // TODO (ahcorde): For some unknown reason message_filters::Connection registerFailureCallback is disable // with #if 0 https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/message_filter.h#L463 @@ -221,7 +217,7 @@ TEST(MessageFilter, postTransforms) // // EXPECT_EQ(10, n.count_); // -// node.reset(); +// // } TEST(MessageFilter, setTargetFrame) @@ -249,7 +245,6 @@ TEST(MessageFilter, setTargetFrame) filter.add(msg); EXPECT_EQ(1, n.count_); - node.reset(); } TEST(MessageFilter, multipleTargetFrames) @@ -286,8 +281,6 @@ TEST(MessageFilter, multipleTargetFrames) buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists - - node.reset(); } TEST(MessageFilter, tolerance) @@ -330,8 +323,6 @@ TEST(MessageFilter, tolerance) filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset - - node.reset(); } // TODO(ahcorde): For some unknown reason message_filters::Connection registerFailureCallback is disable @@ -412,8 +403,8 @@ TEST(MessageFilter, tolerance) // // EXPECT_EQ(1, n.count_); // } -// -// + + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From fab5031e6bf4e75176a7d2597490f03689a43b64 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 17:07:06 +0100 Subject: [PATCH 32/44] Commented bullet dependency --- test_tf2/CMakeLists.txt | 73 ++++++++++++++++++++++------------------- 1 file changed, 39 insertions(+), 34 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 336b1e2e5..06b5e40d2 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -13,15 +13,17 @@ find_package(launch_testing_ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(tf2 REQUIRED) -find_package(tf2_bullet REQUIRED) +# TODO (ahcorde): activate when tf2_bullet is merged +# find_package(tf2_bullet REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_kdl REQUIRED) ament_find_gtest() -find_package(PkgConfig REQUIRED) -pkg_check_modules(bullet REQUIRED bullet) -include_directories(include ${bullet_INCLUDE_DIRS}) +# TODO (ahcorde): activate when tf2_bullet is merged +# find_package(PkgConfig REQUIRED) +# pkg_check_modules(bullet REQUIRED bullet) +# include_directories(include ${bullet_INCLUDE_DIRS}) ament_add_gtest(buffer_core_test test/buffer_core_test.cpp) if(TARGET buffer_core_test) @@ -41,13 +43,14 @@ if(TARGET test_message_filter) ) endif() -ament_add_gtest(test_convert test/test_convert.cpp) -if(TARGET test_convert) - ament_target_dependencies(test_convert - tf2 - tf2_bullet - ) -endif() +# TODO (ahcorde): activate when tf2_bullet is merged +# ament_add_gtest(test_convert test/test_convert.cpp) +# if(TARGET test_convert) +# ament_target_dependencies(test_convert +# tf2 +# tf2_bullet +# ) +# endif() ament_add_gtest(test_utils test/test_utils.cpp) if(TARGET test_utils) @@ -58,28 +61,30 @@ if(TARGET test_utils) ) endif() -add_executable(test_buffer_server test/test_buffer_server.cpp) -if(TARGET test_buffer_server) - ament_target_dependencies(test_buffer_server - rclcpp - rclcpp_action - tf2_bullet - tf2_ros - ) -endif() +# TODO (ahcorde): activate when tf2_bullet is merged +# add_executable(test_buffer_server test/test_buffer_server.cpp) +# if(TARGET test_buffer_server) +# ament_target_dependencies(test_buffer_server +# rclcpp +# rclcpp_action +# tf2_bullet +# tf2_ros +# ) +# endif() -add_executable(test_buffer_client test/test_buffer_client.cpp) -if(TARGET test_buffer_client) - ament_target_dependencies(test_buffer_client - rclcpp - rclcpp_action - tf2_bullet - tf2_kdl - tf2_ros - ) - target_link_libraries(test_buffer_client ${GTEST_LIBRARIES}) - add_launch_test(test/buffer_client_tester.launch.py) -endif() +# TODO (ahcorde): activate when tf2_bullet is merged +# add_executable(test_buffer_client test/test_buffer_client.cpp) +# if(TARGET test_buffer_client) +# ament_target_dependencies(test_buffer_client +# rclcpp +# rclcpp_action +# tf2_bullet +# tf2_kdl +# tf2_ros +# ) +# target_link_libraries(test_buffer_client ${GTEST_LIBRARIES}) +# add_launch_test(test/buffer_client_tester.launch.py) +# endif() add_executable(test_static_publisher test/test_static_publisher.cpp) if(TARGET test_static_publisher) @@ -108,8 +113,8 @@ endif() # install executables install(TARGETS - test_buffer_client - test_buffer_server + # test_buffer_client + # test_buffer_server test_static_publisher DESTINATION lib/${PROJECT_NAME} ) From b6b21cd5a0652eb17b36e0bcd2f7ad260873f090 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 17:07:22 +0100 Subject: [PATCH 33/44] Improved package.xml test_tf2 --- test_tf2/package.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index 18f0fc961..3f5300a51 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -13,12 +13,16 @@ ament_cmake + geometry_msgs rclcpp - tf2_bullet + rclcpp_action tf2 + tf2_geometry_msgs + tf2_kdl tf2_ros ament_cmake_gtest + launch_testing_ament_cmake ament_cmake From a099594da8bcc833e5b880e1c5ad15f9e906dcfc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 14 Jan 2020 19:16:37 +0100 Subject: [PATCH 34/44] Fixed cmakelists --- test_tf2/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 06b5e40d2..ebbd9229c 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -20,6 +20,8 @@ find_package(tf2_kdl REQUIRED) ament_find_gtest() +include_directories(${GTEST_INCLUDE_DIRS}) + # TODO (ahcorde): activate when tf2_bullet is merged # find_package(PkgConfig REQUIRED) # pkg_check_modules(bullet REQUIRED bullet) @@ -30,6 +32,7 @@ if(TARGET buffer_core_test) ament_target_dependencies(buffer_core_test rclcpp tf2 + tf2_geometry_msgs ) endif() From ef1c040d9cbee7eebdc435e2cd0f63eb5ee62313 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 15 Jan 2020 12:45:40 +0100 Subject: [PATCH 35/44] Update package.xml test_tf2 --- test_tf2/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index 3f5300a51..1ca90a7e2 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -16,6 +16,8 @@ geometry_msgs rclcpp rclcpp_action + rcpputils + rcutils tf2 tf2_geometry_msgs tf2_kdl From aeee191b30cc4ddd897a86a3356516463a825c5e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 15 Jan 2020 15:18:25 +0100 Subject: [PATCH 36/44] test_tf2 including math contanst in Windows --- test_tf2/test/buffer_core_test.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 7c3b11e75..b57a3e392 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -27,6 +27,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#if _WIN32 +#define _USE_MATH_DEFINES +#include +#endif + #include #include #include "tf2/exceptions.h" From 9ab51fa837835ee592f008cf10d98e3d53e96b66 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 15 Jan 2020 15:25:30 +0100 Subject: [PATCH 37/44] Added launch_ros depend in package.xml --- test_tf2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index 1ca90a7e2..252786c37 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -22,6 +22,7 @@ tf2_geometry_msgs tf2_kdl tf2_ros + launch_ros ament_cmake_gtest launch_testing_ament_cmake From 740a310cd93421f68c7168ac055f71acee6859dd Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 15 Jan 2020 16:02:37 +0100 Subject: [PATCH 38/44] test_tf2 changed 0.0/0.0 for NaN --- test_tf2/test/buffer_core_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index b57a3e392..c33494bc1 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -326,7 +326,7 @@ TEST(BufferCore_setTransform, NoInsertWithNan) tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = "other_frame"; EXPECT_TRUE(mBC.setTransform(tranStamped, "authority")); - tranStamped.transform.translation.x = 0.0/0.0; + tranStamped.transform.translation.x = std::numeric_limits::quiet_NaN(); EXPECT_TRUE(std::isnan(tranStamped.transform.translation.x)); EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); } From f720bbba85ca38af0d8443159e719c9aea76f183 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Thu, 16 Jan 2020 13:13:47 +0100 Subject: [PATCH 39/44] Fixed permutter define --- test_tf2/test/permuter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 676cf1b0d..25ef932ff 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -31,7 +31,7 @@ /** \author Tully Foote */ #ifndef ROSTEST_PERMUTER_HPP -#define ROSTEST_PERMUTER_H +#define ROSTEST_PERMUTER_HPP #include #include @@ -166,4 +166,4 @@ class Permuter } -#endif //ROSTEST_PERMUTER_H +#endif //ROSTEST_PERMUTER_HPP From 22c41a5457682ad452dc1906597dda384db5a407 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Thu, 16 Jan 2020 13:13:26 +0100 Subject: [PATCH 40/44] Fixed MACOS issue with gtest --- test_tf2/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index ebbd9229c..76b0483b6 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -4,6 +4,8 @@ if(NOT BUILD_TESTING) return() endif() +set(CMAKE_CXX_STANDARD 14) + project(test_tf2) find_package(ament_cmake REQUIRED) From 68e2608d638f41f85e2daccd009978fb49997292 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 15 Jan 2020 19:08:07 +0100 Subject: [PATCH 41/44] fixed compiler warnings --- test_tf2/test/buffer_core_test.cpp | 10 +++++----- test_tf2/test/test_message_filter.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index c33494bc1..833e1efc6 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -45,7 +45,7 @@ void seed_rand() { //Seed random number generator with current time. - srand(std::chrono::system_clock::now().time_since_epoch().count()); + srand((unsigned) time(0)); }; void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) @@ -237,7 +237,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte if (time_seconds > time_interpolation_space ) { double time_stamp = time_seconds - time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp*1e9); + ts.header.stamp = rclcpp::Time(static_cast(time_stamp*1e9)); } else { @@ -254,7 +254,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte { // TODO (ahcorde): review this double time_stamp = time_seconds;// + time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp*1e9); + ts.header.stamp = rclcpp::Time(static_cast(time_stamp*1e9)); EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } @@ -285,7 +285,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte double time_interpolation_space = tf2::durationToSec(interpolation_space) * .5; if (time_seconds > time_interpolation_space ){ double time_stamp = time_seconds - time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp*1e9); + ts.header.stamp = rclcpp::Time(static_cast(time_stamp*1e9)); } else { @@ -299,7 +299,7 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const builtin_inte { // TODO (ahcorde): review this double time_stamp = time_seconds;// + time_interpolation_space; - ts.header.stamp = rclcpp::Time(time_stamp*1e9); + ts.header.stamp = rclcpp::Time(static_cast(time_stamp*1e9)); EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 98c3f42b7..36761bace 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -311,7 +311,7 @@ TEST(MessageFilter, tolerance) EXPECT_EQ(0, n.count_); //No return due to lack of space for offset double time_stamp = (stamp.sec + stamp.nanosec/1e9) + tf2::durationToSec(offset)*1.1; - builtin_interfaces::msg::Time stamp_transform = rclcpp::Time((int)time_stamp, (time_stamp - (int)time_stamp)*1e9); + builtin_interfaces::msg::Time stamp_transform = rclcpp::Time(static_cast((int)time_stamp), static_cast((time_stamp - (int)time_stamp)*1e9)); buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp_transform, "frame1", "frame2"), "me"); @@ -319,7 +319,7 @@ TEST(MessageFilter, tolerance) time_stamp = (stamp.sec + stamp.nanosec/1e9) + tf2::durationToSec(offset); - msg->header.stamp = rclcpp::Time(time_stamp); + msg->header.stamp = rclcpp::Time(static_cast(time_stamp)); filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset From 562ac611ef25ec234e0ade38218054eefdfef9bc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 15 Jan 2020 17:13:51 +0100 Subject: [PATCH 42/44] replaced deprecated constPtr --- test_tf2/test/test_message_filter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 36761bace..ea8e93dd4 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -49,12 +49,12 @@ class Notification { } - void notify(const geometry_msgs::msg::PointStamped::ConstPtr& message) + void notify(const geometry_msgs::msg::PointStamped::ConstSharedPtr& message) { ++count_; } - void failure(const geometry_msgs::msg::PointStamped::ConstPtr& message, tf2_ros::filter_failure_reasons::FilterFailureReason reason) + void failure(const geometry_msgs::msg::PointStamped::ConstSharedPtr& message, tf2_ros::filter_failure_reasons::FilterFailureReason reason) { ++failure_count_; } From fbfb21c8e7110973fc5b6acf7d3d44378ac1ff76 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 20 Jan 2020 09:13:11 +0100 Subject: [PATCH 43/44] Included suggestions --- test_tf2/test/buffer_core_test.cpp | 15 --------------- test_tf2/test/permuter.hpp | 2 +- test_tf2/test/test_buffer_client.cpp | 8 +++++--- test_tf2/test/test_message_filter.cpp | 13 +++++-------- test_tf2/test/test_static_publisher.cpp | 13 +++++++------ 5 files changed, 18 insertions(+), 33 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 833e1efc6..eff943f89 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -666,7 +666,6 @@ TEST(BufferCore_lookupTransform, i_configuration) while(permuter.step()) { - tf2::BufferCore mBC; setupTree(mBC, "i", eval_time, interpolation_space); @@ -712,14 +711,12 @@ TEST(BufferCore_lookupTransform, i_configuration) EXPECT_FALSE("i configuration: Shouldn't get here"); printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.sec + eval_time.nanosec /1e9 ); } - // rclcpp::shutdown(); } } /* Check 1 result return false if test parameters unmet */ bool check_1_result(const geometry_msgs::msg::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -745,7 +742,6 @@ bool check_1_result(const geometry_msgs::msg::TransformStamped& outpose, const s } else { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); return false; } return true; @@ -754,7 +750,6 @@ bool check_1_result(const geometry_msgs::msg::TransformStamped& outpose, const s /* Check v result return false if test parameters unmet */ bool check_v_result(const geometry_msgs::msg::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -855,7 +850,6 @@ bool check_v_result(const geometry_msgs::msg::TransformStamped& outpose, const s } else { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); return false; } return true; @@ -864,7 +858,6 @@ bool check_v_result(const geometry_msgs::msg::TransformStamped& outpose, const s /* Check v result return false if test parameters unmet */ bool check_y_result(const geometry_msgs::msg::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const builtin_interfaces::msg::Time& eval_time, double epsilon) { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -965,7 +958,6 @@ bool check_y_result(const geometry_msgs::msg::TransformStamped& outpose, const s } else { - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); return false; } return true; @@ -1485,15 +1477,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) { tf2::BufferCore mBC; - // std::cerr << "interpolation_space " << tf2::durationToSec(interpolation_space) << std::endl; - // std::cerr << "eval_time " << eval_time.sec << " " << eval_time.nanosec << std::endl; - setupTree(mBC, "ring_45", eval_time, interpolation_space); geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time)); - //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); - // std::cerr << "outpose " << outpose.header.stamp.sec << " " << outpose.header.stamp.nanosec << std::endl; - EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); @@ -2868,7 +2854,6 @@ TEST(tf2_stamped, OperatorEqual) */ int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); - // builtin_interfaces::msg::Time::init(); //needed for builtin_interfaces::msg::Time::now() rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/test_tf2/test/permuter.hpp b/test_tf2/test/permuter.hpp index 25ef932ff..07cc57f77 100644 --- a/test_tf2/test/permuter.hpp +++ b/test_tf2/test/permuter.hpp @@ -33,8 +33,8 @@ #ifndef ROSTEST_PERMUTER_HPP #define ROSTEST_PERMUTER_HPP -#include #include +#include namespace rostest { diff --git a/test_tf2/test/test_buffer_client.cpp b/test_tf2/test/test_buffer_client.cpp index 6bb61381c..1da762bcc 100644 --- a/test_tf2/test/test_buffer_client.cpp +++ b/test_tf2/test/test_buffer_client.cpp @@ -35,15 +35,17 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ #include -#include + #include +#include #include #include -#include +#include + #include -#include #include #include +#include static const double EPS = 1e-3; diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index ea8e93dd4..15397a69f 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -29,17 +29,16 @@ /** \author Josh Faust */ +#include -#include -#include -#include #include - #include +#include +#include +#include -#include -#include #include +#include class Notification { @@ -276,8 +275,6 @@ TEST(MessageFilter, multipleTargetFrames) EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) - //builtin_interfaces::msg::Time::setNow(builtin_interfaces::msg::Time::now() + tf2::durationFromSec(1.0)); - buffer.setTransform(createTransform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index 545a3b073..90ec9b698 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -28,13 +28,13 @@ */ #include -#include -#include "tf2/exceptions.h" -#include -#include -#include "permuter.hpp" +#include "permuter.hpp" +#include "tf2/exceptions.h" #include "tf2_ros/transform_listener.h" +#include +#include +#include TEST(StaticTransformPublisher, a_b_different_times) { @@ -180,7 +180,8 @@ TEST(StaticTransformPublisher, multiple_parent_test) node.reset(); }; -// TODO (ahcorde) Load transform from yaml +// TODO (ahcorde) static_transform_publisher allows to load transforma from yaml files +// Revisit this test when "Load transform from yaml" is available // TEST(StaticTransformPublisher, tf_from_param_server_valid) // { // // This TF is loaded from the parameter server; ensure it is valid. From 3dd26d9dd145535e88f1ef01cecb31a330dc7442 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 22 Jan 2020 16:50:43 +0100 Subject: [PATCH 44/44] fixed ring_45_configuration test_tf2 --- test_tf2/test/buffer_core_test.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index eff943f89..e8bd841ee 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1477,6 +1477,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) { tf2::BufferCore mBC; + setupTree(mBC, "ring_45", eval_time, interpolation_space); geometry_msgs::msg::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, tf2_ros::fromMsg(eval_time));