pax_global_header 0000666 0000000 0000000 00000000064 13605745704 0014525 g ustar 00root root 0000000 0000000 52 comment=739da8720b63fb99b9f73cdafe585a26528939de
geometry2-0.6.6/ 0000775 0000000 0000000 00000000000 13605745704 0013453 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/.gitignore 0000664 0000000 0000000 00000000014 13605745704 0015436 0 ustar 00root root 0000000 0000000 *~
*.pyc
\#* geometry2-0.6.6/geometry2/ 0000775 0000000 0000000 00000000000 13605745704 0015370 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/geometry2/CHANGELOG.rst 0000664 0000000 0000000 00000001247 13605745704 0017415 0 ustar 00root root 0000000 0000000 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package geometry2
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.6.6 (2020-01-09)
------------------
0.6.5 (2018-11-16)
------------------
0.6.4 (2018-11-06)
------------------
0.6.3 (2018-07-09)
------------------
0.6.2 (2018-05-02)
------------------
0.6.1 (2018-03-21)
------------------
0.6.0 (2018-03-21)
------------------
0.5.17 (2018-01-01)
-------------------
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
geometry2-0.6.6/geometry2/CMakeLists.txt 0000664 0000000 0000000 00000000154 13605745704 0020130 0 ustar 00root root 0000000 0000000 cmake_minimum_required(VERSION 2.8.3)
project(geometry2)
find_package(catkin REQUIRED)
catkin_metapackage()
geometry2-0.6.6/geometry2/package.xml 0000664 0000000 0000000 00000001537 13605745704 0017513 0 ustar 00root root 0000000 0000000
geometry2
0.6.6
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
geometry2-0.6.6/test_tf2/ 0000775 0000000 0000000 00000000000 13605745704 0015205 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/test_tf2/CHANGELOG.rst 0000664 0000000 0000000 00000021636 13605745704 0017236 0 ustar 00root root 0000000 0000000 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package test_tf2
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.6.6 (2020-01-09)
------------------
* Update shebang and add launch prefixes for python3 support (`#421 `_)
* Always call catkin_package() (`#418 `_)
* Remove roslib.load_manifest `#404 `_ from otamachan/remove-load-manifest
* Contributors: Shane Loretz, Tamaki Nishino, Tully Foote
0.6.5 (2018-11-16)
------------------
0.6.4 (2018-11-06)
------------------
0.6.3 (2018-07-09)
------------------
* use correct unit test for test_tf2_bullet (`#301 `_)
* update cmake order (`#298 `_)
* Contributors: Tully Foote
0.6.2 (2018-05-02)
------------------
0.6.1 (2018-03-21)
------------------
0.6.0 (2018-03-21)
------------------
0.5.17 (2018-01-01)
-------------------
* Merge pull request `#257 `_ from delftrobotics-forks/python3
Make tf2_py python3 compatible again
* Use python3 print function.
* Contributors: Maarten de Vries, Tully Foote
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
geometry2-0.6.6/test_tf2/CMakeLists.txt 0000664 0000000 0000000 00000004352 13605745704 0017751 0 ustar 00root root 0000000 0000000 cmake_minimum_required(VERSION 2.8.3)
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)
catkin_package()
if(NOT CATKIN_ENABLE_TESTING)
return()
endif()
include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS})
link_directories(${orocos_kdl_LIBRARY_DIRS})
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()
geometry2-0.6.6/test_tf2/mainpage.dox 0000664 0000000 0000000 00000001112 13605745704 0017475 0 ustar 00root root 0000000 0000000 /**
\mainpage
\htmlinclude manifest.html
\b test_tf2 is ...
\section codeapi Code API
*/
geometry2-0.6.6/test_tf2/package.xml 0000664 0000000 0000000 00000002372 13605745704 0017326 0 ustar 00root root 0000000 0000000
test_tf2
0.6.6
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
geometry2-0.6.6/test_tf2/test/ 0000775 0000000 0000000 00000000000 13605745704 0016164 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/test_tf2/test/buffer_client_tester.launch 0000664 0000000 0000000 00000000501 13605745704 0023551 0 ustar 00root root 0000000 0000000
geometry2-0.6.6/test_tf2/test/buffer_core_test.cpp 0000664 0000000 0000000 00000302627 13605745704 0022222 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();
}
geometry2-0.6.6/test_tf2/test/static_publisher.launch 0000664 0000000 0000000 00000003036 13605745704 0022726 0 ustar 00root root 0000000 0000000
geometry2-0.6.6/test_tf2/test/test_buffer_client.cpp 0000664 0000000 0000000 00000007357 13605745704 0022552 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();
}
geometry2-0.6.6/test_tf2/test/test_buffer_client.py 0000775 0000000 0000000 00000006254 13605745704 0022416 0 ustar 00root root 0000000 0000000 #! /usr/bin/env 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 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)
geometry2-0.6.6/test_tf2/test/test_buffer_server.cpp 0000664 0000000 0000000 00000004203 13605745704 0022565 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();
}
geometry2-0.6.6/test_tf2/test/test_convert.cpp 0000664 0000000 0000000 00000007145 13605745704 0021416 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();
}
geometry2-0.6.6/test_tf2/test/test_convert.py 0000775 0000000 0000000 00000004773 13605745704 0021273 0 ustar 00root root 0000000 0000000 #! /usr/bin/env 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
#***********************************************************
from __future__ import print_function
PKG = 'test_tf2'
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)
geometry2-0.6.6/test_tf2/test/test_message_filter.cpp 0000664 0000000 0000000 00000024245 13605745704 0022727 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;
}
geometry2-0.6.6/test_tf2/test/test_static_publisher.cpp 0000664 0000000 0000000 00000012476 13605745704 0023305 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();
}
geometry2-0.6.6/test_tf2/test/test_static_publisher.py 0000775 0000000 0000000 00000010056 13605745704 0023146 0 ustar 00root root 0000000 0000000 #! /usr/bin/env 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'
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)
geometry2-0.6.6/test_tf2/test/test_tf2_bullet.cpp 0000664 0000000 0000000 00000007075 13605745704 0022002 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;
}
geometry2-0.6.6/test_tf2/test/test_tf2_bullet.launch 0000664 0000000 0000000 00000000161 13605745704 0022457 0 ustar 00root root 0000000 0000000
geometry2-0.6.6/test_tf2/test/test_tf_invalid.yaml 0000664 0000000 0000000 00000000132 13605745704 0022222 0 ustar 00root root 0000000 0000000 # This is not a valid TF.
child_frame_id: calibration
some_data:
- 1
- 2
- 3
geometry2-0.6.6/test_tf2/test/test_tf_valid.yaml 0000664 0000000 0000000 00000000421 13605745704 0021674 0 ustar 00root root 0000000 0000000 header:
seq: 0
stamp:
secs: 1619
nsecs: 601000000
frame_id: world
child_frame_id: robot_calibration
transform:
translation:
x: 0.75
y: 0.5
z: 1.0
rotation:
x: -0.62908825919
y: 0.210952809338
z: 0.640171445021
w: 0.38720459109
geometry2-0.6.6/test_tf2/test/test_utils.cpp 0000664 0000000 0000000 00000004716 13605745704 0021077 0 ustar 00root root 0000000 0000000 // Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include
#include
#include
#include
#include
double epsilon = 1e-9;
template
void yprTest(const T& t, double yaw1, double pitch1, double roll1) {
double yaw2, pitch2, roll2;
tf2::getEulerYPR(t, yaw2, pitch2, roll2);
EXPECT_NEAR(yaw1, yaw2, epsilon);
EXPECT_NEAR(pitch1, pitch2, epsilon);
EXPECT_NEAR(roll1, roll2, epsilon);
EXPECT_NEAR(tf2::getYaw(t), yaw1, epsilon);
}
TEST(tf2Utils, yaw)
{
double x, y, z, w;
x = 0.4;
y = 0.5;
z = 0.6;
w = 0.7;
double yaw1, pitch1, roll1;
// Compute results one way with KDL
KDL::Rotation::Quaternion(x, y, z, w).GetRPY(roll1, pitch1, yaw1);
{
// geometry_msgs::Quaternion
geometry_msgs::Quaternion q;
q.x = x; q.y =y; q.z = z; q.w = w;
yprTest(q, yaw1, pitch1, roll1);
// geometry_msgs::QuaternionStamped
geometry_msgs::QuaternionStamped qst;
qst.quaternion = q;
yprTest(qst, yaw1, pitch1, roll1);
}
{
// tf2::Quaternion
tf2::Quaternion q(x, y, z, w);
yprTest(q, yaw1, pitch1, roll1);
// tf2::Stamped
tf2::Stamped sq;
sq.setData(q);
yprTest(sq, yaw1, pitch1, roll1);
}
}
TEST(tf2Utils, identity)
{
geometry_msgs::Transform t;
t.translation.x = 0.1;
t.translation.y = 0.2;
t.translation.z = 0.3;
t.rotation.x = 0.4;
t.rotation.y = 0.5;
t.rotation.z = 0.6;
t.rotation.w = 0.7;
// Test identity
t = tf2::getTransformIdentity();
EXPECT_EQ(t.translation.x, 0);
EXPECT_EQ(t.translation.y, 0);
EXPECT_EQ(t.translation.z, 0);
EXPECT_EQ(t.rotation.x, 0);
EXPECT_EQ(t.rotation.y, 0);
EXPECT_EQ(t.rotation.z, 0);
EXPECT_EQ(t.rotation.w, 1);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
geometry2-0.6.6/tf2/ 0000775 0000000 0000000 00000000000 13605745704 0014146 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/tf2/CHANGELOG.rst 0000664 0000000 0000000 00000047153 13605745704 0016201 0 ustar 00root root 0000000 0000000 ^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package tf2
^^^^^^^^^^^^^^^^^^^^^^^^^
0.6.6 (2020-01-09)
------------------
* Fix compile error missing ros/ros.h (`#400 `_)
* ros/ros.h -> ros/time.h
* tf2_eigen doesn't need ros/ros.h
* rework Eigen functions namespace hack
* separate transform function declarations into transform_functions.h
* use ROS_DEPRECATED macro for portability (`#362 `_)
* Remove `signals` from find_package(Boost COMPONENTS ...).
* Remove legacy inclusion in CMakeLists of tf2.
* Contributors: James Xu, Maarten de Vries, Marco Tranzatto, Shane Loretz, Tully Foote
0.6.5 (2018-11-16)
------------------
0.6.4 (2018-11-06)
------------------
* Resolved pedantic warnings
* fix issue `#315 `_
* fixed nan interpoaltion issue
* Contributors: Keller Fabian Rudolf (CC-AD/EYC3), Kuang Fangjun, Martin Ganeff
0.6.3 (2018-07-09)
------------------
* preserve constness of const argument to avoid warnings (`#307 `_)
* Change comment style for unused doxygen (`#297 `_)
* Contributors: Jacob Perron, Tully Foote
0.6.2 (2018-05-02)
------------------
0.6.1 (2018-03-21)
------------------
* Replaced deprecated console_bridge macro calls (tests)
* Contributors: Johannes Meyer, Tully Foote
0.6.0 (2018-03-21)
------------------
* Replaced deprecated log macro calls
* Contributors: Tim Rakowski, Tully Foote
0.5.17 (2018-01-01)
-------------------
* Merge pull request `#278 `_ from ros/chain_as_vec_test2
Clean up results of _chainAsVector
* Simple test to check BufferCore::_chainAsVector.
Unit tests for walk and chain passing now.
* Merge pull request `#267 `_ from at-wat/speedup-timecache-for-large-buffer
Speed-up TimeCache search for large cache time.
* Merge pull request `#265 `_ from vsherrod/interpolation_fix
Corrected time output on interpolation function.
* Add time_interval option to tf2 speed-test.
* Merge pull request `#269 `_ from ros/frames_as_yaml
allFrameAsYaml consistently outputting a dict
* resolve https://github.com/ros/geometry/pull/153 at the source instead of needing the workaround.
* Speed-up TimeCache search for large cache time.
* Modified tests for correct time in interpolation to existing tests.
* Corrected time output on interpolation function.
Added unit test to check for this.
* Contributors: Atsushi Watanabe, Miguel Prada, Tully Foote, Vallan Sherrod
0.5.16 (2017-07-14)
-------------------
* remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation.
* Merge pull request `#144 `_ from clearpathrobotics/dead_lock_fix
Solve a bug that causes a deadlock in MessageFilter
* Resolve 2 places where the error_msg would not be propogated.
Fixes `#198 `_
* Remove generate_rand_vectors() from a number of tests. (`#227 `_)
* fixing include directory order to support overlays (`#231 `_)
* replaced dependencies on tf2_msgs_gencpp by exported dependencies
* Document the lifetime of the returned reference for getFrameId getTimestamp
* relax normalization tolerance. `#196 `_ was too strict for some use cases. (`#220 `_)
* Solve a bug that causes a deadlock in MessageFilter
* Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Tully Foote, dhood
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
* fixes `#194 `_ check for quaternion normalization before inserting into storage (`#196 `_)
* check for quaternion normalization before inserting into storage
* Add test to check for transform failure on invalid quaternion input
* updating getAngleShortestPath() (`#187 `_)
* Move internal cache functions into a namespace
Fixes https://github.com/ros/geometry2/issues/175
* Link properly to convert.h
* Landing page for tf2 describing the conversion interface
* Fix comment on BufferCore::MAX_GRAPH_DEPTH.
* Contributors: Jackie Kay, Phil Osteen, Tully Foote, alex, gavanderhoorn
0.5.13 (2016-03-04)
-------------------
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)
-------------------
* move lct_cache into function local memoryfor `#92 `_
* Clean up range checking. Re: `#92 `_
* Fixed chainToVector
* release lock before possibly invoking user callbacks. Fixes `#91 `_
* Contributors: Jackie Kay, Tully Foote
0.5.9 (2015-03-25)
------------------
* fixing edge case where two no frame id lookups matched in getLatestCommonTime
* Contributors: Tully Foote
0.5.8 (2015-03-17)
------------------
* change from default argument to overload to avoid linking issue `#84 `_
* remove useless Makefile files
* Remove unused assignments in max/min functions
* change _allFramesAsDot() -> _allFramesAsDot(double current_time)
* Contributors: Jon Binney, Kei Okada, Tully Foote, Vincent Rabaud
0.5.7 (2014-12-23)
------------------
0.5.6 (2014-09-18)
------------------
0.5.5 (2014-06-23)
------------------
* convert to use console bridge from upstream debian package https://github.com/ros/rosdistro/issues/4633
* Fix format string
* Contributors: Austin, Tully Foote
0.5.4 (2014-05-07)
------------------
* switch to boost signals2 following `ros/ros_comm#267 `_, blocking `ros/geometry#23 `_
* 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)
-------------------
* updated error message. fixes `#38 `_
* tf2: add missing console bridge include directories (fix `#48 `_)
* Fix const correctness of tf2::Vector3 rotate() method
The method does not modify the class thus should be const.
This has already been fixed in Bullet itself.
* Contributors: Dirk Thomas, Timo Rohling, Tully Foote
0.4.9 (2013-11-06)
------------------
0.4.8 (2013-11-06)
------------------
* moving python documentation to tf2_ros from tf2 to follow the code
* removing legacy rospy dependency. implementation removed in 0.4.0 fixes `#27 `_
0.4.7 (2013-08-28)
------------------
* switching to use allFramesAsStringNoLock inside of getLatestCommonTime and walkToParent and locking in public API _getLatestCommonTime instead re `#23 `_
* Fixes a crash in tf's view_frames related to dot code generation in allFramesAsDot
0.4.6 (2013-08-28)
------------------
* cleaner fix for `#19 `_
* fix pointer initialization. Fixes `#19 `_
* fixes `#18 `_ for hydro
* package.xml: corrected typo in description
0.4.5 (2013-07-11)
------------------
* adding _chainAsVector method for https://github.com/ros/geometry/issues/18
* adding _allFramesAsDot for backwards compatability https://github.com/ros/geometry/issues/18
0.4.4 (2013-07-09)
------------------
* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change.
* tf2: Fixes a warning on OS X, but generally safer
Replaces the use of pointers with shared_ptrs,
this allows the polymorphism and makes it so that
the compiler doesn't yell at us about calling
delete on a class with a public non-virtual
destructor.
* tf2: Fixes compiler warnings on OS X
This exploited a gcc specific extension and is not
C++ standard compliant. There used to be a "fix"
for OS X which no longer applies. I think it is ok
to use this as an int instead of a double, but
another way to fix it would be to use a define.
* tf2: Fixes linkedit errors on OS X
0.4.3 (2013-07-05)
------------------
0.4.2 (2013-07-05)
------------------
* adding getCacheLength() to parallel old tf API
* removing legacy static const variable MAX_EXTRAPOLATION_DISTANCE copied from tf unnecessesarily
0.4.1 (2013-07-05)
------------------
* adding old style callback notifications to BufferCore to enable backwards compatability of message filters
* exposing dedicated thread logic in BufferCore and checking in Buffer
* more methods to expose, and check for empty cache before getting latest timestamp
* adding methods to enable backwards compatability for passing through to tf::Transformer
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
* 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
* suppressing bullet LinearMath copy inside of tf2, so it will not collide, and should not be used externally.
* Restoring test packages and bullet packages.
reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented
reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_
* fixing includes in unit tests
* Make PythonLibs find_package python2 specific
On systems with python 3 installed and default, find_package(PythonLibs) will find the python 3 paths and libraries. However, the c++ include structure seems to be different in python 3 and tf2 uses includes that are no longer present or deprecated.
Until the includes are made to be python 3 compliant, we should specify that the version of python found must be python 2.
0.3.6 (2013-03-03)
------------------
0.3.5 (2013-02-15 14:46)
------------------------
* 0.3.4 -> 0.3.5
0.3.4 (2013-02-15 13:14)
------------------------
* 0.3.3 -> 0.3.4
* moving LinearMath includes to include/tf2
0.3.3 (2013-02-15 11:30)
------------------------
* 0.3.2 -> 0.3.3
* fixing include installation of tf2
0.3.2 (2013-02-15 00:42)
------------------------
* 0.3.1 -> 0.3.2
* fixed missing include export & tf2_ros dependecy
0.3.1 (2013-02-14)
------------------
* 0.3.0 -> 0.3.1
* fixing PYTHON installation directory
0.3.0 (2013-02-13)
------------------
* switching to version 0.3.0
* adding setup.py to tf2 package
* fixed tf2 exposing python functionality
* removed line that was killing tf2_ros.so
* fixing catkin message dependencies
* removing packages with missing deps
* adding missing package.xml
* adding missing package.xml
* adding missing package.xml
* catkinizing geometry-experimental
* removing bullet headers from use in header files
* removing bullet headers from use in header files
* merging my recent changes
* setting child_frame_id overlooked in revision 6a0eec022be0 which fixed failing tests
* allFramesAsString public and internal methods seperated. Public method is locked, private method is not
* fixing another scoped lock
* fixing one scoped lock
* fixing test compilation
* merge
* Error message fix, ros-pkg5085
* Check if target equals to source before validation
* When target_frame == source_frame, just returns an identity transform.
* adding addition ros header includes for strictness
* Fixed optimized lookups with compound transforms
* Fixed problem in tf2 optimized branch. Quaternion multiplication order was incorrect
* fix compilation on 32-bit
* Josh fix: Final inverse transform composition (missed multiplying the sourcd->top vector by the target->top inverse orientation). b44877d2b054
* Josh change: fix first/last time case. 46bf33868e0d
* fix transform accumulation to parent
* fix parent lookup, now works on the real pr2's tree
* move the message filter to tf2_ros
* 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
* Don't add the request if the transform is already available. Add some new tests
* working transformable callbacks with a simple (incomplete) test case
* first pass at a transformable callback api, not tested yet
* add interpolation cases
* fix getLatestCommonTime -- no longer returns the latest of any of the times
* Some more optimization -- allow findClosest to inline
* another minor speedup
* Minorly speed up canTransform by not requiring the full data lookup, and only looking up the parent
* Add explicit operator= so that we can see the time in it on a profile graph. Also some minor cleanup
* minor cleanup
* add 3 more cases to the speed test
* Remove use of btTransform at all from transform accumulation, since the conversion to/from is unnecessary, expensive, and can introduce floating point error
* Don't use btTransform as an intermediate when accumulating transforms, as constructing them takes quite a bit of time
* Completely remove lookupLists(). canTransform() now uses the same walking code as lookupTransform(). Also fixed a bug in the static transform publisher test
* Genericise the walk-to-top-parent code in lookupTransform so that it will be able to be used by canTransform as well (minus the cost of actually computing the transform)
* remove id lookup that wasn't doing anything
* Some more optimization:
* Reduce # of TransformStorage copies made in TimeCache::getData()
* Remove use of lookupLists from getLatestCommonTime
* lookupTransform() no longer uses lookupLists unless it's called with Time(0). Removes lots of object construction/destruction due to removal of pushing back on the lists
* Remove CompactFrameID in favor of a typedef
* these mode checks are no longer necessary
* Fix crash when testing extrapolation on the forward transforms
* Update cache unit tests to work with the changes TransformStorage.
Also make sure that BT_USE_DOUBLE_PRECISION is set for tf2.
* remove exposure of time_cache.h from buffer_core.h
* Removed the mutex from TimeCache, as it's unnecessary (BufferCore needs to have its own mutex locked anyway), and this speeds things up by about 20%
Also fixed a number of thread-safety problems
* Optimize test_extrapolation a bit, 25% speedup of lookupTransform
* use a hash map for looking up frame numbers, speeds up lookupTransform by ~8%
* Cache vectors used for looking up transforms. Speeds up lookupTransform by another 10%
* speed up lookupTransform by another 25%
* speed up lookupTransform by another 2x. also reduces the memory footprint of the cache significantly
* sped up lookupTransform by another 2x
* First add of a simple speed test
Sped up lookupTransform 2x
* roscpp dependency explicit, instead of relying on implicit
* static transform tested and working
* tests passing and all throw catches removed too\!
* validating frame_ids up front for lookup exceptions
* working with single base class vector
* tests passing for static storage
* making method private for clarity
* static cache implementation and test
* cleaning up API doc typos
* sphinx docs for Buffer
* new dox mainpage
* update tf2 manifest
* commenting out twist
* Changed cache_time to cache_time_ to follow C++ style guide, also initialized it to actually get things to work
* no more rand in cache tests
* Changing tf2_py.cpp to use underscores instead of camelCase
* removing all old converter functions from transform_datatypes.h
* removing last references to transform_datatypes.h in tf2
* transform conversions internalized
* removing unused datatypes
* copying bullet transform headers into tf2 and breaking bullet dependency
* merge
* removing dependency on tf
* removing include of old tf from tf2
* update doc
* merge
* kdl unittest passing
* Spaces instead of tabs in YAML grrrr
* Adding quotes for parent
* canTransform advanced ported
* Hopefully fixing YAML syntax
* new version of view_frames in new tf2_tools package
* testing new argument validation and catching bug
* Python support for debugging
* merge
* adding validation of frame_ids in queries with warnings and exceptions where appropriate
* Exposing ability to get frames as a string
* A compiling version of YAML debugging interface for BufferCore
* placeholder for tf debug
* fixing tf:: to tf2:: ns issues and stripping slashes on set in tf2 for backwards compatiabily
* Adding a python version of the BufferClient
* moving test to new package
* merging
* working unit test for BufferCore::lookupTransform
* removing unused method test and converting NO_PARENT test to new API
* Adding some comments
* Moving the python bindings for tf2 to the tf2 package from the tf2_py package
* buffercore tests upgraded
* porting tf_unittest while running incrmentally instead of block copy
* BufferCore::clear ported forward
* successfully changed lookupTransform advanced to new version
* switching to new implementation of lookupTransform tests still passing
* compiling lookupTransform new version
* removing tf_prefix from BufferCore. BuferCore is independent of any frame_ids. tf_prefix should be implemented at the ROS API level.
* initializing tf_prefix
* adding missing initialization
* suppressing warnings
* more tests ported
* removing tests for apis not ported forward
* setTransform tests ported
* old tests in new package passing due to backwards dependency. now for the fun, port all 1500 lines :-)
* setTransform working in new framework as well as old
* porting more methods
* more compatability
* bringing in helper functions for buffer_core from tf.h/cpp
* rethrowing to new exceptions
* converting Storage to geometry_msgs::TransformStamped
* removing deprecated useage
* cleaning up includes
* moving all implementations into cpp file
* switching test to new class from old one
* Compiling version of the buffer client
* moving listener to tf_cpp
* removing listener, it should be in another package
* most of listener
* add cantransform implementation
* removing deprecated API usage
* initial import of listener header
* move implementation into library
* 2 tests of buffer
* moving executables back into bin
* compiling again with new design
* rename tfcore to buffercore
* almost compiling version of template code
* compiling tf2_core simple test
* add test to start compiling
* copying in tf_unittest for tf_core testing template
* prototype of tf2_core implemented using old tf.
* first version of template functions
* remove timeouts
* properly naming tf2_core.h from tf_core.h
* working cache test with tf2 lib
* first unit test passing, not yet ported
* tf_core api
* tf2 v2
* aborting port
* moving across time cache tf and datatypes headers
* copying exceptions from tf
* switching to tf2 from tf_core
geometry2-0.6.6/tf2/CMakeLists.txt 0000664 0000000 0000000 00000003500 13605745704 0016704 0 ustar 00root root 0000000 0000000 cmake_minimum_required(VERSION 2.8.3)
project(tf2)
find_package(console_bridge REQUIRED)
find_package(catkin REQUIRED COMPONENTS geometry_msgs rostime tf2_msgs)
find_package(Boost REQUIRED COMPONENTS system thread)
catkin_package(
INCLUDE_DIRS include
LIBRARIES tf2
DEPENDS console_bridge
CATKIN_DEPENDS geometry_msgs tf2_msgs rostime)
include_directories(include ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS})
# export user definitions
#CPP Libraries
add_library(tf2 src/cache.cpp src/buffer_core.cpp src/static_cache.cpp)
target_link_libraries(tf2 ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
add_dependencies(tf2 ${catkin_EXPORTED_TARGETS})
install(TARGETS tf2
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
# Tests
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(test_cache_unittest test/cache_unittest.cpp)
target_link_libraries(test_cache_unittest tf2 ${console_bridge_LIBRARIES})
add_dependencies(test_cache_unittest ${catkin_EXPORTED_TARGETS})
catkin_add_gtest(test_static_cache_unittest test/static_cache_test.cpp)
target_link_libraries(test_static_cache_unittest tf2 ${console_bridge_LIBRARIES})
add_dependencies(test_static_cache_unittest ${catkin_EXPORTED_TARGETS})
catkin_add_gtest(test_simple test/simple_tf2_core.cpp)
target_link_libraries(test_simple tf2 ${console_bridge_LIBRARIES})
add_dependencies(test_simple ${catkin_EXPORTED_TARGETS})
add_executable(speed_test EXCLUDE_FROM_ALL test/speed_test.cpp)
target_link_libraries(speed_test tf2 ${console_bridge_LIBRARIES})
add_dependencies(tests speed_test)
add_dependencies(tests ${catkin_EXPORTED_TARGETS})
endif()
geometry2-0.6.6/tf2/include/ 0000775 0000000 0000000 00000000000 13605745704 0015571 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/tf2/include/tf2/ 0000775 0000000 0000000 00000000000 13605745704 0016264 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/tf2/include/tf2/LinearMath/ 0000775 0000000 0000000 00000000000 13605745704 0020310 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/tf2/include/tf2/LinearMath/Matrix3x3.h 0000664 0000000 0000000 00000051721 13605745704 0022271 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef TF2_MATRIX3x3_H
#define TF2_MATRIX3x3_H
#include "Vector3.h"
#include "Quaternion.h"
#include
namespace tf2
{
#define Matrix3x3Data Matrix3x3DoubleData
/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3.
* Make sure to only include a pure orthogonal matrix without scaling. */
class Matrix3x3 {
///Data storage for the matrix, each vector is a row of the matrix
Vector3 m_el[3];
public:
/** @brief No initializaion constructor */
Matrix3x3 () {}
// explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); }
/**@brief Constructor from Quaternion */
explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
/*
template
Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
{
setEulerYPR(yaw, pitch, roll);
}
*/
/** @brief Constructor with row major formatting */
Matrix3x3(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
{
setValue(xx, xy, xz,
yx, yy, yz,
zx, zy, zz);
}
/** @brief Copy constructor */
TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
{
m_el[0] = other.m_el[0];
m_el[1] = other.m_el[1];
m_el[2] = other.m_el[2];
}
/** @brief Assignment Operator */
TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
{
m_el[0] = other.m_el[0];
m_el[1] = other.m_el[1];
m_el[2] = other.m_el[2];
return *this;
}
/** @brief Get a column of the matrix as a vector
* @param i Column number 0 indexed */
TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const
{
return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
}
/** @brief Get a row of the matrix as a vector
* @param i Row number 0 indexed */
TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const
{
tf2FullAssert(0 <= i && i < 3);
return m_el[i];
}
/** @brief Get a mutable reference to a row of the matrix as a vector
* @param i Row number 0 indexed */
TF2SIMD_FORCE_INLINE Vector3& operator[](int i)
{
tf2FullAssert(0 <= i && i < 3);
return m_el[i];
}
/** @brief Get a const reference to a row of the matrix as a vector
* @param i Row number 0 indexed */
TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const
{
tf2FullAssert(0 <= i && i < 3);
return m_el[i];
}
/** @brief Multiply by the target matrix on the right
* @param m Rotation matrix to be applied
* Equivilant to this = this * m */
Matrix3x3& operator*=(const Matrix3x3& m);
/** @brief Set from a carray of tf2Scalars
* @param m A pointer to the beginning of an array of 9 tf2Scalars */
void setFromOpenGLSubMatrix(const tf2Scalar *m)
{
m_el[0].setValue(m[0],m[4],m[8]);
m_el[1].setValue(m[1],m[5],m[9]);
m_el[2].setValue(m[2],m[6],m[10]);
}
/** @brief Set the values of the matrix explicitly (row major)
* @param xx Top left
* @param xy Top Middle
* @param xz Top Right
* @param yx Middle Left
* @param yy Middle Middle
* @param yz Middle Right
* @param zx Bottom Left
* @param zy Bottom Middle
* @param zz Bottom Right*/
void setValue(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz,
const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz,
const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz)
{
m_el[0].setValue(xx,xy,xz);
m_el[1].setValue(yx,yy,yz);
m_el[2].setValue(zx,zy,zz);
}
/** @brief Set the matrix from a quaternion
* @param q The Quaternion to match */
void setRotation(const Quaternion& q)
{
tf2Scalar d = q.length2();
tf2FullAssert(d != tf2Scalar(0.0));
tf2Scalar s = tf2Scalar(2.0) / d;
tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy));
}
/** @brief Set the matrix from euler angles using YPR around ZYX respectively
* @param yaw Yaw about Z axis
* @param pitch Pitch about Y axis
* @param roll Roll about X axis
*/
ROS_DEPRECATED void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
{
setEulerYPR(yaw, pitch, roll);
}
/** @brief Set the matrix from euler angles YPR around ZYX axes
* @param eulerZ Yaw aboud Z axis
* @param eulerY Pitch around Y axis
* @param eulerX Roll about X axis
*
* These angles are used to produce a rotation matrix. The euler
* angles are applied in ZYX order. I.e a vector is first rotated
* about X then Y and then Z
**/
void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) {
tf2Scalar ci ( tf2Cos(eulerX));
tf2Scalar cj ( tf2Cos(eulerY));
tf2Scalar ch ( tf2Cos(eulerZ));
tf2Scalar si ( tf2Sin(eulerX));
tf2Scalar sj ( tf2Sin(eulerY));
tf2Scalar sh ( tf2Sin(eulerZ));
tf2Scalar cc = ci * ch;
tf2Scalar cs = ci * sh;
tf2Scalar sc = si * ch;
tf2Scalar ss = si * sh;
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
cj * sh, sj * ss + cc, sj * cs - sc,
-sj, cj * si, cj * ci);
}
/** @brief Set the matrix using RPY about XYZ fixed axes
* @param roll Roll about X axis
* @param pitch Pitch around Y axis
* @param yaw Yaw aboud Z axis
*
**/
void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) {
setEulerYPR(yaw, pitch, roll);
}
/**@brief Set the matrix to the identity */
void setIdentity()
{
setValue(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0),
tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0),
tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0));
}
static const Matrix3x3& getIdentity()
{
static const Matrix3x3 identityMatrix(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0),
tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0),
tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0));
return identityMatrix;
}
/**@brief Fill the values of the matrix into a 9 element array
* @param m The array to be filled */
void getOpenGLSubMatrix(tf2Scalar *m) const
{
m[0] = tf2Scalar(m_el[0].x());
m[1] = tf2Scalar(m_el[1].x());
m[2] = tf2Scalar(m_el[2].x());
m[3] = tf2Scalar(0.0);
m[4] = tf2Scalar(m_el[0].y());
m[5] = tf2Scalar(m_el[1].y());
m[6] = tf2Scalar(m_el[2].y());
m[7] = tf2Scalar(0.0);
m[8] = tf2Scalar(m_el[0].z());
m[9] = tf2Scalar(m_el[1].z());
m[10] = tf2Scalar(m_el[2].z());
m[11] = tf2Scalar(0.0);
}
/**@brief Get the matrix represented as a quaternion
* @param q The quaternion which will be set */
void getRotation(Quaternion& q) const
{
tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
tf2Scalar temp[4];
if (trace > tf2Scalar(0.0))
{
tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0));
temp[3]=(s * tf2Scalar(0.5));
s = tf2Scalar(0.5) / s;
temp[0]=((m_el[2].y() - m_el[1].z()) * s);
temp[1]=((m_el[0].z() - m_el[2].x()) * s);
temp[2]=((m_el[1].x() - m_el[0].y()) * s);
}
else
{
int i = m_el[0].x() < m_el[1].y() ?
(m_el[1].y() < m_el[2].z() ? 2 : 1) :
(m_el[0].x() < m_el[2].z() ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0));
temp[i] = s * tf2Scalar(0.5);
s = tf2Scalar(0.5) / s;
temp[3] = (m_el[k][j] - m_el[j][k]) * s;
temp[j] = (m_el[j][i] + m_el[i][j]) * s;
temp[k] = (m_el[k][i] + m_el[i][k]) * s;
}
q.setValue(temp[0],temp[1],temp[2],temp[3]);
}
/**@brief Get the matrix represented as euler angles around ZYX
* @param yaw Yaw around Z axis
* @param pitch Pitch around Y axis
* @param roll around X axis
* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
ROS_DEPRECATED void getEulerZYX(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
{
getEulerYPR(yaw, pitch, roll, solution_number);
};
/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
* @param yaw Yaw around Z axis
* @param pitch Pitch around Y axis
* @param roll around X axis */
void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const
{
struct Euler
{
tf2Scalar yaw;
tf2Scalar pitch;
tf2Scalar roll;
};
Euler euler_out;
Euler euler_out2; //second solution
//get the pointer to the raw data
// Check that pitch is not at a singularity
// Check that pitch is not at a singularity
if (tf2Fabs(m_el[2].x()) >= 1)
{
euler_out.yaw = 0;
euler_out2.yaw = 0;
// From difference of angles formula
tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z());
if (m_el[2].x() < 0) //gimbal locked down
{
euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0);
euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
else // gimbal locked up
{
euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
}
else
{
euler_out.pitch = - tf2Asin(m_el[2].x());
euler_out2.pitch = TF2SIMD_PI - euler_out.pitch;
euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch),
m_el[2].z()/tf2Cos(euler_out.pitch));
euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch),
m_el[2].z()/tf2Cos(euler_out2.pitch));
euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch),
m_el[0].x()/tf2Cos(euler_out.pitch));
euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch),
m_el[0].x()/tf2Cos(euler_out2.pitch));
}
if (solution_number == 1)
{
yaw = euler_out.yaw;
pitch = euler_out.pitch;
roll = euler_out.roll;
}
else
{
yaw = euler_out2.yaw;
pitch = euler_out2.pitch;
roll = euler_out2.roll;
}
}
/**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ
* @param roll around X axis
* @param pitch Pitch around Y axis
* @param yaw Yaw around Z axis
* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const
{
getEulerYPR(yaw, pitch, roll, solution_number);
}
/**@brief Create a scaled copy of the matrix
* @param s Scaling vector The elements of the vector will scale each column */
Matrix3x3 scaled(const Vector3& s) const
{
return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
}
/**@brief Return the determinant of the matrix */
tf2Scalar determinant() const;
/**@brief Return the adjoint of the matrix */
Matrix3x3 adjoint() const;
/**@brief Return the matrix with all values non negative */
Matrix3x3 absolute() const;
/**@brief Return the transpose of the matrix */
Matrix3x3 transpose() const;
/**@brief Return the inverse of the matrix */
Matrix3x3 inverse() const;
Matrix3x3 transposeTimes(const Matrix3x3& m) const;
Matrix3x3 timesTranspose(const Matrix3x3& m) const;
TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3& v) const
{
return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
}
TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3& v) const
{
return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
}
TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3& v) const
{
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
}
/**@brief diagonalizes this matrix by the Jacobi method.
* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
* coordinate system, i.e., old_this = rot * new_this * rot^T.
* @param threshold See iteration
* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
* by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
*
* Note that this matrix is assumed to be symmetric.
*/
void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps)
{
rot.setIdentity();
for (int step = maxSteps; step > 0; step--)
{
// find off-diagonal element [p][q] with largest magnitude
int p = 0;
int q = 1;
int r = 2;
tf2Scalar max = tf2Fabs(m_el[0][1]);
tf2Scalar v = tf2Fabs(m_el[0][2]);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = tf2Fabs(m_el[1][2]);
if (v > max)
{
p = 1;
q = 2;
r = 0;
max = v;
}
tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2]));
if (max <= t)
{
if (max <= TF2SIMD_EPSILON * t)
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
tf2Scalar mpq = m_el[p][q];
tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
tf2Scalar theta2 = theta * theta;
tf2Scalar cos;
tf2Scalar sin;
if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON))
{
t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2))
: 1 / (theta - tf2Sqrt(1 + theta2));
cos = 1 / tf2Sqrt(1 + t * t);
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2));
cos = 1 - tf2Scalar(0.5) * t * t;
sin = cos * t;
}
// apply rotation to matrix (this = J^T * this * J)
m_el[p][q] = m_el[q][p] = 0;
m_el[p][p] -= t * mpq;
m_el[q][q] += t * mpq;
tf2Scalar mrp = m_el[r][p];
tf2Scalar mrq = m_el[r][q];
m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for (int i = 0; i < 3; i++)
{
Vector3& row = rot[i];
mrp = row[p];
mrq = row[q];
row[p] = cos * mrp - sin * mrq;
row[q] = cos * mrq + sin * mrp;
}
}
}
/**@brief Calculate the matrix cofactor
* @param r1 The first row to use for calculating the cofactor
* @param c1 The first column to use for calculating the cofactor
* @param r1 The second row to use for calculating the cofactor
* @param c1 The second column to use for calculating the cofactor
* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
*/
tf2Scalar cofac(int r1, int c1, int r2, int c2) const
{
return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
}
void serialize(struct Matrix3x3Data& dataOut) const;
void serializeFloat(struct Matrix3x3FloatData& dataOut) const;
void deSerialize(const struct Matrix3x3Data& dataIn);
void deSerializeFloat(const struct Matrix3x3FloatData& dataIn);
void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn);
};
TF2SIMD_FORCE_INLINE Matrix3x3&
Matrix3x3::operator*=(const Matrix3x3& m)
{
setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
return *this;
}
TF2SIMD_FORCE_INLINE tf2Scalar
Matrix3x3::determinant() const
{
return tf2Triple((*this)[0], (*this)[1], (*this)[2]);
}
TF2SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::absolute() const
{
return Matrix3x3(
tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()),
tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()),
tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z()));
}
TF2SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::transpose() const
{
return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
m_el[0].y(), m_el[1].y(), m_el[2].y(),
m_el[0].z(), m_el[1].z(), m_el[2].z());
}
TF2SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::adjoint() const
{
return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
}
TF2SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::inverse() const
{
Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
tf2Scalar det = (*this)[0].dot(co);
tf2FullAssert(det != tf2Scalar(0.0));
tf2Scalar s = tf2Scalar(1.0) / det;
return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
}
TF2SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::transposeTimes(const Matrix3x3& m) const
{
return Matrix3x3(
m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
}
TF2SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::timesTranspose(const Matrix3x3& m) const
{
return Matrix3x3(
m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
}
TF2SIMD_FORCE_INLINE Vector3
operator*(const Matrix3x3& m, const Vector3& v)
{
return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
}
TF2SIMD_FORCE_INLINE Vector3
operator*(const Vector3& v, const Matrix3x3& m)
{
return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
}
TF2SIMD_FORCE_INLINE Matrix3x3
operator*(const Matrix3x3& m1, const Matrix3x3& m2)
{
return Matrix3x3(
m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
}
/*
TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) {
return Matrix3x3(
m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
}
*/
/**@brief Equality operator between two matrices
* It will test all elements are equal. */
TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
{
return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
}
///for serialization
struct Matrix3x3FloatData
{
Vector3FloatData m_el[3];
};
///for serialization
struct Matrix3x3DoubleData
{
Vector3DoubleData m_el[3];
};
TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serialize(dataOut.m_el[i]);
}
TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serializeFloat(dataOut.m_el[i]);
}
TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerialize(dataIn.m_el[i]);
}
TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeFloat(dataIn.m_el[i]);
}
TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeDouble(dataIn.m_el[i]);
}
}
#endif //TF2_MATRIX3x3_H
geometry2-0.6.6/tf2/include/tf2/LinearMath/MinMax.h 0000664 0000000 0000000 00000003261 13605745704 0021654 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GEN_MINMAX_H
#define GEN_MINMAX_H
template
TF2SIMD_FORCE_INLINE const T& tf2Min(const T& a, const T& b)
{
return a < b ? a : b ;
}
template
TF2SIMD_FORCE_INLINE const T& tf2Max(const T& a, const T& b)
{
return a > b ? a : b;
}
template
TF2SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub)
{
return a < lb ? lb : (ub < a ? ub : a);
}
template
TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b)
{
if (b < a)
{
a = b;
}
}
template
TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b)
{
if (a < b)
{
a = b;
}
}
template
TF2SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub)
{
if (a < lb)
{
a = lb;
}
else if (ub < a)
{
a = ub;
}
}
#endif
geometry2-0.6.6/tf2/include/tf2/LinearMath/QuadWord.h 0000664 0000000 0000000 00000013706 13605745704 0022216 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef TF2SIMD_QUADWORD_H
#define TF2SIMD_QUADWORD_H
#include "Scalar.h"
#include "MinMax.h"
#if defined (__CELLOS_LV2) && defined (__SPU__)
#include
#endif
namespace tf2
{
/**@brief The QuadWord class is base class for Vector3 and Quaternion.
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/
#ifndef USE_LIBSPE2
ATTRIBUTE_ALIGNED16(class) QuadWord
#else
class QuadWord
#endif
{
protected:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
union {
vec_float4 mVec128;
tf2Scalar m_floats[4];
};
public:
vec_float4 get128() const
{
return mVec128;
}
protected:
#else //__CELLOS_LV2__ __SPU__
tf2Scalar m_floats[4];
#endif //__CELLOS_LV2__ __SPU__
public:
/**@brief Return the x value */
TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;};
/**@brief Set the y value */
TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;};
/**@brief Set the z value */
TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) { m_floats[2] = z;};
/**@brief Set the w value */
TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;};
/**@brief Return the x value */
TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; }
//TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; }
TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; }
TF2SIMD_FORCE_INLINE bool operator==(const QuadWord& other) const
{
return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
}
TF2SIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const
{
return !(*this == other);
}
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3] = 0.f;
}
/* void getValue(tf2Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] = m_floats[2];
}
*/
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3]=w;
}
/**@brief No initialization constructor */
TF2SIMD_FORCE_INLINE QuadWord()
// :m_floats[0](tf2Scalar(0.)),m_floats[1](tf2Scalar(0.)),m_floats[2](tf2Scalar(0.)),m_floats[3](tf2Scalar(0.))
{
}
/**@brief Three argument constructor (zeros w)
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z)
{
m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f;
}
/**@brief Initializing constructor
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w)
{
m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w;
}
/**@brief Set each element to the max of the current values and the values of another QuadWord
* @param other The other QuadWord to compare with
*/
TF2SIMD_FORCE_INLINE void setMax(const QuadWord& other)
{
tf2SetMax(m_floats[0], other.m_floats[0]);
tf2SetMax(m_floats[1], other.m_floats[1]);
tf2SetMax(m_floats[2], other.m_floats[2]);
tf2SetMax(m_floats[3], other.m_floats[3]);
}
/**@brief Set each element to the min of the current values and the values of another QuadWord
* @param other The other QuadWord to compare with
*/
TF2SIMD_FORCE_INLINE void setMin(const QuadWord& other)
{
tf2SetMin(m_floats[0], other.m_floats[0]);
tf2SetMin(m_floats[1], other.m_floats[1]);
tf2SetMin(m_floats[2], other.m_floats[2]);
tf2SetMin(m_floats[3], other.m_floats[3]);
}
};
}
#endif //TF2SIMD_QUADWORD_H
geometry2-0.6.6/tf2/include/tf2/LinearMath/Quaternion.h 0000664 0000000 0000000 00000037416 13605745704 0022621 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef TF2_QUATERNION_H_
#define TF2_QUATERNION_H_
#include "Vector3.h"
#include "QuadWord.h"
#include
namespace tf2
{
/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */
class Quaternion : public QuadWord {
public:
/**@brief No initialization constructor */
Quaternion() {}
// template
// explicit Quaternion(const tf2Scalar *v) : Tuple4(v) {}
/**@brief Constructor from scalars */
Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w)
: QuadWord(x, y, z, w)
{}
/**@brief Axis angle Constructor
* @param axis The axis which the rotation is around
* @param angle The magnitude of the rotation around the angle (Radians) */
Quaternion(const Vector3& axis, const tf2Scalar& angle)
{
setRotation(axis, angle);
}
/**@brief Constructor from Euler angles
* @param yaw Angle around Y unless TF2_EULER_DEFAULT_ZYX defined then Z
* @param pitch Angle around X unless TF2_EULER_DEFAULT_ZYX defined then Y
* @param roll Angle around Z unless TF2_EULER_DEFAULT_ZYX defined then X */
ROS_DEPRECATED Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
{
#ifndef TF2_EULER_DEFAULT_ZYX
setEuler(yaw, pitch, roll);
#else
setRPY(roll, pitch, yaw);
#endif
}
/**@brief Set the rotation using axis angle notation
* @param axis The axis around which to rotate
* @param angle The magnitude of the rotation in Radians */
void setRotation(const Vector3& axis, const tf2Scalar& angle)
{
tf2Scalar d = axis.length();
tf2Assert(d != tf2Scalar(0.0));
tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d;
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
tf2Cos(angle * tf2Scalar(0.5)));
}
/**@brief Set the quaternion using Euler angles
* @param yaw Angle around Y
* @param pitch Angle around X
* @param roll Angle around Z */
void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
{
tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
tf2Scalar cosYaw = tf2Cos(halfYaw);
tf2Scalar sinYaw = tf2Sin(halfYaw);
tf2Scalar cosPitch = tf2Cos(halfPitch);
tf2Scalar sinPitch = tf2Sin(halfPitch);
tf2Scalar cosRoll = tf2Cos(halfRoll);
tf2Scalar sinRoll = tf2Sin(halfRoll);
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
}
/**@brief Set the quaternion using fixed axis RPY
* @param roll Angle around X
* @param pitch Angle around Y
* @param yaw Angle around Z*/
void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)
{
tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
tf2Scalar cosYaw = tf2Cos(halfYaw);
tf2Scalar sinYaw = tf2Sin(halfYaw);
tf2Scalar cosPitch = tf2Cos(halfPitch);
tf2Scalar sinPitch = tf2Sin(halfPitch);
tf2Scalar cosRoll = tf2Cos(halfRoll);
tf2Scalar sinRoll = tf2Sin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
}
/**@brief Set the quaternion using euler angles
* @param yaw Angle around Z
* @param pitch Angle around Y
* @param roll Angle around X */
ROS_DEPRECATED void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
{
setRPY(roll, pitch, yaw);
}
/**@brief Add two quaternions
* @param q The quaternion to add to this one */
TF2SIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q)
{
m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
return *this;
}
/**@brief Sutf2ract out a quaternion
* @param q The quaternion to sutf2ract from this one */
Quaternion& operator-=(const Quaternion& q)
{
m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
return *this;
}
/**@brief Scale this quaternion
* @param s The scalar to scale by */
Quaternion& operator*=(const tf2Scalar& s)
{
m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
return *this;
}
/**@brief Multiply this quaternion by q on the right
* @param q The other quaternion
* Equivilant to this = this * q */
Quaternion& operator*=(const Quaternion& q)
{
setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
return *this;
}
/**@brief Return the dot product between this quaternion and another
* @param q The other quaternion */
tf2Scalar dot(const Quaternion& q) const
{
return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
}
/**@brief Return the length squared of the quaternion */
tf2Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the quaternion */
tf2Scalar length() const
{
return tf2Sqrt(length2());
}
/**@brief Normalize the quaternion
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
Quaternion& normalize()
{
return *this /= length();
}
/**@brief Return a scaled version of this quaternion
* @param s The scale factor */
TF2SIMD_FORCE_INLINE Quaternion
operator*(const tf2Scalar& s) const
{
return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
}
/**@brief Return an inversely scaled versionof this quaternion
* @param s The inverse scale factor */
Quaternion operator/(const tf2Scalar& s) const
{
tf2Assert(s != tf2Scalar(0.0));
return *this * (tf2Scalar(1.0) / s);
}
/**@brief Inversely scale this quaternion
* @param s The scale factor */
Quaternion& operator/=(const tf2Scalar& s)
{
tf2Assert(s != tf2Scalar(0.0));
return *this *= tf2Scalar(1.0) / s;
}
/**@brief Return a normalized version of this quaternion */
Quaternion normalized() const
{
return *this / length();
}
/**@brief Return the ***half*** angle between this quaternion and the other
* @param q The other quaternion */
tf2Scalar angle(const Quaternion& q) const
{
tf2Scalar s = tf2Sqrt(length2() * q.length2());
tf2Assert(s != tf2Scalar(0.0));
return tf2Acos(dot(q) / s);
}
/**@brief Return the angle between this quaternion and the other along the shortest path
* @param q The other quaternion */
tf2Scalar angleShortestPath(const Quaternion& q) const
{
tf2Scalar s = tf2Sqrt(length2() * q.length2());
tf2Assert(s != tf2Scalar(0.0));
if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
else
return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
}
/**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
tf2Scalar getAngle() const
{
tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
return s;
}
/**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */
tf2Scalar getAngleShortestPath() const
{
tf2Scalar s;
if (m_floats[3] >= 0)
s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
else
s = tf2Scalar(2.) * tf2Acos(-m_floats[3]);
return s;
}
/**@brief Return the axis of the rotation represented by this quaternion */
Vector3 getAxis() const
{
tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.));
if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero
return Vector3(1.0, 0.0, 0.0); // Arbitrary
tf2Scalar s = tf2Sqrt(s_squared);
return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
}
/**@brief Return the inverse of this quaternion */
Quaternion inverse() const
{
return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
}
/**@brief Return the sum of this quaternion and the other
* @param q2 The other quaternion */
TF2SIMD_FORCE_INLINE Quaternion
operator+(const Quaternion& q2) const
{
const Quaternion& q1 = *this;
return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
}
/**@brief Return the difference between this quaternion and the other
* @param q2 The other quaternion */
TF2SIMD_FORCE_INLINE Quaternion
operator-(const Quaternion& q2) const
{
const Quaternion& q1 = *this;
return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
}
/**@brief Return the negative of this quaternion
* This simply negates each element */
TF2SIMD_FORCE_INLINE Quaternion operator-() const
{
const Quaternion& q2 = *this;
return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
}
/**@todo document this and it's use */
TF2SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const
{
Quaternion diff,sum;
diff = *this - qd;
sum = *this + qd;
if( diff.dot(diff) > sum.dot(sum) )
return qd;
return (-qd);
}
/**@todo document this and it's use */
TF2SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const
{
Quaternion diff,sum;
diff = *this - qd;
sum = *this + qd;
if( diff.dot(diff) < sum.dot(sum) )
return qd;
return (-qd);
}
/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
* Slerp interpolates assuming constant velocity. */
Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const
{
tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0);
if (theta != tf2Scalar(0.0))
{
tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta);
tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta);
tf2Scalar s1 = tf2Sin(t * theta);
if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
(m_floats[1] * s0 + -q.y() * s1) * d,
(m_floats[2] * s0 + -q.z() * s1) * d,
(m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
else
return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
(m_floats[1] * s0 + q.y() * s1) * d,
(m_floats[2] * s0 + q.z() * s1) * d,
(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
}
else
{
return *this;
}
}
static const Quaternion& getIdentity()
{
static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.));
return identityQuat;
}
TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; }
};
/**@brief Return the negative of a quaternion */
TF2SIMD_FORCE_INLINE Quaternion
operator-(const Quaternion& q)
{
return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
}
/**@brief Return the product of two quaternions */
TF2SIMD_FORCE_INLINE Quaternion
operator*(const Quaternion& q1, const Quaternion& q2) {
return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
}
TF2SIMD_FORCE_INLINE Quaternion
operator*(const Quaternion& q, const Vector3& w)
{
return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
}
TF2SIMD_FORCE_INLINE Quaternion
operator*(const Vector3& w, const Quaternion& q)
{
return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
}
/**@brief Calculate the dot product between two quaternions */
TF2SIMD_FORCE_INLINE tf2Scalar
dot(const Quaternion& q1, const Quaternion& q2)
{
return q1.dot(q2);
}
/**@brief Return the length of a quaternion */
TF2SIMD_FORCE_INLINE tf2Scalar
length(const Quaternion& q)
{
return q.length();
}
/**@brief Return the ***half*** angle between two quaternions*/
TF2SIMD_FORCE_INLINE tf2Scalar
angle(const Quaternion& q1, const Quaternion& q2)
{
return q1.angle(q2);
}
/**@brief Return the shortest angle between two quaternions*/
TF2SIMD_FORCE_INLINE tf2Scalar
angleShortestPath(const Quaternion& q1, const Quaternion& q2)
{
return q1.angleShortestPath(q2);
}
/**@brief Return the inverse of a quaternion*/
TF2SIMD_FORCE_INLINE Quaternion
inverse(const Quaternion& q)
{
return q.inverse();
}
/**@brief Return the result of spherical linear interpolation betwen two quaternions
* @param q1 The first quaternion
* @param q2 The second quaternion
* @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
* Slerp assumes constant velocity between positions. */
TF2SIMD_FORCE_INLINE Quaternion
slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t)
{
return q1.slerp(q2, t);
}
TF2SIMD_FORCE_INLINE Vector3
quatRotate(const Quaternion& rotation, const Vector3& v)
{
Quaternion q = rotation * v;
q *= rotation.inverse();
return Vector3(q.getX(),q.getY(),q.getZ());
}
TF2SIMD_FORCE_INLINE Quaternion
shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
{
Vector3 c = v0.cross(v1);
tf2Scalar d = v0.dot(v1);
if (d < -1.0 + TF2SIMD_EPSILON)
{
Vector3 n,unused;
tf2PlaneSpace1(v0,n,unused);
return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
}
tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f);
tf2Scalar rs = 1.0f / s;
return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
}
TF2SIMD_FORCE_INLINE Quaternion
shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
{
v0.normalize();
v1.normalize();
return shortestArcQuat(v0,v1);
}
}
#endif
geometry2-0.6.6/tf2/include/tf2/LinearMath/Scalar.h 0000664 0000000 0000000 00000032175 13605745704 0021676 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef TF2_SCALAR_H
#define TF2_SCALAR_H
#ifdef TF2_MANAGED_CODE
//Aligned data types not supported in managed code
#pragma unmanaged
#endif
#include
#include //size_t for MSVC 6.0
#include
#include
#include
#if defined(DEBUG) || defined (_DEBUG)
#define TF2_DEBUG
#endif
#ifdef _WIN32
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
#define TF2SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#else
//#define TF2_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
// #pragma warning(disable:4786) // Disable the "debug name too long" warning
#define TF2SIMD_FORCE_INLINE __forceinline
#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
#ifdef _XBOX
#define TF2_USE_VMX128
#include
#define TF2_HAVE_NATIVE_FSEL
#define tf2Fsel(a,b,c) __fsel((a),(b),(c))
#else
#endif//_XBOX
#endif //__MINGW32__
#include
#ifdef TF2_DEBUG
#define tf2Assert assert
#else
#define tf2Assert(x)
#endif
//tf2FullAssert is optional, slows down a lot
#define tf2FullAssert(x)
#define tf2Likely(_c) _c
#define tf2Unlikely(_c) _c
#else
#if defined (__CELLOS_LV2__)
#define TF2SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert
#include
#endif
#ifdef TF2_DEBUG
#define tf2Assert assert
#else
#define tf2Assert(x)
#endif
//tf2FullAssert is optional, slows down a lot
#define tf2FullAssert(x)
#define tf2Likely(_c) _c
#define tf2Unlikely(_c) _c
#else
#ifdef USE_LIBSPE2
#define TF2SIMD_FORCE_INLINE __inline
#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert
#include
#endif
#ifdef TF2_DEBUG
#define tf2Assert assert
#else
#define tf2Assert(x)
#endif
//tf2FullAssert is optional, slows down a lot
#define tf2FullAssert(x)
#define tf2Likely(_c) __builtin_expect((_c), 1)
#define tf2Unlikely(_c) __builtin_expect((_c), 0)
#else
//non-windows systems
#define TF2SIMD_FORCE_INLINE inline
///@todo: check out alignment methods for other platforms/compilers
///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#ifndef assert
#include
#endif
#if defined(DEBUG) || defined (_DEBUG)
#define tf2Assert assert
#else
#define tf2Assert(x)
#endif
//tf2FullAssert is optional, slows down a lot
#define tf2FullAssert(x)
#define tf2Likely(_c) _c
#define tf2Unlikely(_c) _c
#endif // LIBSPE2
#endif //__CELLOS_LV2__
#endif
///The tf2Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
typedef double tf2Scalar;
//this number could be bigger in double precision
#define TF2_LARGE_FLOAT 1e30
#define TF2_DECLARE_ALIGNED_ALLOCATOR() \
TF2SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \
TF2SIMD_FORCE_INLINE void operator delete(void* ptr) { tf2AlignedFree(ptr); } \
TF2SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
TF2SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
TF2SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \
TF2SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf2AlignedFree(ptr); } \
TF2SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
TF2SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x) { return sqrt(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x) { return fabs(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x) { return cos(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x) { return sin(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Tan(tf2Scalar x) { return tan(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return acos(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return asin(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan(tf2Scalar x) { return atan(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y) { return atan2(x, y); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Exp(tf2Scalar x) { return exp(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Log(tf2Scalar x) { return log(x); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x,tf2Scalar y) { return pow(x,y); }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x,tf2Scalar y) { return fmod(x,y); }
#define TF2SIMD_2_PI tf2Scalar(6.283185307179586232)
#define TF2SIMD_PI (TF2SIMD_2_PI * tf2Scalar(0.5))
#define TF2SIMD_HALF_PI (TF2SIMD_2_PI * tf2Scalar(0.25))
#define TF2SIMD_RADS_PER_DEG (TF2SIMD_2_PI / tf2Scalar(360.0))
#define TF2SIMD_DEGS_PER_RAD (tf2Scalar(360.0) / TF2SIMD_2_PI)
#define TF2SIMDSQRT12 tf2Scalar(0.7071067811865475244008443621048490)
#define tf2RecipSqrt(x) ((tf2Scalar)(tf2Scalar(1.0)/tf2Sqrt(tf2Scalar(x)))) /* reciprocal square root */
#define TF2SIMD_EPSILON DBL_EPSILON
#define TF2SIMD_INFINITY DBL_MAX
TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x)
{
tf2Scalar coeff_1 = TF2SIMD_PI / 4.0f;
tf2Scalar coeff_2 = 3.0f * coeff_1;
tf2Scalar abs_y = tf2Fabs(y);
tf2Scalar angle;
if (x >= 0.0f) {
tf2Scalar r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
tf2Scalar r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
return (y < 0.0f) ? -angle : angle;
}
TF2SIMD_FORCE_INLINE bool tf2FuzzyZero(tf2Scalar x) { return tf2Fabs(x) < TF2SIMD_EPSILON; }
TF2SIMD_FORCE_INLINE bool tf2Equal(tf2Scalar a, tf2Scalar eps) {
return (((a) <= eps) && !((a) < -eps));
}
TF2SIMD_FORCE_INLINE bool tf2GreaterEqual (tf2Scalar a, tf2Scalar eps) {
return (!((a) <= eps));
}
TF2SIMD_FORCE_INLINE int tf2IsNegative(tf2Scalar x) {
return x < tf2Scalar(0.0) ? 1 : 0;
}
TF2SIMD_FORCE_INLINE tf2Scalar tf2Radians(tf2Scalar x) { return x * TF2SIMD_RADS_PER_DEG; }
TF2SIMD_FORCE_INLINE tf2Scalar tf2Degrees(tf2Scalar x) { return x * TF2SIMD_DEGS_PER_RAD; }
#define TF2_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#ifndef tf2Fsel
TF2SIMD_FORCE_INLINE tf2Scalar tf2Fsel(tf2Scalar a, tf2Scalar b, tf2Scalar c)
{
return a >= 0 ? b : c;
}
#endif
#define tf2Fsels(a,b,c) (tf2Scalar)tf2Fsel(a,b,c)
TF2SIMD_FORCE_INLINE bool tf2MachineIsLittleEndian()
{
long int i = 1;
const char *p = (const char *) &i;
if (p[0] == 1) // Lowest address contains the least significant byte
return true;
else
return false;
}
///tf2Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
TF2SIMD_FORCE_INLINE unsigned tf2Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
{
// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
// Rely on positive value or'ed with its negative having sign bit on
// and zero value or'ed with its negative (which is still zero) having sign bit off
// Use arithmetic shift right, shifting the sign bit through all 32 bits
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
TF2SIMD_FORCE_INLINE int tf2Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
{
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
TF2SIMD_FORCE_INLINE float tf2Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
{
#ifdef TF2_HAVE_NATIVE_FSEL
return (float)tf2Fsel((tf2Scalar)condition - tf2Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#else
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
#endif
}
template TF2SIMD_FORCE_INLINE void tf2Swap(T& a, T& b)
{
T tmp = a;
a = b;
b = tmp;
}
//PCK: endian swapping functions
TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(unsigned val)
{
return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
}
TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(unsigned short val)
{
return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
}
TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(int val)
{
return tf2SwapEndian((unsigned)val);
}
TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(short val)
{
return tf2SwapEndian((unsigned short) val);
}
///tf2SwapFloat uses using char pointers to swap the endianness
////tf2SwapFloat/tf2SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
///so instead of returning a float/double, we return integer/long long integer
TF2SIMD_FORCE_INLINE unsigned int tf2SwapEndianFloat(float d)
{
unsigned int a = 0;
unsigned char *dst = (unsigned char *)&a;
unsigned char *src = (unsigned char *)&d;
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
return a;
}
// unswap using char pointers
TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a)
{
float d = 0.0f;
unsigned char *src = (unsigned char *)&a;
unsigned char *dst = (unsigned char *)&d;
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
return d;
}
// swap using char pointers
TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char* dst)
{
unsigned char *src = (unsigned char *)&d;
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
}
// unswap using char pointers
TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src)
{
double d = 0.0;
unsigned char *dst = (unsigned char *)&d;
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
return d;
}
// returns normalized value in range [-TF2SIMD_PI, TF2SIMD_PI]
TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians)
{
angleInRadians = tf2Fmod(angleInRadians, TF2SIMD_2_PI);
if(angleInRadians < -TF2SIMD_PI)
{
return angleInRadians + TF2SIMD_2_PI;
}
else if(angleInRadians > TF2SIMD_PI)
{
return angleInRadians - TF2SIMD_2_PI;
}
else
{
return angleInRadians;
}
}
///rudimentary class to provide type info
struct tf2TypedObject
{
tf2TypedObject(int objectType)
:m_objectType(objectType)
{
}
int m_objectType;
inline int getObjectType() const
{
return m_objectType;
}
};
#endif //TF2SIMD___SCALAR_H
geometry2-0.6.6/tf2/include/tf2/LinearMath/Transform.h 0000664 0000000 0000000 00000020637 13605745704 0022444 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef tf2_Transform_H
#define tf2_Transform_H
#include "Matrix3x3.h"
namespace tf2
{
#define TransformData TransformDoubleData
/**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */
class Transform {
///Storage for the rotation
Matrix3x3 m_basis;
///Storage for the translation
Vector3 m_origin;
public:
/**@brief No initialization constructor */
Transform() {}
/**@brief Constructor from Quaternion (optional Vector3 )
* @param q Rotation from quaternion
* @param c Translation from Vector (default 0,0,0) */
explicit TF2SIMD_FORCE_INLINE Transform(const Quaternion& q,
const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
: m_basis(q),
m_origin(c)
{}
/**@brief Constructor from Matrix3x3 (optional Vector3)
* @param b Rotation from Matrix
* @param c Translation from Vector default (0,0,0)*/
explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b,
const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
: m_basis(b),
m_origin(c)
{}
/**@brief Copy constructor */
TF2SIMD_FORCE_INLINE Transform (const Transform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
{
}
/**@brief Assignment Operator */
TF2SIMD_FORCE_INLINE Transform& operator=(const Transform& other)
{
m_basis = other.m_basis;
m_origin = other.m_origin;
return *this;
}
/**@brief Set the current transform as the value of the product of two transforms
* @param t1 Transform 1
* @param t2 Transform 2
* This = Transform1 * Transform2 */
TF2SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) {
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
/* void multInverseLeft(const Transform& t1, const Transform& t2) {
Vector3 v = t2.m_origin - t1.m_origin;
m_basis = tf2MultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis;
}
*/
/**@brief Return the transform of the vector */
TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const
{
return Vector3(m_basis[0].dot(x) + m_origin.x(),
m_basis[1].dot(x) + m_origin.y(),
m_basis[2].dot(x) + m_origin.z());
}
/**@brief Return the transform of the vector */
TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const
{
return (*this)(x);
}
/**@brief Return the transform of the Quaternion */
TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const
{
return getRotation() * q;
}
/**@brief Return the basis matrix for the rotation */
TF2SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
TF2SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; }
/**@brief Return the origin vector translation */
TF2SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
TF2SIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; }
/**@brief Return a quaternion representing the rotation */
Quaternion getRotation() const {
Quaternion q;
m_basis.getRotation(q);
return q;
}
/**@brief Set from an array
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void setFromOpenGLMatrix(const tf2Scalar *m)
{
m_basis.setFromOpenGLSubMatrix(m);
m_origin.setValue(m[12],m[13],m[14]);
}
/**@brief Fill an array representation
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void getOpenGLMatrix(tf2Scalar *m) const
{
m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin.x();
m[13] = m_origin.y();
m[14] = m_origin.z();
m[15] = tf2Scalar(1.0);
}
/**@brief Set the translational element
* @param origin The vector to set the translation to */
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin)
{
m_origin = origin;
}
TF2SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const;
/**@brief Set the rotational element by Matrix3x3 */
TF2SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis)
{
m_basis = basis;
}
/**@brief Set the rotational element by Quaternion */
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion& q)
{
m_basis.setRotation(q);
}
/**@brief Set this transformation to the identity */
void setIdentity()
{
m_basis.setIdentity();
m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
}
/**@brief Multiply this Transform by another(this = this * another)
* @param t The other transform */
Transform& operator*=(const Transform& t)
{
m_origin += m_basis * t.m_origin;
m_basis *= t.m_basis;
return *this;
}
/**@brief Return the inverse of this transform */
Transform inverse() const
{
Matrix3x3 inv = m_basis.transpose();
return Transform(inv, inv * -m_origin);
}
/**@brief Return the inverse of this transform times the other transform
* @param t The other transform
* return this.inverse() * the other */
Transform inverseTimes(const Transform& t) const;
/**@brief Return the product of this transform and the other */
Transform operator*(const Transform& t) const;
/**@brief Return an identity transform */
static const Transform& getIdentity()
{
static const Transform identityTransform(Matrix3x3::getIdentity());
return identityTransform;
}
void serialize(struct TransformData& dataOut) const;
void serializeFloat(struct TransformFloatData& dataOut) const;
void deSerialize(const struct TransformData& dataIn);
void deSerializeDouble(const struct TransformDoubleData& dataIn);
void deSerializeFloat(const struct TransformFloatData& dataIn);
};
TF2SIMD_FORCE_INLINE Vector3
Transform::invXform(const Vector3& inVec) const
{
Vector3 v = inVec - m_origin;
return (m_basis.transpose() * v);
}
TF2SIMD_FORCE_INLINE Transform
Transform::inverseTimes(const Transform& t) const
{
Vector3 v = t.getOrigin() - m_origin;
return Transform(m_basis.transposeTimes(t.m_basis),
v * m_basis);
}
TF2SIMD_FORCE_INLINE Transform
Transform::operator*(const Transform& t) const
{
return Transform(m_basis * t.m_basis,
(*this)(t.m_origin));
}
/**@brief Test if two transforms have all elements equal */
TF2SIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2)
{
return ( t1.getBasis() == t2.getBasis() &&
t1.getOrigin() == t2.getOrigin() );
}
///for serialization
struct TransformFloatData
{
Matrix3x3FloatData m_basis;
Vector3FloatData m_origin;
};
struct TransformDoubleData
{
Matrix3x3DoubleData m_basis;
Vector3DoubleData m_origin;
};
TF2SIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
m_origin.serialize(dataOut.m_origin);
}
TF2SIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const
{
m_basis.serializeFloat(dataOut.m_basis);
m_origin.serializeFloat(dataOut.m_origin);
}
TF2SIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
m_origin.deSerialize(dataIn.m_origin);
}
TF2SIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn)
{
m_basis.deSerializeFloat(dataIn.m_basis);
m_origin.deSerializeFloat(dataIn.m_origin);
}
TF2SIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn)
{
m_basis.deSerializeDouble(dataIn.m_basis);
m_origin.deSerializeDouble(dataIn.m_origin);
}
}
#endif
geometry2-0.6.6/tf2/include/tf2/LinearMath/Vector3.h 0000664 0000000 0000000 00000047516 13605745704 0022023 0 ustar 00root root 0000000 0000000 /*
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef TF2_VECTOR3_H
#define TF2_VECTOR3_H
#include "Scalar.h"
#include "MinMax.h"
namespace tf2
{
#define Vector3Data Vector3DoubleData
#define Vector3DataName "Vector3DoubleData"
/**@brief tf2::Vector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when tf2::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
* Ideally, this class should be replaced by a platform optimized TF2SIMD version that keeps the data in registers
*/
ATTRIBUTE_ALIGNED16(class) Vector3
{
public:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
tf2Scalar m_floats[4];
public:
TF2SIMD_FORCE_INLINE const vec_float4& get128() const
{
return *((const vec_float4*)&m_floats[0]);
}
public:
#else //__CELLOS_LV2__ __SPU__
#ifdef TF2_USE_SSE // _WIN32
union {
__m128 mVec128;
tf2Scalar m_floats[4];
};
TF2SIMD_FORCE_INLINE __m128 get128() const
{
return mVec128;
}
TF2SIMD_FORCE_INLINE void set128(__m128 v128)
{
mVec128 = v128;
}
#else
tf2Scalar m_floats[4];
#endif
#endif //__CELLOS_LV2__ __SPU__
public:
/**@brief No initialization constructor */
TF2SIMD_FORCE_INLINE Vector3() {}
/**@brief Constructor from scalars
* @param x X value
* @param y Y value
* @param z Z value
*/
TF2SIMD_FORCE_INLINE Vector3(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z)
{
m_floats[0] = x;
m_floats[1] = y;
m_floats[2] = z;
m_floats[3] = tf2Scalar(0.);
}
/**@brief Add a vector to this one
* @param The vector to add to this one */
TF2SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v)
{
m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
return *this;
}
/**@brief Sutf2ract a vector from this one
* @param The vector to sutf2ract */
TF2SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v)
{
m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
return *this;
}
/**@brief Scale the vector
* @param s Scale factor */
TF2SIMD_FORCE_INLINE Vector3& operator*=(const tf2Scalar& s)
{
m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
return *this;
}
/**@brief Inversely scale the vector
* @param s Scale factor to divide by */
TF2SIMD_FORCE_INLINE Vector3& operator/=(const tf2Scalar& s)
{
tf2FullAssert(s != tf2Scalar(0.0));
return *this *= tf2Scalar(1.0) / s;
}
/**@brief Return the dot product
* @param v The other vector in the dot product */
TF2SIMD_FORCE_INLINE tf2Scalar dot(const Vector3& v) const
{
return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
}
/**@brief Return the length of the vector squared */
TF2SIMD_FORCE_INLINE tf2Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the vector */
TF2SIMD_FORCE_INLINE tf2Scalar length() const
{
return tf2Sqrt(length2());
}
/**@brief Return the distance squared between the ends of this and another vector
* This is symantically treating the vector like a point */
TF2SIMD_FORCE_INLINE tf2Scalar distance2(const Vector3& v) const;
/**@brief Return the distance between the ends of this and another vector
* This is symantically treating the vector like a point */
TF2SIMD_FORCE_INLINE tf2Scalar distance(const Vector3& v) const;
/**@brief Normalize this vector
* x^2 + y^2 + z^2 = 1 */
TF2SIMD_FORCE_INLINE Vector3& normalize()
{
return *this /= length();
}
/**@brief Return a normalized version of this vector */
TF2SIMD_FORCE_INLINE Vector3 normalized() const;
/**@brief Rotate this vector
* @param wAxis The axis to rotate about
* @param angle The angle to rotate by */
TF2SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf2Scalar angle ) const;
/**@brief Return the angle between this and another vector
* @param v The other vector */
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Vector3& v) const
{
tf2Scalar s = tf2Sqrt(length2() * v.length2());
tf2FullAssert(s != tf2Scalar(0.0));
return tf2Acos(dot(v) / s);
}
/**@brief Return a vector will the absolute values of each element */
TF2SIMD_FORCE_INLINE Vector3 absolute() const
{
return Vector3(
tf2Fabs(m_floats[0]),
tf2Fabs(m_floats[1]),
tf2Fabs(m_floats[2]));
}
/**@brief Return the cross product between this and another vector
* @param v The other vector */
TF2SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const
{
return Vector3(
m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
}
TF2SIMD_FORCE_INLINE tf2Scalar triple(const Vector3& v1, const Vector3& v2) const
{
return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
}
/**@brief Return the axis with the smallest value
* Note return values are 0,1,2 for x, y, or z */
TF2SIMD_FORCE_INLINE int minAxis() const
{
return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */
TF2SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf2Scalar& t) const
{
return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
}
/**@brief Elementwise multiply this vector by the other
* @param v The other vector */
TF2SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v)
{
m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
return *this;
}
/**@brief Return the x value */
TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;};
/**@brief Set the y value */
TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;};
/**@brief Set the z value */
TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) {m_floats[2] = z;};
/**@brief Set the w value */
TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;};
/**@brief Return the x value */
TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; }
//TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; }
TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; }
TF2SIMD_FORCE_INLINE bool operator==(const Vector3& other) const
{
return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
}
TF2SIMD_FORCE_INLINE bool operator!=(const Vector3& other) const
{
return !(*this == other);
}
/**@brief Set each element to the max of the current values and the values of another Vector3
* @param other The other Vector3 to compare with
*/
TF2SIMD_FORCE_INLINE void setMax(const Vector3& other)
{
tf2SetMax(m_floats[0], other.m_floats[0]);
tf2SetMax(m_floats[1], other.m_floats[1]);
tf2SetMax(m_floats[2], other.m_floats[2]);
tf2SetMax(m_floats[3], other.w());
}
/**@brief Set each element to the min of the current values and the values of another Vector3
* @param other The other Vector3 to compare with
*/
TF2SIMD_FORCE_INLINE void setMin(const Vector3& other)
{
tf2SetMin(m_floats[0], other.m_floats[0]);
tf2SetMin(m_floats[1], other.m_floats[1]);
tf2SetMin(m_floats[2], other.m_floats[2]);
tf2SetMin(m_floats[3], other.w());
}
TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3] = tf2Scalar(0.);
}
void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const
{
v0->setValue(0. ,-z() ,y());
v1->setValue(z() ,0. ,-x());
v2->setValue(-y() ,x() ,0.);
}
void setZero()
{
setValue(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.));
}
TF2SIMD_FORCE_INLINE bool isZero() const
{
return m_floats[0] == tf2Scalar(0) && m_floats[1] == tf2Scalar(0) && m_floats[2] == tf2Scalar(0);
}
TF2SIMD_FORCE_INLINE bool fuzzyZero() const
{
return length2() < TF2SIMD_EPSILON;
}
TF2SIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const;
TF2SIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn);
TF2SIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const;
TF2SIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn);
TF2SIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const;
TF2SIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn);
};
/**@brief Return the sum of two vectors (Point symantics)*/
TF2SIMD_FORCE_INLINE Vector3
operator+(const Vector3& v1, const Vector3& v2)
{
return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
}
/**@brief Return the elementwise product of two vectors */
TF2SIMD_FORCE_INLINE Vector3
operator*(const Vector3& v1, const Vector3& v2)
{
return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
}
/**@brief Return the difference between two vectors */
TF2SIMD_FORCE_INLINE Vector3
operator-(const Vector3& v1, const Vector3& v2)
{
return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
}
/**@brief Return the negative of the vector */
TF2SIMD_FORCE_INLINE Vector3
operator-(const Vector3& v)
{
return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
}
/**@brief Return the vector scaled by s */
TF2SIMD_FORCE_INLINE Vector3
operator*(const Vector3& v, const tf2Scalar& s)
{
return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
}
/**@brief Return the vector scaled by s */
TF2SIMD_FORCE_INLINE Vector3
operator*(const tf2Scalar& s, const Vector3& v)
{
return v * s;
}
/**@brief Return the vector inversely scaled by s */
TF2SIMD_FORCE_INLINE Vector3
operator/(const Vector3& v, const tf2Scalar& s)
{
tf2FullAssert(s != tf2Scalar(0.0));
return v * (tf2Scalar(1.0) / s);
}
/**@brief Return the vector inversely scaled by s */
TF2SIMD_FORCE_INLINE Vector3
operator/(const Vector3& v1, const Vector3& v2)
{
return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
}
/**@brief Return the dot product between two vectors */
TF2SIMD_FORCE_INLINE tf2Scalar
tf2Dot(const Vector3& v1, const Vector3& v2)
{
return v1.dot(v2);
}
/**@brief Return the distance squared between two vectors */
TF2SIMD_FORCE_INLINE tf2Scalar
tf2Distance2(const Vector3& v1, const Vector3& v2)
{
return v1.distance2(v2);
}
/**@brief Return the distance between two vectors */
TF2SIMD_FORCE_INLINE tf2Scalar
tf2Distance(const Vector3& v1, const Vector3& v2)
{
return v1.distance(v2);
}
/**@brief Return the angle between two vectors */
TF2SIMD_FORCE_INLINE tf2Scalar
tf2Angle(const Vector3& v1, const Vector3& v2)
{
return v1.angle(v2);
}
/**@brief Return the cross product of two vectors */
TF2SIMD_FORCE_INLINE Vector3
tf2Cross(const Vector3& v1, const Vector3& v2)
{
return v1.cross(v2);
}
TF2SIMD_FORCE_INLINE tf2Scalar
tf2Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3)
{
return v1.triple(v2, v3);
}
/**@brief Return the linear interpolation between two vectors
* @param v1 One vector
* @param v2 The other vector
* @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
TF2SIMD_FORCE_INLINE Vector3
lerp(const Vector3& v1, const Vector3& v2, const tf2Scalar& t)
{
return v1.lerp(v2, t);
}
TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance2(const Vector3& v) const
{
return (v - *this).length2();
}
TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance(const Vector3& v) const
{
return (v - *this).length();
}
TF2SIMD_FORCE_INLINE Vector3 Vector3::normalized() const
{
return *this / length();
}
TF2SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf2Scalar angle ) const
{
// wAxis must be a unit lenght vector
Vector3 o = wAxis * wAxis.dot( *this );
Vector3 x = *this - o;
Vector3 y;
y = wAxis.cross( *this );
return ( o + x * tf2Cos( angle ) + y * tf2Sin( angle ) );
}
class tf2Vector4 : public Vector3
{
public:
TF2SIMD_FORCE_INLINE tf2Vector4() {}
TF2SIMD_FORCE_INLINE tf2Vector4(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w)
: Vector3(x,y,z)
{
m_floats[3] = w;
}
TF2SIMD_FORCE_INLINE tf2Vector4 absolute4() const
{
return tf2Vector4(
tf2Fabs(m_floats[0]),
tf2Fabs(m_floats[1]),
tf2Fabs(m_floats[2]),
tf2Fabs(m_floats[3]));
}
tf2Scalar getW() const { return m_floats[3];}
TF2SIMD_FORCE_INLINE int maxAxis4() const
{
int maxIndex = -1;
tf2Scalar maxVal = tf2Scalar(-TF2_LARGE_FLOAT);
if (m_floats[0] > maxVal)
{
maxIndex = 0;
maxVal = m_floats[0];
}
if (m_floats[1] > maxVal)
{
maxIndex = 1;
maxVal = m_floats[1];
}
if (m_floats[2] > maxVal)
{
maxIndex = 2;
maxVal =m_floats[2];
}
if (m_floats[3] > maxVal)
{
maxIndex = 3;
}
return maxIndex;
}
TF2SIMD_FORCE_INLINE int minAxis4() const
{
int minIndex = -1;
tf2Scalar minVal = tf2Scalar(TF2_LARGE_FLOAT);
if (m_floats[0] < minVal)
{
minIndex = 0;
minVal = m_floats[0];
}
if (m_floats[1] < minVal)
{
minIndex = 1;
minVal = m_floats[1];
}
if (m_floats[2] < minVal)
{
minIndex = 2;
minVal =m_floats[2];
}
if (m_floats[3] < minVal)
{
minIndex = 3;
}
return minIndex;
}
TF2SIMD_FORCE_INLINE int closestAxis4() const
{
return absolute4().maxAxis4();
}
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
/* void getValue(tf2Scalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] =m_floats[2];
}
*/
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3]=w;
}
};
///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
TF2SIMD_FORCE_INLINE void tf2SwapScalarEndian(const tf2Scalar& sourceVal, tf2Scalar& destVal)
{
unsigned char* dest = (unsigned char*) &destVal;
const unsigned char* src = (const unsigned char*) &sourceVal;
dest[0] = src[7];
dest[1] = src[6];
dest[2] = src[5];
dest[3] = src[4];
dest[4] = src[3];
dest[5] = src[2];
dest[6] = src[1];
dest[7] = src[0];
}
///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
TF2SIMD_FORCE_INLINE void tf2SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec)
{
for (int i=0;i<4;i++)
{
tf2SwapScalarEndian(sourceVec[i],destVec[i]);
}
}
///tf2UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
TF2SIMD_FORCE_INLINE void tf2UnSwapVector3Endian(Vector3& vector)
{
Vector3 swappedVec;
for (int i=0;i<4;i++)
{
tf2SwapScalarEndian(vector[i],swappedVec[i]);
}
vector = swappedVec;
}
TF2SIMD_FORCE_INLINE void tf2PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q)
{
if (tf2Fabs(n.z()) > TF2SIMDSQRT12) {
// choose p in y-z plane
tf2Scalar a = n[1]*n[1] + n[2]*n[2];
tf2Scalar k = tf2RecipSqrt (a);
p.setValue(0,-n[2]*k,n[1]*k);
// set q = n x p
q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
}
else {
// choose p in x-y plane
tf2Scalar a = n.x()*n.x() + n.y()*n.y();
tf2Scalar k = tf2RecipSqrt (a);
p.setValue(-n.y()*k,n.x()*k,0);
// set q = n x p
q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
}
}
struct Vector3FloatData
{
float m_floats[4];
};
struct Vector3DoubleData
{
double m_floats[4];
};
TF2SIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = float(m_floats[i]);
}
TF2SIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = tf2Scalar(dataIn.m_floats[i]);
}
TF2SIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = double(m_floats[i]);
}
TF2SIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = tf2Scalar(dataIn.m_floats[i]);
}
TF2SIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = m_floats[i];
}
TF2SIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = dataIn.m_floats[i];
}
}
#endif //TF2_VECTOR3_H
geometry2-0.6.6/tf2/include/tf2/buffer_core.h 0000664 0000000 0000000 00000045126 13605745704 0020726 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 Tully Foote */
#ifndef TF2_BUFFER_CORE_H
#define TF2_BUFFER_CORE_H
#include "transform_storage.h"
#include
#include
#include "ros/duration.h"
#include "ros/time.h"
//#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/TransformStamped.h"
//////////////////////////backwards startup for porting
//#include "tf/tf.h"
#include
#include
#include
#include
namespace tf2
{
typedef std::pair P_TimeAndFrameID;
typedef uint32_t TransformableCallbackHandle;
typedef uint64_t TransformableRequestHandle;
class TimeCacheInterface;
typedef boost::shared_ptr TimeCacheInterfacePtr;
enum TransformableResult
{
TransformAvailable,
TransformFailure,
};
/** \brief A Class which provides coordinate transforms between any two frames in a system.
*
* This class provides a simple interface to allow recording and lookup of
* relationships between arbitrary frames of the system.
*
* libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames.
* For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head.
* But Base to Hand really is composed of base to shoulder to elbow to wrist to hand.
* libTF is designed to take care of all the intermediate steps for you.
*
* Internal Representation
* libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame.
* Frames are designated using an std::string
* 0 is a frame without a parent (the top of a tree)
* The positions of frames over time must be pushed in.
*
* All function calls which pass frame ids can potentially throw the exception tf::LookupException
*/
class BufferCore
{
public:
/************* Constants ***********************/
static const int DEFAULT_CACHE_TIME = 10; //!< The default amount of time to cache data in seconds
static const uint32_t MAX_GRAPH_DEPTH = 1000UL; //!< Maximum graph search depth (deeper graphs will be assumed to have loops)
/** Constructor
* \param interpolating Whether to interpolate, if this is false the closest value will be returned
* \param cache_time How long to keep a history of transforms in nanoseconds
*
*/
BufferCore(ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
virtual ~BufferCore(void);
/** \brief Clear all data */
void clear();
/** \brief Add transform information to the tf data structure
* \param transform The transform to store
* \param authority The source of the information for this transform
* \param is_static Record this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.)
* \return True unless an error occured
*/
bool setTransform(const geometry_msgs::TransformStamped& transform, const std::string & authority, bool is_static = false);
/*********** Accessors *************/
/** \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
geometry_msgs::TransformStamped
lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time) const;
/** \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed
* \param target_time The time to which the data should be transformed. (0 will get the latest)
* \param source_frame The frame where the data originated
* \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
geometry_msgs::TransformStamped
lookupTransform(const std::string& target_frame, const ros::Time& target_time,
const std::string& source_frame, const ros::Time& source_time,
const std::string& fixed_frame) const;
/* \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point
* \param tracking_frame The frame to track
* \param observation_frame The frame from which to measure the twist
* \param reference_frame The reference frame in which to express the twist
* \param reference_point The reference point with which to express the twist
* \param reference_point_frame The frame_id in which the reference point is expressed
* \param time The time at which to get the velocity
* \param duration The period over which to average
* \return twist The twist output
*
* This will compute the average velocity on the interval
* (time - duration/2, time+duration/2). If that is too close to the most
* recent reading, in which case it will shift the interval up to
* duration/2 to prevent extrapolation.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*
* New in geometry 1.1
*/
/*
geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
const tf::Point & reference_point, const std::string& reference_point_frame,
const ros::Time& time, const ros::Duration& averaging_interval) const;
*/
/* \brief lookup the twist of the tracking frame with respect to the observational frame
*
* This is a simplified version of
* lookupTwist with it assumed that the reference point is the
* origin of the tracking frame, and the reference frame is the
* observation frame.
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*
* New in geometry 1.1
*/
/*
geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
const ros::Time& time, const ros::Duration& averaging_interval) const;
*/
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param source_frame The frame from which to transform
* \param time The time at which to transform
* \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
bool canTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, std::string* error_msg = NULL) const;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param target_time The time into which to transform
* \param source_frame The frame from which to transform
* \param source_time The time from which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time
* \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
bool canTransform(const std::string& target_frame, const ros::Time& target_time,
const std::string& source_frame, const ros::Time& source_time,
const std::string& fixed_frame, std::string* error_msg = NULL) const;
/** \brief A way to see what frames have been cached in yaml format
* Useful for debugging tools
*/
std::string allFramesAsYAML(double current_time) const;
/** Backwards compatibility for #84
*/
std::string allFramesAsYAML() const;
/** \brief A way to see what frames have been cached
* Useful for debugging
*/
std::string allFramesAsString() const;
typedef boost::function TransformableCallback;
/// \brief Internal use only
TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb);
/// \brief Internal use only
void removeTransformableCallback(TransformableCallbackHandle handle);
/// \brief Internal use only
TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time);
/// \brief Internal use only
void cancelTransformableRequest(TransformableRequestHandle handle);
// Tell the buffer that there are multiple threads serviciing it.
// This is useful for derived classes to know if they can block or not.
void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
// Get the state of using_dedicated_thread_
bool isUsingDedicatedThread() const { return using_dedicated_thread_;};
/* Backwards compatability section for tf::Transformer you should not use these
*/
/**
* \brief Add a callback that happens when a new transform has arrived
*
* \param callback The callback, of the form void func();
* \return A boost::signals2::connection object that can be used to remove this
* listener
*/
boost::signals2::connection _addTransformsChangedListener(boost::function callback);
void _removeTransformsChangedListener(boost::signals2::connection c);
/**@brief Check if a frame exists in the tree
* @param frame_id_str The frame id in question */
bool _frameExists(const std::string& frame_id_str) const;
/**@brief Fill the parent of a frame.
* @param frame_id The frame id of the frame in question
* @param parent The reference to the string to fill the parent
* Returns true unless "NO_PARENT" */
bool _getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
/** \brief A way to get a std::vector of available frame ids */
void _getFrameStrings(std::vector& ids) const;
CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const {
return lookupFrameNumber(frameid_str);
}
CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) {
return lookupOrInsertFrameNumber(frameid_str);
}
int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const {
boost::mutex::scoped_lock lock(frame_mutex_);
return getLatestCommonTime(target_frame, source_frame, time, error_string);
}
CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const {
return validateFrameId(function_name_arg, frame_id);
}
/**@brief Get the duration over which this transformer will cache */
ros::Duration getCacheLength() { return cache_time_;}
/** \brief Backwards compatabilityA way to see what frames have been cached
* Useful for debugging
*/
std::string _allFramesAsDot(double current_time) const;
std::string _allFramesAsDot() const;
/** \brief Backwards compatabilityA way to see what frames are in a chain
* Useful for debugging
*/
void _chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector& output) const;
private:
/** \brief A way to see what frames have been cached
* Useful for debugging. Use this call internally.
*/
std::string allFramesAsStringNoLock() const;
/******************** Internal Storage ****************/
/** \brief The pointers to potential frames that the tree can be made of.
* The frames will be dynamically allocated at run time when set the first time. */
typedef std::vector V_TimeCacheInterface;
V_TimeCacheInterface frames_;
/** \brief A mutex to protect testing and allocating new frames on the above vector. */
mutable boost::mutex frame_mutex_;
/** \brief A map from string frame ids to CompactFrameID */
typedef boost::unordered_map M_StringToCompactFrameID;
M_StringToCompactFrameID frameIDs_;
/** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */
std::vector frameIDs_reverse;
/** \brief A map to lookup the most recent authority for a given frame */
std::map frame_authority_;
/// How long to cache transform history
ros::Duration cache_time_;
typedef boost::unordered_map M_TransformableCallback;
M_TransformableCallback transformable_callbacks_;
uint32_t transformable_callbacks_counter_;
boost::mutex transformable_callbacks_mutex_;
struct TransformableRequest
{
ros::Time time;
TransformableRequestHandle request_handle;
TransformableCallbackHandle cb_handle;
CompactFrameID target_id;
CompactFrameID source_id;
std::string target_string;
std::string source_string;
};
typedef std::vector V_TransformableRequest;
V_TransformableRequest transformable_requests_;
boost::mutex transformable_requests_mutex_;
uint64_t transformable_requests_counter_;
struct RemoveRequestByCallback;
struct RemoveRequestByID;
// Backwards compatability for tf message_filter
typedef boost::signals2::signal TransformsChangedSignal;
/// Signal which is fired whenever new transform data has arrived, from the thread the data arrived in
TransformsChangedSignal _transforms_changed_;
/************************* Internal Functions ****************************/
/** \brief An accessor to get a frame, which will throw an exception if the frame is no there.
* \param frame_number The frameID of the desired Reference Frame
*
* This is an internal function which will get the pointer to the frame associated with the frame id
* Possible Exception: tf::LookupException
*/
TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const;
TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static);
bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const;
CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const;
/// String to number for frame lookup with dynamic allocation of new frames
CompactFrameID lookupFrameNumber(const std::string& frameid_str) const;
/// String to number for frame lookup with dynamic allocation of new frames
CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str);
///Number to string frame lookup may throw LookupException if number invalid
const std::string& lookupFrameString(CompactFrameID frame_id_num) const;
void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
/**@brief Return the latest rostime which is common across the spanning set
* zero if fails to cross */
int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const;
template
int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
/**@brief Traverse the transform tree. If frame_chain is not NULL, store the traversed frame tree in vector frame_chain.
* */
template
int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const;
void testTransformableRequests();
bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
const ros::Time& time, std::string* error_msg) const;
bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
const ros::Time& time, std::string* error_msg) const;
//Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.)
bool using_dedicated_thread_;
public:
friend class TestBufferCore; // For unit testing
};
/** A helper class for testing internal APIs */
class TestBufferCore
{
public:
int _walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const;
const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const
{
return buffer.lookupFrameString(frame_id_num);
}
};
}
#endif //TF2_CORE_H
geometry2-0.6.6/tf2/include/tf2/convert.h 0000664 0000000 0000000 00000004744 13605745704 0020126 0 ustar 00root root 0000000 0000000 /*
* Copyright (c) 2013, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Tully Foote */
#ifndef TF2_CONVERT_H
#define TF2_CONVERT_H
#include
#include
namespace tf2 {
/** Function that converts any type to any type (messages or not).
* Matching toMsg and from Msg conversion functions need to exist.
* If they don't exist or do not apply (for example, if your two
* classes are ROS messages), just write a specialization of the function.
* \param a an object to convert from
* \param b the object to convert to
*/
template
void convert(const A& a, B& b)
{
//printf("In double type convert\n");
impl::Converter::value, ros::message_traits::IsMessage::value>::convert(a, b);
}
template
void convert(const A& a1, A& a2)
{
//printf("In single type convert\n");
if(&a1 != &a2)
a2 = a1;
}
}
#endif //TF2_CONVERT_H
geometry2-0.6.6/tf2/include/tf2/exceptions.h 0000664 0000000 0000000 00000007456 13605745704 0020632 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 Tully Foote */
#ifndef TF2_EXCEPTIONS_H
#define TF2_EXCEPTIONS_H
#include
namespace tf2{
/** \brief A base class for all tf2 exceptions
* This inherits from ros::exception
* which inherits from std::runtime_exception
*/
class TransformException: public std::runtime_error
{
public:
TransformException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
};
/** \brief An exception class to notify of no connection
*
* This is an exception class to be thrown in the case
* that the Reference Frame tree is not connected between
* the frames requested. */
class ConnectivityException:public TransformException
{
public:
ConnectivityException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; };
};
/** \brief An exception class to notify of bad frame number
*
* This is an exception class to be thrown in the case that
* a frame not in the graph has been attempted to be accessed.
* The most common reason for this is that the frame is not
* being published, or a parent frame was not set correctly
* causing the tree to be broken.
*/
class LookupException: public TransformException
{
public:
LookupException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; };
};
/** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits.
*
*/
class ExtrapolationException: public TransformException
{
public:
ExtrapolationException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; };
};
/** \brief An exception class to notify that one of the arguments is invalid
*
* usually it's an uninitalized Quaternion (0,0,0,0)
*
*/
class InvalidArgumentException: public TransformException
{
public:
InvalidArgumentException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; };
};
/** \brief An exception class to notify that a timeout has occured
*
*
*/
class TimeoutException: public TransformException
{
public:
TimeoutException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; };
};
}
#endif //TF2_EXCEPTIONS_H
geometry2-0.6.6/tf2/include/tf2/impl/ 0000775 0000000 0000000 00000000000 13605745704 0017225 5 ustar 00root root 0000000 0000000 geometry2-0.6.6/tf2/include/tf2/impl/convert.h 0000664 0000000 0000000 00000005311 13605745704 0021056 0 ustar 00root root 0000000 0000000 /*
* Copyright (c) 2013, Open Source Robotics Foundation
* 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.
*/
#ifndef TF2_IMPL_CONVERT_H
#define TF2_IMPL_CONVERT_H
namespace tf2 {
namespace impl {
template
class Converter {
public:
template
static void convert(const A& a, B& b);
};
// The case where both A and B are messages should not happen: if you have two
// messages that are interchangeable, well, that's against the ROS purpose:
// only use one type. Worst comes to worst, specialize the original convert
// function for your types.
// if B == A, the templated version of convert with only one argument will be
// used.
//
//template <>
//template
//inline void Converter::convert(const A& a, B& b);
template <>
template
inline void Converter::convert(const A& a, B& b)
{
fromMsg(a, b);
}
template <>
template