diff --git a/.gitmodules b/.gitmodules index 609c208..06447a1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,6 +22,4 @@ [submodule "src/Libraries/data_convert"] path = src/Libraries/data_convert url = https://git.pnkr.asia/DuongTD/data_convert.git -[submodule "src/Libraries/tf3"] - path = src/Libraries/tf3 - url = https://github.com/Huandaotien/tf3.git + diff --git a/src/Libraries/tf3/.gitignore b/src/Libraries/tf3/.gitignore new file mode 100644 index 0000000..b098b7a --- /dev/null +++ b/src/Libraries/tf3/.gitignore @@ -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 diff --git a/src/Libraries/tf3/CHANGELOG.rst b/src/Libraries/tf3/CHANGELOG.rst new file mode 100644 index 0000000..8db46e0 --- /dev/null +++ b/src/Libraries/tf3/CHANGELOG.rst @@ -0,0 +1,524 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2 +^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.7.10 (2025-05-02) +------------------- +* Fix race conditions in MessageFilter (`#539 `_) +* Contributors: Robert Haschke + +0.7.9 (2025-04-25) +------------------ + +0.7.8 (2025-04-10) +------------------ +* Longer char array for null termination needed (`#514 `_) +* Fixed error message when fixed_frame is not found (`#559 `_) +* Add missing #include to buffer_core.cpp (`#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 `_) +* Add parent frame to warning logs (`#533 `_) +* Contributors: Jack Zender, Stephan + +0.7.6 (2022-10-11) +------------------ +* Fix dead loop in message filter (`#532 `_) +* Restore time difference order so future extrapolation exceptions don't show non-sensical negative seconds into the future (`#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 `_) +* 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 `_) +* Output time difference of extrapolation exceptions (`#477 `_) +* Cherry-picking various commits from Melodic (`#471 `_) + * Revert "rework Eigen functions namespace hack" (`#436 `_) + * Fixed warnings in message_filter.h (`#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 `_) + * 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::operator=() to fix warnings downstream (`#453 `_) + * Add tf2::Stamped::operator=() +* [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) + * [Windows][melodic-devel] Fix install locations (`#442 `_) + * fixed install locations of tf2 + * [windows][melodic] more portable fixes. (`#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 `_) + Signed-off-by: Shane Loretz +* Fix compile error missing ros/ros.h (`#400 `_) + * ros/ros.h -> ros/time.h + tf2 package depends on rostime + * tf2_bullet doesn't need ros.h + Signed-off-by: Shane Loretz + * tf2_eigen doesn't need ros/ros.h + Signed-off-by: Shane Loretz +* Merge pull request `#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 `_) + * use ROS_DEPRECATED for better portability + * change ROS_DEPRECATED position (`#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 `_ +* fixed nan interpoaltion issue +* Contributors: Keller Fabian Rudolf (CC-AD/EYC3), Kuang Fangjun, Martin Ganeff + +0.6.3 (2018-07-09) +------------------ +* preserve constness of const argument to avoid warnings (`#307 `_) +* Change comment style for unused doxygen (`#297 `_) +* Contributors: Jacob Perron, Tully Foote + +0.6.2 (2018-05-02) +------------------ + +0.6.1 (2018-03-21) +------------------ +* Replaced deprecated console_bridge macro calls (tests) +* Contributors: Johannes Meyer, Tully Foote + +0.6.0 (2018-03-21) +------------------ +* Replaced deprecated log macro calls +* Contributors: Tim Rakowski, Tully Foote + +0.5.17 (2018-01-01) +------------------- +* Merge pull request `#278 `_ from ros/chain_as_vec_test2 + Clean up results of _chainAsVector +* Simple test to check BufferCore::_chainAsVector. + Unit tests for walk and chain passing now. +* Merge pull request `#267 `_ from at-wat/speedup-timecache-for-large-buffer + Speed-up TimeCache search for large cache time. +* Merge pull request `#265 `_ from vsherrod/interpolation_fix + Corrected time output on interpolation function. +* Add time_interval option to tf2 speed-test. +* Merge pull request `#269 `_ from ros/frames_as_yaml + allFrameAsYaml consistently outputting a dict +* resolve https://github.com/ros/geometry/pull/153 at the source instead of needing the workaround. +* Speed-up TimeCache search for large cache time. +* Modified tests for correct time in interpolation to existing tests. +* Corrected time output on interpolation function. + Added unit test to check for this. +* Contributors: Atsushi Watanabe, Miguel Prada, Tully Foote, Vallan Sherrod + +0.5.16 (2017-07-14) +------------------- +* remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation. +* Merge pull request `#144 `_ from clearpathrobotics/dead_lock_fix + Solve a bug that causes a deadlock in MessageFilter +* Resolve 2 places where the error_msg would not be propogated. + Fixes `#198 `_ +* Remove generate_rand_vectors() from a number of tests. (`#227 `_) +* fixing include directory order to support overlays (`#231 `_) +* replaced dependencies on tf2_msgs_gencpp by exported dependencies +* Document the lifetime of the returned reference for getFrameId getTimestamp +* relax normalization tolerance. `#196 `_ was too strict for some use cases. (`#220 `_) +* Solve a bug that causes a deadlock in MessageFilter +* Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Tully Foote, dhood + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* fixes `#194 `_ check for quaternion normalization before inserting into storage (`#196 `_) + * check for quaternion normalization before inserting into storage + * Add test to check for transform failure on invalid quaternion input +* updating getAngleShortestPath() (`#187 `_) +* Move internal cache functions into a namespace + Fixes https://github.com/ros/geometry2/issues/175 +* Link properly to convert.h +* Landing page for tf2 describing the conversion interface +* Fix comment on BufferCore::MAX_GRAPH_DEPTH. +* Contributors: Jackie Kay, Phil Osteen, Tully Foote, alex, gavanderhoorn + +0.5.13 (2016-03-04) +------------------- + +0.5.12 (2015-08-05) +------------------- +* add utilities to get yaw, pitch, roll and identity transform +* provide more conversions between types + The previous conversion always assumed that it was converting a + non-message type to a non-message type. Now, one, both or none + can be a message or a non-message. +* Contributors: Vincent Rabaud + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- +* move lct_cache into function local memoryfor `#92 `_ +* Clean up range checking. Re: `#92 `_ +* Fixed chainToVector +* release lock before possibly invoking user callbacks. Fixes `#91 `_ +* Contributors: Jackie Kay, Tully Foote + +0.5.9 (2015-03-25) +------------------ +* fixing edge case where two no frame id lookups matched in getLatestCommonTime +* Contributors: Tully Foote + +0.5.8 (2015-03-17) +------------------ +* change from default argument to overload to avoid linking issue `#84 `_ +* remove useless Makefile files +* Remove unused assignments in max/min functions +* change _allFramesAsDot() -> _allFramesAsDot(double current_time) +* Contributors: Jon Binney, Kei Okada, Tully Foote, Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ +* convert to use console bridge from upstream debian package https://github.com/ros/rosdistro/issues/4633 +* Fix format string +* Contributors: Austin, Tully Foote + +0.5.4 (2014-05-07) +------------------ +* switch to boost signals2 following `ros/ros_comm#267 `_, blocking `ros/geometry#23 `_ +* Contributors: Tully Foote + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- +* updated error message. fixes `#38 `_ +* tf2: add missing console bridge include directories (fix `#48 `_) +* Fix const correctness of tf2::Vector3 rotate() method + The method does not modify the class thus should be const. + This has already been fixed in Bullet itself. +* Contributors: Dirk Thomas, Timo Rohling, Tully Foote + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ +* moving python documentation to tf2_ros from tf2 to follow the code +* removing legacy rospy dependency. implementation removed in 0.4.0 fixes `#27 `_ + +0.4.7 (2013-08-28) +------------------ +* switching to use allFramesAsStringNoLock inside of getLatestCommonTime and walkToParent and locking in public API _getLatestCommonTime instead re `#23 `_ +* Fixes a crash in tf's view_frames related to dot code generation in allFramesAsDot + +0.4.6 (2013-08-28) +------------------ +* cleaner fix for `#19 `_ +* fix pointer initialization. Fixes `#19 `_ +* fixes `#18 `_ for hydro +* package.xml: corrected typo in description + +0.4.5 (2013-07-11) +------------------ +* adding _chainAsVector method for https://github.com/ros/geometry/issues/18 +* adding _allFramesAsDot for backwards compatability https://github.com/ros/geometry/issues/18 + +0.4.4 (2013-07-09) +------------------ +* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. +* tf2: Fixes a warning on OS X, but generally safer + Replaces the use of pointers with shared_ptrs, + this allows the polymorphism and makes it so that + the compiler doesn't yell at us about calling + delete on a class with a public non-virtual + destructor. +* tf2: Fixes compiler warnings on OS X + This exploited a gcc specific extension and is not + C++ standard compliant. There used to be a "fix" + for OS X which no longer applies. I think it is ok + to use this as an int instead of a double, but + another way to fix it would be to use a define. +* tf2: Fixes linkedit errors on OS X + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ +* adding getCacheLength() to parallel old tf API +* removing legacy static const variable MAX_EXTRAPOLATION_DISTANCE copied from tf unnecessesarily + +0.4.1 (2013-07-05) +------------------ +* adding old style callback notifications to BufferCore to enable backwards compatability of message filters +* exposing dedicated thread logic in BufferCore and checking in Buffer +* more methods to expose, and check for empty cache before getting latest timestamp +* adding methods to enable backwards compatability for passing through to tf::Transformer + +0.4.0 (2013-06-27) +------------------ +* splitting rospy dependency into tf2_py so tf2 is pure c++ library. +* switching to console_bridge from rosconsole +* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 +* Cleaning up unnecessary dependency on roscpp +* Cleaning up packaging of tf2 including: + removing unused nodehandle + fixing overmatch on search and replace + cleaning up a few dependencies and linking + removing old backup of package.xml + making diff minimally different from tf version of library +* suppressing bullet LinearMath copy inside of tf2, so it will not collide, and should not be used externally. +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ +* fixing includes in unit tests +* Make PythonLibs find_package python2 specific + On systems with python 3 installed and default, find_package(PythonLibs) will find the python 3 paths and libraries. However, the c++ include structure seems to be different in python 3 and tf2 uses includes that are no longer present or deprecated. + Until the includes are made to be python 3 compliant, we should specify that the version of python found must be python 2. + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 +* moving LinearMath includes to include/tf2 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 +* fixing include installation of tf2 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 +* fixed missing include export & tf2_ros dependecy + +0.3.1 (2013-02-14) +------------------ +* 0.3.0 -> 0.3.1 +* fixing PYTHON installation directory + +0.3.0 (2013-02-13) +------------------ +* switching to version 0.3.0 +* adding setup.py to tf2 package +* fixed tf2 exposing python functionality +* removed line that was killing tf2_ros.so +* fixing catkin message dependencies +* removing packages with missing deps +* adding missing package.xml +* adding missing package.xml +* adding missing package.xml +* catkinizing geometry-experimental +* removing bullet headers from use in header files +* removing bullet headers from use in header files +* merging my recent changes +* setting child_frame_id overlooked in revision 6a0eec022be0 which fixed failing tests +* allFramesAsString public and internal methods seperated. Public method is locked, private method is not +* fixing another scoped lock +* fixing one scoped lock +* fixing test compilation +* merge +* Error message fix, ros-pkg5085 +* Check if target equals to source before validation +* When target_frame == source_frame, just returns an identity transform. +* adding addition ros header includes for strictness +* Fixed optimized lookups with compound transforms +* Fixed problem in tf2 optimized branch. Quaternion multiplication order was incorrect +* fix compilation on 32-bit +* Josh fix: Final inverse transform composition (missed multiplying the sourcd->top vector by the target->top inverse orientation). b44877d2b054 +* Josh change: fix first/last time case. 46bf33868e0d +* fix transform accumulation to parent +* fix parent lookup, now works on the real pr2's tree +* move the message filter to tf2_ros +* tf2::MessageFilter + tests. Still need to change it around to pass in a callback queue, since we're being triggered directly from the tf2 buffer +* Don't add the request if the transform is already available. Add some new tests +* working transformable callbacks with a simple (incomplete) test case +* first pass at a transformable callback api, not tested yet +* add interpolation cases +* fix getLatestCommonTime -- no longer returns the latest of any of the times +* Some more optimization -- allow findClosest to inline +* another minor speedup +* Minorly speed up canTransform by not requiring the full data lookup, and only looking up the parent +* Add explicit operator= so that we can see the time in it on a profile graph. Also some minor cleanup +* minor cleanup +* add 3 more cases to the speed test +* Remove use of btTransform at all from transform accumulation, since the conversion to/from is unnecessary, expensive, and can introduce floating point error +* Don't use btTransform as an intermediate when accumulating transforms, as constructing them takes quite a bit of time +* Completely remove lookupLists(). canTransform() now uses the same walking code as lookupTransform(). Also fixed a bug in the static transform publisher test +* Genericise the walk-to-top-parent code in lookupTransform so that it will be able to be used by canTransform as well (minus the cost of actually computing the transform) +* remove id lookup that wasn't doing anything +* Some more optimization: + * Reduce # of TransformStorage copies made in TimeCache::getData() + * Remove use of lookupLists from getLatestCommonTime +* lookupTransform() no longer uses lookupLists unless it's called with Time(0). Removes lots of object construction/destruction due to removal of pushing back on the lists +* Remove CompactFrameID in favor of a typedef +* these mode checks are no longer necessary +* Fix crash when testing extrapolation on the forward transforms +* Update cache unit tests to work with the changes TransformStorage. + Also make sure that BT_USE_DOUBLE_PRECISION is set for tf2. +* remove exposure of time_cache.h from buffer_core.h +* Removed the mutex from TimeCache, as it's unnecessary (BufferCore needs to have its own mutex locked anyway), and this speeds things up by about 20% + Also fixed a number of thread-safety problems +* Optimize test_extrapolation a bit, 25% speedup of lookupTransform +* use a hash map for looking up frame numbers, speeds up lookupTransform by ~8% +* Cache vectors used for looking up transforms. Speeds up lookupTransform by another 10% +* speed up lookupTransform by another 25% +* speed up lookupTransform by another 2x. also reduces the memory footprint of the cache significantly +* sped up lookupTransform by another 2x +* First add of a simple speed test + Sped up lookupTransform 2x +* roscpp dependency explicit, instead of relying on implicit +* static transform tested and working +* tests passing and all throw catches removed too\! +* validating frame_ids up front for lookup exceptions +* working with single base class vector +* tests passing for static storage +* making method private for clarity +* static cache implementation and test +* cleaning up API doc typos +* sphinx docs for Buffer +* new dox mainpage +* update tf2 manifest +* commenting out twist +* Changed cache_time to cache_time_ to follow C++ style guide, also initialized it to actually get things to work +* no more rand in cache tests +* Changing tf2_py.cpp to use underscores instead of camelCase +* removing all old converter functions from transform_datatypes.h +* removing last references to transform_datatypes.h in tf2 +* transform conversions internalized +* removing unused datatypes +* copying bullet transform headers into tf2 and breaking bullet dependency +* merge +* removing dependency on tf +* removing include of old tf from tf2 +* update doc +* merge +* kdl unittest passing +* Spaces instead of tabs in YAML grrrr +* Adding quotes for parent +* canTransform advanced ported +* Hopefully fixing YAML syntax +* new version of view_frames in new tf2_tools package +* testing new argument validation and catching bug +* Python support for debugging +* merge +* adding validation of frame_ids in queries with warnings and exceptions where appropriate +* Exposing ability to get frames as a string +* A compiling version of YAML debugging interface for BufferCore +* placeholder for tf debug +* fixing tf:: to tf2:: ns issues and stripping slashes on set in tf2 for backwards compatiabily +* Adding a python version of the BufferClient +* moving test to new package +* merging +* working unit test for BufferCore::lookupTransform +* removing unused method test and converting NO_PARENT test to new API +* Adding some comments +* Moving the python bindings for tf2 to the tf2 package from the tf2_py package +* buffercore tests upgraded +* porting tf_unittest while running incrmentally instead of block copy +* BufferCore::clear ported forward +* successfully changed lookupTransform advanced to new version +* switching to new implementation of lookupTransform tests still passing +* compiling lookupTransform new version +* removing tf_prefix from BufferCore. BuferCore is independent of any frame_ids. tf_prefix should be implemented at the ROS API level. +* initializing tf_prefix +* adding missing initialization +* suppressing warnings +* more tests ported +* removing tests for apis not ported forward +* setTransform tests ported +* old tests in new package passing due to backwards dependency. now for the fun, port all 1500 lines :-) +* setTransform working in new framework as well as old +* porting more methods +* more compatability +* bringing in helper functions for buffer_core from tf.h/cpp +* rethrowing to new exceptions +* converting Storage to geometry_msgs::TransformStamped +* removing deprecated useage +* cleaning up includes +* moving all implementations into cpp file +* switching test to new class from old one +* Compiling version of the buffer client +* moving listener to tf_cpp +* removing listener, it should be in another package +* most of listener +* add cantransform implementation +* removing deprecated API usage +* initial import of listener header +* move implementation into library +* 2 tests of buffer +* moving executables back into bin +* compiling again with new design +* rename tfcore to buffercore +* almost compiling version of template code +* compiling tf2_core simple test +* add test to start compiling +* copying in tf_unittest for tf_core testing template +* prototype of tf2_core implemented using old tf. +* first version of template functions +* remove timeouts +* properly naming tf2_core.h from tf_core.h +* working cache test with tf2 lib +* first unit test passing, not yet ported +* tf_core api +* tf2 v2 +* aborting port +* moving across time cache tf and datatypes headers +* copying exceptions from tf +* switching to tf2 from tf_core diff --git a/src/Libraries/tf3/CMakeLists.txt b/src/Libraries/tf3/CMakeLists.txt new file mode 100644 index 0000000..65cb09b --- /dev/null +++ b/src/Libraries/tf3/CMakeLists.txt @@ -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 + $ + $) + +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() diff --git a/src/Libraries/tf3/README.md b/src/Libraries/tf3/README.md new file mode 100644 index 0000000..2ca5db2 --- /dev/null +++ b/src/Libraries/tf3/README.md @@ -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 + ``` \ No newline at end of file diff --git a/src/Libraries/tf3/examples/simple_tf3_example.cpp b/src/Libraries/tf3/examples/simple_tf3_example.cpp new file mode 100644 index 0000000..a599d28 --- /dev/null +++ b/src/Libraries/tf3/examples/simple_tf3_example.cpp @@ -0,0 +1,91 @@ +#include +#include +#include + +#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; +} diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h b/src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h new file mode 100644 index 0000000..d8a3044 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/Matrix3x3.h @@ -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 + 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 + diff --git a/src/Libraries/tf3/include/tf3/LinearMath/MinMax.h b/src/Libraries/tf3/include/tf3/LinearMath/MinMax.h new file mode 100644 index 0000000..4eabd57 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/MinMax.h @@ -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 +TF3SIMD_FORCE_INLINE const T& tf3Min(const T& a, const T& b) +{ + return a < b ? a : b ; +} + +template +TF3SIMD_FORCE_INLINE const T& tf3Max(const T& a, const T& b) +{ + return a > b ? a : b; +} + +template +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 +TF3SIMD_FORCE_INLINE void tf3SetMin(T& a, const T& b) +{ + if (b < a) + { + a = b; + } +} + +template +TF3SIMD_FORCE_INLINE void tf3SetMax(T& a, const T& b) +{ + if (a < b) + { + a = b; + } +} + +template +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 diff --git a/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h b/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h new file mode 100644 index 0000000..871696e --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h @@ -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 +#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 diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h b/src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h new file mode 100644 index 0000000..44ec992 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/Quaternion.h @@ -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 + // explicit Quaternion(const tf3Scalar *v) : Tuple4(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 + + + + diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Scalar.h b/src/Libraries/tf3/include/tf3/LinearMath/Scalar.h new file mode 100644 index 0000000..c7a0f2b --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/Scalar.h @@ -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 +#include //size_t for MSVC 6.0 +#include +#include +#include + +#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 + #define TF3_HAVE_NATIVE_FSEL + #define tf3Fsel(a,b,c) __fsel((a),(b),(c)) + #else + + + #endif//_XBOX + + #endif //__MINGW32__ + + #include +#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 + #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 + #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 + #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 (xtf3Scalar(1)) x=tf3Scalar(1); return acos(x); } +TF3SIMD_FORCE_INLINE tf3Scalar tf3Asin(tf3Scalar x) { if (xtf3Scalar(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((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 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(((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 diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Transform.h b/src/Libraries/tf3/include/tf3/LinearMath/Transform.h new file mode 100644 index 0000000..ef676f3 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/Transform.h @@ -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 + + + + + + diff --git a/src/Libraries/tf3/include/tf3/LinearMath/Vector3.h b/src/Libraries/tf3/include/tf3/LinearMath/Vector3.h new file mode 100644 index 0000000..42416fc --- /dev/null +++ b/src/Libraries/tf3/include/tf3/LinearMath/Vector3.h @@ -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] 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 diff --git a/src/Libraries/tf3/include/tf3/TF3Error.h b/src/Libraries/tf3/include/tf3/TF3Error.h new file mode 100644 index 0000000..de12fbc --- /dev/null +++ b/src/Libraries/tf3/include/tf3/TF3Error.h @@ -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 diff --git a/src/Libraries/tf3/include/tf3/buffer_core.h b/src/Libraries/tf3/include/tf3/buffer_core.h new file mode 100644 index 0000000..2e6d8f3 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/buffer_core.h @@ -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 + +#include + +#include "tf3/compat.h" +#include "tf3/time.h" + + +//////////////////////////backwards startup for porting +//#include "tf/tf.h" + +#include +#include +#include +#include + +namespace tf3 +{ + +typedef std::pair P_TimeAndFrameID; +typedef uint32_t TransformableCallbackHandle; +typedef uint64_t TransformableRequestHandle; + +class TimeCacheInterface; +typedef boost::shared_ptr TimeCacheInterfacePtr; + +enum TransformableResult +{ + TransformAvailable, + TransformFailure, +}; + +/** \brief A Class which provides coordinate transforms between any two frames in a system. + * + * This class provides a simple interface to allow recording and lookup of + * relationships between arbitrary frames of the system. + * + * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. + * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. + * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. + * libTF is designed to take care of all the intermediate steps for you. + * + * Internal Representation + * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. + * Frames are designated using an std::string + * 0 is a frame without a parent (the top of a tree) + * The positions of frames over time must be pushed in. + * + * All function calls which pass frame ids can potentially throw the exception tf::LookupException + */ +class BufferCore +{ +public: + /************* Constants ***********************/ + static const int DEFAULT_CACHE_TIME = 10; //!< The default amount of time to cache data in seconds + static const uint32_t MAX_GRAPH_DEPTH = 1000UL; //!< Maximum graph search depth (deeper graphs will be assumed to have loops) + + /** Constructor + * \param interpolating Whether to interpolate, if this is false the closest value will be returned + * \param cache_time How long to keep a history of transforms in nanoseconds + * + */ + BufferCore(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 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 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& 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& output) const; + +private: + + /** \brief A way to see what frames have been cached + * Useful for debugging. Use this call internally. + */ + std::string allFramesAsStringNoLock() const; + + + /******************** Internal Storage ****************/ + + /** \brief The pointers to potential frames that the tree can be made of. + * The frames will be dynamically allocated at run time when set the first time. */ + typedef std::vector V_TimeCacheInterface; + V_TimeCacheInterface frames_; + + /** \brief A mutex to protect testing and allocating new frames on the above vector. */ + mutable boost::mutex frame_mutex_; + + /** \brief A map from string frame ids to CompactFrameID */ + typedef boost::unordered_map M_StringToCompactFrameID; + M_StringToCompactFrameID frameIDs_; + /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */ + std::vector frameIDs_reverse; + /** \brief A map to lookup the most recent authority for a given frame */ + std::map frame_authority_; + + + /// How long to cache transform history + tf3::Duration cache_time_; + + typedef boost::unordered_map M_TransformableCallback; + M_TransformableCallback transformable_callbacks_; + uint32_t transformable_callbacks_counter_; + boost::mutex transformable_callbacks_mutex_; + + struct TransformableRequest + { + 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 V_TransformableRequest; + V_TransformableRequest transformable_requests_; + boost::mutex transformable_requests_mutex_; + uint64_t transformable_requests_counter_; + + struct RemoveRequestByCallback; + struct RemoveRequestByID; + + // Backwards compatability for tf message_filter + typedef boost::signals2::signal TransformsChangedSignal; + /// Signal which is fired whenever new transform data has arrived, from the thread the data arrived in + TransformsChangedSignal _transforms_changed_; + + + /************************* Internal Functions ****************************/ + + /** \brief An accessor to get a frame, which will throw an exception if the frame is no there. + * \param frame_number The frameID of the desired Reference Frame + * + * This is an internal function which will get the pointer to the frame associated with the frame id + * Possible Exception: tf::LookupException + */ + TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const; + + TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static); + + + bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const; + CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const; + + /// String to number for frame lookup with dynamic allocation of new frames + CompactFrameID lookupFrameNumber(const std::string& frameid_str) const; + + /// String to number for frame lookup with dynamic allocation of new frames + CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str); + + ///Number to string frame lookup may throw LookupException if number invalid + const std::string& lookupFrameString(CompactFrameID frame_id_num) const; + + void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const; + + /**@brief Return the latest rostime which is common across the spanning set + * zero if fails to cross */ + int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, tf3::Time& time, std::string* error_string) const; + + template + 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 + int walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const; + + void testTransformableRequests(); + bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, + const 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 *frame_chain) const; + const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const + { + return buffer.lookupFrameString(frame_id_num); + } +}; +} + +#endif //TF3_CORE_H diff --git a/src/Libraries/tf3/include/tf3/compat.h b/src/Libraries/tf3/include/tf3/compat.h new file mode 100644 index 0000000..318049e --- /dev/null +++ b/src/Libraries/tf3/include/tf3/compat.h @@ -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 + +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 diff --git a/src/Libraries/tf3/include/tf3/convert.h b/src/Libraries/tf3/include/tf3/convert.h new file mode 100644 index 0000000..6516370 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/convert.h @@ -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 +#include +#include +#include +#include + +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 + 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 + 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 + const std::string& getFrameId(const T& t); + + + +/* An implementation for Stamped

datatypes */ +template + const tf3::Time& getTimestamp(const tf3::Stamped

& t) + { + return t.stamp_; + } + +/* An implementation for Stamped

datatypes */ +template + const std::string& getFrameId(const tf3::Stamped

& 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 + 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 + 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 + void convert(const A& a, B& b) + { + //printf("In double type convert\n"); + impl::Converter::value, tf3::message_traits::IsMessage::value>::convert(a, b); + } + +template + void convert(const A& a1, A& a2) + { + //printf("In single type convert\n"); + if(&a1 != &a2) + a2 = a1; + } + + +} + +#endif //TF3_CONVERT_H diff --git a/src/Libraries/tf3/include/tf3/exceptions.h b/src/Libraries/tf3/include/tf3/exceptions.h new file mode 100644 index 0000000..f22734e --- /dev/null +++ b/src/Libraries/tf3/include/tf3/exceptions.h @@ -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 + +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 diff --git a/src/Libraries/tf3/include/tf3/impl/convert.h b/src/Libraries/tf3/include/tf3/impl/convert.h new file mode 100644 index 0000000..8b16331 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/impl/convert.h @@ -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 +class Converter { +public: + template + static void convert(const A& a, B& b); +}; + +// The case where both A and B are messages should not happen: if you have two +// messages that are interchangeable, well, that's against the ROS purpose: +// only use one type. Worst comes to worst, specialize the original convert +// function for your types. +// if B == A, the templated version of convert with only one argument will be +// used. +// +//template <> +//template +//inline void Converter::convert(const A& a, B& b); + +template <> +template +inline void Converter::convert(const A& a, B& b) +{ +#ifdef _MSC_VER + tf3::fromMsg(a, b); +#else + fromMsg(a, b); +#endif +} + +template <> +template +inline void Converter::convert(const A& a, B& b) +{ +#ifdef _MSC_VER + b = tf3::toMsg(a); +#else + b = toMsg(a); +#endif +} + +template <> +template +inline void Converter::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 diff --git a/src/Libraries/tf3/include/tf3/impl/utils.h b/src/Libraries/tf3/include/tf3/impl/utils.h new file mode 100644 index 0000000..8411f92 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/impl/utils.h @@ -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 +#include +#include + +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 + tf3::Quaternion toQuaternion(const tf3::Stamped& 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 + 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 diff --git a/src/Libraries/tf3/include/tf3/macros.h b/src/Libraries/tf3/include/tf3/macros.h new file mode 100644 index 0000000..00b26d2 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/macros.h @@ -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_ diff --git a/src/Libraries/tf3/include/tf3/message_traits.h b/src/Libraries/tf3/include/tf3/message_traits.h new file mode 100644 index 0000000..d826231 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/message_traits.h @@ -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 + struct IsMessage { static const bool value = false; }; + } +} + +#endif // TF3_MESSAGE_TRAITS_H diff --git a/src/Libraries/tf3/include/tf3/time.h b/src/Libraries/tf3/include/tf3/time.h new file mode 100644 index 0000000..11652ab --- /dev/null +++ b/src/Libraries/tf3/include/tf3/time.h @@ -0,0 +1,152 @@ +// Minimal tf3::Time and tf3::Duration (non-ROS) for standalone tf3 +#ifndef TF3_TIME_H +#define TF3_TIME_H + +#include +#include +#include +#include +#include + +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(now).count(); + Time t; + t.sec = static_cast(ns / 1000000000ULL); + t.nsec = static_cast(ns % 1000000000ULL); + return t; + } + + static Time fromSec(double s) + { + Time t; + t.sec = static_cast(std::floor(s)); + t.nsec = static_cast((s - t.sec) * 1e9); + return t; + } + + double toSec() const { return static_cast(sec) + static_cast(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::max(), std::numeric_limits::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(std::floor(seconds)); + nsec = static_cast((seconds - sec) * 1e9); + normalize(); + } + + static Duration fromSec(double s) { return Duration(s); } + static Duration fromNSec(int64_t ns_total) + { + int32_t s = static_cast(ns_total / 1000000000LL); + int32_t ns = static_cast(ns_total % 1000000000LL); + return Duration(s, ns); + } + + double toSec() const { return static_cast(sec) + static_cast(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(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(a.sec) - static_cast(b.sec); + int32_t ns = static_cast(a.nsec) - static_cast(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(t.sec) * 1000000000LL + t.nsec + static_cast(d.sec) * 1000000000LL + d.nsec; + Time out; + out.sec = static_cast(total_ns / 1000000000LL); + out.nsec = static_cast(total_ns % 1000000000LL); + return out; +} + +inline Time operator-(const Time& t, const Duration& d) +{ + int64_t total_ns = static_cast(t.sec) * 1000000000LL + t.nsec - (static_cast(d.sec) * 1000000000LL + d.nsec); + if (total_ns < 0) total_ns = 0; + Time out; + out.sec = static_cast(total_ns / 1000000000LL); + out.nsec = static_cast(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 diff --git a/src/Libraries/tf3/include/tf3/time_cache.h b/src/Libraries/tf3/include/tf3/time_cache.h new file mode 100644 index 0000000..30e4883 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/time_cache.h @@ -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 + +#include "tf3/time.h" + +#include + +namespace tf3 +{ + +typedef std::pair 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 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 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 diff --git a/src/Libraries/tf3/include/tf3/transform_datatypes.h b/src/Libraries/tf3/include/tf3/transform_datatypes.h new file mode 100644 index 0000000..e760059 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/transform_datatypes.h @@ -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 +#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 +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& s): + T (s), + stamp_(s.stamp_), + frame_id_(s.frame_id_) {} + + /** Copy assignment operator */ + Stamped & operator=(const Stamped & 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(this) = input;}; +}; + +/** \brief Comparison Operator for Stamped datatypes */ +template +bool operator==(const Stamped &a, const Stamped &b) { + return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); +} + + +} +#endif //TF3_TRANSFORM_DATATYPES_H diff --git a/src/Libraries/tf3/include/tf3/transform_storage.h b/src/Libraries/tf3/include/tf3/transform_storage.h new file mode 100644 index 0000000..f966386 --- /dev/null +++ b/src/Libraries/tf3/include/tf3/transform_storage.h @@ -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 +#include + + +#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 + diff --git a/src/Libraries/tf3/include/tf3/utils.h b/src/Libraries/tf3/include/tf3/utils.h new file mode 100644 index 0000000..60bc46a --- /dev/null +++ b/src/Libraries/tf3/include/tf3/utils.h @@ -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 +#include +#include + +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 + 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 + 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 + A getTransformIdentity() + { + tf3::Transform t; + t.setIdentity(); + A a; + convert(t, a); + return a; + } + +} + +#endif //TF3_UTILS_H diff --git a/src/Libraries/tf3/index.rst b/src/Libraries/tf3/index.rst new file mode 100644 index 0000000..5001a33 --- /dev/null +++ b/src/Libraries/tf3/index.rst @@ -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` diff --git a/src/Libraries/tf3/mainpage.dox b/src/Libraries/tf3/mainpage.dox new file mode 100644 index 0000000..2cba60c --- /dev/null +++ b/src/Libraries/tf3/mainpage.dox @@ -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 ROS Wiki. + +*/ diff --git a/src/Libraries/tf3/package.xml b/src/Libraries/tf3/package.xml new file mode 100644 index 0000000..5e4811d --- /dev/null +++ b/src/Libraries/tf3/package.xml @@ -0,0 +1,26 @@ + + tf3 + 0.7.10 + + 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. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/tf3 + + catkin + + libconsole-bridge-dev + + libconsole-bridge-dev + + \ No newline at end of file diff --git a/src/Libraries/tf3/src/buffer_core.cpp b/src/Libraries/tf3/src/buffer_core.cpp new file mode 100644 index 0000000..6c5a19c --- /dev/null +++ b/src/Libraries/tf3/src/buffer_core.cpp @@ -0,0 +1,1645 @@ +/* + * 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 */ + +#include "tf3/buffer_core.h" +#include "tf3/time_cache.h" +#include "tf3/exceptions.h" +#include "tf3/TF3Error.h" + +#include +#include +#include "tf3/LinearMath/Transform.h" + +namespace tf3 +{ + +// Tolerance for acceptable quaternion normalization +static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3; + +/** \brief convert Transform msg to Transform */ +void transformMsgToTF3(const tf3::TransformMsg& msg, tf3::Transform& tf3) +{ tf3 = tf3::Transform(tf3::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf3::Vector3(msg.translation.x, msg.translation.y, msg.translation.z)); } + +/** \brief convert Transform to Transform msg*/ +void transformTF3ToMsg(const tf3::Transform& tf3, tf3::TransformMsg& msg) +{ + msg.translation.x = tf3.getOrigin().x(); + msg.translation.y = tf3.getOrigin().y(); + msg.translation.z = tf3.getOrigin().z(); + msg.rotation.x = tf3.getRotation().x(); + msg.rotation.y = tf3.getRotation().y(); + msg.rotation.z = tf3.getRotation().z(); + msg.rotation.w = tf3.getRotation().w(); +} + +/** \brief convert Transform to Transform msg*/ +void transformTF3ToMsg(const tf3::Transform& tf3, tf3::TransformStampedMsg& msg, tf3::Time stamp, const std::string& frame_id, const std::string& child_frame_id) +{ + transformTF3ToMsg(tf3, msg.transform); + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + msg.child_frame_id = child_frame_id; +} + +void transformTF3ToMsg(const tf3::Quaternion& orient, const tf3::Vector3& pos, tf3::TransformMsg& msg) +{ + msg.translation.x = pos.x(); + msg.translation.y = pos.y(); + msg.translation.z = pos.z(); + msg.rotation.x = orient.x(); + msg.rotation.y = orient.y(); + msg.rotation.z = orient.z(); + msg.rotation.w = orient.w(); +} + +void transformTF3ToMsg(const tf3::Quaternion& orient, const tf3::Vector3& pos, tf3::TransformStampedMsg& msg, tf3::Time stamp, const std::string& frame_id, const std::string& child_frame_id) +{ + transformTF3ToMsg(orient, pos, msg.transform); + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + msg.child_frame_id = child_frame_id; +} + +void setIdentity(tf3::TransformMsg& tx) +{ + tx.translation.x = 0; + tx.translation.y = 0; + tx.translation.z = 0; + tx.rotation.x = 0; + tx.rotation.y = 0; + tx.rotation.z = 0; + tx.rotation.w = 1; +} + +bool startsWithSlash(const std::string& frame_id) +{ + if (frame_id.size() > 0) + if (frame_id[0] == '/') + return true; + return false; +} + +std::string stripSlash(const std::string& in) +{ + std::string out = in; + if (startsWithSlash(in)) + out.erase(0,1); + return out; +} + + +bool BufferCore::warnFrameId(const char* function_name_arg, const std::string& frame_id) const +{ + if (frame_id.size() == 0) + { + std::stringstream ss; + ss << "Invalid argument passed to "<< function_name_arg <<" in tf3 frame_ids cannot be empty"; + CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); + return true; + } + + if (startsWithSlash(frame_id)) + { + std::stringstream ss; + ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf3 frame_ids cannot start with a '/' like: "; + CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); + return true; + } + + return false; +} + +CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const +{ + if (frame_id.empty()) + { + std::stringstream ss; + ss << "Invalid argument passed to "<< function_name_arg <<" in tf3 frame_ids cannot be empty"; + throw tf3::InvalidArgumentException(ss.str().c_str()); + } + + if (startsWithSlash(frame_id)) + { + std::stringstream ss; + ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf3 frame_ids cannot start with a '/' like: "; + throw tf3::InvalidArgumentException(ss.str().c_str()); + } + + CompactFrameID id = lookupFrameNumber(frame_id); + if (id == 0) + { + std::stringstream ss; + ss << "\"" << frame_id << "\" passed to "<< function_name_arg <<" does not exist. "; + throw tf3::LookupException(ss.str().c_str()); + } + + return id; +} + +BufferCore::BufferCore(tf3::Duration cache_time) +: transformable_callbacks_counter_(0), + transformable_requests_counter_(0), + using_dedicated_thread_(false), + cache_time_(cache_time) +{ + frameIDs_["NO_PARENT"] = 0; + frames_.push_back(TimeCacheInterfacePtr()); + frameIDs_reverse.push_back("NO_PARENT"); +} + +BufferCore::~BufferCore() +{ + +} + +void BufferCore::clear() +{ + //old_tf_.clear(); + + + boost::mutex::scoped_lock lock(frame_mutex_); + if ( frames_.size() > 1 ) + { + for (std::vector::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it) + { + if (*cache_it) + (*cache_it)->clearList(); + } + } + +} + +bool BufferCore::setTransform(const tf3::TransformStampedMsg& transform_in, const std::string& authority, bool is_static) +{ + + /////BACKEARDS COMPATABILITY + /* tf::StampedTransform tf_transform; + tf::transformStampedMsgToTF(transform_in, tf_transform); + if (!old_tf_.setTransform(tf_transform, authority)) + { + printf("Warning old setTransform Failed but was not caught\n"); + }*/ + + /////// New implementation + tf3::TransformStampedMsg stripped = transform_in; + stripped.header.frame_id = stripSlash(stripped.header.frame_id); + stripped.child_frame_id = stripSlash(stripped.child_frame_id); + + + bool error_exists = false; + if (stripped.child_frame_id == stripped.header.frame_id) + { + CONSOLE_BRIDGE_logError("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), stripped.child_frame_id.c_str()); + error_exists = true; + } + + if (stripped.child_frame_id == "") + { + CONSOLE_BRIDGE_logError("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str()); + error_exists = true; + } + + if (stripped.header.frame_id == "") + { + CONSOLE_BRIDGE_logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", stripped.child_frame_id.c_str(), authority.c_str()); + error_exists = true; + } + + if (std::isnan(stripped.transform.translation.x) || std::isnan(stripped.transform.translation.y) || std::isnan(stripped.transform.translation.z)|| + std::isnan(stripped.transform.rotation.x) || std::isnan(stripped.transform.rotation.y) || std::isnan(stripped.transform.rotation.z) || std::isnan(stripped.transform.rotation.w)) + { + CONSOLE_BRIDGE_logError("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)", + stripped.child_frame_id.c_str(), authority.c_str(), + stripped.transform.translation.x, stripped.transform.translation.y, stripped.transform.translation.z, + stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w + ); + error_exists = true; + } + + bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w + + stripped.transform.rotation.x * stripped.transform.rotation.x + + stripped.transform.rotation.y * stripped.transform.rotation.y + + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE; + + if (!valid) + { + CONSOLE_BRIDGE_logError("TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)", + stripped.child_frame_id.c_str(), authority.c_str(), + stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w); + error_exists = true; + } + + if (error_exists) + return false; + + { + boost::mutex::scoped_lock lock(frame_mutex_); + CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id); + TimeCacheInterfacePtr frame = getFrame(frame_number); + if (frame == NULL) + frame = allocateFrame(frame_number, is_static); + + std::string error_string; + if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), &error_string)) + { + frame_authority_[frame_number] = authority; + } + else + { + CONSOLE_BRIDGE_logWarn((error_string+" for frame %s (parent %s) at time %lf according to authority %s").c_str(), stripped.child_frame_id.c_str(), stripped.header.frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); + return false; + } + } + + testTransformableRequests(); + + return true; +} + +TimeCacheInterfacePtr BufferCore::allocateFrame(CompactFrameID cfid, bool is_static) +{ + TimeCacheInterfacePtr frame_ptr = frames_[cfid]; + if (is_static) { + frames_[cfid] = TimeCacheInterfacePtr(new StaticCache()); + } else { + frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_)); + } + + return frames_[cfid]; +} + +enum WalkEnding +{ + Identity, + TargetParentOfSource, + SourceParentOfTarget, + FullPath, +}; + +// TODO for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI +template +int BufferCore::walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const +{ + return walkToTopParent(f, time, target_id, source_id, error_string, NULL); +} + +template +int BufferCore::walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, + CompactFrameID source_id, std::string* error_string, std::vector + *frame_chain) const +{ + if (frame_chain) + frame_chain->clear(); + + // Short circuit if zero length transform to allow lookups on non existant links + if (source_id == target_id) + { + f.finalize(Identity, time); + return tf3::TF3Error::NO_ERROR; + } + + //If getting the latest get the latest common time + if (time == tf3::Time()) + { + int retval = getLatestCommonTime(target_id, source_id, time, error_string); + if (retval != tf3::TF3Error::NO_ERROR) + { + return retval; + } + } + + // Walk the tree to its root from the source frame, accumulating the transform + CompactFrameID frame = source_id; + CompactFrameID top_parent = frame; + uint32_t depth = 0; + + std::string extrapolation_error_string; + bool extrapolation_might_have_occurred = false; + + while (frame != 0) + { + TimeCacheInterfacePtr cache = getFrame(frame); + if (frame_chain) + frame_chain->push_back(frame); + + if (!cache) + { + // There will be no cache for the very root of the tree + top_parent = frame; + break; + } + + CompactFrameID parent = f.gather(cache, time, error_string ? &extrapolation_error_string : NULL); + if (parent == 0) + { + // Just break out here... there may still be a path from source -> target + top_parent = frame; + extrapolation_might_have_occurred = true; + break; + } + + // Early out... target frame is a direct parent of the source frame + if (frame == target_id) + { + f.finalize(TargetParentOfSource, time); + return tf3::TF3Error::NO_ERROR; + } + + f.accum(true); + + top_parent = frame; + frame = parent; + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss << "The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf3::TF3Error::LOOKUP_ERROR; + } + } + + // Now walk to the top parent from the target frame, accumulating its transform + frame = target_id; + depth = 0; + std::vector reverse_frame_chain; + + while (frame != top_parent) + { + TimeCacheInterfacePtr cache = getFrame(frame); + if (frame_chain) + reverse_frame_chain.push_back(frame); + + if (!cache) + { + break; + } + + CompactFrameID parent = f.gather(cache, time, error_string); + if (parent == 0) + { + if (error_string) + { + // optimize performance by not using stringstream + char str[1000]; + snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", error_string->c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); + *error_string = str; + } + + return tf3::TF3Error::EXTRAPOLATION_ERROR; + } + + // Early out... source frame is a direct parent of the target frame + if (frame == source_id) + { + f.finalize(SourceParentOfTarget, time); + if (frame_chain) + { + // Use the walk we just did + frame_chain->swap(reverse_frame_chain); + // Reverse it before returning because this is the reverse walk. + std::reverse(frame_chain->begin(), frame_chain->end()); + } + return tf3::TF3Error::NO_ERROR; + } + + f.accum(false); + + frame = parent; + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss << "The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf3::TF3Error::LOOKUP_ERROR; + } + } + + if (frame != top_parent) + { + if (extrapolation_might_have_occurred) + { + if (error_string) + { + // optimize performance by not using stringstream + char str[1000]; + snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", extrapolation_error_string.c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); + *error_string = str; + } + + return tf3::TF3Error::EXTRAPOLATION_ERROR; + + } + + createConnectivityErrorString(source_id, target_id, error_string); + return tf3::TF3Error::CONNECTIVITY_ERROR; + } + else if (frame_chain){ + // append top_parent to reverse_frame_chain for easier matching/trimming + reverse_frame_chain.push_back(frame); + } + + f.finalize(FullPath, time); + if (frame_chain) + { + // Pruning: Compare the chains starting at the parent (end) until they differ + int m = reverse_frame_chain.size()-1; + int n = frame_chain->size()-1; + for (; m >= 0 && n >= 0; --m, --n) + { + if ((*frame_chain)[n] != reverse_frame_chain[m]) + { + break; + } + } + // Erase all duplicate items from frame_chain + if (n > 0) + { + // N is offset by 1 and leave the common parent for this result + frame_chain->erase(frame_chain->begin() + (n + 2), frame_chain->end()); + } + if (m < reverse_frame_chain.size()) + { + for (int i = m; i >= 0; --i) + { + frame_chain->push_back(reverse_frame_chain[i]); + } + } + } + + return tf3::TF3Error::NO_ERROR; +} + + + +struct TransformAccum +{ + TransformAccum() + : source_to_top_quat(0.0, 0.0, 0.0, 1.0) + , source_to_top_vec(0.0, 0.0, 0.0) + , target_to_top_quat(0.0, 0.0, 0.0, 1.0) + , target_to_top_vec(0.0, 0.0, 0.0) + , result_quat(0.0, 0.0, 0.0, 1.0) + , result_vec(0.0, 0.0, 0.0) + { + } + + CompactFrameID gather(TimeCacheInterfacePtr cache, tf3::Time time, std::string* error_string) + { + if (!cache->getData(time, st, error_string)) + { + return 0; + } + + return st.frame_id_; + } + + void accum(bool source) + { + if (source) + { + source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_; + source_to_top_quat = st.rotation_ * source_to_top_quat; + } + else + { + target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; + target_to_top_quat = st.rotation_ * target_to_top_quat; + } + } + + void finalize(WalkEnding end, tf3::Time _time) + { + switch (end) + { + case Identity: + break; + case TargetParentOfSource: + result_vec = source_to_top_vec; + result_quat = source_to_top_quat; + break; + case SourceParentOfTarget: + { + tf3::Quaternion inv_target_quat = target_to_top_quat.inverse(); + tf3::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); + result_vec = inv_target_vec; + result_quat = inv_target_quat; + break; + } + case FullPath: + { + tf3::Quaternion inv_target_quat = target_to_top_quat.inverse(); + tf3::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); + + result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec; + result_quat = inv_target_quat * source_to_top_quat; + } + break; + }; + + time = _time; + } + + TransformStorage st; + tf3::Time time; + tf3::Quaternion source_to_top_quat; + tf3::Vector3 source_to_top_vec; + tf3::Quaternion target_to_top_quat; + tf3::Vector3 target_to_top_vec; + + tf3::Quaternion result_quat; + tf3::Vector3 result_vec; +}; + +tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_frame, + const std::string& source_frame, + const tf3::Time& time) const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + + if (target_frame == source_frame) { + tf3::TransformStampedMsg identity; + identity.header.frame_id = target_frame; + identity.child_frame_id = source_frame; + identity.transform.rotation.w = 1; + + if (time == tf3::Time()) + { + CompactFrameID target_id = lookupFrameNumber(target_frame); + TimeCacheInterfacePtr cache = getFrame(target_id); + if (cache) + identity.header.stamp = cache->getLatestTimestamp(); + else + identity.header.stamp = time; + } + else + identity.header.stamp = time; + + return identity; + } + + //Identify case does not need to be validated above + CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame); + CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame); + + std::string error_string; + TransformAccum accum; + int retval = walkToTopParent(accum, time, target_id, source_id, &error_string); + if (retval != tf3::TF3Error::NO_ERROR) + { + switch (retval) + { + case tf3::TF3Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf3::TF3Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf3::TF3Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); + } + } + + tf3::TransformStampedMsg output_transform; + transformTF3ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame); + return output_transform; +} + +tf3::TransformStampedMsg BufferCore::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 +{ + validateFrameId("lookupTransform argument target_frame", target_frame); + validateFrameId("lookupTransform argument source_frame", source_frame); + validateFrameId("lookupTransform argument fixed_frame", fixed_frame); + + tf3::TransformStampedMsg output; + tf3::TransformStampedMsg temp1 = lookupTransform(fixed_frame, source_frame, source_time); + tf3::TransformStampedMsg temp2 = lookupTransform(target_frame, fixed_frame, target_time); + + tf3::Transform tf1, tfb; + transformMsgToTF3(temp1.transform, tf1); + transformMsgToTF3(temp2.transform, tfb); + transformTF3ToMsg(tfb*tf1, output.transform); + output.header.stamp = temp2.header.stamp; + output.header.frame_id = target_frame; + output.child_frame_id = source_frame; + return output; +} + + + +/* +geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, + const std::string& observation_frame, + const ros::Time& time, + const ros::Duration& averaging_interval) const +{ + try + { + geometry_msgs::Twist t; + old_tf_.lookupTwist(tracking_frame, observation_frame, + time, averaging_interval, t); + return t; + } + catch (tf::LookupException& ex) + { + throw tf3::LookupException(ex.what()); + } + catch (tf::ConnectivityException& ex) + { + throw tf3::ConnectivityException(ex.what()); + } + catch (tf::ExtrapolationException& ex) + { + throw tf3::ExtrapolationException(ex.what()); + } + catch (tf::InvalidArgument& ex) + { + throw tf3::InvalidArgumentException(ex.what()); + } +} + +geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, + const std::string& observation_frame, + const std::string& reference_frame, + const tf3::Point & reference_point, + const std::string& reference_point_frame, + const ros::Time& time, + const ros::Duration& averaging_interval) const +{ + try{ + geometry_msgs::Twist t; + old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, + time, averaging_interval, t); + return t; + } + catch (tf::LookupException& ex) + { + throw tf3::LookupException(ex.what()); + } + catch (tf::ConnectivityException& ex) + { + throw tf3::ConnectivityException(ex.what()); + } + catch (tf::ExtrapolationException& ex) + { + throw tf3::ExtrapolationException(ex.what()); + } + catch (tf::InvalidArgument& ex) + { + throw tf3::InvalidArgumentException(ex.what()); + } +} +*/ + +struct CanTransformAccum +{ + CompactFrameID gather(TimeCacheInterfacePtr cache, tf3::Time time, std::string* error_string) + { + return cache->getParent(time, error_string); + } + + void accum(bool source) + { + } + + void finalize(WalkEnding end, tf3::Time _time) + { + } + + TransformStorage st; +}; + +bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, + const tf3::Time& time, std::string* error_msg) const +{ + if (target_id == 0 || source_id == 0) + { + if (error_msg) + { + if (target_id == 0) + { + *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist."); + } + } + return false; + } + + if (target_id == source_id) + { + return true; + } + + CanTransformAccum accum; + if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf3::TF3Error::NO_ERROR) + { + return true; + } + + return false; +} + +bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, + const tf3::Time& time, std::string* error_msg) const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + return canTransformNoLock(target_id, source_id, time, error_msg); +} + +bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame, + const tf3::Time& time, std::string* error_msg) const +{ + // Short circuit if target_frame == source_frame + if (target_frame == source_frame) + return true; + + if (warnFrameId("canTransform argument target_frame", target_frame)) + return false; + if (warnFrameId("canTransform argument source_frame", source_frame)) + return false; + + boost::mutex::scoped_lock lock(frame_mutex_); + + CompactFrameID target_id = lookupFrameNumber(target_frame); + CompactFrameID source_id = lookupFrameNumber(source_frame); + + if (target_id == 0 || source_id == 0) + { + if (error_msg) + { + if (target_id == 0) + { + *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); + } + } + return false; + } + return canTransformNoLock(target_id, source_id, time, error_msg); +} + +bool BufferCore::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) const +{ + if (warnFrameId("canTransform argument target_frame", target_frame)) + return false; + if (warnFrameId("canTransform argument source_frame", source_frame)) + return false; + if (warnFrameId("canTransform argument fixed_frame", fixed_frame)) + return false; + + boost::mutex::scoped_lock lock(frame_mutex_); + CompactFrameID target_id = lookupFrameNumber(target_frame); + CompactFrameID source_id = lookupFrameNumber(source_frame); + CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); + + if (target_id == 0 || source_id == 0 || fixed_id == 0) + { + if (error_msg) + { + if (target_id == 0) + { + *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); + } + if (fixed_id == 0) + { + if (target_id == 0 || source_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist."); + } + } + return false; + } + return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg); +} + + +tf3::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const +{ + if (frame_id >= frames_.size()) + return TimeCacheInterfacePtr(); + else + { + return frames_[frame_id]; + } +} + +CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const +{ + CompactFrameID retval; + M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str); + if (map_it == frameIDs_.end()) + { + retval = CompactFrameID(0); + } + else + retval = map_it->second; + return retval; +} + +CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str) +{ + CompactFrameID retval = 0; + M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str); + if (map_it == frameIDs_.end()) + { + retval = CompactFrameID(frames_.size()); + frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration + frameIDs_[frameid_str] = retval; + frameIDs_reverse.push_back(frameid_str); + } + else + retval = frameIDs_[frameid_str]; + + return retval; +} + +const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const +{ + if (frame_id_num >= frameIDs_reverse.size()) + { + std::stringstream ss; + ss << "Reverse lookup of frame id " << frame_id_num << " failed!"; + throw tf3::LookupException(ss.str()); + } + else + return frameIDs_reverse[frame_id_num]; +} + +void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const +{ + if (!out) + { + return; + } + *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+ + lookupFrameString(source_frame)+"' because they are not part of the same tree."+ + "Tf has two or more unconnected trees."); +} + +std::string BufferCore::allFramesAsString() const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + return this->allFramesAsStringNoLock(); +} + +std::string BufferCore::allFramesAsStringNoLock() const +{ + std::stringstream mstream; + + TransformStorage temp; + + // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) + + ///regular transforms + for (unsigned int counter = 1; counter < frames_.size(); counter ++) + { + TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter)); + if (frame_ptr == NULL) + continue; + CompactFrameID frame_id_num; + if( frame_ptr->getData(tf3::Time(), temp)) + frame_id_num = temp.frame_id_; + else + { + frame_id_num = 0; + } + mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <getLatestTimestamp(); + else + time = tf3::Time(); + return tf3::TF3Error::NO_ERROR; + } + + std::vector lct_cache; + + // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time + // in the target is a direct parent + CompactFrameID frame = source_id; + P_TimeAndFrameID temp; + uint32_t depth = 0; + tf3::Time common_time = tf3::Time::TIME_MAX(); + while (frame != 0) + { + TimeCacheInterfacePtr cache = getFrame(frame); + + if (!cache) + { + // There will be no cache for the very root of the tree + break; + } + + P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); + + if (latest.second == 0) + { + // Just break out here... there may still be a path from source -> target + break; + } + + if (!latest.first.isZero()) + { + common_time = std::min(latest.first, common_time); + } + + lct_cache.push_back(latest); + + frame = latest.second; + + // Early out... target frame is a direct parent of the source frame + if (frame == target_id) + { + time = common_time; + if (time == tf3::Time::TIME_MAX()) + { + time = tf3::Time(); + } + return tf3::TF3Error::NO_ERROR; + } + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss<<"The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf3::TF3Error::LOOKUP_ERROR; + } + } + + // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent + frame = target_id; + depth = 0; + common_time = tf3::Time::TIME_MAX(); + CompactFrameID common_parent = 0; + while (true) + { + TimeCacheInterfacePtr cache = getFrame(frame); + + if (!cache) + { + break; + } + + P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); + + if (latest.second == 0) + { + break; + } + + if (!latest.first.isZero()) + { + common_time = std::min(latest.first, common_time); + } + + std::vector::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second)); + if (it != lct_cache.end()) // found a common parent + { + common_parent = it->second; + break; + } + + frame = latest.second; + + // Early out... source frame is a direct parent of the target frame + if (frame == source_id) + { + time = common_time; + if (time == tf3::Time::TIME_MAX()) + { + time = tf3::Time(); + } + return tf3::TF3Error::NO_ERROR; + } + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss<<"The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf3::TF3Error::LOOKUP_ERROR; + } + } + + if (common_parent == 0) + { + createConnectivityErrorString(source_id, target_id, error_string); + return tf3::TF3Error::CONNECTIVITY_ERROR; + } + + // Loop through the source -> root list until we hit the common parent + { + std::vector::iterator it = lct_cache.begin(); + std::vector::iterator end = lct_cache.end(); + for (; it != end; ++it) + { + if (!it->first.isZero()) + { + common_time = std::min(common_time, it->first); + } + + if (it->second == common_parent) + { + break; + } + } + } + + if (common_time == tf3::Time::TIME_MAX()) + { + common_time = tf3::Time(); + } + + time = common_time; + return tf3::TF3Error::NO_ERROR; +} + +std::string BufferCore::allFramesAsYAML(double current_time) const +{ + std::stringstream mstream; + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformStorage temp; + + if (frames_.size() ==1) + mstream <<"{}"; + + mstream.precision(3); + mstream.setf(std::ios::fixed,std::ios::floatfield); + + // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) + for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame + { + CompactFrameID cfid = CompactFrameID(counter); + CompactFrameID frame_id_num; + TimeCacheInterfacePtr cache = getFrame(cfid); + if (!cache) + { + continue; + } + + if(!cache->getData(tf3::Time(), temp)) + { + continue; + } + + frame_id_num = temp.frame_id_; + + std::string authority = "no recorded authority"; + std::map::const_iterator it = frame_authority_.find(cfid); + if (it != frame_authority_.end()) { + authority = it->second; + } + + double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() - + cache->getOldestTimestamp().toSec() ), 0.0001); + + mstream << std::fixed; //fixed point notation + mstream.precision(3); //3 decimal places + mstream << frameIDs_reverse[cfid] << ": " << std::endl; + mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl; + mstream << " broadcaster: '" << authority << "'" << std::endl; + mstream << " rate: " << rate << std::endl; + mstream << " most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl; + mstream << " oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl; + if ( current_time > 0 ) { + mstream << " transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl; + } + mstream << " buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl; + } + + return mstream.str(); +} + +std::string BufferCore::allFramesAsYAML() const +{ + return this->allFramesAsYAML(0.0); +} + +TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb) +{ + boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); + TransformableCallbackHandle handle = ++transformable_callbacks_counter_; + while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second) + { + handle = ++transformable_callbacks_counter_; + } + + return handle; +} + +struct BufferCore::RemoveRequestByCallback +{ + RemoveRequestByCallback(TransformableCallbackHandle handle) + : handle_(handle) + {} + + bool operator()(const TransformableRequest& req) + { + return req.cb_handle == handle_; + } + + TransformableCallbackHandle handle_; +}; + +void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle) +{ + { + boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); + transformable_callbacks_.erase(handle); + } + + { + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + auto it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle)); + if (it != transformable_requests_.end()) + { + transformable_requests_.erase(it, transformable_requests_.end()); + } + } +} + +TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, tf3::Time time) +{ + // shortcut if target == source + if (target_frame == source_frame) + { + return 0; + } + + TransformableRequest req; + req.target_id = lookupFrameNumber(target_frame); + req.source_id = lookupFrameNumber(source_frame); + + // First check if the request is already transformable. If it is, return immediately + if (canTransformInternal(req.target_id, req.source_id, time, 0)) + { + return 0; + } + + // Might not be transformable at all, ever (if it's too far in the past) + if (req.target_id && req.source_id) + { + tf3::Time latest_time; + // TODO: This is incorrect, but better than nothing. Really we want the latest time for + // any of the frames + getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); + if (!latest_time.isZero() && time + cache_time_ < latest_time) + { + return 0xffffffffffffffffULL; + } + } + + req.cb_handle = handle; + req.time = time; + req.request_handle = ++transformable_requests_counter_; + if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL) + { + req.request_handle = 1; + } + + if (req.target_id == 0) + { + req.target_string = target_frame; + } + + if (req.source_id == 0) + { + req.source_string = source_frame; + } + + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + transformable_requests_.push_back(req); + + return req.request_handle; +} + +struct BufferCore::RemoveRequestByID +{ + RemoveRequestByID(TransformableRequestHandle handle) + : handle_(handle) + {} + + bool operator()(const TransformableRequest& req) + { + return req.request_handle == handle_; + } + + TransformableCallbackHandle handle_; +}; + +void BufferCore::cancelTransformableRequest(TransformableRequestHandle handle) +{ + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + auto it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle)); + if (it != transformable_requests_.end()) + { + transformable_requests_.erase(it, transformable_requests_.end()); + } +} + + + +// backwards compability for tf methods +boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function callback) +{ + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + return _transforms_changed_.connect(callback); +} + +void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c) +{ + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + c.disconnect(); +} + + +bool BufferCore::_frameExists(const std::string& frame_id_str) const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + return frameIDs_.count(frame_id_str); +} + +bool BufferCore::_getParent(const std::string& frame_id, tf3::Time time, std::string& parent) const +{ + + boost::mutex::scoped_lock lock(frame_mutex_); + CompactFrameID frame_number = lookupFrameNumber(frame_id); + TimeCacheInterfacePtr frame = getFrame(frame_number); + + if (! frame) + return false; + + CompactFrameID parent_id = frame->getParent(time, NULL); + if (parent_id == 0) + return false; + + parent = lookupFrameString(parent_id); + return true; +}; + +void BufferCore::_getFrameStrings(std::vector & vec) const +{ + vec.clear(); + + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformStorage temp; + + // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) + for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++) + { + vec.push_back(frameIDs_reverse[counter]); + } + return; +} + + + + +void BufferCore::testTransformableRequests() +{ + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + V_TransformableRequest::iterator it = transformable_requests_.begin(); + for (; it != transformable_requests_.end();) + { + TransformableRequest& req = *it; + + // One or both of the frames may not have existed when the request was originally made. + if (req.target_id == 0) + { + req.target_id = lookupFrameNumber(req.target_string); + } + + if (req.source_id == 0) + { + req.source_id = lookupFrameNumber(req.source_string); + } + + tf3::Time latest_time; + bool do_cb = false; + TransformableResult result = TransformAvailable; + // TODO: This is incorrect, but better than nothing. Really we want the latest time for + // any of the frames + getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); + if (!latest_time.isZero() && req.time + cache_time_ < latest_time) + { + do_cb = true; + result = TransformFailure; + } + else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) + { + do_cb = true; + result = TransformAvailable; + } + + if (do_cb) + { + { + boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_); + M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle); + if (it != transformable_callbacks_.end()) + { + const TransformableCallback& cb = it->second; + cb(req.request_handle, lookupFrameString(req.target_id), lookupFrameString(req.source_id), req.time, result); + } + } + + if (transformable_requests_.size() > 1) + { + transformable_requests_[it - transformable_requests_.begin()] = transformable_requests_.back(); + } + + transformable_requests_.erase(transformable_requests_.end() - 1); + } + else + { + ++it; + } + } + + // unlock before allowing possible user callbacks to avoid potential deadlock (#91) + lock.unlock(); + + // Backwards compatability callback for tf + _transforms_changed_(); +} + + +std::string BufferCore::_allFramesAsDot(double current_time) const +{ + std::stringstream mstream; + mstream << "digraph G {" << std::endl; + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformStorage temp; + + if (frames_.size() == 1) { + mstream <<"\"no tf data recieved\""; + } + mstream.precision(3); + mstream.setf(std::ios::fixed,std::ios::floatfield); + + for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame + { + unsigned int frame_id_num; + TimeCacheInterfacePtr counter_frame = getFrame(counter); + if (!counter_frame) { + continue; + } + if(!counter_frame->getData(tf3::Time(), temp)) { + continue; + } else { + frame_id_num = temp.frame_id_; + } + std::string authority = "no recorded authority"; + std::map::const_iterator it = frame_authority_.find(counter); + if (it != frame_authority_.end()) + authority = it->second; + + double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() - + counter_frame->getOldestTimestamp().toSec()), 0.0001); + + mstream << std::fixed; //fixed point notation + mstream.precision(3); //3 decimal places + mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> " + << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\"" + //<< "Time: " << current_time.toSec() << "\\n" + << "Broadcaster: " << authority << "\\n" + << "Average rate: " << rate << " Hz\\n" + << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" "; + if (current_time > 0) + mstream << "( "<< current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)"; + mstream << "\\n" + // << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n" + // << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n" + // << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n" + << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n" + <<"\"];" < 0) { + mstream << "edge [style=invis];" <" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; + } + continue; + } + if (counter_frame->getData(tf3::Time(), temp)) { + frame_id_num = temp.frame_id_; + } else { + frame_id_num = 0; + } + + if(frameIDs_reverse[frame_id_num]=="NO_PARENT") + { + mstream << "edge [style=invis];" < 0) + mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "; + mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; + } + } + mstream << "}"; + return mstream.str(); +} + +std::string BufferCore::_allFramesAsDot() const +{ + return _allFramesAsDot(0.0); +} + +void BufferCore::_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& output) const +{ + std::string error_string; + + output.clear(); //empty vector + + std::stringstream mstream; + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformAccum accum; + + // Get source frame/time using getFrame + CompactFrameID source_id = lookupFrameNumber(source_frame); + CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); + CompactFrameID target_id = lookupFrameNumber(target_frame); + + std::vector source_frame_chain; + int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain); + + if (retval != tf3::TF3Error::NO_ERROR) + { + switch (retval) + { + case tf3::TF3Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf3::TF3Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf3::TF3Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); + } + } + + std::vector target_frame_chain; + retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain); + + if (retval != tf3::TF3Error::NO_ERROR) + { + switch (retval) + { + case tf3::TF3Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf3::TF3Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf3::TF3Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); + } + } + // If the two chains overlap clear the overlap + if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 && + source_frame_chain.back() == target_frame_chain.front()) + { + source_frame_chain.pop_back(); + } + // Join the two walks + for (unsigned int i = 0; i < target_frame_chain.size(); ++i) + { + source_frame_chain.push_back(target_frame_chain[i]); + } + + + // Write each element of source_frame_chain as string + for (unsigned int i = 0; i < source_frame_chain.size(); ++i) + { + output.push_back(lookupFrameString(source_frame_chain[i])); + } +} + +int TestBufferCore::_walkToTopParent(BufferCore& buffer, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const +{ + TransformAccum accum; + return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain); +} + +} // namespace tf3 diff --git a/src/Libraries/tf3/src/cache.cpp b/src/Libraries/tf3/src/cache.cpp new file mode 100644 index 0000000..6cb778a --- /dev/null +++ b/src/Libraries/tf3/src/cache.cpp @@ -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 +#include +#include +#include "tf3/compat.h" +#include + +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()); + + //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 +} diff --git a/src/Libraries/tf3/src/static_cache.cpp b/src/Libraries/tf3/src/static_cache.cpp new file mode 100644 index 0000000..b7e2e85 --- /dev/null +++ b/src/Libraries/tf3/src/static_cache.cpp @@ -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(); +}; + diff --git a/src/Libraries/tf3/test/cache_unittest.cpp b/src/Libraries/tf3/test/cache_unittest.cpp new file mode 100644 index 0000000..c0f4bd8 --- /dev/null +++ b/src/Libraries/tf3/test/cache_unittest.cpp @@ -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 +#include +#include "tf3/LinearMath/Quaternion.h" +#include + +#include + +#include + +std::vector 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 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 xvalues(2); + std::vector yvalues(2); + std::vector 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 xvalues(2); + std::vector yvalues(2); + std::vector 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 yawvalues(2); + std::vector pitchvalues(2); + std::vector rollvalues(2); + uint64_t offset = 200; + + std::vector 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(); +} + diff --git a/src/Libraries/tf3/test/simple_tf3_core.cpp b/src/Libraries/tf3/test/simple_tf3_core.cpp new file mode 100644 index 0000000..5fc8cb7 --- /dev/null +++ b/src/Libraries/tf3/test/simple_tf3_core.cpp @@ -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 +#include +#include +#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 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 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 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(); +} diff --git a/src/Libraries/tf3/test/speed_test.cpp b/src/Libraries/tf3/test/speed_test.cpp new file mode 100644 index 0000000..5dbeb2c --- /dev/null +++ b/src/Libraries/tf3/test/speed_test.cpp @@ -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 + +#include +#include + +#include + +int main(int argc, char** argv) +{ + uint32_t num_levels = 10; + if (argc > 1) + { + num_levels = boost::lexical_cast(argv[1]); + } + double time_interval = 1.0; + if (argc > 2) + { + time_interval = boost::lexical_cast(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(num_levels - 1); + std::string v_frame1 = boost::lexical_cast(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 +} diff --git a/src/Libraries/tf3/test/static_cache_test.cpp b/src/Libraries/tf3/test/static_cache_test.cpp new file mode 100644 index 0000000..11e6c23 --- /dev/null +++ b/src/Libraries/tf3/test/static_cache_test.cpp @@ -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 +#include +#include + +#include + +#include + +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(); +} diff --git a/src/Libraries/tf3/test/test_transform_datatypes.cpp b/src/Libraries/tf3/test/test_transform_datatypes.cpp new file mode 100644 index 0000000..55df3d4 --- /dev/null +++ b/src/Libraries/tf3/test/test_transform_datatypes.cpp @@ -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 + +#include "tf3/transform_datatypes.h" + +#include + + +TEST(Stamped, assignment) +{ + tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); + tf3::Stamped second("baz", ros::Time(0), "my_frame_id"); + + EXPECT_NE(second, first); + second = first; + EXPECT_EQ(second, first); +} + +TEST(Stamped, setData) +{ + tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); + tf3::Stamped 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 first("foobar", ros::Time(0), "my_frame_id"); + tf3::Stamped second(first); + + EXPECT_EQ(second, first); +} + +TEST(Stamped, default_constructor) +{ + tf3::Stamped first("foobar", ros::Time(0), "my_frame_id"); + tf3::Stamped second; + + EXPECT_NE(second, first); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::Time::init(); + return RUN_ALL_TESTS(); +} diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index c12c68b..ec6fd21 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -89,7 +89,6 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) private_nh_.getParam("global_frame", global_frame, std::string("map")); robot_base_frame_ = robot_base_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; private_nh_.getParam("planner_frequency", planner_frequency, 0.0); planner_frequency_ = planner_frequency;