HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add tf3

Browse Source
This commit is contained in:
HiepLM 2025-12-05 16:47:04 +07:00
parent 5db993383b
commit b41d614d1b
39 changed files with 8407 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitmodules vendored
View File

@ -22,6 +22,4 @@
[submodule "src/Libraries/data_convert"] [submodule "src/Libraries/data_convert"]
path = src/Libraries/data_convert path = src/Libraries/data_convert
url = https://git.pnkr.asia/DuongTD/data_convert.git url = https://git.pnkr.asia/DuongTD/data_convert.git
[submodule "src/Libraries/tf3"]
path = src/Libraries/tf3
url = https://github.com/Huandaotien/tf3.git

18
src/Libraries/tf3/.gitignore vendored Normal file
View File

@ -0,0 +1,18 @@
# .gitignore for a C++ project using CMake
/build/
*.o
*.obj
*.exe
*.out
*.log
# IDE specific files
.vscode/
.idea/
*.user
# CMake temp files
CMakeCache.txt
CMakeFiles/
cmake_install.cmake
Makefile

524
src/Libraries/tf3/CHANGELOG.rst Normal file
View File

@ -0,0 +1,524 @@
^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package tf2
^^^^^^^^^^^^^^^^^^^^^^^^^
0.7.10 (2025-05-02)
-------------------
* Fix race conditions in MessageFilter (`#539 <https://github.com/ros/geometry2/issues/539>`_)
* Contributors: Robert Haschke
0.7.9 (2025-04-25)
------------------
0.7.8 (2025-04-10)
------------------
* Longer char array for null termination needed (`#514 <https://github.com/ros/geometry2/issues/514>`_)
* Fixed error message when fixed_frame is not found (`#559 <https://github.com/ros/geometry2/issues/559>`_)
* Add missing #include to buffer_core.cpp (`#558 <https://github.com/ros/geometry2/issues/558>`_)
* Contributors: Lucas Walter, Martin Pecka, vslashg
0.7.7 (2023-10-13)
------------------
* fix extra comma that gives annoying build warnings with -Wall and -Wpedantic with g++-9 and assuming most other compilers (`#550 <https://github.com/ros/geometry2/issues/550>`_)
* Add parent frame to warning logs (`#533 <https://github.com/ros/geometry2/issues/533>`_)
* Contributors: Jack Zender, Stephan
0.7.6 (2022-10-11)
------------------
* Fix dead loop in message filter (`#532 <https://github.com/ros/geometry2/issues/532>`_)
* Restore time difference order so future extrapolation exceptions don't show non-sensical negative seconds into the future (`#522 <https://github.com/ros/geometry2/issues/522>`_)
* Contributors: Feng Zhaolin, Lucas Walter
0.7.5 (2020-09-01)
------------------
* restore buffer sizes
* Contributors: Tully Foote
0.7.4 (2020-09-01)
------------------
* Fix potential buffer overrun of snprintf (`#479 <https://github.com/ros/geometry2/issues/479>`_)
* Contributors: Atsushi Watanabe
0.7.3 (2020-08-25)
------------------
* Use snprintf instead of stringstream to increase performance of lookupTransform() in error cases.
* Do not waste time constructing error string if nobody is interested in it in canTransform(). (`#469 <https://github.com/ros/geometry2/issues/469>`_)
* Output time difference of extrapolation exceptions (`#477 <https://github.com/ros/geometry2/issues/477>`_)
* Cherry-picking various commits from Melodic (`#471 <https://github.com/ros/geometry2/issues/471>`_)
* Revert "rework Eigen functions namespace hack" (`#436 <https://github.com/ros/geometry2/issues/436>`_)
* Fixed warnings in message_filter.h (`#434 <https://github.com/ros/geometry2/issues/434>`_)
the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall
* Fix ambiguous call for tf2::convert on MSVC (`#444 <https://github.com/ros/geometry2/issues/444>`_)
* rework ambiguous call on MSVC.
* Contributors: Lucas Walter, Martin Pecka, Robert Haschke
0.7.2 (2020-06-08)
------------------
0.7.1 (2020-05-13)
------------------
* Fix to improper ring_45 test, where 'anchor' frame for both inverse and normal transform was frame 'b' instead of frame 'a', thus creating a problem
* Don't insert a TF frame is one of the same timestamp already exists, instead just overwrite it.
* [Noetic] Add tf2::Stamped<T>::operator=() to fix warnings downstream (`#453 <https://github.com/ros/geometry2/issues/453>`_)
* Add tf2::Stamped<T>::operator=()
* [noetic] cherry-pick Windows fixes from melodic-devel (`#450 <https://github.com/ros/geometry2/issues/450>`_)
* [Windows][melodic-devel] Fix install locations (`#442 <https://github.com/ros/geometry2/issues/442>`_)
* fixed install locations of tf2
* [windows][melodic] more portable fixes. (`#443 <https://github.com/ros/geometry2/issues/443>`_)
* more portable fixes.
* Contributors: Patrick Beeson, Robert Haschke, Sean Yen, Shane Loretz
0.7.0 (2020-03-09)
------------------
* Bump CMake version to avoid CMP0048 warning (`#445 <https://github.com/ros/geometry2/issues/445>`_)
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>
* Fix compile error missing ros/ros.h (`#400 <https://github.com/ros/geometry2/issues/400>`_)
* ros/ros.h -> ros/time.h
tf2 package depends on rostime
* tf2_bullet doesn't need ros.h
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>
* tf2_eigen doesn't need ros/ros.h
Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>
* Merge pull request `#367 <https://github.com/ros/geometry2/issues/367>`_ from kejxu/add_tf2_namespace_to_avoid_name_collision
rework Eigen functions namespace hack
* separate transform function declarations into transform_functions.h
* use ROS_DEPRECATED macro for portability (`#362 <https://github.com/ros/geometry2/issues/362>`_)
* use ROS_DEPRECATED for better portability
* change ROS_DEPRECATED position (`#5 <https://github.com/ros/geometry2/issues/5>`_)
* Remove `signals` from find_package(Boost COMPONENTS ...).
tf2 is using signals2, which is not the same library.
Additionally, signals2 has always been header only, and header only
libraries must not be listed in find_package.
Boost 1.69 removed the old signals library entirely, so the otherwise
useless `COMPONENTS signals` actually breaks the build.
* 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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/307>`_)
* Change comment style for unused doxygen (`#297 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/267>`_ from at-wat/speedup-timecache-for-large-buffer
Speed-up TimeCache search for large cache time.
* Merge pull request `#265 <https://github.com/ros/geometry2/issues/265>`_ from vsherrod/interpolation_fix
Corrected time output on interpolation function.
* Add time_interval option to tf2 speed-test.
* Merge pull request `#269 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/198>`_
* Remove generate_rand_vectors() from a number of tests. (`#227 <https://github.com/ros/geometry2/issues/227>`_)
* fixing include directory order to support overlays (`#231 <https://github.com/ros/geometry2/issues/231>`_)
* replaced dependencies on tf2_msgs_gencpp by exported dependencies
* Document the lifetime of the returned reference for getFrameId getTimestamp
* relax normalization tolerance. `#196 <https://github.com/ros/geometry2/issues/196>`_ was too strict for some use cases. (`#220 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/194>`_ check for quaternion normalization before inserting into storage (`#196 <https://github.com/ros/geometry2/issues/196>`_)
* check for quaternion normalization before inserting into storage
* Add test to check for transform failure on invalid quaternion input
* updating getAngleShortestPath() (`#187 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/92>`_
* Clean up range checking. Re: `#92 <https://github.com/ros/geometry_experimental/issues/92>`_
* Fixed chainToVector
* release lock before possibly invoking user callbacks. Fixes `#91 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/ros_comm/issues/267>`_, blocking `ros/geometry#23 <https://github.com/ros/geometry/issues/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 <https://github.com/ros/geometry_experimental/issues/38>`_
* tf2: add missing console bridge include directories (fix `#48 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/19>`_
* fix pointer initialization. Fixes `#19 <https://github.com/ros/geometry_experimental/issues/19>`_
* fixes `#18 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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

68
src/Libraries/tf3/CMakeLists.txt Normal file
View File

@ -0,0 +1,68 @@
cmake_minimum_required(VERSION 3.0.2)
project(tf3)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
find_package(console_bridge REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread)
include_directories(include ${console_bridge_INCLUDE_DIRS})
# export user definitions
#CPP Libraries
add_library(tf3 src/cache.cpp src/buffer_core.cpp src/static_cache.cpp)
target_link_libraries(tf3 ${Boost_LIBRARIES} ${console_bridge_LIBRARIES})
target_include_directories(tf3 PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
add_executable(simple_tf3_example examples/simple_tf3_example.cpp)
target_include_directories(simple_tf3_example PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(simple_tf3_example PRIVATE tf3 pthread ${console_bridge_LIBRARIES})
install(TARGETS tf3
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
)
install(TARGETS tf3
EXPORT tf3-targets)
# Export targets
install(EXPORT tf3-targets
FILE tf3-targets.cmake
NAMESPACE tf3::
DESTINATION lib/cmake/tf3)
# Tests
option(BUILD_TESTING "Build tests" OFF)
if(BUILD_TESTING)
enable_testing()
add_executable(test_cache_unittest test/cache_unittest.cpp)
target_link_libraries(test_cache_unittest tf3 ${console_bridge_LIBRARIES})
add_executable(test_static_cache_unittest test/static_cache_test.cpp)
target_link_libraries(test_static_cache_unittest tf3 ${console_bridge_LIBRARIES})
add_executable(test_simple test/simple_tf3_core.cpp)
target_link_libraries(test_simple tf3 ${console_bridge_LIBRARIES})
add_executable(speed_test test/speed_test.cpp)
target_link_libraries(speed_test tf3 ${console_bridge_LIBRARIES})
add_executable(test_transform_datatypes test/test_transform_datatypes.cpp)
target_link_libraries(test_transform_datatypes tf3 ${console_bridge_LIBRARIES})
add_test(NAME test_cache_unittest COMMAND test_cache_unittest)
add_test(NAME test_static_cache_unittest COMMAND test_static_cache_unittest)
add_test(NAME test_simple COMMAND test_simple)
add_test(NAME speed_test COMMAND speed_test)
add_test(NAME test_transform_datatypes COMMAND test_transform_datatypes)
endif()

13
src/Libraries/tf3/README.md Normal file
View File

@ -0,0 +1,13 @@
# Requirements:
- Ubuntu 20.04.6 LTS / 22.04 LTS
# Build tf3 project:
- Build and install tf3 library:
```
cd tf3
mkdir -p build
cd build
cmake .. -DBUILD_SHARED_LIBS=ON -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DCMAKE_INSTALL_PREFIX=/usr/local
make -j$(nproc)
make install
```

91
src/Libraries/tf3/examples/simple_tf3_example.cpp Normal file
View File

@ -0,0 +1,91 @@
#include <iostream>
#include <thread>
#include <chrono>
#include "tf3/buffer_core.h"
#include "tf3/compat.h"
#include "tf3/LinearMath/Transform.h"
using namespace tf3;
static void printTransform(const TransformStampedMsg & t)
{
std::cout << "Transform: [" << t.header.frame_id << "] -> [" << t.child_frame_id << "]\n";
std::cout << " stamp: " << t.header.stamp.toSec() << "\n";
std::cout << " translation: (" << t.transform.translation.x << ", " << t.transform.translation.y << ", " << t.transform.translation.z << ")\n";
std::cout << " rotation: (" << t.transform.rotation.x << ", " << t.transform.rotation.y << ", " << t.transform.rotation.z << ", " << t.transform.rotation.w << ")\n";
}
int main()
{
// Create a buffer with 10 seconds cache
BufferCore buffer(tf3::Duration(10));
// Create a transform from "world" -> "robot"
TransformStampedMsg world_to_robot;
world_to_robot.header.stamp = tf3::Time::fromSec(1.0);
world_to_robot.header.frame_id = "world";
world_to_robot.child_frame_id = "robot";
world_to_robot.transform.translation.x = 1.0;
world_to_robot.transform.translation.y = 2.0;
world_to_robot.transform.translation.z = 0.0;
// identity rotation
world_to_robot.transform.rotation.x = 0.0;
world_to_robot.transform.rotation.y = 0.0;
world_to_robot.transform.rotation.z = 0.0;
world_to_robot.transform.rotation.w = 1.0;
// Insert the transform into the buffer (this simulates broadcasting)
buffer.setTransform(world_to_robot, "example_authority", false);
std::cout << "Inserted world->robot transform:\n";
printTransform(world_to_robot);
// Create another transform robot -> sensor
TransformStampedMsg robot_to_sensor;
robot_to_sensor.header.stamp = tf3::Time::fromSec(1.0);
robot_to_sensor.header.frame_id = "robot";
robot_to_sensor.child_frame_id = "sensor";
robot_to_sensor.transform.translation.x = 0.1;
robot_to_sensor.transform.translation.y = 0.0;
robot_to_sensor.transform.translation.z = 0.2;
robot_to_sensor.transform.rotation.x = 0.0;
robot_to_sensor.transform.rotation.y = 0.0;
robot_to_sensor.transform.rotation.z = 0.0;
robot_to_sensor.transform.rotation.w = 1.0;
buffer.setTransform(robot_to_sensor, "example_authority", false);
std::cout << "Inserted robot->sensor transform:\n";
printTransform(robot_to_sensor);
// Query: get transform from sensor -> world (composed path)
try
{
// lookupTransform(target_frame, source_frame, time)
// returns the transform that maps a point in `source_frame` into `target_frame`.
TransformStampedMsg sensor_to_world = buffer.lookupTransform("world", "sensor", tf3::Time());
std::cout << "Lookup: sensor -> world (composed):\n";
printTransform(sensor_to_world);
}
catch (const std::exception & ex)
{
std::cerr << "lookupTransform failed: " << ex.what() << std::endl;
}
// Simulate broadcasting updates over time (update robot position)
for (int i = 0; i < 3; ++i)
{
double tsec = 2.0 + i * 1.0;
world_to_robot.header.stamp = tf3::Time::fromSec(tsec);
world_to_robot.transform.translation.x += 0.5; // robot moves in x
buffer.setTransform(world_to_robot, "example_authority", false);
std::cout << "Updated world->robot at t=" << tsec << "\n";
// lookup at latest
TransformStampedMsg latest = buffer.lookupTransform("world", "sensor", tf3::Time());
std::cout << "Latest composed transform (sensor->world):\n";
printTransform(latest);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return 0;
}

696
src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h Normal file
View File

@ -0,0 +1,696 @@
/*
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 TF3_MATRIX3x3_H
#define TF3_MATRIX3x3_H
#include "Vector3.h"
#include "Quaternion.h"
#include "../macros.h"
namespace tf3
{
#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 tf3Scalar *m) { setFromOpenGLSubMatrix(m); }
/**@brief Constructor from Quaternion */
explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
/*
template <typename tf3Scalar>
Matrix3x3(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
{
setEulerYPR(yaw, pitch, roll);
}
*/
/** @brief Constructor with row major formatting */
Matrix3x3(const tf3Scalar& xx, const tf3Scalar& xy, const tf3Scalar& xz,
const tf3Scalar& yx, const tf3Scalar& yy, const tf3Scalar& yz,
const tf3Scalar& zx, const tf3Scalar& zy, const tf3Scalar& zz)
{
setValue(xx, xy, xz,
yx, yy, yz,
zx, zy, zz);
}
/** @brief Copy constructor */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE const Vector3& getRow(int i) const
{
tf3FullAssert(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 */
TF3SIMD_FORCE_INLINE Vector3& operator[](int i)
{
tf3FullAssert(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 */
TF3SIMD_FORCE_INLINE const Vector3& operator[](int i) const
{
tf3FullAssert(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 tf3Scalars
* @param m A pointer to the beginning of an array of 9 tf3Scalars */
void setFromOpenGLSubMatrix(const tf3Scalar *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 tf3Scalar& xx, const tf3Scalar& xy, const tf3Scalar& xz,
const tf3Scalar& yx, const tf3Scalar& yy, const tf3Scalar& yz,
const tf3Scalar& zx, const tf3Scalar& zy, const tf3Scalar& 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)
{
tf3Scalar d = q.length2();
tf3FullAssert(d != tf3Scalar(0.0));
tf3Scalar s = tf3Scalar(2.0) / d;
tf3Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
tf3Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
tf3Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
tf3Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
setValue(tf3Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, tf3Scalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, tf3Scalar(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 tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& 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(tf3Scalar eulerZ, tf3Scalar eulerY,tf3Scalar eulerX) {
tf3Scalar ci ( tf3Cos(eulerX));
tf3Scalar cj ( tf3Cos(eulerY));
tf3Scalar ch ( tf3Cos(eulerZ));
tf3Scalar si ( tf3Sin(eulerX));
tf3Scalar sj ( tf3Sin(eulerY));
tf3Scalar sh ( tf3Sin(eulerZ));
tf3Scalar cc = ci * ch;
tf3Scalar cs = ci * sh;
tf3Scalar sc = si * ch;
tf3Scalar 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(tf3Scalar roll, tf3Scalar pitch,tf3Scalar yaw) {
setEulerYPR(yaw, pitch, roll);
}
/**@brief Set the matrix to the identity */
void setIdentity()
{
setValue(tf3Scalar(1.0), tf3Scalar(0.0), tf3Scalar(0.0),
tf3Scalar(0.0), tf3Scalar(1.0), tf3Scalar(0.0),
tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(1.0));
}
static const Matrix3x3& getIdentity()
{
static const Matrix3x3 identityMatrix(tf3Scalar(1.0), tf3Scalar(0.0), tf3Scalar(0.0),
tf3Scalar(0.0), tf3Scalar(1.0), tf3Scalar(0.0),
tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(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(tf3Scalar *m) const
{
m[0] = tf3Scalar(m_el[0].x());
m[1] = tf3Scalar(m_el[1].x());
m[2] = tf3Scalar(m_el[2].x());
m[3] = tf3Scalar(0.0);
m[4] = tf3Scalar(m_el[0].y());
m[5] = tf3Scalar(m_el[1].y());
m[6] = tf3Scalar(m_el[2].y());
m[7] = tf3Scalar(0.0);
m[8] = tf3Scalar(m_el[0].z());
m[9] = tf3Scalar(m_el[1].z());
m[10] = tf3Scalar(m_el[2].z());
m[11] = tf3Scalar(0.0);
}
/**@brief Get the matrix represented as a quaternion
* @param q The quaternion which will be set */
void getRotation(Quaternion& q) const
{
tf3Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
tf3Scalar temp[4];
if (trace > tf3Scalar(0.0))
{
tf3Scalar s = tf3Sqrt(trace + tf3Scalar(1.0));
temp[3]=(s * tf3Scalar(0.5));
s = tf3Scalar(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;
tf3Scalar s = tf3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf3Scalar(1.0));
temp[i] = s * tf3Scalar(0.5);
s = tf3Scalar(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(tf3Scalar& yaw, tf3Scalar& pitch, tf3Scalar& 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(tf3Scalar& yaw, tf3Scalar& pitch, tf3Scalar& roll, unsigned int solution_number = 1) const
{
struct Euler
{
tf3Scalar yaw;
tf3Scalar pitch;
tf3Scalar 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 (tf3Fabs(m_el[2].x()) >= 1)
{
euler_out.yaw = 0;
euler_out2.yaw = 0;
// From difference of angles formula
tf3Scalar delta = tf3Atan2(m_el[2].y(),m_el[2].z());
if (m_el[2].x() < 0) //gimbal locked down
{
euler_out.pitch = TF3SIMD_PI / tf3Scalar(2.0);
euler_out2.pitch = TF3SIMD_PI / tf3Scalar(2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
else // gimbal locked up
{
euler_out.pitch = -TF3SIMD_PI / tf3Scalar(2.0);
euler_out2.pitch = -TF3SIMD_PI / tf3Scalar(2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
}
else
{
euler_out.pitch = - tf3Asin(m_el[2].x());
euler_out2.pitch = TF3SIMD_PI - euler_out.pitch;
euler_out.roll = tf3Atan2(m_el[2].y()/tf3Cos(euler_out.pitch),
m_el[2].z()/tf3Cos(euler_out.pitch));
euler_out2.roll = tf3Atan2(m_el[2].y()/tf3Cos(euler_out2.pitch),
m_el[2].z()/tf3Cos(euler_out2.pitch));
euler_out.yaw = tf3Atan2(m_el[1].x()/tf3Cos(euler_out.pitch),
m_el[0].x()/tf3Cos(euler_out.pitch));
euler_out2.yaw = tf3Atan2(m_el[1].x()/tf3Cos(euler_out2.pitch),
m_el[0].x()/tf3Cos(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(tf3Scalar& roll, tf3Scalar& pitch, tf3Scalar& 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 */
tf3Scalar 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;
TF3SIMD_FORCE_INLINE tf3Scalar tdotx(const Vector3& v) const
{
return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
}
TF3SIMD_FORCE_INLINE tf3Scalar tdoty(const Vector3& v) const
{
return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
}
TF3SIMD_FORCE_INLINE tf3Scalar 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, tf3Scalar 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;
tf3Scalar max = tf3Fabs(m_el[0][1]);
tf3Scalar v = tf3Fabs(m_el[0][2]);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = tf3Fabs(m_el[1][2]);
if (v > max)
{
p = 1;
q = 2;
r = 0;
max = v;
}
tf3Scalar t = threshold * (tf3Fabs(m_el[0][0]) + tf3Fabs(m_el[1][1]) + tf3Fabs(m_el[2][2]));
if (max <= t)
{
if (max <= TF3SIMD_EPSILON * t)
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
tf3Scalar mpq = m_el[p][q];
tf3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
tf3Scalar theta2 = theta * theta;
tf3Scalar cos;
tf3Scalar sin;
if (theta2 * theta2 < tf3Scalar(10 / TF3SIMD_EPSILON))
{
t = (theta >= 0) ? 1 / (theta + tf3Sqrt(1 + theta2))
: 1 / (theta - tf3Sqrt(1 + theta2));
cos = 1 / tf3Sqrt(1 + t * t);
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + tf3Scalar(0.5) / theta2));
cos = 1 - tf3Scalar(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;
tf3Scalar mrp = m_el[r][p];
tf3Scalar 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
*/
tf3Scalar 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);
};
TF3SIMD_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;
}
TF3SIMD_FORCE_INLINE tf3Scalar
Matrix3x3::determinant() const
{
return tf3Triple((*this)[0], (*this)[1], (*this)[2]);
}
TF3SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::absolute() const
{
return Matrix3x3(
tf3Fabs(m_el[0].x()), tf3Fabs(m_el[0].y()), tf3Fabs(m_el[0].z()),
tf3Fabs(m_el[1].x()), tf3Fabs(m_el[1].y()), tf3Fabs(m_el[1].z()),
tf3Fabs(m_el[2].x()), tf3Fabs(m_el[2].y()), tf3Fabs(m_el[2].z()));
}
TF3SIMD_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());
}
TF3SIMD_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));
}
TF3SIMD_FORCE_INLINE Matrix3x3
Matrix3x3::inverse() const
{
Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
tf3Scalar det = (*this)[0].dot(co);
tf3FullAssert(det != tf3Scalar(0.0));
tf3Scalar s = tf3Scalar(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);
}
TF3SIMD_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());
}
TF3SIMD_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]));
}
TF3SIMD_FORCE_INLINE Vector3
operator*(const Matrix3x3& m, const Vector3& v)
{
return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
}
TF3SIMD_FORCE_INLINE Vector3
operator*(const Vector3& v, const Matrix3x3& m)
{
return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
}
TF3SIMD_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]));
}
/*
TF3SIMD_FORCE_INLINE Matrix3x3 tf3MultTransposeLeft(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. */
TF3SIMD_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];
};
TF3SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serialize(dataOut.m_el[i]);
}
TF3SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serializeFloat(dataOut.m_el[i]);
}
TF3SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerialize(dataIn.m_el[i]);
}
TF3SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeFloat(dataIn.m_el[i]);
}
TF3SIMD_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 //TF3_MATRIX3x3_H

69
src/Libraries/tf3/include/tf3/LinearMath/MinMax.h Normal file
View File

@ -0,0 +1,69 @@
/*
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 <class T>
TF3SIMD_FORCE_INLINE const T& tf3Min(const T& a, const T& b)
{
return a < b ? a : b ;
}
template <class T>
TF3SIMD_FORCE_INLINE const T& tf3Max(const T& a, const T& b)
{
return a > b ? a : b;
}
template <class T>
TF3SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub)
{
return a < lb ? lb : (ub < a ? ub : a);
}
template <class T>
TF3SIMD_FORCE_INLINE void tf3SetMin(T& a, const T& b)
{
if (b < a)
{
a = b;
}
}
template <class T>
TF3SIMD_FORCE_INLINE void tf3SetMax(T& a, const T& b)
{
if (a < b)
{
a = b;
}
}
template <class T>
TF3SIMD_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

183
src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h Normal file
View File

@ -0,0 +1,183 @@
/*
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 TF3SIMD_QUADWORD_H
#define TF3SIMD_QUADWORD_H
#include "Scalar.h"
#include "MinMax.h"
#if defined (__CELLOS_LV2) && defined (__SPU__)
#include <altivec.h>
#endif
namespace tf3
{
/**@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;
tf3Scalar m_floats[4];
};
public:
vec_float4 get128() const
{
return mVec128;
}
protected:
#else //__CELLOS_LV2__ __SPU__
tf3Scalar m_floats[4];
#endif //__CELLOS_LV2__ __SPU__
public:
/**@brief Return the x value */
TF3SIMD_FORCE_INLINE const tf3Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
TF3SIMD_FORCE_INLINE const tf3Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
TF3SIMD_FORCE_INLINE const tf3Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
TF3SIMD_FORCE_INLINE void setX(tf3Scalar x) { m_floats[0] = x;};
/**@brief Set the y value */
TF3SIMD_FORCE_INLINE void setY(tf3Scalar y) { m_floats[1] = y;};
/**@brief Set the z value */
TF3SIMD_FORCE_INLINE void setZ(tf3Scalar z) { m_floats[2] = z;};
/**@brief Set the w value */
TF3SIMD_FORCE_INLINE void setW(tf3Scalar w) { m_floats[3] = w;};
/**@brief Return the x value */
TF3SIMD_FORCE_INLINE const tf3Scalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
TF3SIMD_FORCE_INLINE const tf3Scalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
TF3SIMD_FORCE_INLINE const tf3Scalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
TF3SIMD_FORCE_INLINE const tf3Scalar& w() const { return m_floats[3]; }
//TF3SIMD_FORCE_INLINE tf3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//TF3SIMD_FORCE_INLINE const tf3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator tf3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
TF3SIMD_FORCE_INLINE operator tf3Scalar *() { return &m_floats[0]; }
TF3SIMD_FORCE_INLINE operator const tf3Scalar *() const { return &m_floats[0]; }
TF3SIMD_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]));
}
TF3SIMD_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
*/
TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3] = 0.f;
}
/* void getValue(tf3Scalar *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
*/
TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3]=w;
}
/**@brief No initialization constructor */
TF3SIMD_FORCE_INLINE QuadWord()
// :m_floats[0](tf3Scalar(0.)),m_floats[1](tf3Scalar(0.)),m_floats[2](tf3Scalar(0.)),m_floats[3](tf3Scalar(0.))
{
}
/**@brief Three argument constructor (zeros w)
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
TF3SIMD_FORCE_INLINE QuadWord(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& 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
*/
TF3SIMD_FORCE_INLINE QuadWord(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& 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
*/
TF3SIMD_FORCE_INLINE void setMax(const QuadWord& other)
{
tf3SetMax(m_floats[0], other.m_floats[0]);
tf3SetMax(m_floats[1], other.m_floats[1]);
tf3SetMax(m_floats[2], other.m_floats[2]);
tf3SetMax(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
*/
TF3SIMD_FORCE_INLINE void setMin(const QuadWord& other)
{
tf3SetMin(m_floats[0], other.m_floats[0]);
tf3SetMin(m_floats[1], other.m_floats[1]);
tf3SetMin(m_floats[2], other.m_floats[2]);
tf3SetMin(m_floats[3], other.m_floats[3]);
}
};
}
#endif //TF3SIMD_QUADWORD_H

477
src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h Normal file
View File

@ -0,0 +1,477 @@
/*
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 TF3_QUATERNION_H_
#define TF3_QUATERNION_H_
#include "Vector3.h"
#include "QuadWord.h"
#include "../macros.h"
namespace tf3
{
/**@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 <typename tf3Scalar>
// explicit Quaternion(const tf3Scalar *v) : Tuple4<tf3Scalar>(v) {}
/**@brief Constructor from scalars */
Quaternion(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z, const tf3Scalar& 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 tf3Scalar& angle)
{
setRotation(axis, angle);
}
/**@brief Constructor from Euler angles
* @param yaw Angle around Y unless TF3_EULER_DEFAULT_ZYX defined then Z
* @param pitch Angle around X unless TF3_EULER_DEFAULT_ZYX defined then Y
* @param roll Angle around Z unless TF3_EULER_DEFAULT_ZYX defined then X */
ROS_DEPRECATED Quaternion(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
{
#ifndef TF3_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 tf3Scalar& angle)
{
tf3Scalar d = axis.length();
tf3Assert(d != tf3Scalar(0.0));
tf3Scalar s = tf3Sin(angle * tf3Scalar(0.5)) / d;
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
tf3Cos(angle * tf3Scalar(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 tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
{
tf3Scalar halfYaw = tf3Scalar(yaw) * tf3Scalar(0.5);
tf3Scalar halfPitch = tf3Scalar(pitch) * tf3Scalar(0.5);
tf3Scalar halfRoll = tf3Scalar(roll) * tf3Scalar(0.5);
tf3Scalar cosYaw = tf3Cos(halfYaw);
tf3Scalar sinYaw = tf3Sin(halfYaw);
tf3Scalar cosPitch = tf3Cos(halfPitch);
tf3Scalar sinPitch = tf3Sin(halfPitch);
tf3Scalar cosRoll = tf3Cos(halfRoll);
tf3Scalar sinRoll = tf3Sin(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 tf3Scalar& roll, const tf3Scalar& pitch, const tf3Scalar& yaw)
{
tf3Scalar halfYaw = tf3Scalar(yaw) * tf3Scalar(0.5);
tf3Scalar halfPitch = tf3Scalar(pitch) * tf3Scalar(0.5);
tf3Scalar halfRoll = tf3Scalar(roll) * tf3Scalar(0.5);
tf3Scalar cosYaw = tf3Cos(halfYaw);
tf3Scalar sinYaw = tf3Sin(halfYaw);
tf3Scalar cosPitch = tf3Cos(halfPitch);
tf3Scalar sinPitch = tf3Sin(halfPitch);
tf3Scalar cosRoll = tf3Cos(halfRoll);
tf3Scalar sinRoll = tf3Sin(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 tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
{
setRPY(roll, pitch, yaw);
}
/**@brief Add two quaternions
* @param q The quaternion to add to this one */
TF3SIMD_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 Sutf3ract out a quaternion
* @param q The quaternion to sutf3ract 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 tf3Scalar& 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 */
tf3Scalar 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 */
tf3Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the quaternion */
tf3Scalar length() const
{
return tf3Sqrt(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 */
TF3SIMD_FORCE_INLINE Quaternion
operator*(const tf3Scalar& 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 tf3Scalar& s) const
{
tf3Assert(s != tf3Scalar(0.0));
return *this * (tf3Scalar(1.0) / s);
}
/**@brief Inversely scale this quaternion
* @param s The scale factor */
Quaternion& operator/=(const tf3Scalar& s)
{
tf3Assert(s != tf3Scalar(0.0));
return *this *= tf3Scalar(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 */
tf3Scalar angle(const Quaternion& q) const
{
tf3Scalar s = tf3Sqrt(length2() * q.length2());
tf3Assert(s != tf3Scalar(0.0));
return tf3Acos(dot(q) / s);
}
/**@brief Return the angle between this quaternion and the other along the shortest path
* @param q The other quaternion */
tf3Scalar angleShortestPath(const Quaternion& q) const
{
tf3Scalar s = tf3Sqrt(length2() * q.length2());
tf3Assert(s != tf3Scalar(0.0));
if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
return tf3Acos(dot(-q) / s) * tf3Scalar(2.0);
else
return tf3Acos(dot(q) / s) * tf3Scalar(2.0);
}
/**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
tf3Scalar getAngle() const
{
tf3Scalar s = tf3Scalar(2.) * tf3Acos(m_floats[3]);
return s;
}
/**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */
tf3Scalar getAngleShortestPath() const
{
tf3Scalar s;
if (m_floats[3] >= 0)
s = tf3Scalar(2.) * tf3Acos(m_floats[3]);
else
s = tf3Scalar(2.) * tf3Acos(-m_floats[3]);
return s;
}
/**@brief Return the axis of the rotation represented by this quaternion */
Vector3 getAxis() const
{
tf3Scalar s_squared = tf3Scalar(1.) - tf3Pow(m_floats[3], tf3Scalar(2.));
if (s_squared < tf3Scalar(10.) * TF3SIMD_EPSILON) //Check for divide by zero
return Vector3(1.0, 0.0, 0.0); // Arbitrary
tf3Scalar s = tf3Sqrt(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 */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_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 tf3Scalar& t) const
{
tf3Scalar theta = angleShortestPath(q) / tf3Scalar(2.0);
if (theta != tf3Scalar(0.0))
{
tf3Scalar d = tf3Scalar(1.0) / tf3Sin(theta);
tf3Scalar s0 = tf3Sin((tf3Scalar(1.0) - t) * theta);
tf3Scalar s1 = tf3Sin(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(tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(1.));
return identityQuat;
}
TF3SIMD_FORCE_INLINE const tf3Scalar& getW() const { return m_floats[3]; }
};
/**@brief Return the negative of a quaternion */
TF3SIMD_FORCE_INLINE Quaternion
operator-(const Quaternion& q)
{
return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
}
/**@brief Return the product of two quaternions */
TF3SIMD_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());
}
TF3SIMD_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());
}
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE tf3Scalar
dot(const Quaternion& q1, const Quaternion& q2)
{
return q1.dot(q2);
}
/**@brief Return the length of a quaternion */
TF3SIMD_FORCE_INLINE tf3Scalar
length(const Quaternion& q)
{
return q.length();
}
/**@brief Return the ***half*** angle between two quaternions*/
TF3SIMD_FORCE_INLINE tf3Scalar
angle(const Quaternion& q1, const Quaternion& q2)
{
return q1.angle(q2);
}
/**@brief Return the shortest angle between two quaternions*/
TF3SIMD_FORCE_INLINE tf3Scalar
angleShortestPath(const Quaternion& q1, const Quaternion& q2)
{
return q1.angleShortestPath(q2);
}
/**@brief Return the inverse of a quaternion*/
TF3SIMD_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. */
TF3SIMD_FORCE_INLINE Quaternion
slerp(const Quaternion& q1, const Quaternion& q2, const tf3Scalar& t)
{
return q1.slerp(q2, t);
}
TF3SIMD_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());
}
TF3SIMD_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);
tf3Scalar d = v0.dot(v1);
if (d < -1.0 + TF3SIMD_EPSILON)
{
Vector3 n,unused;
tf3PlaneSpace1(v0,n,unused);
return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
}
tf3Scalar s = tf3Sqrt((1.0f + d) * 2.0f);
tf3Scalar rs = 1.0f / s;
return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
}
TF3SIMD_FORCE_INLINE Quaternion
shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
{
v0.normalize();
v1.normalize();
return shortestArcQuat(v0,v1);
}
}
#endif

417
src/Libraries/tf3/include/tf3/LinearMath/Scalar.h Normal file
View File

@ -0,0 +1,417 @@
/*
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 TF3_SCALAR_H
#define TF3_SCALAR_H
#ifdef TF3_MANAGED_CODE
//Aligned data types not supported in managed code
#pragma unmanaged
#endif
#include <math.h>
#include <stdlib.h>//size_t for MSVC 6.0
#include <cstdlib>
#include <cfloat>
#include <float.h>
#if defined(DEBUG) || defined (_DEBUG)
#define TF3_DEBUG
#endif
#ifdef _WIN32
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
#define TF3SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#else
//#define TF3_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 TF3SIMD_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 TF3_USE_VMX128
#include <ppcintrinsics.h>
#define TF3_HAVE_NATIVE_FSEL
#define tf3Fsel(a,b,c) __fsel((a),(b),(c))
#else
#endif//_XBOX
#endif //__MINGW32__
#include <assert.h>
#ifdef TF3_DEBUG
#define tf3Assert assert
#else
#define tf3Assert(x)
#endif
//tf3FullAssert is optional, slows down a lot
#define tf3FullAssert(x)
#define tf3Likely(_c) _c
#define tf3Unlikely(_c) _c
#else
#if defined (__CELLOS_LV2__)
#define TF3SIMD_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 <assert.h>
#endif
#ifdef TF3_DEBUG
#define tf3Assert assert
#else
#define tf3Assert(x)
#endif
//tf3FullAssert is optional, slows down a lot
#define tf3FullAssert(x)
#define tf3Likely(_c) _c
#define tf3Unlikely(_c) _c
#else
#ifdef USE_LIBSPE2
#define TF3SIMD_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 <assert.h>
#endif
#ifdef TF3_DEBUG
#define tf3Assert assert
#else
#define tf3Assert(x)
#endif
//tf3FullAssert is optional, slows down a lot
#define tf3FullAssert(x)
#define tf3Likely(_c) __builtin_expect((_c), 1)
#define tf3Unlikely(_c) __builtin_expect((_c), 0)
#else
//non-windows systems
#define TF3SIMD_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 <assert.h>
#endif
#if defined(DEBUG) || defined (_DEBUG)
#define tf3Assert assert
#else
#define tf3Assert(x)
#endif
//tf3FullAssert is optional, slows down a lot
#define tf3FullAssert(x)
#define tf3Likely(_c) _c
#define tf3Unlikely(_c) _c
#endif // LIBSPE2
#endif //__CELLOS_LV2__
#endif
///The tf3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
typedef double tf3Scalar;
//this number could be bigger in double precision
#define TF3_LARGE_FLOAT 1e30
#define TF3_DECLARE_ALIGNED_ALLOCATOR() \
TF3SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf3AlignedAlloc(sizeInBytes,16); } \
TF3SIMD_FORCE_INLINE void operator delete(void* ptr) { tf3AlignedFree(ptr); } \
TF3SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
TF3SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
TF3SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf3AlignedAlloc(sizeInBytes,16); } \
TF3SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf3AlignedFree(ptr); } \
TF3SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
TF3SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
TF3SIMD_FORCE_INLINE tf3Scalar tf3Sqrt(tf3Scalar x) { return sqrt(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Fabs(tf3Scalar x) { return fabs(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Cos(tf3Scalar x) { return cos(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Sin(tf3Scalar x) { return sin(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Tan(tf3Scalar x) { return tan(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Acos(tf3Scalar x) { if (x<tf3Scalar(-1)) x=tf3Scalar(-1); if (x>tf3Scalar(1)) x=tf3Scalar(1); return acos(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Asin(tf3Scalar x) { if (x<tf3Scalar(-1)) x=tf3Scalar(-1); if (x>tf3Scalar(1)) x=tf3Scalar(1); return asin(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan(tf3Scalar x) { return atan(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan2(tf3Scalar x, tf3Scalar y) { return atan2(x, y); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Exp(tf3Scalar x) { return exp(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Log(tf3Scalar x) { return log(x); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Pow(tf3Scalar x,tf3Scalar y) { return pow(x,y); }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Fmod(tf3Scalar x,tf3Scalar y) { return fmod(x,y); }
#define TF3SIMD_2_PI tf3Scalar(6.283185307179586232)
#define TF3SIMD_PI (TF3SIMD_2_PI * tf3Scalar(0.5))
#define TF3SIMD_HALF_PI (TF3SIMD_2_PI * tf3Scalar(0.25))
#define TF3SIMD_RADS_PER_DEG (TF3SIMD_2_PI / tf3Scalar(360.0))
#define TF3SIMD_DEGS_PER_RAD (tf3Scalar(360.0) / TF3SIMD_2_PI)
#define TF3SIMDSQRT12 tf3Scalar(0.7071067811865475244008443621048490)
#define tf3RecipSqrt(x) ((tf3Scalar)(tf3Scalar(1.0)/tf3Sqrt(tf3Scalar(x)))) /* reciprocal square root */
#define TF3SIMD_EPSILON DBL_EPSILON
#define TF3SIMD_INFINITY DBL_MAX
TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan2Fast(tf3Scalar y, tf3Scalar x)
{
tf3Scalar coeff_1 = TF3SIMD_PI / 4.0f;
tf3Scalar coeff_2 = 3.0f * coeff_1;
tf3Scalar abs_y = tf3Fabs(y);
tf3Scalar angle;
if (x >= 0.0f) {
tf3Scalar r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
tf3Scalar r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
return (y < 0.0f) ? -angle : angle;
}
TF3SIMD_FORCE_INLINE bool tf3FuzzyZero(tf3Scalar x) { return tf3Fabs(x) < TF3SIMD_EPSILON; }
TF3SIMD_FORCE_INLINE bool tf3Equal(tf3Scalar a, tf3Scalar eps) {
return (((a) <= eps) && !((a) < -eps));
}
TF3SIMD_FORCE_INLINE bool tf3GreaterEqual (tf3Scalar a, tf3Scalar eps) {
return (!((a) <= eps));
}
TF3SIMD_FORCE_INLINE int tf3IsNegative(tf3Scalar x) {
return x < tf3Scalar(0.0) ? 1 : 0;
}
TF3SIMD_FORCE_INLINE tf3Scalar tf3Radians(tf3Scalar x) { return x * TF3SIMD_RADS_PER_DEG; }
TF3SIMD_FORCE_INLINE tf3Scalar tf3Degrees(tf3Scalar x) { return x * TF3SIMD_DEGS_PER_RAD; }
#define TF3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#ifndef tf3Fsel
TF3SIMD_FORCE_INLINE tf3Scalar tf3Fsel(tf3Scalar a, tf3Scalar b, tf3Scalar c)
{
return a >= 0 ? b : c;
}
#endif
#define tf3Fsels(a,b,c) (tf3Scalar)tf3Fsel(a,b,c)
TF3SIMD_FORCE_INLINE bool tf3MachineIsLittleEndian()
{
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;
}
///tf3Select 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
TF3SIMD_FORCE_INLINE unsigned tf3Select(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));
}
TF3SIMD_FORCE_INLINE int tf3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
{
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
TF3SIMD_FORCE_INLINE float tf3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
{
#ifdef TF3_HAVE_NATIVE_FSEL
return (float)tf3Fsel((tf3Scalar)condition - tf3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#else
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
#endif
}
template<typename T> TF3SIMD_FORCE_INLINE void tf3Swap(T& a, T& b)
{
T tmp = a;
a = b;
b = tmp;
}
//PCK: endian swapping functions
TF3SIMD_FORCE_INLINE unsigned tf3SwapEndian(unsigned val)
{
return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
}
TF3SIMD_FORCE_INLINE unsigned short tf3SwapEndian(unsigned short val)
{
return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
}
TF3SIMD_FORCE_INLINE unsigned tf3SwapEndian(int val)
{
return tf3SwapEndian((unsigned)val);
}
TF3SIMD_FORCE_INLINE unsigned short tf3SwapEndian(short val)
{
return tf3SwapEndian((unsigned short) val);
}
///tf3SwapFloat uses using char pointers to swap the endianness
////tf3SwapFloat/tf3SwapDouble 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
TF3SIMD_FORCE_INLINE unsigned int tf3SwapEndianFloat(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
TF3SIMD_FORCE_INLINE float tf3UnswapEndianFloat(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
TF3SIMD_FORCE_INLINE void tf3SwapEndianDouble(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
TF3SIMD_FORCE_INLINE double tf3UnswapEndianDouble(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 [-TF3SIMD_PI, TF3SIMD_PI]
TF3SIMD_FORCE_INLINE tf3Scalar tf3NormalizeAngle(tf3Scalar angleInRadians)
{
angleInRadians = tf3Fmod(angleInRadians, TF3SIMD_2_PI);
if(angleInRadians < -TF3SIMD_PI)
{
return angleInRadians + TF3SIMD_2_PI;
}
else if(angleInRadians > TF3SIMD_PI)
{
return angleInRadians - TF3SIMD_2_PI;
}
else
{
return angleInRadians;
}
}
///rudimentary class to provide type info
struct tf3TypedObject
{
tf3TypedObject(int objectType)
:m_objectType(objectType)
{
}
int m_objectType;
inline int getObjectType() const
{
return m_objectType;
}
};
#endif //TF3SIMD___SCALAR_H

305
src/Libraries/tf3/include/tf3/LinearMath/Transform.h Normal file
View File

@ -0,0 +1,305 @@
/*
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 tf3_Transform_H
#define tf3_Transform_H
#include "Matrix3x3.h"
namespace tf3
{
#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 TF3SIMD_FORCE_INLINE Transform(const Quaternion& q,
const Vector3& c = Vector3(tf3Scalar(0), tf3Scalar(0), tf3Scalar(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 TF3SIMD_FORCE_INLINE Transform(const Matrix3x3& b,
const Vector3& c = Vector3(tf3Scalar(0), tf3Scalar(0), tf3Scalar(0)))
: m_basis(b),
m_origin(c)
{}
/**@brief Copy constructor */
TF3SIMD_FORCE_INLINE Transform (const Transform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
{
}
/**@brief Assignment Operator */
TF3SIMD_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 */
TF3SIMD_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 = tf3MultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis;
}
*/
/**@brief Return the transform of the vector */
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const
{
return (*this)(x);
}
/**@brief Return the transform of the Quaternion */
TF3SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const
{
return getRotation() * q;
}
/**@brief Return the basis matrix for the rotation */
TF3SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
TF3SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; }
/**@brief Return the origin vector translation */
TF3SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
TF3SIMD_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 tf3Scalar *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(tf3Scalar *m) const
{
m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin.x();
m[13] = m_origin.y();
m[14] = m_origin.z();
m[15] = tf3Scalar(1.0);
}
/**@brief Set the translational element
* @param origin The vector to set the translation to */
TF3SIMD_FORCE_INLINE void setOrigin(const Vector3& origin)
{
m_origin = origin;
}
TF3SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const;
/**@brief Set the rotational element by Matrix3x3 */
TF3SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis)
{
m_basis = basis;
}
/**@brief Set the rotational element by Quaternion */
TF3SIMD_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(tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(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);
};
TF3SIMD_FORCE_INLINE Vector3
Transform::invXform(const Vector3& inVec) const
{
Vector3 v = inVec - m_origin;
return (m_basis.transpose() * v);
}
TF3SIMD_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);
}
TF3SIMD_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 */
TF3SIMD_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;
};
TF3SIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
m_origin.serialize(dataOut.m_origin);
}
TF3SIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const
{
m_basis.serializeFloat(dataOut.m_basis);
m_origin.serializeFloat(dataOut.m_origin);
}
TF3SIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
m_origin.deSerialize(dataIn.m_origin);
}
TF3SIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn)
{
m_basis.deSerializeFloat(dataIn.m_basis);
m_origin.deSerializeFloat(dataIn.m_origin);
}
TF3SIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn)
{
m_basis.deSerializeDouble(dataIn.m_basis);
m_origin.deSerializeDouble(dataIn.m_origin);
}
}
#endif

731
src/Libraries/tf3/include/tf3/LinearMath/Vector3.h Normal file
View File

@ -0,0 +1,731 @@
/*
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 TF3_VECTOR3_H
#define TF3_VECTOR3_H
#include "Scalar.h"
#include "MinMax.h"
namespace tf3
{
#define Vector3Data Vector3DoubleData
#define Vector3DataName "Vector3DoubleData"
/**@brief tf3::Vector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when tf3::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 TF3SIMD version that keeps the data in registers
*/
ATTRIBUTE_ALIGNED16(class) Vector3
{
public:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
tf3Scalar m_floats[4];
public:
TF3SIMD_FORCE_INLINE const vec_float4& get128() const
{
return *((const vec_float4*)&m_floats[0]);
}
public:
#else //__CELLOS_LV2__ __SPU__
#ifdef TF3_USE_SSE // _WIN32
union {
__m128 mVec128;
tf3Scalar m_floats[4];
};
TF3SIMD_FORCE_INLINE __m128 get128() const
{
return mVec128;
}
TF3SIMD_FORCE_INLINE void set128(__m128 v128)
{
mVec128 = v128;
}
#else
tf3Scalar m_floats[4];
#endif
#endif //__CELLOS_LV2__ __SPU__
public:
/**@brief No initialization constructor */
TF3SIMD_FORCE_INLINE Vector3() {}
/**@brief Constructor from scalars
* @param x X value
* @param y Y value
* @param z Z value
*/
TF3SIMD_FORCE_INLINE Vector3(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)
{
m_floats[0] = x;
m_floats[1] = y;
m_floats[2] = z;
m_floats[3] = tf3Scalar(0.);
}
/**@brief Add a vector to this one
* @param The vector to add to this one */
TF3SIMD_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 Sutf3ract a vector from this one
* @param The vector to sutf3ract */
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE Vector3& operator*=(const tf3Scalar& 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 */
TF3SIMD_FORCE_INLINE Vector3& operator/=(const tf3Scalar& s)
{
tf3FullAssert(s != tf3Scalar(0.0));
return *this *= tf3Scalar(1.0) / s;
}
/**@brief Return the dot product
* @param v The other vector in the dot product */
TF3SIMD_FORCE_INLINE tf3Scalar 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 */
TF3SIMD_FORCE_INLINE tf3Scalar length2() const
{
return dot(*this);
}
/**@brief Return the length of the vector */
TF3SIMD_FORCE_INLINE tf3Scalar length() const
{
return tf3Sqrt(length2());
}
/**@brief Return the distance squared between the ends of this and another vector
* This is symantically treating the vector like a point */
TF3SIMD_FORCE_INLINE tf3Scalar 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 */
TF3SIMD_FORCE_INLINE tf3Scalar distance(const Vector3& v) const;
/**@brief Normalize this vector
* x^2 + y^2 + z^2 = 1 */
TF3SIMD_FORCE_INLINE Vector3& normalize()
{
return *this /= length();
}
/**@brief Return a normalized version of this vector */
TF3SIMD_FORCE_INLINE Vector3 normalized() const;
/**@brief Rotate this vector
* @param wAxis The axis to rotate about
* @param angle The angle to rotate by */
TF3SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf3Scalar angle ) const;
/**@brief Return the angle between this and another vector
* @param v The other vector */
TF3SIMD_FORCE_INLINE tf3Scalar angle(const Vector3& v) const
{
tf3Scalar s = tf3Sqrt(length2() * v.length2());
tf3FullAssert(s != tf3Scalar(0.0));
return tf3Acos(dot(v) / s);
}
/**@brief Return a vector will the absolute values of each element */
TF3SIMD_FORCE_INLINE Vector3 absolute() const
{
return Vector3(
tf3Fabs(m_floats[0]),
tf3Fabs(m_floats[1]),
tf3Fabs(m_floats[2]));
}
/**@brief Return the cross product between this and another vector
* @param v The other vector */
TF3SIMD_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]);
}
TF3SIMD_FORCE_INLINE tf3Scalar 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 */
TF3SIMD_FORCE_INLINE int minAxis() const
{
return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
}
/**@brief Return the axis with the largest value
* Note return values are 0,1,2 for x, y, or z */
TF3SIMD_FORCE_INLINE int maxAxis() const
{
return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
}
TF3SIMD_FORCE_INLINE int furthestAxis() const
{
return absolute().minAxis();
}
TF3SIMD_FORCE_INLINE int closestAxis() const
{
return absolute().maxAxis();
}
TF3SIMD_FORCE_INLINE void setInterpolate3(const Vector3& v0, const Vector3& v1, tf3Scalar rt)
{
tf3Scalar s = tf3Scalar(1.0) - rt;
m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
//don't do the unused w component
// m_co[3] = s * v0[3] + rt * v1[3];
}
/**@brief Return the linear interpolation between this and another vector
* @param v The other vector
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
TF3SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf3Scalar& 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 */
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE const tf3Scalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
TF3SIMD_FORCE_INLINE const tf3Scalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
TF3SIMD_FORCE_INLINE const tf3Scalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
TF3SIMD_FORCE_INLINE void setX(tf3Scalar x) { m_floats[0] = x;};
/**@brief Set the y value */
TF3SIMD_FORCE_INLINE void setY(tf3Scalar y) { m_floats[1] = y;};
/**@brief Set the z value */
TF3SIMD_FORCE_INLINE void setZ(tf3Scalar z) {m_floats[2] = z;};
/**@brief Set the w value */
TF3SIMD_FORCE_INLINE void setW(tf3Scalar w) { m_floats[3] = w;};
/**@brief Return the x value */
TF3SIMD_FORCE_INLINE const tf3Scalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
TF3SIMD_FORCE_INLINE const tf3Scalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
TF3SIMD_FORCE_INLINE const tf3Scalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
TF3SIMD_FORCE_INLINE const tf3Scalar& w() const { return m_floats[3]; }
//TF3SIMD_FORCE_INLINE tf3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//TF3SIMD_FORCE_INLINE const tf3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator tf3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
TF3SIMD_FORCE_INLINE operator tf3Scalar *() { return &m_floats[0]; }
TF3SIMD_FORCE_INLINE operator const tf3Scalar *() const { return &m_floats[0]; }
TF3SIMD_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]));
}
TF3SIMD_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
*/
TF3SIMD_FORCE_INLINE void setMax(const Vector3& other)
{
tf3SetMax(m_floats[0], other.m_floats[0]);
tf3SetMax(m_floats[1], other.m_floats[1]);
tf3SetMax(m_floats[2], other.m_floats[2]);
tf3SetMax(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
*/
TF3SIMD_FORCE_INLINE void setMin(const Vector3& other)
{
tf3SetMin(m_floats[0], other.m_floats[0]);
tf3SetMin(m_floats[1], other.m_floats[1]);
tf3SetMin(m_floats[2], other.m_floats[2]);
tf3SetMin(m_floats[3], other.w());
}
TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3] = tf3Scalar(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(tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(0.));
}
TF3SIMD_FORCE_INLINE bool isZero() const
{
return m_floats[0] == tf3Scalar(0) && m_floats[1] == tf3Scalar(0) && m_floats[2] == tf3Scalar(0);
}
TF3SIMD_FORCE_INLINE bool fuzzyZero() const
{
return length2() < TF3SIMD_EPSILON;
}
TF3SIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const;
TF3SIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn);
TF3SIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const;
TF3SIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn);
TF3SIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const;
TF3SIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn);
};
/**@brief Return the sum of two vectors (Point symantics)*/
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE Vector3
operator*(const Vector3& v, const tf3Scalar& 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 */
TF3SIMD_FORCE_INLINE Vector3
operator*(const tf3Scalar& s, const Vector3& v)
{
return v * s;
}
/**@brief Return the vector inversely scaled by s */
TF3SIMD_FORCE_INLINE Vector3
operator/(const Vector3& v, const tf3Scalar& s)
{
tf3FullAssert(s != tf3Scalar(0.0));
return v * (tf3Scalar(1.0) / s);
}
/**@brief Return the vector inversely scaled by s */
TF3SIMD_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 */
TF3SIMD_FORCE_INLINE tf3Scalar
tf3Dot(const Vector3& v1, const Vector3& v2)
{
return v1.dot(v2);
}
/**@brief Return the distance squared between two vectors */
TF3SIMD_FORCE_INLINE tf3Scalar
tf3Distance2(const Vector3& v1, const Vector3& v2)
{
return v1.distance2(v2);
}
/**@brief Return the distance between two vectors */
TF3SIMD_FORCE_INLINE tf3Scalar
tf3Distance(const Vector3& v1, const Vector3& v2)
{
return v1.distance(v2);
}
/**@brief Return the angle between two vectors */
TF3SIMD_FORCE_INLINE tf3Scalar
tf3Angle(const Vector3& v1, const Vector3& v2)
{
return v1.angle(v2);
}
/**@brief Return the cross product of two vectors */
TF3SIMD_FORCE_INLINE Vector3
tf3Cross(const Vector3& v1, const Vector3& v2)
{
return v1.cross(v2);
}
TF3SIMD_FORCE_INLINE tf3Scalar
tf3Triple(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) */
TF3SIMD_FORCE_INLINE Vector3
lerp(const Vector3& v1, const Vector3& v2, const tf3Scalar& t)
{
return v1.lerp(v2, t);
}
TF3SIMD_FORCE_INLINE tf3Scalar Vector3::distance2(const Vector3& v) const
{
return (v - *this).length2();
}
TF3SIMD_FORCE_INLINE tf3Scalar Vector3::distance(const Vector3& v) const
{
return (v - *this).length();
}
TF3SIMD_FORCE_INLINE Vector3 Vector3::normalized() const
{
return *this / length();
}
TF3SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf3Scalar 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 * tf3Cos( angle ) + y * tf3Sin( angle ) );
}
class tf3Vector4 : public Vector3
{
public:
TF3SIMD_FORCE_INLINE tf3Vector4() {}
TF3SIMD_FORCE_INLINE tf3Vector4(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w)
: Vector3(x,y,z)
{
m_floats[3] = w;
}
TF3SIMD_FORCE_INLINE tf3Vector4 absolute4() const
{
return tf3Vector4(
tf3Fabs(m_floats[0]),
tf3Fabs(m_floats[1]),
tf3Fabs(m_floats[2]),
tf3Fabs(m_floats[3]));
}
tf3Scalar getW() const { return m_floats[3];}
TF3SIMD_FORCE_INLINE int maxAxis4() const
{
int maxIndex = -1;
tf3Scalar maxVal = tf3Scalar(-TF3_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;
}
TF3SIMD_FORCE_INLINE int minAxis4() const
{
int minIndex = -1;
tf3Scalar minVal = tf3Scalar(TF3_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;
}
TF3SIMD_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(tf3Scalar *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
*/
TF3SIMD_FORCE_INLINE void setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3]=w;
}
};
///tf3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
TF3SIMD_FORCE_INLINE void tf3SwapScalarEndian(const tf3Scalar& sourceVal, tf3Scalar& 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];
}
///tf3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
TF3SIMD_FORCE_INLINE void tf3SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec)
{
for (int i=0;i<4;i++)
{
tf3SwapScalarEndian(sourceVec[i],destVec[i]);
}
}
///tf3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
TF3SIMD_FORCE_INLINE void tf3UnSwapVector3Endian(Vector3& vector)
{
Vector3 swappedVec;
for (int i=0;i<4;i++)
{
tf3SwapScalarEndian(vector[i],swappedVec[i]);
}
vector = swappedVec;
}
TF3SIMD_FORCE_INLINE void tf3PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q)
{
if (tf3Fabs(n.z()) > TF3SIMDSQRT12) {
// choose p in y-z plane
tf3Scalar a = n[1]*n[1] + n[2]*n[2];
tf3Scalar k = tf3RecipSqrt (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
tf3Scalar a = n.x()*n.x() + n.y()*n.y();
tf3Scalar k = tf3RecipSqrt (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];
};
TF3SIMD_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]);
}
TF3SIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = tf3Scalar(dataIn.m_floats[i]);
}
TF3SIMD_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]);
}
TF3SIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = tf3Scalar(dataIn.m_floats[i]);
}
TF3SIMD_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];
}
TF3SIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = dataIn.m_floats[i];
}
}
#endif //TF3_VECTOR3_H

11
src/Libraries/tf3/include/tf3/TF3Error.h Normal file
View File

@ -0,0 +1,11 @@
// TF3 error codes in tf3 namespace (no tf3_msgs dependency)
#ifndef TF3_ERROR_TF3_H
#define TF3_ERROR_TF3_H
namespace tf3 {
struct TF3Error {
enum { NO_ERROR = 0, LOOKUP_ERROR = 1, CONNECTIVITY_ERROR = 2, EXTRAPOLATION_ERROR = 3 };
};
}
#endif // TF3_ERROR_TF3_H

432
src/Libraries/tf3/include/tf3/buffer_core.h Normal file
View File

@ -0,0 +1,432 @@
/*
* 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 TF3_BUFFER_CORE_H
#define TF3_BUFFER_CORE_H
#include "transform_storage.h"
#include <boost/signals2.hpp>
#include <string>
#include "tf3/compat.h"
#include "tf3/time.h"
//////////////////////////backwards startup for porting
//#include "tf/tf.h"
#include <boost/unordered_map.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
namespace tf3
{
typedef std::pair<tf3::Time, CompactFrameID> P_TimeAndFrameID;
typedef uint32_t TransformableCallbackHandle;
typedef uint64_t TransformableRequestHandle;
class TimeCacheInterface;
typedef boost::shared_ptr<TimeCacheInterface> 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(tf3::Duration cache_time_ = tf3::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 tf3::TransformStampedMsg& 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 tf3::LookupException, tf3::ConnectivityException,
* tf3::ExtrapolationException, tf3::InvalidArgumentException
*/
tf3::TransformStampedMsg
lookupTransform(const std::string& target_frame, const std::string& source_frame,
const tf3::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 tf3::LookupException, tf3::ConnectivityException,
* tf3::ExtrapolationException, tf3::InvalidArgumentException
*/
tf3::TransformStampedMsg
lookupTransform(const std::string& target_frame, const tf3::Time& target_time,
const std::string& source_frame, const tf3::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 tf3::LookupException, tf3::ConnectivityException,
* tf3::ExtrapolationException, tf3::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 tf3::LookupException, tf3::ConnectivityException,
* tf3::ExtrapolationException, tf3::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 tf3::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 tf3::Time& target_time,
const std::string& source_frame, const tf3::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<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
tf3::Time time, TransformableResult result)> 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, tf3::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<void(void)> 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, tf3::Time time, std::string& parent) const;
/** \brief A way to get a std::vector of available frame ids */
void _getFrameStrings(std::vector<std::string>& 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, tf3::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 */
tf3::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, tf3::Time target_time, const std::string & source_frame, tf3::Time source_time, const std::string & fixed_frame, std::vector<std::string>& 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<TimeCacheInterfacePtr> 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<std::string, CompactFrameID> M_StringToCompactFrameID;
M_StringToCompactFrameID frameIDs_;
/** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */
std::vector<std::string> frameIDs_reverse;
/** \brief A map to lookup the most recent authority for a given frame */
std::map<CompactFrameID, std::string> frame_authority_;
/// How long to cache transform history
tf3::Duration cache_time_;
typedef boost::unordered_map<TransformableCallbackHandle, TransformableCallback> M_TransformableCallback;
M_TransformableCallback transformable_callbacks_;
uint32_t transformable_callbacks_counter_;
boost::mutex transformable_callbacks_mutex_;
struct TransformableRequest
{
tf3::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<TransformableRequest> 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<void(void)> 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, tf3::Time& time, std::string* error_string) const;
template<typename F>
int walkToTopParent(F& f, tf3::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<typename F>
int walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
void testTransformableRequests();
bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
const tf3::Time& time, std::string* error_msg) const;
bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
const tf3::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, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const
{
return buffer.lookupFrameString(frame_id_num);
}
};
}
#endif //TF3_CORE_H

31
src/Libraries/tf3/include/tf3/compat.h Normal file
View File

@ -0,0 +1,31 @@
// Compatibility types to avoid using ros:: or geometry_msgs:: namespaces inside tf3
#ifndef TF3_COMPAT_H
#define TF3_COMPAT_H
#include "tf3/time.h"
#include <string>
namespace tf3 {
// Minimal header/message equivalents owned by tf3 (no ros:: or geometry_msgs::)
struct HeaderMsg
{
uint32_t seq;
Time stamp;
std::string frame_id;
};
struct Vector3Msg { double x, y, z; };
struct QuaternionMsg { double x, y, z, w; };
struct TransformMsg { Vector3Msg translation; QuaternionMsg rotation; };
struct TransformStampedMsg
{
HeaderMsg header;
std::string child_frame_id;
TransformMsg transform;
};
} // namespace tf3
#endif // TF3_COMPAT_H

131
src/Libraries/tf3/include/tf3/convert.h Normal file
View File

@ -0,0 +1,131 @@
/*
* 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 TF3_CONVERT_H
#define TF3_CONVERT_H
#include <tf3/transform_datatypes.h>
#include <tf3/exceptions.h>
#include <tf3/message_traits.h>
#include <tf3/impl/convert.h>
#include <tf3/compat.h>
namespace tf3 {
/**\brief The templated function expected to be able to do a transform
*
* This is the method which tf3 will use to try to apply a transform for any given datatype.
* \param data_in The data to be transformed.
* \param data_out A reference to the output data. Note this can point to data in and the method should be mutation safe.
* \param transform The transform to apply to data_in to fill data_out.
*
* This method needs to be implemented by client library developers
*/
template <class T>
void doTransform(const T& data_in, T& data_out, const tf3::TransformStampedMsg& transform);
/**\brief Get the timestamp from data
* \param t The data input.
* \return The timestamp associated with the data. The lifetime of the returned
* reference is bound to the lifetime of the argument.
*/
template <class T>
const tf3::Time& getTimestamp(const T& t);
/**\brief Get the frame_id from data
* \param t The data input.
* \return The frame_id associated with the data. The lifetime of the returned
* reference is bound to the lifetime of the argument.
*/
template <class T>
const std::string& getFrameId(const T& t);
/* An implementation for Stamped<P> datatypes */
template <class P>
const tf3::Time& getTimestamp(const tf3::Stamped<P>& t)
{
return t.stamp_;
}
/* An implementation for Stamped<P> datatypes */
template <class P>
const std::string& getFrameId(const tf3::Stamped<P>& t)
{
return t.frame_id_;
}
/** Function that converts from one type to a ROS message type. It has to be
* implemented by each data type in tf3_* (except ROS messages) as it is
* used in the "convert" function.
* \param a an object of whatever type
* \return the conversion as a ROS message
*/
template<typename A, typename B>
B toMsg(const A& a);
/** Function that converts from a ROS message type to another type. It has to be
* implemented by each data type in tf3_* (except ROS messages) as it is used
* in the "convert" function.
* \param a a ROS message to convert from
* \param b the object to convert to
*/
template<typename A, typename B>
void fromMsg(const A&, B& b);
/** 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 <class A, class B>
void convert(const A& a, B& b)
{
//printf("In double type convert\n");
impl::Converter<tf3::message_traits::IsMessage<A>::value, tf3::message_traits::IsMessage<B>::value>::convert(a, b);
}
template <class A>
void convert(const A& a1, A& a2)
{
//printf("In single type convert\n");
if(&a1 != &a2)
a2 = a1;
}
}
#endif //TF3_CONVERT_H

110
src/Libraries/tf3/include/tf3/exceptions.h Normal file
View File

@ -0,0 +1,110 @@
/*
* 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 TF3_EXCEPTIONS_H
#define TF3_EXCEPTIONS_H
#include <stdexcept>
namespace tf3{
/** \brief A base class for all tf3 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) : tf3::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) : tf3::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) : tf3::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) : tf3::TransformException(errorDescription) { ; };
};
/** \brief An exception class to notify that a timeout has occured
*
*
*/
class TimeoutException: public TransformException
{
public:
TimeoutException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; };
};
}
#endif //TF3_EXCEPTIONS_H

90
src/Libraries/tf3/include/tf3/impl/convert.h Normal file
View File

@ -0,0 +1,90 @@
/*
* 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 TF3_IMPL_CONVERT_H
#define TF3_IMPL_CONVERT_H
namespace tf3 {
namespace impl {
template <bool IS_MESSAGE_A, bool IS_MESSAGE_B>
class Converter {
public:
template<typename A, typename B>
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 <typename A, typename B>
//inline void Converter<true, true>::convert(const A& a, B& b);
template <>
template <typename A, typename B>
inline void Converter<true, false>::convert(const A& a, B& b)
{
#ifdef _MSC_VER
tf3::fromMsg(a, b);
#else
fromMsg(a, b);
#endif
}
template <>
template <typename A, typename B>
inline void Converter<false, true>::convert(const A& a, B& b)
{
#ifdef _MSC_VER
b = tf3::toMsg(a);
#else
b = toMsg(a);
#endif
}
template <>
template <typename A, typename B>
inline void Converter<false, false>::convert(const A& a, B& b)
{
#ifdef _MSC_VER
tf3::fromMsg(tf3::toMsg(a), b);
#else
fromMsg(toMsg(a), b);
#endif
}
}
}
#endif //TF3_IMPL_CONVERT_H

142
src/Libraries/tf3/include/tf3/impl/utils.h Normal file
View File

@ -0,0 +1,142 @@
// 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.
#ifndef TF3_IMPL_UTILS_H
#define TF3_IMPL_UTILS_H
#include <tf3/transform_datatypes.h>
#include <tf3/LinearMath/Quaternion.h>
#include <tf3/compat.h>
namespace tf3 {
namespace impl {
/** Function needed for the generalization of toQuaternion
* \param q a tf3::Quaternion
* \return a copy of the same quaternion
*/
inline
tf3::Quaternion toQuaternion(const tf3::Quaternion& q) {
return q;
}
/** Function needed for the generalization of toQuaternion
* \param q a tf3::QuaternionMsg wrapped in a stamped container
* \return a copy of the same quaternion as a tf3::Quaternion
*/
inline
tf3::Quaternion toQuaternion(const tf3::QuaternionMsg& q) {
tf3::Quaternion res;
res.setValue(q.x, q.y, q.z, q.w);
return res;
}
/** Function needed for the generalization of toQuaternion
* \param t some tf3::Stamped object
* \return a copy of the same quaternion as a tf3::Quaternion
*/
template<typename T>
tf3::Quaternion toQuaternion(const tf3::Stamped<T>& t) {
tf3::QuaternionMsg q = toMsg(t);
return toQuaternion(q);
}
/** Generic version of toQuaternion. It tries to convert the argument
* to a geometry_msgs::Quaternion
* \param t some object
* \return a copy of the same quaternion as a tf3::Quaternion
*/
template<typename T>
tf3::Quaternion toQuaternion(const T& t) {
tf3::QuaternionMsg q = toMsg(t);
return toQuaternion(q);
}
/** The code below is blantantly copied from urdfdom_headers
* only the normalization has been added.
* It computes the Euler roll, pitch yaw from a tf3::Quaternion
* It is equivalent to tf3::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
* \param q a tf3::Quaternion
* \param yaw the computed yaw
* \param pitch the computed pitch
* \param roll the computed roll
*/
inline
void getEulerYPR(const tf3::Quaternion& q, double &yaw, double &pitch, double &roll)
{
double sqw;
double sqx;
double sqy;
double sqz;
sqx = q.x() * q.x();
sqy = q.y() * q.y();
sqz = q.z() * q.z();
sqw = q.w() * q.w();
// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
if (sarg <= -0.99999) {
pitch = -0.5*M_PI;
roll = 0;
yaw = -2 * atan2(q.y(), q.x());
} else if (sarg >= 0.99999) {
pitch = 0.5*M_PI;
roll = 0;
yaw = 2 * atan2(q.y(), q.x());
} else {
pitch = asin(sarg);
roll = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz);
yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
}
}
/** The code below is a simplified version of getEulerRPY that only
* returns the yaw. It is mostly useful in navigation where only yaw
* matters
* \param q a tf3::Quaternion
* \return the computed yaw
*/
inline
double getYaw(const tf3::Quaternion& q)
{
double yaw;
double sqw;
double sqx;
double sqy;
double sqz;
sqx = q.x() * q.x();
sqy = q.y() * q.y();
sqz = q.z() * q.z();
sqw = q.w() * q.w();
// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
if (sarg <= -0.99999) {
yaw = -2 * atan2(q.y(), q.x());
} else if (sarg >= 0.99999) {
yaw = 2 * atan2(q.y(), q.x());
} else {
yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
}
return yaw;
}
}
}
#endif //TF3_IMPL_UTILS_H

13
src/Libraries/tf3/include/tf3/macros.h Normal file
View File

@ -0,0 +1,13 @@
// Minimal compatibility macros used by tf3 to avoid depending on ROS
#ifndef TF3_MACROS_H_
#define TF3_MACROS_H_
#if defined(__clang__) || defined(__GNUC__)
# define ROS_DEPRECATED __attribute__((deprecated))
#elif defined(_MSC_VER)
# define ROS_DEPRECATED __declspec(deprecated)
#else
# define ROS_DEPRECATED
#endif
#endif // TF3_MACROS_H_

12
src/Libraries/tf3/include/tf3/message_traits.h Normal file
View File

@ -0,0 +1,12 @@
// Minimal message_traits implementation for tf3 (no ROS dependency)
#ifndef TF3_MESSAGE_TRAITS_H
#define TF3_MESSAGE_TRAITS_H
namespace tf3 {
namespace message_traits {
template <typename T>
struct IsMessage { static const bool value = false; };
}
}
#endif // TF3_MESSAGE_TRAITS_H

152
src/Libraries/tf3/include/tf3/time.h Normal file
View File

@ -0,0 +1,152 @@
// Minimal tf3::Time and tf3::Duration (non-ROS) for standalone tf3
#ifndef TF3_TIME_H
#define TF3_TIME_H
#include <cstdint>
#include <limits>
#include <cmath>
#include <chrono>
#include <thread>
namespace tf3 {
class Duration;
class Time {
public:
uint32_t sec;
uint32_t nsec;
Time(): sec(0), nsec(0) {}
Time(uint32_t s, uint32_t ns): sec(s), nsec(ns) {}
static Time now()
{
using namespace std::chrono;
auto now = system_clock::now().time_since_epoch();
auto ns = duration_cast<nanoseconds>(now).count();
Time t;
t.sec = static_cast<uint32_t>(ns / 1000000000ULL);
t.nsec = static_cast<uint32_t>(ns % 1000000000ULL);
return t;
}
static Time fromSec(double s)
{
Time t;
t.sec = static_cast<uint32_t>(std::floor(s));
t.nsec = static_cast<uint32_t>((s - t.sec) * 1e9);
return t;
}
double toSec() const { return static_cast<double>(sec) + static_cast<double>(nsec) * 1e-9; }
bool isZero() const { return sec == 0 && nsec == 0; }
bool operator==(const Time& o) const { return sec == o.sec && nsec == o.nsec; }
bool operator!=(const Time& o) const { return !(*this == o); }
bool operator<(const Time& o) const { return sec < o.sec || (sec == o.sec && nsec < o.nsec); }
bool operator>(const Time& o) const { return o < *this; }
bool operator<=(const Time& o) const { return !(*this > o); }
bool operator>=(const Time& o) const { return !(*this < o); }
friend Duration operator-(const Time& a, const Time& b);
friend Time operator+(const Time& t, const Duration& d);
friend Time operator-(const Time& t, const Duration& d);
static const Time& TIME_MAX()
{
static Time tm{std::numeric_limits<uint32_t>::max(), std::numeric_limits<uint32_t>::max()};
return tm;
}
};
class Duration {
public:
int32_t sec;
int32_t nsec;
Duration(): sec(0), nsec(0) {}
Duration(int32_t s, int32_t ns): sec(s), nsec(ns) { normalize(); }
explicit Duration(double seconds)
{
sec = static_cast<int32_t>(std::floor(seconds));
nsec = static_cast<int32_t>((seconds - sec) * 1e9);
normalize();
}
static Duration fromSec(double s) { return Duration(s); }
static Duration fromNSec(int64_t ns_total)
{
int32_t s = static_cast<int32_t>(ns_total / 1000000000LL);
int32_t ns = static_cast<int32_t>(ns_total % 1000000000LL);
return Duration(s, ns);
}
double toSec() const { return static_cast<double>(sec) + static_cast<double>(nsec) * 1e-9; }
void normalize()
{
if (nsec >= 1000000000L)
{
sec += nsec / 1000000000L;
nsec = nsec % 1000000000L;
}
else if (nsec < 0)
{
int32_t borrow = (std::abs(nsec) / 1000000000L) + 1;
sec -= borrow;
nsec += borrow * 1000000000L;
}
}
Duration operator+(const Duration& other) const { return Duration(sec + other.sec, nsec + other.nsec); }
Duration operator-(const Duration& other) const { return Duration(sec - other.sec, nsec - other.nsec); }
Duration& operator+=(const Duration& other) { sec += other.sec; nsec += other.nsec; normalize(); return *this; }
Duration& operator-=(const Duration& other) { sec -= other.sec; nsec -= other.nsec; normalize(); return *this; }
bool operator==(const Duration& o) const { return sec == o.sec && nsec == o.nsec; }
bool operator!=(const Duration& o) const { return !(*this == o); }
bool operator<(const Duration& o) const { return sec < o.sec || (sec == o.sec && nsec < o.nsec); }
bool operator>(const Duration& o) const { return o < *this; }
bool operator<=(const Duration& o) const { return !(*this > o); }
bool operator>=(const Duration& o) const { return !(*this < o); }
void sleep() const { if (sec < 0 || nsec < 0) return; auto ns_total = static_cast<int64_t>(sec) * 1000000000LL + nsec; std::this_thread::sleep_for(std::chrono::nanoseconds(ns_total)); }
};
inline Duration operator-(const Time& a, const Time& b)
{
int32_t s = static_cast<int32_t>(a.sec) - static_cast<int32_t>(b.sec);
int32_t ns = static_cast<int32_t>(a.nsec) - static_cast<int32_t>(b.nsec);
Duration d(s, ns);
d.normalize();
return d;
}
inline Time operator+(const Time& t, const Duration& d)
{
int64_t total_ns = static_cast<int64_t>(t.sec) * 1000000000LL + t.nsec + static_cast<int64_t>(d.sec) * 1000000000LL + d.nsec;
Time out;
out.sec = static_cast<uint32_t>(total_ns / 1000000000LL);
out.nsec = static_cast<uint32_t>(total_ns % 1000000000LL);
return out;
}
inline Time operator-(const Time& t, const Duration& d)
{
int64_t total_ns = static_cast<int64_t>(t.sec) * 1000000000LL + t.nsec - (static_cast<int64_t>(d.sec) * 1000000000LL + d.nsec);
if (total_ns < 0) total_ns = 0;
Time out;
out.sec = static_cast<uint32_t>(total_ns / 1000000000LL);
out.nsec = static_cast<uint32_t>(total_ns % 1000000000LL);
return out;
}
// Provide tf3::TIME_MAX symbol
namespace {
static const Time TIME_MAX = Time::TIME_MAX();
}
} // namespace tf3
#endif // TF3_TIME_H

155
src/Libraries/tf3/include/tf3/time_cache.h Normal file
View File

@ -0,0 +1,155 @@
/*
* 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 TF3_TIME_CACHE_H
#define TF3_TIME_CACHE_H
#include "transform_storage.h"
#include <deque>
#include "tf3/time.h"
#include <boost/shared_ptr.hpp>
namespace tf3
{
typedef std::pair<tf3::Time, CompactFrameID> P_TimeAndFrameID;
class TimeCacheInterface
{
public:
/** \brief Access data from the cache */
virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception
/** \brief Insert data into the cache */
virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0)=0;
/** @brief Clear the list of stored values */
virtual void clearList()=0;
/** \brief Retrieve the parent at a specific time */
virtual CompactFrameID getParent(tf3::Time time, std::string* error_str) = 0;
/**
* \brief Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no data.
*/
virtual P_TimeAndFrameID getLatestTimeAndParent() = 0;
/// Debugging information methods
/** @brief Get the length of the stored list */
virtual unsigned int getListLength()=0;
/** @brief Get the latest timestamp cached */
virtual tf3::Time getLatestTimestamp()=0;
/** @brief Get the oldest timestamp cached */
virtual tf3::Time getOldestTimestamp()=0;
};
typedef boost::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
/** \brief A class to keep a sorted linked list in time
* This builds and maintains a list of timestamped
* data. And provides lookup functions to get
* data out as a function of time. */
class TimeCache : public TimeCacheInterface
{
public:
static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below.
static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory.
static const int64_t DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000LL; //!< default value of 10 seconds storage
TimeCache(tf3::Duration max_storage_time = tf3::Duration::fromNSec(DEFAULT_MAX_STORAGE_TIME));
/// Virtual methods
virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0);
virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0);
virtual void clearList();
virtual CompactFrameID getParent(tf3::Time time, std::string* error_str);
virtual P_TimeAndFrameID getLatestTimeAndParent();
/// Debugging information methods
virtual unsigned int getListLength();
virtual tf3::Time getLatestTimestamp();
virtual tf3::Time getOldestTimestamp();
private:
typedef std::deque<TransformStorage> L_TransformStorage;
L_TransformStorage storage_;
tf3::Duration max_storage_time_;
/// A helper function for getData
//Assumes storage is already locked for it
inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, tf3::Time target_time, std::string* error_str);
inline void interpolate(const TransformStorage& one, const TransformStorage& two, tf3::Time time, TransformStorage& output);
void pruneList();
};
class StaticCache : public TimeCacheInterface
{
public:
/// Virtual methods
virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception
virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0);
virtual void clearList();
virtual CompactFrameID getParent(tf3::Time time, std::string* error_str);
virtual P_TimeAndFrameID getLatestTimeAndParent();
/// Debugging information methods
virtual unsigned int getListLength();
virtual tf3::Time getLatestTimestamp();
virtual tf3::Time getOldestTimestamp();
private:
TransformStorage storage_;
};
}
#endif // TF3_TIME_CACHE_H

84
src/Libraries/tf3/include/tf3/transform_datatypes.h Normal file
View File

@ -0,0 +1,84 @@
/*
* 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 TF3_TRANSFORM_DATATYPES_H
#define TF3_TRANSFORM_DATATYPES_H
#include <string>
#include "tf3/time.h"
namespace tf3
{
/** \brief The data type which will be cross compatable with geometry_msgs
* This is the tf3 datatype equivilant of a MessageStamped */
template <typename T>
class Stamped : public T{
public:
tf3::Time stamp_; ///< The timestamp associated with this data
std::string frame_id_; ///< The frame_id associated this data
/** Default constructor */
Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation
/** Full constructor */
Stamped(const T& input, const tf3::Time& timestamp, const std::string & frame_id) :
T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ;
/** Copy Constructor */
Stamped(const Stamped<T>& s):
T (s),
stamp_(s.stamp_),
frame_id_(s.frame_id_) {}
/** Copy assignment operator */
Stamped<T> & operator=(const Stamped<T> & rhs)
{
T::operator=(rhs);
stamp_ = rhs.stamp_;
frame_id_ = rhs.frame_id_;
return *this;
}
/** Set the data element */
void setData(const T& input){*static_cast<T*>(this) = input;};
};
/** \brief Comparison Operator for Stamped datatypes */
template <typename T>
bool operator==(const Stamped<T> &a, const Stamped<T> &b) {
return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast<const T&>(a) == static_cast<const T&>(b);
}
}
#endif //TF3_TRANSFORM_DATATYPES_H

80
src/Libraries/tf3/include/tf3/transform_storage.h Normal file
View File

@ -0,0 +1,80 @@
/*
* Copyright (c) 2010, 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 TF3_TRANSFORM_STORAGE_H
#define TF3_TRANSFORM_STORAGE_H
#include <tf3/LinearMath/Vector3.h>
#include <tf3/LinearMath/Quaternion.h>
#include "tf3/compat.h"
namespace tf3
{
typedef uint32_t CompactFrameID;
/** \brief Storage for transforms and their parent */
class TransformStorage
{
public:
TransformStorage();
TransformStorage(const tf3::TransformStampedMsg& data, CompactFrameID frame_id, CompactFrameID child_frame_id);
TransformStorage(const TransformStorage& rhs)
{
*this = rhs;
}
TransformStorage& operator=(const TransformStorage& rhs)
{
#if 01
rotation_ = rhs.rotation_;
translation_ = rhs.translation_;
stamp_ = rhs.stamp_;
frame_id_ = rhs.frame_id_;
child_frame_id_ = rhs.child_frame_id_;
#endif
return *this;
}
tf3::Quaternion rotation_;
tf3::Vector3 translation_;
tf3::Time stamp_;
CompactFrameID frame_id_;
CompactFrameID child_frame_id_;
};
}
#endif // TF3_TRANSFORM_STORAGE_H

66
src/Libraries/tf3/include/tf3/utils.h Normal file
View File

@ -0,0 +1,66 @@
// 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.
#ifndef TF3_UTILS_H
#define TF3_UTILS_H
#include <tf3/LinearMath/Transform.h>
#include <tf3/LinearMath/Quaternion.h>
#include <tf3/impl/utils.h>
namespace tf3 {
/** Return the yaw, pitch, roll of anything that can be converted to a tf3::Quaternion
* The conventions are the usual ROS ones defined in tf3/LineMath/Matrix3x3.h
* \param a the object to get data from (it represents a rotation/quaternion)
* \param yaw yaw
* \param pitch pitch
* \param roll roll
*/
template <class A>
void getEulerYPR(const A& a, double& yaw, double& pitch, double& roll)
{
tf3::Quaternion q = impl::toQuaternion(a);
impl::getEulerYPR(q, yaw, pitch, roll);
}
/** Return the yaw of anything that can be converted to a tf3::Quaternion
* The conventions are the usual ROS ones defined in tf3/LineMath/Matrix3x3.h
* This function is a specialization of getEulerYPR and is useful for its
* wide-spread use in navigation
* \param a the object to get data from (it represents a rotation/quaternion)
* \param yaw yaw
*/
template <class A>
double getYaw(const A& a)
{
tf3::Quaternion q = impl::toQuaternion(a);
return impl::getYaw(q);
}
/** Return the identity for any type that can be converted to a tf3::Transform
* \return an object of class A that is an identity transform
*/
template <class A>
A getTransformIdentity()
{
tf3::Transform t;
t.setIdentity();
A a;
convert(t, a);
return a;
}
}
#endif //TF3_UTILS_H

15
src/Libraries/tf3/index.rst Normal file
View File

@ -0,0 +1,15 @@
tf2
=====
This is the Python API reference of the tf2 package.
.. toctree::
:maxdepth: 2
tf2
Indices and tables
==================
* :ref:`genindex`
* :ref:`search`

36
src/Libraries/tf3/mainpage.dox Normal file
View File

@ -0,0 +1,36 @@
/**
\mainpage
\htmlinclude manifest.html
\b tf2 is the second generation of the tf library.
This library implements the interface defined by tf2::BufferCore.
There is also a Python wrapper with the same API that class this library using CPython bindings.
\section codeapi Code API
The main interface is through the tf2::BufferCore interface.
It uses the exceptions in exceptions.h and the Stamped datatype
in transform_datatypes.h.
\section conversions Conversion Interface
tf2 offers a templated conversion interface for external libraries to specify conversions between
tf2-specific data types and user-defined data types. Various templated functions in tf2_ros use the
conversion interface to apply transformations from the tf server to these custom datatypes.
The conversion interface is defined in tf2/convert.h.
Some packages that implement this interface:
- tf2_bullet
- tf2_eigen
- tf2_geometry_msgs
- tf2_kdl
- tf2_sensor_msgs
More documentation for the conversion interface is available on the <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">ROS Wiki</A>.
*/

26
src/Libraries/tf3/package.xml Normal file
View File

@ -0,0 +1,26 @@
<package>
<name>tf3</name>
<version>0.7.10</version>
<description>
tf3 is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. tf3
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
point in time.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/tf3</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
</package>

1645
src/Libraries/tf3/src/buffer_core.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

338
src/Libraries/tf3/src/cache.cpp Normal file
View File

@ -0,0 +1,338 @@
/*
* 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 */
#include "tf3/time_cache.h"
#include "tf3/exceptions.h"
#include <tf3/LinearMath/Vector3.h>
#include <tf3/LinearMath/Quaternion.h>
#include <tf3/LinearMath/Transform.h>
#include "tf3/compat.h"
#include <assert.h>
namespace tf3 {
TransformStorage::TransformStorage()
{
}
TransformStorage::TransformStorage(const tf3::TransformStampedMsg& data, CompactFrameID frame_id,
CompactFrameID child_frame_id)
: stamp_(data.header.stamp)
, frame_id_(frame_id)
, child_frame_id_(child_frame_id)
{
const tf3::QuaternionMsg& o = data.transform.rotation;
rotation_ = tf3::Quaternion(o.x, o.y, o.z, o.w);
const tf3::Vector3Msg& v = data.transform.translation;
translation_ = tf3::Vector3(v.x, v.y, v.z);
}
TimeCache::TimeCache(tf3::Duration max_storage_time)
: max_storage_time_(max_storage_time)
{}
namespace cache { // Avoid ODR collisions https://github.com/ros/geometry2/issues/175
// hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10%
void createExtrapolationException1(tf3::Time t0, tf3::Time t1, std::string* error_str)
{
if (error_str)
{
char str[117]; // Text without formatting strings has 77, each timestamp has up to 20
snprintf(str, sizeof(str), "Lookup would require extrapolation at time %.09f, but only time %.09f is in the buffer", t0.toSec(), t1.toSec());
*error_str = str;
}
}
void createExtrapolationException2(tf3::Time t0, tf3::Time t1, std::string* error_str)
{
if (error_str)
{
// Want this to come out positive, because this is a future extrapolation problem with t0
// t0 needs to come first because it will be bigger than t1
tf3::Duration tdiff = t0 - t1;
char str[163]; // Text without formatting strings has 102, each timestamp has up to 20
snprintf(
str, sizeof(str),
"Lookup would require extrapolation %.09fs into the future. Requested time %.09f but the latest data is at time %.09f",
tdiff.toSec(), t0.toSec(), t1.toSec());
*error_str = str;
}
}
void createExtrapolationException3(tf3::Time t0, tf3::Time t1, std::string* error_str)
{
if (error_str)
{
tf3::Duration tdiff = t1 - t0;
char str[163]; // Text without formatting strings has 102, each timestamp has up to 20
snprintf(
str, sizeof(str),
"Lookup would require extrapolation %.09fs into the past. Requested time %.09f but the earliest data is at time %.09f",
tdiff.toSec(), t0.toSec(), t1.toSec());
*error_str = str;
}
}
} // namespace cache
bool operator>(const TransformStorage& lhs, const TransformStorage& rhs)
{
return lhs.stamp_ > rhs.stamp_;
}
uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, tf3::Time target_time, std::string* error_err)
{
//No values stored
if (storage_.empty())
{
return 0;
}
//If time == 0 return the latest
if (target_time.isZero())
{
one = &storage_.front();
return 1;
}
// One value stored
if (++storage_.begin() == storage_.end())
{
TransformStorage& ts = *storage_.begin();
if (ts.stamp_ == target_time)
{
one = &ts;
return 1;
}
else
{
cache::createExtrapolationException1(target_time, ts.stamp_, error_err);
return 0;
}
}
tf3::Time latest_time = (*storage_.begin()).stamp_;
tf3::Time earliest_time = (*(storage_.rbegin())).stamp_;
if (target_time == latest_time)
{
one = &(*storage_.begin());
return 1;
}
else if (target_time == earliest_time)
{
one = &(*storage_.rbegin());
return 1;
}
// Catch cases that would require extrapolation
else if (target_time > latest_time)
{
cache::createExtrapolationException2(target_time, latest_time, error_err);
return 0;
}
else if (target_time < earliest_time)
{
cache::createExtrapolationException3(target_time, earliest_time, error_err);
return 0;
}
//At least 2 values stored
//Find the first value less than the target value
L_TransformStorage::iterator storage_it;
TransformStorage storage_target_time;
storage_target_time.stamp_ = target_time;
storage_it = std::lower_bound(
storage_.begin(),
storage_.end(),
storage_target_time, std::greater<TransformStorage>());
//Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
one = &*(storage_it); //Older
two = &*(--storage_it); //Newer
return 2;
}
void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, tf3::Time time, TransformStorage& output)
{
// Check for zero distance case
if( two.stamp_ == one.stamp_ )
{
output = two;
return;
}
//Calculate the ratio
tf3Scalar ratio = (time - one.stamp_).toSec() / (two.stamp_ - one.stamp_).toSec();
//Interpolate translation
output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
//Interpolate rotation
output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
output.stamp_ = time;
output.frame_id_ = one.frame_id_;
output.child_frame_id_ = one.child_frame_id_;
}
bool TimeCache::getData(tf3::Time time, TransformStorage & data_out, std::string* error_err) //returns false if data not available
{
TransformStorage* p_temp_1;
TransformStorage* p_temp_2;
int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_err);
if (num_nodes == 0)
{
return false;
}
else if (num_nodes == 1)
{
data_out = *p_temp_1;
}
else if (num_nodes == 2)
{
if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
{
interpolate(*p_temp_1, *p_temp_2, time, data_out);
}
else
{
data_out = *p_temp_1;
}
}
else
{
assert(0);
}
return true;
}
CompactFrameID TimeCache::getParent(tf3::Time time, std::string* error_err)
{
TransformStorage* p_temp_1;
TransformStorage* p_temp_2;
int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_err);
if (num_nodes == 0)
{
return 0;
}
return p_temp_1->frame_id_;
}
bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_str)
{
L_TransformStorage::iterator storage_it = storage_.begin();
if(storage_it != storage_.end())
{
if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_)
{
if (error_str)
{
*error_str = "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)";
}
return false;
}
}
while(storage_it != storage_.end())
{
if (storage_it->stamp_ <= new_data.stamp_)
break;
storage_it++;
}
if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_)
{
if (error_str)
{
*error_str = "TF_REPEATED_DATA ignoring data with redundant timestamp";
}
return false;
}
else
{
storage_.insert(storage_it, new_data);
}
pruneList();
return true;
}
void TimeCache::clearList()
{
storage_.clear();
}
unsigned int TimeCache::getListLength()
{
return storage_.size();
}
P_TimeAndFrameID TimeCache::getLatestTimeAndParent()
{
if (storage_.empty())
{
return std::make_pair(tf3::Time(), 0);
}
const TransformStorage& ts = storage_.front();
return std::make_pair(ts.stamp_, ts.frame_id_);
}
tf3::Time TimeCache::getLatestTimestamp()
{
if (storage_.empty()) return tf3::Time(); //empty list case
return storage_.front().stamp_;
}
tf3::Time TimeCache::getOldestTimestamp()
{
if (storage_.empty()) return tf3::Time(); //empty list case
return storage_.back().stamp_;
}
void TimeCache::pruneList()
{
tf3::Time latest_time = storage_.begin()->stamp_;
while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time)
{
storage_.pop_back();
}
} // namespace tf3
}

81
src/Libraries/tf3/src/static_cache.cpp Normal file
View File

@ -0,0 +1,81 @@
/*
* 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 */
#include "tf3/time_cache.h"
#include "tf3/exceptions.h"
#include "tf3/LinearMath/Transform.h"
#include "tf3/compat.h"
using namespace tf3;
bool StaticCache::getData(tf3::Time time, TransformStorage & data_out, std::string* error_err) //returns false if data not available
{
data_out = storage_;
data_out.stamp_ = time;
return true;
};
bool StaticCache::insertData(const TransformStorage& new_data, std::string* error_str)
{
storage_ = new_data;
return true;
};
void StaticCache::clearList() { return; };
unsigned int StaticCache::getListLength() { return 1; };
CompactFrameID StaticCache::getParent(tf3::Time time, std::string* error_err)
{
return storage_.frame_id_;
}
P_TimeAndFrameID StaticCache::getLatestTimeAndParent()
{
return std::make_pair(tf3::Time(), storage_.frame_id_);
}
tf3::Time StaticCache::getLatestTimestamp()
{
return tf3::Time();
};
tf3::Time StaticCache::getOldestTimestamp()
{
return tf3::Time();
};

414
src/Libraries/tf3/test/cache_unittest.cpp Normal file
View File

@ -0,0 +1,414 @@
/*
* 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 <gtest/gtest.h>
#include <tf3/time_cache.h>
#include "tf3/LinearMath/Quaternion.h"
#include <stdexcept>
#include <geometry_msgs/TransformStamped.h>
#include <cmath>
std::vector<double> values;
unsigned int step = 0;
void seed_rand()
{
values.clear();
for (unsigned int i = 0; i < 1000; i++)
{
int pseudo_rand = std::floor(i * M_PI);
values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
//printf("Seeding with %f\n", values.back());
}
};
double get_rand()
{
if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first");
if (step >= values.size())
step = 0;
else
step++;
return values[step];
}
using namespace tf3;
void setIdentity(TransformStorage& stor)
{
stor.translation_.setValue(0.0, 0.0, 0.0);
stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
}
TEST(TimeCache, Repeatability)
{
unsigned int runs = 100;
tf3::TimeCache cache;
TransformStorage stor;
setIdentity(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
cache.insertData(stor);
}
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
}
}
TEST(TimeCache, RepeatabilityReverseInsertOrder)
{
unsigned int runs = 100;
tf3::TimeCache cache;
TransformStorage stor;
setIdentity(stor);
for ( int i = runs -1; i >= 0 ; i-- )
{
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
cache.insertData(stor);
}
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
}
}
#if 0 // jfaust: this doesn't seem to actually be testing random insertion?
TEST(TimeCache, RepeatabilityRandomInsertOrder)
{
seed_rand();
tf3::TimeCache cache;
double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double));
unsigned int runs = values.size();
TransformStorage stor;
setIdentity(stor);
for ( uint64_t i = 0; i <runs ; i++ )
{
values[i] = 10.0 * get_rand();
std::stringstream ss;
ss << values[i];
stor.header.frame_id = ss.str();
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
cache.insertData(stor);
}
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
std::stringstream ss;
ss << values[i];
EXPECT_EQ(stor.header.frame_id, ss.str());
}
}
#endif
TEST(TimeCache, ZeroAtFront)
{
uint64_t runs = 100;
tf3::TimeCache cache;
TransformStorage stor;
setIdentity(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
cache.insertData(stor);
}
stor.frame_id_ = runs;
stor.stamp_ = ros::Time().fromNSec(runs);
cache.insertData(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
}
cache.getData(ros::Time(), stor);
EXPECT_EQ(stor.frame_id_, runs);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
stor.frame_id_ = runs;
stor.stamp_ = ros::Time().fromNSec(runs+1);
cache.insertData(stor);
//Make sure we get a different value now that a new values is added at the front
cache.getData(ros::Time(), stor);
EXPECT_EQ(stor.frame_id_, runs);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
}
TEST(TimeCache, CartesianInterpolation)
{
uint64_t runs = 100;
double epsilon = 2e-6;
seed_rand();
tf3::TimeCache cache;
std::vector<double> xvalues(2);
std::vector<double> yvalues(2);
std::vector<double> zvalues(2);
uint64_t offset = 200;
TransformStorage stor;
setIdentity(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
for (uint64_t step = 0; step < 2 ; step++)
{
xvalues[step] = 10.0 * get_rand();
yvalues[step] = 10.0 * get_rand();
zvalues[step] = 10.0 * get_rand();
stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
stor.frame_id_ = 2;
stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
cache.insertData(stor);
}
for (int pos = 0; pos < 100 ; pos ++)
{
uint64_t time = offset + pos;
cache.getData(ros::Time().fromNSec(time), stor);
uint64_t time_out = stor.stamp_.toNSec();
double x_out = stor.translation_.x();
double y_out = stor.translation_.y();
double z_out = stor.translation_.z();
// printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out,
// xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.,
// yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0,
// zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0);
EXPECT_EQ(time, time_out);
EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
}
cache.clearList();
}
}
/** \brief Make sure we dont' interpolate across reparented data */
TEST(TimeCache, ReparentingInterpolationProtection)
{
double epsilon = 1e-6;
uint64_t offset = 555;
seed_rand();
tf3::TimeCache cache;
std::vector<double> xvalues(2);
std::vector<double> yvalues(2);
std::vector<double> zvalues(2);
TransformStorage stor;
setIdentity(stor);
for (uint64_t step = 0; step < 2 ; step++)
{
xvalues[step] = 10.0 * get_rand();
yvalues[step] = 10.0 * get_rand();
zvalues[step] = 10.0 * get_rand();
stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
stor.frame_id_ = step + 4;
stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
cache.insertData(stor);
}
for (int pos = 0; pos < 100 ; pos ++)
{
EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
double x_out = stor.translation_.x();
double y_out = stor.translation_.y();
double z_out = stor.translation_.z();
EXPECT_NEAR(xvalues[0], x_out, epsilon);
EXPECT_NEAR(yvalues[0], y_out, epsilon);
EXPECT_NEAR(zvalues[0], z_out, epsilon);
}
}
TEST(Bullet, Slerp)
{
uint64_t runs = 100;
seed_rand();
tf3::Quaternion q1, q2;
q1.setEuler(0,0,0);
for (uint64_t i = 0 ; i < runs ; i++)
{
q2.setEuler(1.0 * get_rand(),
1.0 * get_rand(),
1.0 * get_rand());
tf3::Quaternion q3 = slerp(q1,q2,0.5);
EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
}
}
TEST(TimeCache, AngularInterpolation)
{
uint64_t runs = 100;
double epsilon = 1e-6;
seed_rand();
tf3::TimeCache cache;
std::vector<double> yawvalues(2);
std::vector<double> pitchvalues(2);
std::vector<double> rollvalues(2);
uint64_t offset = 200;
std::vector<tf3::Quaternion> quats(2);
TransformStorage stor;
setIdentity(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
for (uint64_t step = 0; step < 2 ; step++)
{
yawvalues[step] = 10.0 * get_rand() / 100.0;
pitchvalues[step] = 0;//10.0 * get_rand();
rollvalues[step] = 0;//10.0 * get_rand();
quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
stor.rotation_ = quats[step];
stor.frame_id_ = 3;
stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
cache.insertData(stor);
}
for (int pos = 0; pos < 100 ; pos ++)
{
uint64_t time = offset + pos;
cache.getData(ros::Time().fromNSec(time), stor);
uint64_t time_out = stor.stamp_.toNSec();
tf3::Quaternion quat (stor.rotation_);
//Generate a ground truth quaternion directly calling slerp
tf3::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
//Make sure the transformed one and the direct call match
EXPECT_EQ(time, time_out);
EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
}
cache.clearList();
}
}
TEST(TimeCache, DuplicateEntries)
{
TimeCache cache;
TransformStorage stor;
setIdentity(stor);
stor.frame_id_ = 3;
stor.stamp_ = ros::Time().fromNSec(1);
cache.insertData(stor);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(1), stor);
//printf(" stor is %f\n", stor.translation_.x());
EXPECT_TRUE(!std::isnan(stor.translation_.x()));
EXPECT_TRUE(!std::isnan(stor.translation_.y()));
EXPECT_TRUE(!std::isnan(stor.translation_.z()));
EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

320
src/Libraries/tf3/test/simple_tf3_core.cpp Normal file
View File

@ -0,0 +1,320 @@
/*
* 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 <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <ros/time.h>
#include "tf3/LinearMath/Vector3.h"
#include "tf3/exceptions.h"
TEST(tf3, setTransformFail)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
EXPECT_FALSE(tfc.setTransform(st, "authority1"));
}
TEST(tf3, setTransformValid)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 1;
EXPECT_TRUE(tfc.setTransform(st, "authority1"));
}
TEST(tf3, setTransformInvalidQuaternion)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 0;
EXPECT_FALSE(tfc.setTransform(st, "authority1"));
}
TEST(tf3_lookupTransform, LookupException_Nothing_Exists)
{
tf3::BufferCore tfc;
EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf3::LookupException);
}
TEST(tf3_canTransform, Nothing_Exists)
{
tf3::BufferCore tfc;
EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0)));
std::string error_msg = std::string();
EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg));
ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist.");
}
TEST(tf3_lookupTransform, LookupException_One_Exists)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 1;
EXPECT_TRUE(tfc.setTransform(st, "authority1"));
EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf3::LookupException);
}
TEST(tf3_canTransform, One_Exists)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 1;
EXPECT_TRUE(tfc.setTransform(st, "authority1"));
EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0)));
}
TEST(tf3_chainAsVector, chain_v_configuration)
{
tf3::BufferCore mBC;
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
st.header.stamp = ros::Time(0);
st.transform.rotation.w = 1;
st.header.frame_id = "a";
st.child_frame_id = "b";
mBC.setTransform(st, "authority1");
st.header.frame_id = "b";
st.child_frame_id = "c";
mBC.setTransform(st, "authority1");
st.header.frame_id = "a";
st.child_frame_id = "d";
mBC.setTransform(st, "authority1");
st.header.frame_id = "d";
st.child_frame_id = "e";
mBC.setTransform(st, "authority1");
std::vector<std::string> chain;
mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain);
EXPECT_EQ( 0, chain.size());
mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain);
EXPECT_EQ( 5, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain);
EXPECT_EQ( 5, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain);
EXPECT_EQ( 5, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
}
TEST(tf3_walkToTopParent, walk_i_configuration)
{
tf3::BufferCore mBC;
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
st.header.stamp = ros::Time(0);
st.transform.rotation.w = 1;
st.header.frame_id = "a";
st.child_frame_id = "b";
mBC.setTransform(st, "authority1");
st.header.frame_id = "b";
st.child_frame_id = "c";
mBC.setTransform(st, "authority1");
st.header.frame_id = "c";
st.child_frame_id = "d";
mBC.setTransform(st, "authority1");
st.header.frame_id = "d";
st.child_frame_id = "e";
mBC.setTransform(st, "authority1");
std::vector<tf3::CompactFrameID> id_chain;
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size() );
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4]));
id_chain.clear();
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size() );
if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
}
TEST(tf3_walkToTopParent, walk_v_configuration)
{
tf3::BufferCore mBC;
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
st.header.stamp = ros::Time(0);
st.transform.rotation.w = 1;
// st.header.frame_id = "aaa";
// st.child_frame_id = "aa";
// mBC.setTransform(st, "authority1");
//
// st.header.frame_id = "aa";
// st.child_frame_id = "a";
// mBC.setTransform(st, "authority1");
st.header.frame_id = "a";
st.child_frame_id = "b";
mBC.setTransform(st, "authority1");
st.header.frame_id = "b";
st.child_frame_id = "c";
mBC.setTransform(st, "authority1");
st.header.frame_id = "a";
st.child_frame_id = "d";
mBC.setTransform(st, "authority1");
st.header.frame_id = "d";
st.child_frame_id = "e";
mBC.setTransform(st, "authority1");
std::vector<tf3::CompactFrameID> id_chain;
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size());
if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size());
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 3 );
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 3 );
if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 2 );
if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 2 );
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::Time::init(); //needed for ros::TIme::now()
return RUN_ALL_TESTS();
}

252
src/Libraries/tf3/test/speed_test.cpp Normal file
View File

@ -0,0 +1,252 @@
/*
* Copyright (c) 2010, 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 <tf3/buffer_core.h>
#include <ros/time.h>
#include <console_bridge/console.h>
#include <boost/lexical_cast.hpp>
int main(int argc, char** argv)
{
uint32_t num_levels = 10;
if (argc > 1)
{
num_levels = boost::lexical_cast<uint32_t>(argv[1]);
}
double time_interval = 1.0;
if (argc > 2)
{
time_interval = boost::lexical_cast<double>(argv[2]);
}
console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
tf3::BufferCore bc;
geometry_msgs::TransformStamped t;
t.header.stamp = ros::Time(1);
t.header.frame_id = "root";
t.child_frame_id = "0";
t.transform.translation.x = 1;
t.transform.rotation.w = 1.0;
bc.setTransform(t, "me");
t.header.stamp = ros::Time(2);
bc.setTransform(t, "me");
for (uint32_t i = 1; i < num_levels / 2; ++i)
{
for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
{
std::stringstream parent_ss;
parent_ss << (i - 1);
std::stringstream child_ss;
child_ss << i;
t.header.stamp = ros::Time(j);
t.header.frame_id = parent_ss.str();
t.child_frame_id = child_ss.str();
bc.setTransform(t, "me");
}
}
t.header.frame_id = "root";
std::stringstream ss;
ss << num_levels/2;
t.header.stamp = ros::Time(1);
t.child_frame_id = ss.str();
bc.setTransform(t, "me");
t.header.stamp = ros::Time(2);
bc.setTransform(t, "me");
for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
{
for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
{
std::stringstream parent_ss;
parent_ss << (i - 1);
std::stringstream child_ss;
child_ss << i;
t.header.stamp = ros::Time(j);
t.header.frame_id = parent_ss.str();
t.child_frame_id = child_ss.str();
bc.setTransform(t, "me");
}
}
//logInfo_STREAM(bc.allFramesAsYAML());
std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str());
geometry_msgs::TransformStamped out_t;
const uint32_t count = 1000000;
CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(0));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(1));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(2));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(3));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
std::string str;
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(3), &str);
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
}

101
src/Libraries/tf3/test/static_cache_test.cpp Normal file
View File

@ -0,0 +1,101 @@
/*
* 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 <gtest/gtest.h>
#include <tf3/time_cache.h>
#include <stdexcept>
#include <geometry_msgs/TransformStamped.h>
#include <cmath>
using namespace tf3;
void setIdentity(TransformStorage& stor)
{
stor.translation_.setValue(0.0, 0.0, 0.0);
stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
}
TEST(StaticCache, Repeatability)
{
unsigned int runs = 100;
tf3::StaticCache cache;
TransformStorage stor;
setIdentity(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
stor.frame_id_ = CompactFrameID(i);
stor.stamp_ = ros::Time().fromNSec(i);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
}
}
TEST(StaticCache, DuplicateEntries)
{
tf3::StaticCache cache;
TransformStorage stor;
setIdentity(stor);
stor.frame_id_ = CompactFrameID(3);
stor.stamp_ = ros::Time().fromNSec(1);
cache.insertData(stor);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(1), stor);
//printf(" stor is %f\n", stor.transform.translation.x);
EXPECT_TRUE(!std::isnan(stor.translation_.x()));
EXPECT_TRUE(!std::isnan(stor.translation_.y()));
EXPECT_TRUE(!std::isnan(stor.translation_.z()));
EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

77
src/Libraries/tf3/test/test_transform_datatypes.cpp Normal file
View File

@ -0,0 +1,77 @@
/*
* Copyright (c) 2020, Open Source Robotics Foundation, 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 <gtest/gtest.h>
#include "tf3/transform_datatypes.h"
#include <string>
TEST(Stamped, assignment)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> second("baz", ros::Time(0), "my_frame_id");
EXPECT_NE(second, first);
second = first;
EXPECT_EQ(second, first);
}
TEST(Stamped, setData)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> second("baz", ros::Time(0), "my_frame_id");
EXPECT_NE(second, first);
second.setData("foobar");
EXPECT_EQ(second, first);
}
TEST(Stamped, copy_constructor)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> second(first);
EXPECT_EQ(second, first);
}
TEST(Stamped, default_constructor)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> second;
EXPECT_NE(second, first);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::Time::init();
return RUN_ALL_TESTS();
}

1
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@ -89,7 +89,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
private_nh_.getParam("global_frame", global_frame, std::string("map")); private_nh_.getParam("global_frame", global_frame, std::string("map"));
robot_base_frame_ = robot_base_frame; robot_base_frame_ = robot_base_frame;
global_frame_ = global_frame; global_frame_ = global_frame;
printf("[%s:%d] robot_base_frame: %s, global_frame: %s\n", __FILE__, __LINE__, robot_base_frame_.c_str(), global_frame_.c_str());
double planner_frequency; double planner_frequency;
private_nh_.getParam("planner_frequency", planner_frequency, 0.0); private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
planner_frequency_ = planner_frequency; planner_frequency_ = planner_frequency;