pax_global_header 0000666 0000000 0000000 00000000064 13163650501 0014512 g ustar 00root root 0000000 0000000 52 comment=4e49e2fbd15a8f62fa9d500a38920040aa7b7473
ros-geometry2-0.5.16/ 0000775 0000000 0000000 00000000000 13163650501 0014321 5 ustar 00root root 0000000 0000000 ros-geometry2-0.5.16/.gitignore 0000664 0000000 0000000 00000000014 13163650501 0016304 0 ustar 00root root 0000000 0000000 *~
*.pyc
\#* ros-geometry2-0.5.16/geometry2/ 0000775 0000000 0000000 00000000000 13163650501 0016236 5 ustar 00root root 0000000 0000000 ros-geometry2-0.5.16/geometry2/CHANGELOG.rst 0000664 0000000 0000000 00000000555 13163650501 0020264 0 ustar 00root root 0000000 0000000 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package geometry2
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.16 (2017-07-14)
-------------------
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
* create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking.
* Contributors: Tully Foote
ros-geometry2-0.5.16/geometry2/CMakeLists.txt 0000664 0000000 0000000 00000000154 13163650501 0020776 0 ustar 00root root 0000000 0000000 cmake_minimum_required(VERSION 2.8.3)
project(geometry2)
find_package(catkin REQUIRED)
catkin_metapackage()
ros-geometry2-0.5.16/geometry2/package.xml 0000664 0000000 0000000 00000001540 13163650501 0020353 0 ustar 00root root 0000000 0000000
geometry2
0.5.16
A metapackage to bring in the default packages second generation Transform Library in ros, tf2.
Tully Foote
Tully Foote
BSD
http://www.ros.org/wiki/geometry2
catkin
tf2
tf2_bullet
tf2_eigen
tf2_geometry_msgs
tf2_kdl
tf2_msgs
tf2_py
tf2_ros
tf2_sensor_msgs
tf2_tools
ros-geometry2-0.5.16/geometry_experimental/ 0000775 0000000 0000000 00000000000 13163650501 0020731 5 ustar 00root root 0000000 0000000 ros-geometry2-0.5.16/geometry_experimental/CHANGELOG.rst 0000664 0000000 0000000 00000005201 13163650501 0022750 0 ustar 00root root 0000000 0000000 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package geometry_experimental
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.16 (2017-07-14)
-------------------
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
* create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking.
* Contributors: Tully Foote
0.5.13 (2016-03-04)
-------------------
* Remove LGPL from license tags
LGPL was erroneously included in 2a38724. As there are no files with it
in the package.
* add missing dependencies in the meta-package geometry_experimental
This partly fixes the doc jobs in `#120 `_
* Contributors: Jochen Sprickerhof, Vincent Rabaud
0.5.12 (2015-08-05)
-------------------
0.5.11 (2015-04-22)
-------------------
0.5.10 (2015-04-21)
-------------------
0.5.9 (2015-03-25)
------------------
0.5.8 (2015-03-17)
------------------
0.5.7 (2014-12-23)
------------------
0.5.6 (2014-09-18)
------------------
0.5.5 (2014-06-23)
------------------
0.5.4 (2014-05-07)
------------------
0.5.3 (2014-02-21)
------------------
0.5.2 (2014-02-20)
------------------
0.5.1 (2014-02-14)
------------------
0.5.0 (2014-02-14)
------------------
0.4.10 (2013-12-26)
-------------------
0.4.9 (2013-11-06)
------------------
0.4.8 (2013-11-06)
------------------
0.4.7 (2013-08-28)
------------------
0.4.6 (2013-08-28)
------------------
0.4.5 (2013-07-11)
------------------
0.4.4 (2013-07-09)
------------------
0.4.3 (2013-07-05)
------------------
* removing test_tf2 from metapackage and update description
0.4.2 (2013-07-05)
------------------
0.4.1 (2013-07-05)
------------------
0.4.0 (2013-06-27)
------------------
* Restoring test packages and bullet packages.
reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented
reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_
* updating for new metapackage specification
0.3.6 (2013-03-03)
------------------
0.3.5 (2013-02-15 14:46)
------------------------
* 0.3.4 -> 0.3.5
* fixing adding new packages to metapackage
0.3.4 (2013-02-15 13:14)
------------------------
* 0.3.3 -> 0.3.4
0.3.3 (2013-02-15 11:30)
------------------------
* 0.3.2 -> 0.3.3
0.3.2 (2013-02-15 00:42)
------------------------
* 0.3.1 -> 0.3.2
0.3.1 (2013-02-14)
------------------
* 0.3.0 -> 0.3.1
0.3.0 (2013-02-13)
------------------
* switching to version 0.3.0
* renaming geometry_experimental meta package in order to comply with catkin naming convention
ros-geometry2-0.5.16/geometry_experimental/CMakeLists.txt 0000664 0000000 0000000 00000000170 13163650501 0023467 0 ustar 00root root 0000000 0000000 cmake_minimum_required(VERSION 2.8.3)
project(geometry_experimental)
find_package(catkin REQUIRED)
catkin_metapackage()
ros-geometry2-0.5.16/geometry_experimental/package.xml 0000664 0000000 0000000 00000001217 13163650501 0023047 0 ustar 00root root 0000000 0000000
geometry_experimental
0.5.16
The second generation Transform Library in ros. This metapackage is deprecated, but is kept for backwards compatability.
Tully Foote
Eitan Marder-Eppstein
Wim Meeussen
Tully Foote
BSD
http://www.ros.org/wiki/geometry_experimental
catkin
geometry2
ros-geometry2-0.5.16/test_tf2/ 0000775 0000000 0000000 00000000000 13163650501 0016053 5 ustar 00root root 0000000 0000000 ros-geometry2-0.5.16/test_tf2/CHANGELOG.rst 0000664 0000000 0000000 00000017472 13163650501 0020107 0 ustar 00root root 0000000 0000000 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package test_tf2
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.16 (2017-07-14)
-------------------
* Remove generate_rand_vectors() from a number of tests. (`#227 `_)
* Remove a slew of trailing whitespace.
Signed-off-by: Chris Lalancette
* Remove generate_rand_vectors() from a number of tests.
It was never used, so there is no reason to carry it around.
Signed-off-by: Chris Lalancette
* store gtest return value as int (`#229 `_)
* Contributors: Chris Lalancette, dhood
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
* Typos.
* Adds unit tests for TF loaded from parameter server.
This tests both success (loading a valid TF into the param server) and
failures (parameter does not exist, parameter contents are invalid).
* Code linting & reorganization
- whitespace
- indentation
- re-organized code to remove duplications.
whitespace & indentation changes only.
simplified (de-duplicated) duplicate code.
missing a duplicate variable.
whitespace changes only.
* Contributors: Felix Duvallet
0.5.13 (2016-03-04)
-------------------
* Remove LGPL from license tags
LGPL was erroneously included in 2a38724. As there are no files with it
in the package.
* Contributors: Jochen Sprickerhof
0.5.12 (2015-08-05)
-------------------
* add utilities to get yaw, pitch, roll and identity transform
* provide more conversions between types
The previous conversion always assumed that it was converting a
non-message type to a non-message type. Now, one, both or none
can be a message or a non-message.
* Contributors: Vincent Rabaud
0.5.11 (2015-04-22)
-------------------
0.5.10 (2015-04-21)
-------------------
0.5.9 (2015-03-25)
------------------
0.5.8 (2015-03-17)
------------------
* remove useless Makefile files
* Contributors: Vincent Rabaud
0.5.7 (2014-12-23)
------------------
0.5.6 (2014-09-18)
------------------
0.5.5 (2014-06-23)
------------------
* Removed AsyncSpinner workaround
* Contributors: Esteve Fernandez
0.5.4 (2014-05-07)
------------------
* Clean up warnings about autostart and add some assertions for coverage
* Contributors: Tully Foote
0.5.3 (2014-02-21)
------------------
0.5.2 (2014-02-20)
------------------
0.5.1 (2014-02-14)
------------------
0.5.0 (2014-02-14)
------------------
0.4.10 (2013-12-26)
-------------------
* fixing kdl linking for tests
* Contributors: Tully Foote
0.4.9 (2013-11-06)
------------------
0.4.8 (2013-11-06)
------------------
* Fixed static_transform_publisher duplicate check, added rostest.
0.4.7 (2013-08-28)
------------------
0.4.6 (2013-08-28)
------------------
0.4.5 (2013-07-11)
------------------
* fixing quaternion in unit test and adding a timeout on the waitForServer
* fixing usage string to show quaternions and using quaternions in the test app
* removing redundant declaration
* disabling whole cmake invocation in test_tf2 when not CATKIN_ENABLE_TESTING
0.4.4 (2013-07-09)
------------------
0.4.3 (2013-07-05)
------------------
0.4.2 (2013-07-05)
------------------
0.4.1 (2013-07-05)
------------------
* fixing test target dependencies
* fixing colliding target names between geometry and geometry_experimental
* stripping tf2_ros dependency from tf2_bullet. Test was moved to test_tf2
0.4.0 (2013-06-27)
------------------
* splitting rospy dependency into tf2_py so tf2 is pure c++ library.
* switching to console_bridge from rosconsole
* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2
* Cleaning up unnecessary dependency on roscpp
* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace
* Cleaning up packaging of tf2 including:
removing unused nodehandle
fixing overmatch on search and replace
cleaning up a few dependencies and linking
removing old backup of package.xml
making diff minimally different from tf version of library
* Restoring test packages and bullet packages.
reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented
reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_
0.3.6 (2013-03-03)
------------------
0.3.5 (2013-02-15 14:46)
------------------------
0.3.4 (2013-02-15 13:14)
------------------------
0.3.3 (2013-02-15 11:30)
------------------------
0.3.2 (2013-02-15 00:42)
------------------------
0.3.1 (2013-02-14)
------------------
0.3.0 (2013-02-13)
------------------
* removing packages with missing deps
* catkinizing geometry-experimental
* add boost linkage
* fixing test for header cleanup
* fixing usage of bullet for migration to native bullet
* Cleanup on test code, all tests pass
* cleanup on optimized tests, still failing
* Cleanup in compound transform test
* Adding more frames to compound transform case
* Compound transform test fails on optimized case after more frames added
* Compound transform test has more frames in it
* Cleanup of compount transform test
* Compound transform at root node test fails for optimized branch
* compount transform test, non-optimized
* time-varying tests with different time-steps for optimized case
* Time-varying test inserts data at different time-steps for non-optimized case
* Helix (time-varying) test works on optimized branch
* Adding more complicated case to helix test
* Adding helix test for time-varying transforms in non-optimized case
* Corrected ring45 values in buffer core test
* Corrected values of ring45 test for non-optimized case
* Ring 45 test running on non-optimized tf2 branch, from Tully's commit r880
* filling out ring test case which finds errors in the optimization
* Add option to use a callback queue in the message filter
* another out-the-back test
* move the message filter to tf2_ros
* fix warnings
* merge from tf_rework
* tf2::MessageFilter + tests. Still need to change it around to pass in a callback queue, since we're being triggered directly from the tf2 buffer
* adding in y configuration test
* a little more realistic
* Don't add the request if the transform is already available. Add some new tests
* working transformable callbacks with a simple (incomplete) test case
* cleaning up test setup
* check_v implemented and passing v test and multi tree test
* working toward multi configuration tests
* removing restructuring for it won't nest like I thought
* continuing restructuring and filling in test case setup
* restructuring before scaling
* Completely remove lookupLists(). canTransform() now uses the same walking code as lookupTransform(). Also fixed a bug in the static transform publisher test
* testing chaining in a ring
* test dataset generator
* more complicated test with interleaving static and dynamic frames passing
* static transform tested and working
* test in progress, need to unshelve changes.
* tests passing and all throw catches removed too\!
* move to tf2_ros completed. tests pass again
* merge tf2_cpp and tf2_py into tf2_ros
* merging and fixing broken unittest
* Got transform with types working in python
* A working first version of transforming and converting between different types
* removing unused datatypes
* removing include of old tf from tf2
* testing new argument validation and catching bug
* unit test of single link one to try to debug eitan's client bug
* working towards interpolation too
* A working version of a test case for the python buffer client
* merging
* adding else to catch uncovered cases, and changing time for easier use
* Adding a test for the python buffer client
* using permuter now and doing a,b,c to a,b,c, at three different times including 0
* Moving tf2_tests to test_tf2
* moving test to new package
* initial package created for testing tf2
ros-geometry2-0.5.16/test_tf2/CMakeLists.txt 0000664 0000000 0000000 00000004353 13163650501 0020620 0 ustar 00root root 0000000 0000000 cmake_minimum_required(VERSION 2.8.3)
if(NOT CATKIN_ENABLE_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)
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() ros-geometry2-0.5.16/test_tf2/mainpage.dox 0000664 0000000 0000000 00000001112 13163650501 0020343 0 ustar 00root root 0000000 0000000 /**
\mainpage
\htmlinclude manifest.html
\b test_tf2 is ...
\section codeapi Code API
*/
ros-geometry2-0.5.16/test_tf2/package.xml 0000664 0000000 0000000 00000002373 13163650501 0020175 0 ustar 00root root 0000000 0000000
test_tf2
0.5.16
tf2 unit tests
Tully Foote
Eitan Marder-Eppstein
Tully Foote
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
rosconsole
roscpp
rostest
tf
tf2
tf2_bullet
tf2_ros
tf2_geometry_msgs
tf2_kdl
tf2_msgs
rosunit
rosbash
ros-geometry2-0.5.16/test_tf2/test/ 0000775 0000000 0000000 00000000000 13163650501 0017032 5 ustar 00root root 0000000 0000000 ros-geometry2-0.5.16/test_tf2/test/buffer_client_tester.launch 0000664 0000000 0000000 00000000502 13163650501 0024420 0 ustar 00root root 0000000 0000000
ros-geometry2-0.5.16/test_tf2/test/buffer_core_test.cpp 0000664 0000000 0000000 00000302627 13163650501 0023070 0 ustar 00root root 0000000 0000000 /*
* 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
* 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.
*/
#include
#include
#include "tf2/exceptions.h"
#include
#include
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "rostest/permuter.h"
void setIdentity(geometry_msgs::Transform& trans)
{
trans.translation.x = 0;
trans.translation.y = 0;
trans.translation.z = 0;
trans.rotation.x = 0;
trans.rotation.y = 0;
trans.rotation.z = 0;
trans.rotation.w = 1;
}
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);
dy.push_back(0.0);
children.push_back("c");
parents.push_back("b");
dx.push_back(1.0);
dy.push_back(0.0);
}
void push_back_y(std::vector& children, std::vector& parents,
std::vector& dx, std::vector& dy)
{
/*
"a"
v (1,0)
"b" ------(0,1)-----> "d"
v (1,0) v (0,1)
"c" "e"
*/
// a>b
children.push_back("b");
parents.push_back("a");
dx.push_back(1.0);
dy.push_back(0.0);
// b>c
children.push_back("c");
parents.push_back("b");
dx.push_back(1.0);
dy.push_back(0.0);
// b>d
children.push_back("d");
parents.push_back("b");
dx.push_back(0.0);
dy.push_back(1.0);
// d>e
children.push_back("e");
parents.push_back("d");
dx.push_back(0.0);
dy.push_back(1.0);
}
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"
v (1,0)
"c"
*/
// a>b
children.push_back("b");
parents.push_back("a");
dx.push_back(1.0);
dy.push_back(0.0);
// b>c
children.push_back("c");
parents.push_back("b");
dx.push_back(1.0);
dy.push_back(0.0);
// a>f
children.push_back("f");
parents.push_back("a");
dx.push_back(0.0);
dy.push_back(1.0);
// f>g
children.push_back("g");
parents.push_back("f");
dx.push_back(0.0);
dy.push_back(1.0);
}
void push_back_1(std::vector& children, std::vector& parents,
std::vector& dx, std::vector& dy)
{
children.push_back("2");
parents.push_back("1");
dx.push_back(1.0);
dy.push_back(0.0);
}
void setupTree(tf2::BufferCore& mBC, const std::string& mode, const ros::Time & time, const ros::Duration& interpolation_space = ros::Duration())
{
ROS_DEBUG("Clearing Buffer Core for new test setup");
mBC.clear();
ROS_DEBUG("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);
}
else if (mode == "y")
{
push_back_y(children, parents, dx, dy);
}
else if (mode == "v")
{
push_back_v(children, parents, dx, dy);
}
else if (mode == "ring_45")
{
/* 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");
frames.push_back("d");
frames.push_back("e");
frames.push_back("f");
frames.push_back("g");
frames.push_back("h");
frames.push_back("i");
for (uint8_t iteration = 0; iteration < 2; ++iteration)
{
double direction = 1;
std::string frame_prefix;
if (iteration == 0)
{
frame_prefix = "inverse_";
direction = -1;
}
else
frame_prefix ="";
for (uint64_t i = 1; i < frames.size(); i++)
{
geometry_msgs::TransformStamped ts;
setIdentity(ts.transform);
ts.transform.translation.x = direction * ( sqrt(2)/2 - 1);
ts.transform.translation.y = direction * sqrt(2)/2;
ts.transform.rotation.x = 0;
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 > ros::Time() + (interpolation_space * .5))
ts.header.stamp = time - (interpolation_space * .5);
else
ts.header.stamp = ros::Time();
ts.header.frame_id = frame_prefix + frames[i-1];
if (i > 1)
ts.child_frame_id = frame_prefix + frames[i];
else
ts.child_frame_id = frames[i]; // connect first frame
EXPECT_TRUE(mBC.setTransform(ts, "authority"));
if (interpolation_space > ros::Duration())
{
ts.header.stamp = time + interpolation_space * .5;
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);
push_back_v(children, parents, dx, dy);
}
else
EXPECT_FALSE("Undefined mode for tree setup. Test harness improperly setup.");
/// Standard
for (uint64_t i = 0; i < children.size(); i++)
{
geometry_msgs::TransformStamped ts;
setIdentity(ts.transform);
ts.transform.translation.x = dx[i];
ts.transform.translation.y = dy[i];
if (time > ros::Time() + (interpolation_space * .5))
ts.header.stamp = time - (interpolation_space * .5);
else
ts.header.stamp = ros::Time();
ts.header.frame_id = parents[i];
ts.child_frame_id = children[i];
EXPECT_TRUE(mBC.setTransform(ts, "authority"));
if (interpolation_space > ros::Duration())
{
ts.header.stamp = time + interpolation_space * .5;
EXPECT_TRUE(mBC.setTransform(ts, "authority"));
}
}
}
TEST(BufferCore_setTransform, NoInsertOnSelfTransform)
{
tf2::BufferCore mBC;
geometry_msgs::TransformStamped tranStamped;
setIdentity(tranStamped.transform);
tranStamped.header.stamp = ros::Time().fromNSec(10.0);
tranStamped.header.frame_id = "same_frame";
tranStamped.child_frame_id = "same_frame";
EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
}
TEST(BufferCore_setTransform, NoInsertWithNan)
{
tf2::BufferCore mBC;
geometry_msgs::TransformStamped tranStamped;
setIdentity(tranStamped.transform);
tranStamped.header.stamp = ros::Time().fromNSec(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;
setIdentity(tranStamped.transform);
tranStamped.header.stamp = ros::Time().fromNSec(10.0);
tranStamped.header.frame_id = "same_frame";
tranStamped.child_frame_id = "";
EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
tranStamped.child_frame_id = "/";
EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
}
TEST(BufferCore_setTransform, NoInsertWithNoParentID)
{
tf2::BufferCore mBC;
geometry_msgs::TransformStamped tranStamped;
setIdentity(tranStamped.transform);
tranStamped.header.stamp = ros::Time().fromNSec(10.0);
tranStamped.header.frame_id = "";
tranStamped.child_frame_id = "some_frame";
EXPECT_FALSE(mBC.setTransform(tranStamped, "authority"));
tranStamped.header.frame_id = "/";
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])), ros::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)), ros::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])), ros::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])), ros::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)), ros::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])), ros::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)), ros::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])), ros::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])), ros::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)), ros::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])), ros::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])), ros::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)), ros::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])), ros::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])), ros::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)), ros::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(BufferCore_lookupTransform, i_configuration)
{
double epsilon = 1e-6;
rostest::Permuter permuter;
std::vector times;
times.push_back(ros::Time(1.0));
times.push_back(ros::Time(10.0));
times.push_back(ros::Time(0.0));
ros::Time eval_time;
permuter.addOptionSet(times, &eval_time);
std::vector durations;
durations.push_back(ros::Duration(1.0));
durations.push_back(ros::Duration(0.001));
durations.push_back(ros::Duration(0.1));
ros::Duration interpolation_space;
// permuter.addOptionSet(durations, &interpolation_space);
std::vector frames;
frames.push_back("a");
frames.push_back("b");
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())
{
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());
EXPECT_EQ(outpose.header.stamp, eval_time);
EXPECT_EQ(outpose.header.frame_id, source_frame);
EXPECT_EQ(outpose.child_frame_id, target_frame);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
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, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
//Zero distance
if (source_frame == target_frame)
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
}
else if ((source_frame == "a" && target_frame =="b") ||
(source_frame == "b" && target_frame =="c"))
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
}
else if ((source_frame == "b" && target_frame =="a") ||
(source_frame == "c" && target_frame =="b"))
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
}
else if (source_frame == "a" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
}
else if (source_frame == "c" && target_frame =="a")
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
}
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());
}
}
}
/* 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 ros::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);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
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, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
//Zero distance
if (source_frame == target_frame)
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
}
else if (source_frame == "1" && target_frame =="2")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
}
else if (source_frame == "2" && target_frame =="1")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
}
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;
}
/* 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 ros::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);
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, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
//Zero distance
if (source_frame == target_frame)
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
}
else if ((source_frame == "a" && target_frame =="b") ||
(source_frame == "b" && target_frame =="c"))
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if ((source_frame == "b" && target_frame =="a") ||
(source_frame == "c" && target_frame =="b"))
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if ((source_frame == "a" && target_frame =="f") ||
(source_frame == "f" && target_frame =="g"))
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
}
else if ((source_frame == "f" && target_frame =="a") ||
(source_frame == "g" && target_frame =="f"))
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
}
else if (source_frame == "a" && target_frame =="g")
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
}
else if (source_frame == "g" && target_frame =="a")
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
}
else if (source_frame == "a" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if (source_frame == "c" && target_frame =="a")
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if (source_frame == "b" && target_frame =="f")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
}
else if (source_frame == "f" && target_frame =="b")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
}
else if (source_frame == "c" && target_frame =="f")
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
}
else if (source_frame == "f" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
}
else if (source_frame == "b" && target_frame =="g")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
}
else if (source_frame == "g" && target_frame =="b")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
}
else if (source_frame == "c" && target_frame =="g")
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
}
else if (source_frame == "g" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
}
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;
}
/* 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 ros::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);
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, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon);
//Zero distance
if (source_frame == target_frame)
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
}
else if ((source_frame == "a" && target_frame =="b") ||
(source_frame == "b" && target_frame =="c"))
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if ((source_frame == "b" && target_frame =="a") ||
(source_frame == "c" && target_frame =="b"))
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if ((source_frame == "b" && target_frame =="d") ||
(source_frame == "d" && target_frame =="e"))
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
}
else if ((source_frame == "d" && target_frame =="b") ||
(source_frame == "e" && target_frame =="d"))
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
}
else if (source_frame == "b" && target_frame =="e")
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
}
else if (source_frame == "e" && target_frame =="b")
{
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
}
else if (source_frame == "a" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if (source_frame == "c" && target_frame =="a")
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
}
else if (source_frame == "a" && target_frame =="d")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
}
else if (source_frame == "d" && target_frame =="a")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
}
else if (source_frame == "c" && target_frame =="d")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
}
else if (source_frame == "d" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
}
else if (source_frame == "a" && target_frame =="e")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
}
else if (source_frame == "e" && target_frame =="a")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
}
else if (source_frame == "c" && target_frame =="e")
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon);
}
else if (source_frame == "e" && target_frame =="c")
{
EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon);
}
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;
}
TEST(BufferCore_lookupTransform, one_link_configuration)
{
double epsilon = 1e-6;
rostest::Permuter permuter;
std::vector times;
times.push_back(ros::Time(1.0));
times.push_back(ros::Time(10.0));
times.push_back(ros::Time(0.0));
ros::Time eval_time;
permuter.addOptionSet(times, &eval_time);
std::vector durations;
durations.push_back(ros::Duration(1.0));
durations.push_back(ros::Duration(0.001));
durations.push_back(ros::Duration(0.1));
ros::Duration 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);
while (permuter.step())
{
tf2::BufferCore mBC;
setupTree(mBC, "1", eval_time, interpolation_space);
geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon));
}
}
TEST(BufferCore_lookupTransform, v_configuration)
{
double epsilon = 1e-6;
rostest::Permuter permuter;
std::vector times;
times.push_back(ros::Time(1.0));
times.push_back(ros::Time(10.0));
times.push_back(ros::Time(0.0));
ros::Time eval_time;
permuter.addOptionSet(times, &eval_time);
std::vector durations;
durations.push_back(ros::Duration(1.0));
durations.push_back(ros::Duration(0.001));
durations.push_back(ros::Duration(0.1));
ros::Duration interpolation_space;
// permuter.addOptionSet(durations, &interpolation_space);
std::vector frames;
frames.push_back("a");
frames.push_back("b");
frames.push_back("c");
frames.push_back("f");
frames.push_back("g");
std::string source_frame;
permuter.addOptionSet(frames, &source_frame);
std::string target_frame;
permuter.addOptionSet(frames, &target_frame);
while (permuter.step())
{
tf2::BufferCore mBC;
setupTree(mBC, "v", eval_time, interpolation_space);
geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon));
}
}
TEST(BufferCore_lookupTransform, y_configuration)
{
double epsilon = 1e-6;
rostest::Permuter permuter;
std::vector times;
times.push_back(ros::Time(1.0));
times.push_back(ros::Time(10.0));
times.push_back(ros::Time(0.0));
ros::Time eval_time;
permuter.addOptionSet(times, &eval_time);
std::vector durations;
durations.push_back(ros::Duration(1.0));
durations.push_back(ros::Duration(0.001));
durations.push_back(ros::Duration(0.1));
ros::Duration interpolation_space;
// permuter.addOptionSet(durations, &interpolation_space);
std::vector frames;
frames.push_back("a");
frames.push_back("b");
frames.push_back("c");
frames.push_back("d");
frames.push_back("e");
std::string source_frame;
permuter.addOptionSet(frames, &source_frame);
std::string target_frame;
permuter.addOptionSet(frames, &target_frame);
while (permuter.step())
{
tf2::BufferCore mBC;
setupTree(mBC, "y", eval_time, interpolation_space);
geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time);
EXPECT_TRUE(check_y_result(outpose, source_frame, target_frame, eval_time, epsilon));
}
}
TEST(BufferCore_lookupTransform, multi_configuration)
{
double epsilon = 1e-6;
rostest::Permuter permuter;
std::vector times;
times.push_back(ros::Time(1.0));
times.push_back(ros::Time(10.0));
times.push_back(ros::Time(0.0));
ros::Time eval_time;
permuter.addOptionSet(times, &eval_time);
std::vector durations;
durations.push_back(ros::Duration(1.0));
durations.push_back(ros::Duration(0.001));
durations.push_back(ros::Duration(0.1));
ros::Duration interpolation_space;
// permuter.addOptionSet(durations, &interpolation_space);
std::vector frames;
frames.push_back("1");
frames.push_back("2");
frames.push_back("a");
frames.push_back("b");
frames.push_back("c");
frames.push_back("f");
frames.push_back("g");
std::string source_frame;
permuter.addOptionSet(frames, &source_frame);
std::string target_frame;
permuter.addOptionSet(frames, &target_frame);
while (permuter.step())
{
tf2::BufferCore mBC;
setupTree(mBC, "1_v", eval_time, interpolation_space);
if (mBC.canTransform(source_frame, target_frame, eval_time))
{
geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, 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));
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") &&
(target_frame == "1" || target_frame == "2") )
||
((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); \
double angle = q1.angle(q2); \
EXPECT_TRUE(fabs(angle) < _epsilon || fabs(angle - M_PI) < _epsilon); \
}
#define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps) \
EXPECT_NEAR(_out.transform.translation.x, _expected.getOrigin().x(), epsilon); \
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", ros::Time());
CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon);
// root -> b -> c -> d
geometry_msgs::TransformStamped out_rootd = mBC.lookupTransform("root", "d", ros::Time());
CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon);
// a <- root -> b
geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", ros::Time());
CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon);
geometry_msgs::TransformStamped out_ba = mBC.lookupTransform("b", "a", ros::Time());
CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon);
// a <- root -> b -> c
geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", ros::Time());
CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon);
geometry_msgs::TransformStamped out_ca = mBC.lookupTransform("c", "a", ros::Time());
CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon);
// a <- root -> b -> c -> d
geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", ros::Time());
CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon);
geometry_msgs::TransformStamped out_da = mBC.lookupTransform("d", "a", ros::Time());
CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon);
// b -> c
geometry_msgs::TransformStamped out_cb = mBC.lookupTransform("c", "b", ros::Time());
CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon);
geometry_msgs::TransformStamped out_bc = mBC.lookupTransform("b", "c", ros::Time());
CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon);
// b -> c -> d
geometry_msgs::TransformStamped out_bd = mBC.lookupTransform("b", "d", ros::Time());
CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon);
geometry_msgs::TransformStamped out_db = mBC.lookupTransform("d", "b", ros::Time());
CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon);
}
// Time varying transforms, testing interpolation
TEST(BufferCore_lookupTransform, helix_configuration)
{
double epsilon = 2e-5; // Larger epsilon for interpolation values
tf2::BufferCore mBC;
ros::Time t0 = ros::Time() + ros::Duration(10);
ros::Duration step = ros::Duration(0.05);
ros::Duration half_step = ros::Duration(0.025);
ros::Time t1 = t0 + ros::Duration(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 (ros::Time t = t0; t <= t1; t += step)
{
ros::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 (ros::Time t = t0 + half_step; t < t1; t += step)
{
ros::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 (ros::Time t = t0 + half_step; t < t1; t += (step + step))
{
ros::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, ring_45_configuration)
{
double epsilon = 1e-6;
rostest::Permuter permuter;
std::vector times;
times.push_back(ros::Time(1.0));
times.push_back(ros::Time(10.0));
times.push_back(ros::Time(0.0));
ros::Time eval_time;
permuter.addOptionSet(times, &eval_time);
std::vector durations;
durations.push_back(ros::Duration(1.0));
durations.push_back(ros::Duration(0.001));
durations.push_back(ros::Duration(0.1));
ros::Duration interpolation_space;
// permuter.addOptionSet(durations, &interpolation_space);
std::vector frames;
frames.push_back("a");
frames.push_back("b");
frames.push_back("c");
frames.push_back("d");
frames.push_back("e");
frames.push_back("f");
frames.push_back("g");
frames.push_back("h");
frames.push_back("i");
/* frames.push_back("inverse_b");
frames.push_back("inverse_c");
frames.push_back("inverse_d");
frames.push_back("inverse_e");
frames.push_back("inverse_f");
frames.push_back("inverse_g");
frames.push_back("inverse_h");
frames.push_back("inverse_i");*/
std::string source_frame;
permuter.addOptionSet(frames, &source_frame);
std::string target_frame;
permuter.addOptionSet(frames, &target_frame);
while (permuter.step())
{
tf2::BufferCore mBC;
setupTree(mBC, "ring_45", 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());
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") ||
(source_frame == "i" && target_frame == "a") ||
(source_frame == "a" && target_frame == "inverse_i") ||
(source_frame == "inverse_i" && target_frame == "a") )
{
//printf ("here %s %s\n", source_frame.c_str(), target_frame.c_str());
EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
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, 0, epsilon);
EXPECT_NEAR(fabs(outpose.transform.rotation.w), 1, epsilon);
}
// Chaining 1
else if ((source_frame == "a" && target_frame =="b") ||
(source_frame == "b" && target_frame =="c") ||
(source_frame == "c" && target_frame =="d") ||
(source_frame == "d" && target_frame =="e") ||
(source_frame == "e" && target_frame =="f") ||
(source_frame == "f" && target_frame =="g") ||
(source_frame == "g" && target_frame =="h") ||
(source_frame == "h" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
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/8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/8), epsilon);
}
// Inverse Chaining 1
else if ((source_frame == "b" && target_frame =="a") ||
(source_frame == "c" && target_frame =="b") ||
(source_frame == "d" && target_frame =="c") ||
(source_frame == "e" && target_frame =="d") ||
(source_frame == "f" && target_frame =="e") ||
(source_frame == "g" && target_frame =="f") ||
(source_frame == "h" && target_frame =="g") ||
(source_frame == "i" && target_frame =="h")
)
{
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon);
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/8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/8), epsilon);
}
// Chaining 2
else if ((source_frame == "a" && target_frame =="c") ||
(source_frame == "b" && target_frame =="d") ||
(source_frame == "c" && target_frame =="e") ||
(source_frame == "d" && target_frame =="f") ||
(source_frame == "e" && target_frame =="g") ||
(source_frame == "f" && target_frame =="h") ||
(source_frame == "g" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon);
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/4), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/4), epsilon);
}
// Inverse Chaining 2
else if ((source_frame == "c" && target_frame =="a") ||
(source_frame == "d" && target_frame =="b") ||
(source_frame == "e" && target_frame =="c") ||
(source_frame == "f" && target_frame =="d") ||
(source_frame == "g" && target_frame =="e") ||
(source_frame == "h" && target_frame =="f") ||
(source_frame == "i" && target_frame =="g")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon);
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/4), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/4), epsilon);
}
// Chaining 3
else if ((source_frame == "a" && target_frame =="d") ||
(source_frame == "b" && target_frame =="e") ||
(source_frame == "c" && target_frame =="f") ||
(source_frame == "d" && target_frame =="g") ||
(source_frame == "e" && target_frame =="h") ||
(source_frame == "f" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
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);
}
// Inverse Chaining 3
else if ((target_frame == "a" && source_frame =="d") ||
(target_frame == "b" && source_frame =="e") ||
(target_frame == "c" && source_frame =="f") ||
(target_frame == "d" && source_frame =="g") ||
(target_frame == "e" && source_frame =="h") ||
(target_frame == "f" && source_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2)/2 , epsilon);
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);
}
// Chaining 4
else if ((source_frame == "a" && target_frame =="e") ||
(source_frame == "b" && target_frame =="f") ||
(source_frame == "c" && target_frame =="g") ||
(source_frame == "d" && target_frame =="h") ||
(source_frame == "e" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon);
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.w, cos(M_PI/2), epsilon);
}
// Inverse Chaining 4
else if ((target_frame == "a" && source_frame =="e") ||
(target_frame == "b" && source_frame =="f") ||
(target_frame == "c" && source_frame =="g") ||
(target_frame == "d" && source_frame =="h") ||
(target_frame == "e" && source_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon);
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.w, cos(-M_PI/2), epsilon);
}
// Chaining 5
else if ((source_frame == "a" && target_frame =="f") ||
(source_frame == "b" && target_frame =="g") ||
(source_frame == "c" && target_frame =="h") ||
(source_frame == "d" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) /2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2) /2 , epsilon);
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);
}
// Inverse Chaining 5
else if ((target_frame == "a" && source_frame =="f") ||
(target_frame == "b" && source_frame =="g") ||
(target_frame == "c" && source_frame =="h") ||
(target_frame == "d" && source_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
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);
}
// Chaining 6
else if ((source_frame == "a" && target_frame =="g") ||
(source_frame == "b" && target_frame =="h") ||
(source_frame == "c" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon);
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);
}
// Inverse Chaining 6
else if ((target_frame == "a" && source_frame =="g") ||
(target_frame == "b" && source_frame =="h") ||
(target_frame == "c" && source_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon);
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);
}
// Chaining 7
else if ((source_frame == "a" && target_frame =="h") ||
(source_frame == "b" && target_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon);
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);
}
// Inverse Chaining 7
else if ((target_frame == "a" && source_frame =="h") ||
(target_frame == "b" && source_frame =="i")
)
{
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon);
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);
}
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());
}
}
}
TEST(BufferCore_lookupTransform, invalid_arguments)
{
tf2::BufferCore mBC;
setupTree(mBC, "i", ros::Time(1.0));
EXPECT_NO_THROW(mBC.lookupTransform("b", "a", ros::Time()));
//Empty frame_id
EXPECT_THROW(mBC.lookupTransform("", "a", ros::Time()), tf2::InvalidArgumentException);
EXPECT_THROW(mBC.lookupTransform("b", "", ros::Time()), tf2::InvalidArgumentException);
//frame_id with /
EXPECT_THROW(mBC.lookupTransform("/b", "a", ros::Time()), tf2::InvalidArgumentException);
EXPECT_THROW(mBC.lookupTransform("b", "/a", ros::Time()), tf2::InvalidArgumentException);
};
TEST(BufferCore_canTransform, invalid_arguments)
{
tf2::BufferCore mBC;
setupTree(mBC, "i", ros::Time(1.0));
EXPECT_TRUE(mBC.canTransform("b", "a", ros::Time()));
//Empty frame_id
EXPECT_FALSE(mBC.canTransform("", "a", ros::Time()));
EXPECT_FALSE(mBC.canTransform("b", "", ros::Time()));
//frame_id with /
EXPECT_FALSE(mBC.canTransform("/b", "a", ros::Time()));
EXPECT_FALSE(mBC.canTransform("b", "/a", ros::Time()));
};
struct TransformableHelper
{
TransformableHelper()
: called(false)
{}
void callback(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
ros::Time time, tf2::TransformableResult result)
{
called = true;
}
bool called;
};
TEST(BufferCore_transformableCallbacks, alreadyTransformable)
{
tf2::BufferCore b;
TransformableHelper h;
geometry_msgs::TransformStamped t;
t.header.stamp = ros::Time(1);
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", ros::Time(1)), 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", ros::Time(10)), 0U);
geometry_msgs::TransformStamped t;
for (uint32_t i = 1; i <= 10; ++i)
{
t.header.stamp = ros::Time(i);
t.header.frame_id = "a";
t.child_frame_id = "b";
t.transform.rotation.w = 1.0;
b.setTransform(t, "me");
if (i < 10)
{
ASSERT_FALSE(h.called);
}
else
{
ASSERT_TRUE(h.called);
}
}
}
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", ros::Time(1)), 0U);
geometry_msgs::TransformStamped t;
for (uint32_t i = 10; i > 0; --i)
{
t.header.stamp = ros::Time(i);
t.header.frame_id = "a";
t.child_frame_id = "b";
t.transform.rotation.w = 1.0;
b.setTransform(t, "me");
if (i > 1)
{
ASSERT_FALSE(h.called);
}
else
{
ASSERT_TRUE(h.called);
}
}
}
/*
TEST(tf, Exceptions)
{
tf::Transformer mTR(true);
Stamped outpose;
//connectivity when no data
EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000)));
try
{
mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose);
EXPECT_FALSE("ConnectivityException Not Thrown");
}
catch ( tf::LookupException &ex)
{
EXPECT_TRUE("Lookupgh Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me"));
//Extrapolation not valid with one value
EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
try
{
mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
EXPECT_TRUE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_TRUE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me"));
//NO Extration when Interpolating
//inverse list
EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000)));
try
{
mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose);
EXPECT_TRUE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_FALSE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
//forward list
EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000)));
try
{
mTR.transformPose("me",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose);
EXPECT_TRUE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_FALSE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
//Extrapolating backwards
//inverse list
EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000)));
try
{
mTR.transformPose("parent",Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose);
EXPECT_FALSE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_TRUE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
//forwards list
EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000)));
try
{
mTR.transformPose("me",Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose);
EXPECT_FALSE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_TRUE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
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", ros::Time().fromNSec(350000)));
try
{
mTR.transformPose("parent", Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose);
EXPECT_FALSE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_TRUE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
//forward list
EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000)));
try
{
mTR.transformPose("me", Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose);
EXPECT_FALSE("ExtrapolationException Not Thrown");
}
catch ( tf::ExtrapolationException &ex)
{
EXPECT_TRUE("Extrapolation Exception Caught");
}
catch (tf::TransformException& ex)
{
printf("%s\n",ex.what());
EXPECT_FALSE("Other Exception Caught");
}
}
TEST(tf, NoExtrapolationExceptionFromParent)
{
tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "a"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "b"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "b"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent's parent", "parent's parent"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent", "parent"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent's parent", "parent's parent"));
Stamped output;
try
{
mTR.transformPoint( "b", Stamped(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output);
}
catch (ExtrapolationException &ex)
{
EXPECT_FALSE("Shouldn't have gotten this exception");
}
};
TEST(tf, ExtrapolationFromOneValue)
{
tf::Transformer mTR(true, ros::Duration().fromNSec(1000000));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent"));
Stamped output;
bool excepted = false;
//Past time
try
{
mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10), "a"), output);
}
catch (ExtrapolationException &ex)
{
excepted = true;
}
EXPECT_TRUE(excepted);
excepted = false;
//Future one element
try
{
mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output);
}
catch (ExtrapolationException &ex)
{
excepted = true;
}
EXPECT_TRUE(excepted);
//Past multi link
excepted = false;
try
{
mTR.transformPoint( "parent's parent", Stamped(Point(1,1,1), ros::Time().fromNSec(1), "a"), output);
}
catch (ExtrapolationException &ex)
{
excepted = true;
}
EXPECT_TRUE(excepted);
//Future case multi link
excepted = false;
try
{
mTR.transformPoint( "parent's parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
}
catch (ExtrapolationException &ex)
{
excepted = true;
}
EXPECT_TRUE(excepted);
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(20000), "parent", "a"));
excepted = false;
try
{
mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output);
}
catch (ExtrapolationException &ex)
{
excepted = true;
}
EXPECT_FALSE(excepted);
};
TEST(tf, getLatestCommonTime)
{
tf::Transformer mTR(true);
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(2000), "parent's parent", "parent"));
//simple case
ros::Time t;
mTR.getLatestCommonTime("a", "parent's parent", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(1000));
//no connection
EXPECT_EQ(tf::LOOKUP_ERROR, mTR.getLatestCommonTime("a", "not valid", t, NULL));
EXPECT_EQ(t, ros::Time());
//testing with update
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000), "parent", "a"));
mTR.getLatestCommonTime("a", "parent's parent",t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(2000));
//longer chain
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000), "b", "c"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(9000), "c", "d"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(5000), "f", "e"));
//shared parent
mTR.getLatestCommonTime("a", "b",t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(3000));
//two degrees
mTR.getLatestCommonTime("a", "c", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(3000));
//reversed
mTR.getLatestCommonTime("c", "a", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(3000));
//three degrees
mTR.getLatestCommonTime("a", "d", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(3000));
//reversed
mTR.getLatestCommonTime("d", "a", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(3000));
//disconnected tree
mTR.getLatestCommonTime("e", "f", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(5000));
//reversed order
mTR.getLatestCommonTime("f", "e", t, NULL);
EXPECT_EQ(t, ros::Time().fromNSec(5000));
mTR.setExtrapolationLimit(ros::Duration().fromNSec(20000));
//check timestamps resulting
tf::Stamped output, output2;
try
{
mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time(), "b"), output);
mTR.transformPoint( "a", ros::Time(),Stamped(Point(1,1,1), ros::Time(), "b"), "c", output2);
}
catch (tf::TransformException &ex)
{
printf("%s\n", ex.what());
EXPECT_FALSE("Shouldn't get this Exception");
}
EXPECT_EQ(output.stamp_, ros::Time().fromNSec(4000));
EXPECT_EQ(output2.stamp_, ros::Time().fromNSec(3000));
//zero length lookup zero time
ros::Time now1 = ros::Time::now();
ros::Time time_output;
mTR.getLatestCommonTime("a", "a", time_output, NULL);
EXPECT_LE(now1.toSec(), time_output.toSec());
EXPECT_LE(time_output.toSec(), ros::Time::now().toSec());
}
TEST(tf, RepeatedTimes)
{
Transformer mTR;
mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b"));
tf::StampedTransform output;
try{
mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output);
EXPECT_TRUE(!std::isnan(output.getOrigin().x()));
EXPECT_TRUE(!std::isnan(output.getOrigin().y()));
EXPECT_TRUE(!std::isnan(output.getOrigin().z()));
EXPECT_TRUE(!std::isnan(output.getRotation().x()));
EXPECT_TRUE(!std::isnan(output.getRotation().y()));
EXPECT_TRUE(!std::isnan(output.getRotation().z()));
EXPECT_TRUE(!std::isnan(output.getRotation().w()));
}
catch (...)
{
EXPECT_FALSE("Excetion improperly thrown");
}
}
TEST(tf, frameExists)
{
Transformer mTR;
// test with fully qualified name
EXPECT_FALSE(mTR.frameExists("/b"));;
EXPECT_FALSE(mTR.frameExists("/parent"));
EXPECT_FALSE(mTR.frameExists("/other"));
EXPECT_FALSE(mTR.frameExists("/frame"));
//test with resolveping
EXPECT_FALSE(mTR.frameExists("b"));;
EXPECT_FALSE(mTR.frameExists("parent"));
EXPECT_FALSE(mTR.frameExists("other"));
EXPECT_FALSE(mTR.frameExists("frame"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b"));
// test with fully qualified name
EXPECT_TRUE(mTR.frameExists("/b"));
EXPECT_TRUE(mTR.frameExists("/parent"));
EXPECT_FALSE(mTR.frameExists("/other"));
EXPECT_FALSE(mTR.frameExists("/frame"));
//Test with resolveping
EXPECT_TRUE(mTR.frameExists("b"));
EXPECT_TRUE(mTR.frameExists("parent"));
EXPECT_FALSE(mTR.frameExists("other"));
EXPECT_FALSE(mTR.frameExists("frame"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::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"));
EXPECT_TRUE(mTR.frameExists("other"));
EXPECT_TRUE(mTR.frameExists("frame"));
}
TEST(tf, resolve)
{
//no prefix
EXPECT_STREQ("/id", tf::resolve("","id").c_str());
//prefix w/o /
EXPECT_STREQ("/asdf/id", tf::resolve("asdf","id").c_str());
//prefix w /
EXPECT_STREQ("/asdf/id", tf::resolve("/asdf","id").c_str());
// frame_id w / -> no prefix
EXPECT_STREQ("/id", tf::resolve("asdf","/id").c_str());
// frame_id w / -> no prefix
EXPECT_STREQ("/id", tf::resolve("/asdf","/id").c_str());
}
TEST(tf, canTransform)
{
Transformer mTR;
//confirm zero length list disconnected will return true
EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time()));
EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now()));
//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)), ros::Time().fromSec(i), "parent", "child"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child"));
}
// four different timestamps related to tf state
ros::Time zero_time = ros::Time().fromSec(0);
ros::Time old_time = ros::Time().fromSec(5);
ros::Time valid_time = ros::Time().fromSec(15);
ros::Time future_time = ros::Time().fromSec(25);
//confirm zero length list disconnected will return true
EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time));
EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time));
EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time));
EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time));
// Basic API Tests
//Valid data should pass
EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time));
EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time));
//zero data should pass
EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time));
EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time));
//Old data should fail
EXPECT_FALSE(mTR.canTransform("child", "parent", old_time));
EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time));
//Future data should fail
EXPECT_FALSE(mTR.canTransform("child", "parent", future_time));
EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time));
//Same Frame should pass for all times
EXPECT_TRUE(mTR.canTransform("child", "child", zero_time));
EXPECT_TRUE(mTR.canTransform("child", "child", old_time));
EXPECT_TRUE(mTR.canTransform("child", "child", valid_time));
EXPECT_TRUE(mTR.canTransform("child", "child", future_time));
// Advanced API Tests
// Source = Fixed
//zero data in fixed frame should pass
EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child"));
EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child"));
//Old data in fixed frame should pass
EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child"));
EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child"));
//valid data in fixed frame should pass
EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child"));
EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child"));
//future data in fixed frame should pass
EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child"));
EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child"));
//transforming through fixed into the past
EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child"));
EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child"));
//transforming through fixed into the future
EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child"));
EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child"));
// Target = Fixed
//zero data in fixed frame should pass
EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent"));
//Old data in fixed frame should pass
EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent"));
//valid data in fixed frame should pass
EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
//future data in fixed frame should pass
EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent"));
//transforming through fixed into the zero
EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent"));
//transforming through fixed into the past
EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent"));
//transforming through fixed into the valid
EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent"));
//transforming through fixed into the future
EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent"));
}
TEST(tf, lookupTransform)
{
Transformer mTR;
//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)), ros::Time().fromSec(i), "parent", "child"));
mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child"));
}
// four different timestamps related to tf state
ros::Time zero_time = ros::Time().fromSec(0);
ros::Time old_time = ros::Time().fromSec(5);
ros::Time valid_time = ros::Time().fromSec(15);
ros::Time future_time = ros::Time().fromSec(25);
//output
tf::StampedTransform output;
// Basic API Tests
try
{
//confirm zero length list disconnected will return true
mTR.lookupTransform("some_frame","some_frame", zero_time, output);
mTR.lookupTransform("some_frame","some_frame", old_time, output);
mTR.lookupTransform("some_frame","some_frame", valid_time, output);
mTR.lookupTransform("some_frame","some_frame", future_time, output);
mTR.lookupTransform("child","child", future_time, output);
mTR.lookupTransform("other_child","other_child", future_time, output);
//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);
}
catch (tf::TransformException &ex)
{
printf("Exception improperly thrown: %s", ex.what());
EXPECT_FALSE("Exception thrown");
}
try
{
//Old data should fail
mTR.lookupTransform("child", "parent", old_time, output);
EXPECT_FALSE("Exception should have been thrown");
}
catch (tf::TransformException)
{
EXPECT_TRUE("Exception Thrown Correctly");
}
try {
//Future data should fail
mTR.lookupTransform("child", "parent", future_time, output);
EXPECT_FALSE("Exception should have been thrown");
}
catch (tf::TransformException)
{
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);
mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output);
//Old data in fixed frame should pass
mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output);
mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output);
//valid data in fixed frame should pass
mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output);
mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output);
//future data in fixed frame should pass
mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output);
mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output);
}
catch (tf::TransformException &ex)
{
printf("Exception improperly thrown: %s", ex.what());
EXPECT_FALSE("Exception incorrectly thrown");
}
try {
//transforming through fixed into the past
mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output);
EXPECT_FALSE("Exception should have been thrown");
}
catch (tf::TransformException)
{
EXPECT_TRUE("Exception Thrown Correctly");
}
try {
//transforming through fixed into the future
mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output);
EXPECT_FALSE("Exception should have been thrown");
}
catch (tf::TransformException)
{
EXPECT_TRUE("Exception Thrown Correctly");
}
try {
// Target = Fixed
//zero data in fixed frame should pass
mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output);
//valid data in fixed frame should pass
mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
}
catch (tf::TransformException &ex)
{
printf("Exception improperly thrown: %s", ex.what());
EXPECT_FALSE("Exception incorrectly thrown");
}
try {
//Old data in fixed frame should pass
mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output);
EXPECT_FALSE("Exception should have been thrown");
}
catch (tf::TransformException)
{
EXPECT_TRUE("Exception Thrown Correctly");
}
try {
//future data in fixed frame should pass
mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output);
EXPECT_FALSE("Exception should have been thrown");
}
catch (tf::TransformException)
{
EXPECT_TRUE("Exception Thrown Correctly");
}
try {
//transforming through fixed into the zero
mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output);
//transforming through fixed into the past
mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output);
//transforming through fixed into the valid
mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output);
//transforming through fixed into the future
mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output);
}
catch (tf::TransformException &ex)
{
printf("Exception improperly thrown: %s", ex.what());
EXPECT_FALSE("Exception improperly thrown");
}
//make sure zero goes to now for zero length
try
{
ros::Time now1 = ros::Time::now();
mTR.lookupTransform("a", "a", ros::Time(),output);
EXPECT_LE(now1.toSec(), output.stamp_.toSec());
EXPECT_LE(output.stamp_.toSec(), ros::Time::now().toSec());
}
catch (tf::TransformException &ex)
{
printf("Exception improperly thrown: %s", ex.what());
EXPECT_FALSE("Exception improperly thrown");
}
}
TEST(tf, getFrameStrings)
{
Transformer mTR;
mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b"));
std::vector frames_string;
mTR.getFrameStrings(frames_string);
ASSERT_EQ(frames_string.size(), (unsigned)2);
EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
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)), ros::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());
EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str());
EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str());
}
bool expectInvalidQuaternion(tf::Quaternion q)
{
try
{
tf::assertQuaternionValid(q);
printf("this should have thrown\n");
return false;
}
catch (tf::InvalidArgument &ex)
{
return true;
}
catch (...)
{
printf("A different type of exception was expected\n");
return false;
}
return false;
}
bool expectValidQuaternion(tf::Quaternion q)
{
try
{
tf::assertQuaternionValid(q);
}
catch (tf::TransformException &ex)
{
return false;
}
return true;
}
bool expectInvalidQuaternion(geometry_msgs::Quaternion q)
{
try
{
tf::assertQuaternionValid(q);
printf("this should have thrown\n");
return false;
}
catch (tf::InvalidArgument &ex)
{
return true;
}
catch (...)
{
printf("A different type of exception was expected\n");
return false;
}
return false;
}
bool expectValidQuaternion(geometry_msgs::Quaternion q)
{
try
{
tf::assertQuaternionValid(q);
}
catch (tf::TransformException &ex)
{
return false;
}
return true;
}
TEST(tf, assertQuaternionValid)
{
tf::Quaternion q(1,0,0,0);
EXPECT_TRUE(expectValidQuaternion(q));
q.setX(0);
EXPECT_TRUE(expectInvalidQuaternion(q));
q.setY(1);
EXPECT_TRUE(expectValidQuaternion(q));
q.setZ(1);
EXPECT_TRUE(expectInvalidQuaternion(q));
q.setY(0);
EXPECT_TRUE(expectValidQuaternion(q));
q.setW(1);
EXPECT_TRUE(expectInvalidQuaternion(q));
q.setZ(0);
EXPECT_TRUE(expectValidQuaternion(q));
q.setZ(sqrt(2.0)/2.0);
EXPECT_TRUE(expectInvalidQuaternion(q));
q.setW(sqrt(2.0)/2.0);
EXPECT_TRUE(expectValidQuaternion(q));
q.setZ(sqrt(2.0)/2.0 + 0.01);
EXPECT_TRUE(expectInvalidQuaternion(q));
q.setZ(sqrt(2.0)/2.0 - 0.01);
EXPECT_TRUE(expectInvalidQuaternion(q));
EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
// Waiting for gtest 1.1 or later
// EXPECT_NO_THROW(tf::assertQuaternionValid(q));
//q.setX(0);
//EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
//q.setY(1);
//EXPECT_NO_THROW(tf::assertQuaternionValid(q));
}
TEST(tf, assertQuaternionMsgValid)
{
geometry_msgs::Quaternion q;
q.x = 1;//others zeroed to start
EXPECT_TRUE(expectValidQuaternion(q));
q.x = 0;
EXPECT_TRUE(expectInvalidQuaternion(q));
q.y = 1;
EXPECT_TRUE(expectValidQuaternion(q));
q.z = 1;
EXPECT_TRUE(expectInvalidQuaternion(q));
q.y = 0;
EXPECT_TRUE(expectValidQuaternion(q));
q.w = 1;
EXPECT_TRUE(expectInvalidQuaternion(q));
q.z = 0;
EXPECT_TRUE(expectValidQuaternion(q));
q.z = sqrt(2.0)/2.0;
EXPECT_TRUE(expectInvalidQuaternion(q));
q.w = sqrt(2.0)/2.0;
EXPECT_TRUE(expectValidQuaternion(q));
q.z = sqrt(2.0)/2.0 + 0.01;
EXPECT_TRUE(expectInvalidQuaternion(q));
q.z = sqrt(2.0)/2.0 - 0.01;
EXPECT_TRUE(expectInvalidQuaternion(q));
// Waiting for gtest 1.1 or later
// EXPECT_NO_THROW(tf::assertQuaternionValid(q));
//q.x = 0);
//EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument);
//q.y = 1);
//EXPECT_NO_THROW(tf::assertQuaternionValid(q));
}
TEST(tf2_stamped, OperatorEqualEqual)
{
btTransform transform0, transform1, transform0a;
transform0.setIdentity();
transform0a.setIdentity();
transform1.setIdentity();
transform1.setOrigin(btVector3(1, 0, 0));
tf2::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id");
tf2::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id");
EXPECT_TRUE(stamped_transform0A == stamped_transform_reference); // Equal
tf2::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id");
EXPECT_FALSE(stamped_transform0B == stamped_transform_reference); // Different Frame id
tf2::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id");
EXPECT_FALSE(stamped_transform0C == stamped_transform_reference); // Different Time
tf2::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
EXPECT_FALSE(stamped_transform0D == stamped_transform_reference); // Different frame id and time
tf2::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id");
EXPECT_FALSE(stamped_transform0E == stamped_transform_reference); // Different transform, frame id
tf2::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id");
EXPECT_FALSE(stamped_transform0F == stamped_transform_reference); // Different transform, time
tf2::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id");
EXPECT_FALSE(stamped_transform0G == stamped_transform_reference); // Different transform, frame id and time
tf2::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id");
EXPECT_FALSE(stamped_transform0H == stamped_transform_reference); // Different transform
//Different child_frame_id
tf2::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2");
EXPECT_FALSE(stamped_transform1A == stamped_transform_reference); // Equal
tf2::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2");
EXPECT_FALSE(stamped_transform1B == stamped_transform_reference); // Different Frame id
tf2::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2");
EXPECT_FALSE(stamped_transform1C == stamped_transform_reference); // Different Time
tf2::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
EXPECT_FALSE(stamped_transform1D == stamped_transform_reference); // Different frame id and time
tf2::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2");
EXPECT_FALSE(stamped_transform1E == stamped_transform_reference); // Different transform, frame id
tf2::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2");
EXPECT_FALSE(stamped_transform1F == stamped_transform_reference); // Different transform, time
tf2::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2");
EXPECT_FALSE(stamped_transform1G == stamped_transform_reference); // Different transform, frame id and time
tf2::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2");
EXPECT_FALSE(stamped_transform1H == stamped_transform_reference); // Different transform
}
TEST(tf2_stamped, OperatorEqual)
{
btTransform pose0, pose1, pose0a;
pose0.setIdentity();
pose1.setIdentity();
pose1.setOrigin(btVector3(1, 0, 0));
tf2::Stamped stamped_pose0(pose0, ros::Time(), "frame_id");
tf2::Stamped stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal");
EXPECT_FALSE(stamped_pose1 == stamped_pose0);
stamped_pose1 = stamped_pose0;
EXPECT_TRUE(stamped_pose1 == stamped_pose0);
}
*/
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::Time::init(); //needed for ros::TIme::now()
ros::init(argc, argv, "tf_unittest");
return RUN_ALL_TESTS();
}
ros-geometry2-0.5.16/test_tf2/test/static_publisher.launch 0000664 0000000 0000000 00000002746 13163650501 0023603 0 ustar 00root root 0000000 0000000
ros-geometry2-0.5.16/test_tf2/test/test_buffer_client.cpp 0000664 0000000 0000000 00000007357 13163650501 0023420 0 ustar 00root root 0000000 0000000 /*********************************************************************
*
* Software License Agreement (BSD License)
*
* 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 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: Eitan Marder-Eppstein
*********************************************************************/
#include
#include
#include
#include
#include
#include
static const double EPS = 1e-3;
TEST(tf2_ros, buffer_client)
{
tf2_ros::BufferClient client("tf_action");
//make sure that things are set up
ASSERT_TRUE(client.waitForServer(ros::Duration(4.0)));
geometry_msgs::PointStamped p1;
p1.header.frame_id = "a";
p1.header.stamp = ros::Time();
p1.point.x = 0.0;
p1.point.y = 0.0;
p1.point.z = 0.0;
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);
EXPECT_NEAR(p2.point.x, -5.0, EPS);
EXPECT_NEAR(p2.point.y, -6.0, EPS);
EXPECT_NEAR(p2.point.z, -7.0, EPS);
}
catch(tf2::TransformException& ex)
{
ROS_ERROR("Failed to transform: %s", ex.what());
ASSERT_FALSE("Should not get here");
}
}
TEST(tf2_ros, buffer_client_different_types)
{
tf2_ros::BufferClient client("tf_action");
//make sure that things are set up
ASSERT_TRUE(client.waitForServer(ros::Duration(4.0)));
tf2::Stamped k1(KDL::Vector(0, 0, 0), ros::Time(), "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] << ")");
EXPECT_NEAR(b1[0], -5.0, EPS);
EXPECT_NEAR(b1[1], -6.0, EPS);
EXPECT_NEAR(b1[2], -7.0, EPS);
EXPECT_EQ(b1.frame_id_, "b");
EXPECT_EQ(k1.frame_id_, "a");
}
catch(tf2::TransformException& ex)
{
ROS_ERROR("Failed to transform: %s", ex.what());
ASSERT_FALSE("Should not get here");
}
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "buffer_client_test");
return RUN_ALL_TESTS();
}
ros-geometry2-0.5.16/test_tf2/test/test_buffer_client.py 0000775 0000000 0000000 00000006321 13163650501 0023257 0 ustar 00root root 0000000 0000000 #! /usr/bin/python
#***********************************************************
#* Software License Agreement (BSD License)
#*
#* 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 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: 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
class TestBufferClient(unittest.TestCase):
def test_buffer_client(self):
client = tf2_ros.BufferClient("tf_action")
client.wait_for_server()
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
try:
p2 = client.transform(p1, "b")
rospy.loginfo("p1: %s, p2: %s" % (p1, p2))
except tf2.TransformException as e:
rospy.logerr("%s" % e)
def test_transform_type(self):
client = tf2_ros.BufferClient("tf_action")
client.wait_for_server()
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
try:
p2 = client.transform(p1, "b", new_type = PyKDL.Vector)
rospy.loginfo("p1: %s, p2: %s" % (str(p1), str(p2)))
except tf2.TransformException as e:
rospy.logerr("%s" % e)
if __name__ == '__main__':
rospy.init_node("test_buffer_client")
import rostest
rostest.rosrun(PKG, 'test_buffer_client', TestBufferClient)
ros-geometry2-0.5.16/test_tf2/test/test_buffer_server.cpp 0000664 0000000 0000000 00000004203 13163650501 0023433 0 ustar 00root root 0000000 0000000 /*********************************************************************
*
* Software License Agreement (BSD License)
*
* 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 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: Eitan Marder-Eppstein
*********************************************************************/
#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);
server.start();
ros::spin();
}
ros-geometry2-0.5.16/test_tf2/test/test_convert.cpp 0000664 0000000 0000000 00000007145 13163650501 0022264 0 ustar 00root root 0000000 0000000 /*********************************************************************
*
* Software License Agreement (BSD License)
*
* 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 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: Eitan Marder-Eppstein
*********************************************************************/
#include
#include
#include
#include
#include
#include
TEST(tf2Convert, kdlToBullet)
{
double epsilon = 1e-9;
tf2::Stamped b(btVector3(1,2,3), ros::Time(), "my_frame");
tf2::Stamped b1 = b;
tf2::Stamped k1;
tf2::convert(b1, k1);
tf2::Stamped b2;
tf2::convert(k1, b2);
EXPECT_EQ(b.frame_id_, b2.frame_id_);
EXPECT_NEAR(b.stamp_.toSec(), b2.stamp_.toSec(), 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(b1.x(), b2.x(), epsilon);
EXPECT_NEAR(b1.y(), b2.y(), epsilon);
EXPECT_NEAR(b1.z(), b2.z(), epsilon);
}
TEST(tf2Convert, kdlBulletROSConversions)
{
double epsilon = 1e-9;
tf2::Stamped b1(btVector3(1,2,3), ros::Time(), "my_frame"), b2, b3, b4;
geometry_msgs::PointStamped r1, r2, r3;
tf2::Stamped k1, k2, k3;
// Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self -> ROS -> KDL -> bullet -> ROS -> bullet
tf2::convert(b1, b1);
tf2::convert(b1, b2);
tf2::convert(b2, k1);
tf2::convert(k1, k1);
tf2::convert(k1, k2);
tf2::convert(k2, r1);
tf2::convert(r1, r1);
tf2::convert(r1, r2);
tf2::convert(r2, k3);
tf2::convert(k3, b3);
tf2::convert(b3, r3);
tf2::convert(r3, b4);
EXPECT_EQ(b1.frame_id_, b4.frame_id_);
EXPECT_NEAR(b1.stamp_.toSec(), b4.stamp_.toSec(), 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();
}
ros-geometry2-0.5.16/test_tf2/test/test_convert.py 0000775 0000000 0000000 00000004762 13163650501 0022137 0 ustar 00root root 0000000 0000000 #! /usr/bin/python
#***********************************************************
#* Software License Agreement (BSD License)
#*
#* 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 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: 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_geometry_msgs
from geometry_msgs.msg import PointStamped
import rospy
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
p2 = tf2_ros.convert(msg, PyKDL.Vector)
print p2
p2[0] = 100
print p
print p2
print p2.header
if __name__ == '__main__':
import rostest
rostest.unitrun(PKG, 'test_buffer_client', TestConvert)
ros-geometry2-0.5.16/test_tf2/test/test_message_filter.cpp 0000664 0000000 0000000 00000024245 13163650501 0023575 0 ustar 00root root 0000000 0000000 /*
* 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
* 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 Josh Faust */
#include
#include
#include
#include
#include
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include
using namespace tf2;
using namespace tf2_ros;
class Notification
{
public:
Notification(int expected_count) :
count_(0), expected_count_(expected_count), failure_count_(0)
{
}
void notify(const geometry_msgs::PointStamped::ConstPtr& message)
{
++count_;
}
void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason)
{
++failure_count_;
}
int count_;
int expected_count_;
int failure_count_;
};
TEST(MessageFilter, noTransforms)
{
BufferCore bc;
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 = ros::Time(1);
msg->header.frame_id = "frame2";
filter.add(msg);
EXPECT_EQ(0, n.count_);
}
TEST(MessageFilter, noTransformsSameFrame)
{
BufferCore bc;
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 = ros::Time(1);
msg->header.frame_id = "frame1";
filter.add(msg);
EXPECT_EQ(1, n.count_);
}
geometry_msgs::TransformStamped createTransform(Quaternion q, Vector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
{
geometry_msgs::TransformStamped t;
t.header.frame_id = frame1;
t.child_frame_id = frame2;
t.header.stamp = stamp;
t.transform.translation.x = v.x();
t.transform.translation.y = v.y();
t.transform.translation.z = v.z();
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
return t;
}
TEST(MessageFilter, preexistingTransforms)
{
BufferCore bc;
Notification n(1);
MessageFilter filter(bc, "frame1", 1, 0);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp(1);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "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.count_);
}
TEST(MessageFilter, postTransforms)
{
BufferCore bc;
Notification n(1);
MessageFilter filter(bc, "frame1", 1, 0);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp(1);
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_);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
EXPECT_EQ(1, n.count_);
}
TEST(MessageFilter, queueSize)
{
BufferCore bc;
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));
ros::Time stamp(1);
for (int i = 0; i < 20; ++i)
{
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_);
EXPECT_EQ(10, n.failure_count_);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
EXPECT_EQ(10, n.count_);
}
TEST(MessageFilter, setTargetFrame)
{
BufferCore bc;
Notification n(1);
MessageFilter filter(bc, "frame1", 1, 0);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTargetFrame("frame1000");
ros::Time stamp(1);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1000", "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.count_);
}
TEST(MessageFilter, multipleTargetFrames)
{
BufferCore bc;
Notification n(1);
MessageFilter filter(bc, "", 1, 0);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
std::vector target_frames;
target_frames.push_back("frame1");
target_frames.push_back("frame2");
filter.setTargetFrames(target_frames);
ros::Time stamp(1);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame3"), "me");
geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
msg->header.stamp = stamp;
msg->header.frame_id = "frame3";
filter.add(msg);
EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
//ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
EXPECT_EQ(1, n.count_); // frame2->frame3 now exists
}
TEST(MessageFilter, tolerance)
{
ros::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);
ros::Time stamp(1);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "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(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;
Notification n(1);
MessageFilter filter(bc, "frame1", 1, 0);
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
ros::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 + ros::Duration(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));
ros::Time stamp(1);
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_);
EXPECT_EQ(0, n.failure_count_);
bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + ros::Duration(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::PointStampedPtr msg(new geometry_msgs::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::PointStampedPtr msg(new geometry_msgs::PointStamped);
msg->header.stamp = ros::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);
int ret = RUN_ALL_TESTS();
return ret;
}
ros-geometry2-0.5.16/test_tf2/test/test_static_publisher.cpp 0000664 0000000 0000000 00000012476 13163650501 0024153 0 ustar 00root root 0000000 0000000 /*
* 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
* 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.
*/
#include
#include
#include "tf2/exceptions.h"
#include
#include
#include
#include "rostest/permuter.h"
#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", ros::Time(), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "b", ros::Time(100), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "b", ros::Time(1000), ros::Duration(1.0)));
};
TEST(StaticTransformPublisher, a_c_different_times)
{
tf2_ros::Buffer mB;
tf2_ros::TransformListener tfl(mB);
EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(100), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(1000), ros::Duration(1.0)));
};
TEST(StaticTransformPublisher, a_d_different_times)
{
tf2_ros::Buffer mB;
tf2_ros::TransformListener tfl(mB);
geometry_msgs::TransformStamped ts;
ts.transform.rotation.w = 1;
ts.header.frame_id = "c";
ts.header.stamp = ros::Time(10.0);
ts.child_frame_id = "d";
// make sure listener has populated
EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(100), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(1000), ros::Duration(1.0)));
mB.setTransform(ts, "authority");
//printf("%s\n", mB.allFramesAsString().c_str());
EXPECT_TRUE(mB.canTransform("c", "d", ros::Time(10), ros::Duration(0)));
EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(), ros::Duration(0)));
EXPECT_FALSE(mB.canTransform("a", "d", ros::Time(1), ros::Duration(0)));
EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(10), ros::Duration(0)));
EXPECT_FALSE(mB.canTransform("a", "d", ros::Time(100), ros::Duration(0)));
};
TEST(StaticTransformPublisher, multiple_parent_test)
{
tf2_ros::Buffer mB;
tf2_ros::TransformListener tfl(mB);
tf2_ros::StaticTransformBroadcaster stb;
geometry_msgs::TransformStamped ts;
ts.transform.rotation.w = 1;
ts.header.frame_id = "c";
ts.header.stamp = ros::Time(10.0);
ts.child_frame_id = "d";
stb.sendTransform(ts);
// make sure listener has populated
EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(100), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(1000), ros::Duration(1.0)));
// Publish new transform with child 'd', should replace old one in static tf
ts.header.frame_id = "new_parent";
stb.sendTransform(ts);
ts.child_frame_id = "other_child";
stb.sendTransform(ts);
ts.child_frame_id = "other_child2";
stb.sendTransform(ts);
EXPECT_TRUE(mB.canTransform("new_parent", "d", ros::Time(), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("new_parent", "other_child", ros::Time(), ros::Duration(1.0)));
EXPECT_TRUE(mB.canTransform("new_parent", "other_child2", ros::Time(), ros::Duration(1.0)));
EXPECT_FALSE(mB.canTransform("a", "d", ros::Time(), ros::Duration(1.0)));
};
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)));
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "tf_unittest");
return RUN_ALL_TESTS();
}
ros-geometry2-0.5.16/test_tf2/test/test_static_publisher.py 0000775 0000000 0000000 00000010123 13163650501 0024007 0 ustar 00root root 0000000 0000000 #! /usr/bin/python
#***********************************************************
#* Software License Agreement (BSD License)
#*
#* Copyright (c) 2016, Felix Duvallet
#* 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 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: Felix Duvallet
#***********************************************************
import subprocess
import unittest
import rospy
PKG = 'test_tf2'
import roslib; roslib.load_manifest(PKG)
class TestStaticPublisher(unittest.TestCase):
"""
These tests ensure the static transform publisher dies gracefully when
provided with an invalid (or non-existent) transform parameter.
These tests are started by the static_publisher.launch, which loads
parameters into the param server.
We check the output to make sure the correct error is occurring, since the
return code is always -1 (255).
Note that this *could* cause a problem if a valid TF is stored in the param
server for one of the names; in this case the subprocess would never return
and the test would run forever.
"""
def test_publisher_no_args(self):
# Start the publisher with no argument.
cmd = 'rosrun 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)
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'
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)
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'
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)
if __name__ == '__main__':
rospy.init_node("test_static_publisher_py")
import rostest
rostest.rosrun(PKG, 'test_static_publisher_py', TestStaticPublisher)
ros-geometry2-0.5.16/test_tf2/test/test_tf2_bullet.cpp 0000664 0000000 0000000 00000007075 13163650501 0022650 0 ustar 00root root 0000000 0000000 /*
* 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
* 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 Wim Meeussen */
#include
#include
#include
#include
#include
tf2_ros::Buffer* tf_buffer;
static const double EPS = 1e-3;
TEST(TfBullet, Transform)
{
tf2::Stamped v1(btTransform(btQuaternion(1,0,0,0), btVector3(1,2,3)), ros::Time(2.0), "A");
// simple api
btTransform v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.getOrigin().getX(), -9, EPS);
EXPECT_NEAR(v_simple.getOrigin().getY(), 18, EPS);
EXPECT_NEAR(v_simple.getOrigin().getZ(), 27, EPS);
// advanced api
btTransform v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"B", ros::Duration(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), ros::Time(2.0), "A");
// simple api
btVector3 v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.getX(), -9, EPS);
EXPECT_NEAR(v_simple.getY(), 18, EPS);
EXPECT_NEAR(v_simple.getZ(), 27, EPS);
// advanced api
btVector3 v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"B", ros::Duration(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;
tf_buffer = new tf2_ros::Buffer();
// populate buffer
geometry_msgs::TransformStamped t;
t.transform.translation.x = 10;
t.transform.translation.y = 20;
t.transform.translation.z = 30;
t.transform.rotation.x = 1;
t.header.stamp = ros::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;
return ret;
}
ros-geometry2-0.5.16/test_tf2/test/test_tf2_bullet.launch 0000664 0000000 0000000 00000000155 13163650501 0023330 0 ustar 00root root 0000000 0000000