From dd033ec8c3c32eb870a18bcb2fcb49568dde4c8d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 11 Jun 2020 19:14:25 +0000 Subject: [PATCH] Add common linters to tf2. Signed-off-by: Chris Lalancette Signed-off-by: ahcorde --- tf2/CMakeLists.txt | 7 +- tf2/include/tf2/buffer_core.h | 298 +++--- tf2/include/tf2/buffer_core_interface.h | 103 +- tf2/include/tf2/convert.h | 170 ++-- tf2/include/tf2/exceptions.h | 167 +-- tf2/include/tf2/impl/convert.h | 102 +- tf2/include/tf2/impl/utils.h | 97 +- tf2/include/tf2/time.h | 152 ++- tf2/include/tf2/time_cache.h | 153 +-- tf2/include/tf2/transform_datatypes.h | 112 +- tf2/include/tf2/transform_storage.h | 85 +- tf2/include/tf2/utils.h | 53 +- tf2/include/tf2/visibility_control.h | 55 +- tf2/mainpage.dox | 8 +- tf2/package.xml | 3 +- tf2/src/buffer_core.cpp | 1235 +++++++++++------------ tf2/src/cache.cpp | 291 +++--- tf2/src/static_cache.cpp | 98 +- tf2/src/time.cpp | 57 +- tf2/test/cache_unittest.cpp | 304 +++--- tf2/test/simple_tf2_core.cpp | 99 +- tf2/test/speed_test.cpp | 154 ++- tf2/test/static_cache_test.cpp | 98 +- tf2/test/test_time.cpp | 2 +- 24 files changed, 1946 insertions(+), 1957 deletions(-) diff --git a/tf2/CMakeLists.txt b/tf2/CMakeLists.txt index 41ddad3e7..7cdc40e8d 100644 --- a/tf2/CMakeLists.txt +++ b/tf2/CMakeLists.txt @@ -41,9 +41,8 @@ install(DIRECTORY include/${PROJECT_NAME}/ # Tests if(BUILD_TESTING) - # TODO(tfoote) enable linters - # find_package(ament_lint_auto REQUIRED) - # ament_lint_auto_find_test_dependencies() + # TODO(clalancette) enable linters once https://github.com/ament/ament_lint/pull/238 lands + find_package(ament_cmake_gtest) ament_add_gtest(test_cache_unittest test/cache_unittest.cpp) @@ -75,7 +74,7 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_time test/test_time.cpp) - if (TARGET test_time) + if(TARGET test_time) target_link_libraries(test_time tf2) endif() diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index bb6fe3911..d961be161 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -1,56 +1,54 @@ -/* - * 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. - */ +// Copyright 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 {copyright_holder} 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + /** \author Tully Foote */ -#ifndef TF2_BUFFER_CORE_H -#define TF2_BUFFER_CORE_H - -#include "LinearMath/Transform.h" -#include "transform_storage.h" +#ifndef TF2__BUFFER_CORE_H_ +#define TF2__BUFFER_CORE_H_ #include #include -#include - -//#include "geometry_msgs/TwistStamped.h" -#include "geometry_msgs/msg/transform_stamped.hpp" - -#include -#include -#include #include +#include #include +#include +#include +#include +#include +#include -#include -#include -#include +#include "LinearMath/Transform.h" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2/buffer_core_interface.h" +#include "tf2/exceptions.h" +#include "tf2/transform_storage.h" +#include "tf2/visibility_control.h" namespace tf2 { @@ -68,8 +66,8 @@ enum TransformableResult TransformFailure, }; - -static constexpr Duration BUFFER_CORE_DEFAULT_CACHE_TIME = std::chrono::seconds(10); //!< The default amount of time to cache data in seconds +//!< The default amount of time to cache data in seconds +static constexpr Duration BUFFER_CORE_DEFAULT_CACHE_TIME = std::chrono::seconds(10); /** \brief A Class which provides coordinate transforms between any two frames in a system. * @@ -93,8 +91,9 @@ class BufferCore : public BufferCoreInterface { public: /************* Constants ***********************/ + //!< Maximum graph search depth (deeper graphs will be assumed to have loops) TF2_PUBLIC - static const uint32_t MAX_GRAPH_DEPTH = 1000UL; //!< Maximum graph search depth (deeper graphs will be assumed to have loops) + static const uint32_t MAX_GRAPH_DEPTH = 1000UL; /** Constructor * \param interpolating Whether to interpolate, if this is false the closest value will be returned @@ -102,7 +101,7 @@ class BufferCore : public BufferCoreInterface * */ TF2_PUBLIC - BufferCore(tf2::Duration cache_time_ = BUFFER_CORE_DEFAULT_CACHE_TIME); + explicit BufferCore(tf2::Duration cache_time_ = BUFFER_CORE_DEFAULT_CACHE_TIME); TF2_PUBLIC virtual ~BufferCore(void); @@ -118,7 +117,9 @@ class BufferCore : public BufferCoreInterface * \return True unless an error occured */ TF2_PUBLIC - bool setTransform(const geometry_msgs::msg::TransformStamped& transform, const std::string & authority, bool is_static = false); + bool setTransform( + const geometry_msgs::msg::TransformStamped & transform, + const std::string & authority, bool is_static = false); /*********** Accessors *************/ @@ -132,16 +133,17 @@ class BufferCore : public BufferCoreInterface * tf2::ExtrapolationException, tf2::InvalidArgumentException */ TF2_PUBLIC - geometry_msgs::msg::TransformStamped - lookupTransform(const std::string& target_frame, const std::string& source_frame, - const TimePoint& time) const override; + geometry_msgs::msg::TransformStamped + lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time) const override; /** \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. + * \param fixed_frame The frame in which to assume the transform is constant in time. * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, @@ -150,9 +152,10 @@ class BufferCore : public BufferCoreInterface TF2_PUBLIC geometry_msgs::msg::TransformStamped - lookupTransform(const std::string& target_frame, const TimePoint& target_time, - const std::string& source_frame, const TimePoint& source_time, - const std::string& fixed_frame) const override; + lookupTransform( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame) const override; /** \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 @@ -177,37 +180,38 @@ class BufferCore : public BufferCoreInterface /* 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 tf2::TimePoint& time, const tf2::Duration& averaging_interval) const; + const tf::Point & reference_point, const std::string& reference_point_frame, + const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const; */ - /** \brief lookup the twist of the tracking frame with respect to the observational frame - * + /** \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. + * observation frame. * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException - * + * * New in geometry 1.1 */ /* geometry_msgs::Twist - lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, - const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const; + lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, + const tf2::TimePoint& time, const tf2::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 + * \return True if the transform is possible, false otherwise */ TF2_PUBLIC - bool canTransform(const std::string& target_frame, const std::string& source_frame, - const TimePoint& time, std::string* error_msg = NULL) const override; - + bool canTransform( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time, std::string * error_msg = NULL) const override; + /** \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 @@ -215,12 +219,13 @@ class BufferCore : public BufferCoreInterface * \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 + * \return True if the transform is possible, false otherwise */ TF2_PUBLIC - bool canTransform(const std::string& target_frame, const TimePoint& target_time, - const std::string& source_frame, const TimePoint& source_time, - const std::string& fixed_frame, std::string* error_msg = NULL) const override; + bool canTransform( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame, std::string * error_msg = NULL) const override; /** \brief Get all frames that exist in the system. */ @@ -244,34 +249,36 @@ class BufferCore : public BufferCoreInterface TF2_PUBLIC std::string allFramesAsString() const; - using TransformableCallback = std::function; + using TransformableCallback = std::function< + void (TransformableRequestHandle request_handle, const std::string & target_frame, + const std::string & source_frame, + TimePoint time, TransformableResult result)>; /// \brief Internal use only TF2_PUBLIC - TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb); + TransformableCallbackHandle addTransformableCallback(const TransformableCallback & cb); /// \brief Internal use only TF2_PUBLIC void removeTransformableCallback(TransformableCallbackHandle handle); /// \brief Internal use only TF2_PUBLIC - TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, TimePoint time); + TransformableRequestHandle addTransformableRequest( + TransformableCallbackHandle handle, + const std::string & target_frame, + const std::string & source_frame, + TimePoint time); /// \brief Internal use only TF2_PUBLIC 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. + // Tell the buffer that there are multiple threads serviciing it. + // This is useful for derived classes to know if they can block or not. TF2_PUBLIC - void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;}; + void setUsingDedicatedThread(bool value) {using_dedicated_thread_ = value;} // Get the state of using_dedicated_thread_ TF2_PUBLIC - bool isUsingDedicatedThread() const { return using_dedicated_thread_;}; - - + bool isUsingDedicatedThread() const {return using_dedicated_thread_;} /* Backwards compatability section for tf::Transformer you should not use these @@ -280,43 +287,51 @@ class BufferCore : public BufferCoreInterface /**@brief Check if a frame exists in the tree * @param frame_id_str The frame id in question */ TF2_PUBLIC - bool _frameExists(const std::string& frame_id_str) const; + 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" */ TF2_PUBLIC - bool _getParent(const std::string& frame_id, TimePoint time, std::string& parent) const; + bool _getParent(const std::string & frame_id, TimePoint time, std::string & parent) const; /** \brief A way to get a std::vector of available frame ids */ TF2_PUBLIC - void _getFrameStrings(std::vector& ids) const; + void _getFrameStrings(std::vector & ids) const; TF2_PUBLIC - CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const { - return lookupFrameNumber(frameid_str); + CompactFrameID _lookupFrameNumber(const std::string & frameid_str) const + { + return lookupFrameNumber(frameid_str); } TF2_PUBLIC - CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) { - return lookupOrInsertFrameNumber(frameid_str); + CompactFrameID _lookupOrInsertFrameNumber(const std::string & frameid_str) + { + return lookupOrInsertFrameNumber(frameid_str); } TF2_PUBLIC - tf2::TF2Error _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, TimePoint& time, std::string* error_string) const { + tf2::TF2Error _getLatestCommonTime( + CompactFrameID target_frame, CompactFrameID source_frame, + TimePoint & time, std::string * error_string) const + { std::unique_lock lock(frame_mutex_); return getLatestCommonTime(target_frame, source_frame, time, error_string); } TF2_PUBLIC - CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const { + 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 */ TF2_PUBLIC - tf2::Duration getCacheLength() { return cache_time_;} + tf2::Duration getCacheLength() {return cache_time_;} /** \brief Backwards compatabilityA way to see what frames have been cached * Useful for debugging @@ -330,23 +345,26 @@ class BufferCore : public BufferCoreInterface * Useful for debugging */ TF2_PUBLIC - void _chainAsVector(const std::string & target_frame, TimePoint target_time, const std::string & source_frame, TimePoint source_time, const std::string & fixed_frame, std::vector& output) const; + void _chainAsVector( + const std::string & target_frame, TimePoint target_time, + const std::string & source_frame, TimePoint 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. + * Useful for debugging. Use this call internally. */ - std::string allFramesAsStringNoLock() const; + 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 std::mutex frame_mutex_; @@ -362,7 +380,8 @@ class BufferCore : public BufferCoreInterface /// How long to cache transform history tf2::Duration cache_time_; - typedef std::unordered_map M_TransformableCallback; + typedef std::unordered_map M_TransformableCallback; M_TransformableCallback transformable_callbacks_; uint32_t transformable_callbacks_counter_; std::mutex transformable_callbacks_mutex_; @@ -388,15 +407,18 @@ class BufferCore : public BufferCoreInterface /************************* Internal Functions ****************************/ - bool setTransformImpl(const tf2::Transform& transform_in, const std::string frame_id, - const std::string child_frame_id, const TimePoint stamp, - const std::string& authority, bool is_static); - void lookupTransformImpl(const std::string& target_frame, const std::string& source_frame, - const TimePoint& time_in, tf2::Transform& transform, TimePoint& time_out) const; + bool setTransformImpl( + const tf2::Transform & transform_in, const std::string frame_id, + const std::string child_frame_id, const TimePoint stamp, + const std::string & authority, bool is_static); + void lookupTransformImpl( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time_in, tf2::Transform & transform, TimePoint & time_out) const; - void lookupTransformImpl(const std::string& target_frame, const TimePoint& target_time, - const std::string& source_frame, const TimePoint& source_time, - const std::string& fixed_frame, tf2::Transform& transform, TimePoint& time_out) const; + void lookupTransformImpl( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame, tf2::Transform & transform, TimePoint & time_out) const; /** \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 @@ -418,9 +440,9 @@ class BufferCore : public BufferCoreInterface * \return The CompactFrameID of the frame or 0 if not found. */ CompactFrameID validateFrameId( - const char* function_name_arg, - const std::string& frame_id, - std::string* error_msg) const; + const char * function_name_arg, + const std::string & frame_id, + std::string * error_msg) const; /** \brief Validate a frame ID format and look it up its compact ID. * Raise an exception for invalid cases. @@ -431,48 +453,56 @@ class BufferCore : public BufferCoreInterface * \raises InvalidArgumentException if the frame_id string has an invalid format * \raises LookupException if frame_id did not exist */ - CompactFrameID validateFrameId(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. Returns 0 if the frame was not found. - CompactFrameID lookupFrameNumber(const std::string& frameid_str) const; + 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); + 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; + /// 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; + 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 */ - tf2::TF2Error getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, TimePoint& time, std::string* error_string) const; + tf2::TF2Error getLatestCommonTime( + CompactFrameID target_frame, CompactFrameID source_frame, + TimePoint & time, std::string * error_string) const; template - tf2::TF2Error walkToTopParent(F& f, TimePoint time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const; + tf2::TF2Error walkToTopParent( + F & f, TimePoint 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 - tf2::TF2Error walkToTopParent(F& f, TimePoint time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const; + tf2::TF2Error walkToTopParent( + F & f, TimePoint time, CompactFrameID target_id, + CompactFrameID source_id, std::string * error_string, + std::vector * frame_chain) const; void testTransformableRequests(); // Thread safe transform check, acquire lock and call canTransformNoLock. - bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, - const TimePoint& time, std::string* error_msg) const; + bool canTransformInternal( + CompactFrameID target_id, CompactFrameID source_id, + const TimePoint & time, std::string * error_msg) const; // Actual implementation to walk the transform tree and find out if a transform exists. - bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, - const TimePoint& time, std::string* error_msg) const; - + bool canTransformNoLock( + CompactFrameID target_id, CompactFrameID source_id, + const TimePoint & 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.) + // Whether it is safe to use canTransform with a timeout. + // (If another thread is not provided it will always timeout.) bool using_dedicated_thread_; - }; +} // namespace tf2 - - - -} - -#endif //TF2_CORE_H +#endif // TF2__BUFFER_CORE_H_ diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index 989f60042..3dd7cae43 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/tf2/include/tf2/buffer_core_interface.h @@ -1,42 +1,40 @@ -/* - * Copyright (c) 2019, 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. - */ - +// Copyright 2019, 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 Open Source Robotics Foundation 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #ifndef TF2__BUFFER_CORE_INTERFACE_H_ #define TF2__BUFFER_CORE_INTERFACE_H_ #include #include -#include +#include "geometry_msgs/msg/transform_stamped.hpp" -#include -#include +#include "tf2/time.h" +#include "tf2/visibility_control.h" namespace tf2 { @@ -71,9 +69,9 @@ class BufferCoreInterface TF2_PUBLIC virtual geometry_msgs::msg::TransformStamped lookupTransform( - const std::string& target_frame, - const std::string& source_frame, - const tf2::TimePoint& time) const = 0; + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time) const = 0; /** * \brief Get the transform between two frames by frame ID assuming fixed frame. @@ -88,11 +86,11 @@ class BufferCoreInterface TF2_PUBLIC virtual geometry_msgs::msg::TransformStamped lookupTransform( - const std::string& target_frame, - const tf2::TimePoint& target_time, - const std::string& source_frame, - const tf2::TimePoint& source_time, - const std::string& fixed_frame) const = 0; + const std::string & target_frame, + const tf2::TimePoint & target_time, + const std::string & source_frame, + const tf2::TimePoint & source_time, + const std::string & fixed_frame) const = 0; /** * \brief Test if a transform is possible. @@ -106,10 +104,10 @@ class BufferCoreInterface TF2_PUBLIC virtual bool canTransform( - const std::string& target_frame, - const std::string& source_frame, - const tf2::TimePoint& time, - std::string* error_msg) const = 0; + const std::string & target_frame, + const std::string & source_frame, + const tf2::TimePoint & time, + std::string * error_msg) const = 0; /** * \brief Test if a transform is possible. @@ -125,21 +123,22 @@ class BufferCoreInterface TF2_PUBLIC virtual bool canTransform( - const std::string& target_frame, - const tf2::TimePoint& target_time, - const std::string& source_frame, - const tf2::TimePoint& source_time, - const std::string& fixed_frame, - std::string* error_msg) const = 0; + const std::string & target_frame, + const tf2::TimePoint & target_time, + const std::string & source_frame, + const tf2::TimePoint & source_time, + const std::string & fixed_frame, + std::string * error_msg) const = 0; /** * \brief Get all frames that exist in the system. + * \return all frame names in a vector. */ TF2_PUBLIC virtual std::vector getAllFrameNames() const = 0; -}; // class BufferCoreInterface +}; // class BufferCoreInterface } // namespace tf2 -#endif // TF2__BUFFER_CORE_INTERFACE_H_ +#endif // TF2__BUFFER_CORE_INTERFACE_H_ diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 69e90fcf4..7834b14c5 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -1,132 +1,142 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /** \author Tully Foote */ -#ifndef TF2_CONVERT_H -#define TF2_CONVERT_H +#ifndef TF2__CONVERT_H_ +#define TF2__CONVERT_H_ -#include +#include -#include -#include -#include -#include -#include +#include "builtin_interfaces/msg/time.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rosidl_runtime_cpp/traits.hpp" +#include "tf2/exceptions.h" +#include "tf2/impl/convert.h" +#include "tf2/transform_datatypes.h" +#include "tf2/visibility_control.h" -#include - -namespace tf2 { +namespace tf2 +{ /**\brief The templated function expected to be able to do a transform * * This is the method which tf2 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. + * \param data_in[in] The data to be transformed. + * \param data_out[inout] A reference to the output data. Note this can point to data in and the method should be mutation safe. + * \param transform[in] 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 geometry_msgs::msg::TransformStamped& transform); +template +void doTransform( + const T & data_in, T & data_out, + const geometry_msgs::msg::TransformStamped & transform); /**\brief Get the timestamp from data - * \param t The data input. + * \param[in] t The data input. * \return The timestamp associated with the data. */ -template - tf2::TimePoint getTimestamp(const T& t); +template +tf2::TimePoint getTimestamp(const T & t); /**\brief Get the frame_id from data - * \param t The data input. + * \param[in] t The data input. * \return The frame_id associated with the data. */ -template - std::string getFrameId(const T& t); - - +template +std::string getFrameId(const T & t); -/* An implementation for Stamped

datatypes */ -template - tf2::TimePoint getTimestamp(const tf2::Stamped

& t) - { - return t.stamp_; - } +/**\brief Get the frame_id from data + * + * An implementation for Stamped

datatypes. + * + * \param[in] t The data input. + * \return The frame_id associated with the data. + */ +template +tf2::TimePoint getTimestamp(const tf2::Stamped

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

datatypes */ -template - std::string getFrameId(const tf2::Stamped

& t) - { - return t.frame_id_; - } +/**\brief Get the frame_id from data + * + * An implementation for Stamped

datatypes. + * + * \param[in] t The data input. + * \return The frame_id associated with the data. + */ +template +std::string getFrameId(const tf2::Stamped

& t) +{ + return t.frame_id_; +} -/** Function that converts from one type to a ROS message type. It has to be +/**\brief Function that converts from one type to a ROS message type. It has to be * implemented by each data type in tf2_* (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); +B toMsg(const A & a); -/** Function that converts from a ROS message type to another type. It has to be +/**\brief Function that converts from a ROS message type to another type. It has to be * implemented by each data type in tf2_* (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); +void fromMsg(const A &, B & b); -/** Function that converts any type to any type (messages or not). +/**\brief 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) +template +void convert(const A & a, B & b) { impl::Converter::value, - rosidl_generator_traits::is_message::value>::convert(a, b); + rosidl_generator_traits::is_message::value>::convert(a, b); } -template -void convert(const A& a1, A& a2) +template +void convert(const A & a1, A & a2) { - if(&a1 != &a2) + if (&a1 != &a2) { a2 = a1; + } } +} // namespace tf2 - -} - -#endif //TF2_CONVERT_H +#endif // TF2__CONVERT_H_ diff --git a/tf2/include/tf2/exceptions.h b/tf2/include/tf2/exceptions.h index 68f5df309..c516da857 100644 --- a/tf2/include/tf2/exceptions.h +++ b/tf2/include/tf2/exceptions.h @@ -1,45 +1,47 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /** \author Tully Foote */ -#ifndef TF2_EXCEPTIONS_H -#define TF2_EXCEPTIONS_H +#ifndef TF2__EXCEPTIONS_H_ +#define TF2__EXCEPTIONS_H_ #include #include +#include -#include +#include "tf2/visibility_control.h" -namespace tf2{ +namespace tf2 +{ -enum class TF2Error : std::uint8_t { +enum class TF2Error : std::uint8_t +{ NO_ERROR = 0, LOOKUP_ERROR = 1, CONNECTIVITY_ERROR = 2, @@ -49,82 +51,93 @@ enum class TF2Error : std::uint8_t { TRANSFORM_ERROR = 6 }; - -/** \brief A base class for all tf2 exceptions - * This inherits from ros::exception +/** \brief A base class for all tf2 exceptions + * This inherits from ros::exception * which inherits from std::runtime_exception */ -class TransformException: public std::runtime_error -{ +class TransformException : public std::runtime_error +{ public: TF2_PUBLIC - TransformException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; }; + explicit 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 -{ +/** \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: TF2_PUBLIC - ConnectivityException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; + explicit ConnectivityException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } }; -/** \brief An exception class to notify of bad frame number - * - * This is an exception class to be thrown in the case that +/** \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. + * being published, or a parent frame was not set correctly + * causing the tree to be broken. */ -class LookupException: public TransformException -{ +class LookupException : public TransformException +{ public: TF2_PUBLIC - LookupException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; + explicit LookupException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } }; - /** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits. - * - */ -class ExtrapolationException: public TransformException -{ +/** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits. + * + */ +class ExtrapolationException : public TransformException +{ public: TF2_PUBLIC - ExtrapolationException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; + explicit ExtrapolationException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } }; /** \brief An exception class to notify that one of the arguments is invalid - * + * * usually it's an uninitalized Quaternion (0,0,0,0) - * + * */ -class InvalidArgumentException: public TransformException -{ +class InvalidArgumentException : public TransformException +{ public: TF2_PUBLIC - InvalidArgumentException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; + explicit InvalidArgumentException(const std::string errorDescription) + : tf2::TransformException(errorDescription) {} }; /** \brief An exception class to notify that a timeout has occured - * - * + * + * */ -class TimeoutException: public TransformException -{ +class TimeoutException : public TransformException +{ public: TF2_PUBLIC - TimeoutException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; + explicit TimeoutException(const std::string errorDescription) + : tf2::TransformException(errorDescription) + { + } }; - - -} - - -#endif //TF2_EXCEPTIONS_H +} // namespace tf2 +#endif // TF2__EXCEPTIONS_H_ diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index bc76640ef..3821564a0 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -1,43 +1,45 @@ -/* - * 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. - */ +// Copyright 2013, 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 Open Source Robotics Foundation 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. -#ifndef TF2_IMPL_CONVERT_H -#define TF2_IMPL_CONVERT_H +#ifndef TF2__IMPL__CONVERT_H_ +#define TF2__IMPL__CONVERT_H_ -namespace tf2 { -namespace impl { +namespace tf2 +{ +namespace impl +{ -template -class Converter { +template +class Converter +{ public: template - static void convert(const A& a, B& b); + static void convert(const A & a, B & b); }; // The case where both A and B are messages should not happen: if you have two @@ -47,32 +49,32 @@ class Converter { // 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); -template <> -template -inline void Converter::convert(const A& a, B& b) +template< > +template +inline void Converter::convert(const A & a, B & b) { fromMsg(a, b); } -template <> -template -inline void Converter::convert(const A& a, B& b) +template< > +template +inline void Converter::convert(const A & a, B & b) { b = toMsg(a); } -template <> -template -inline void Converter::convert(const A& a, B& b) +template< > +template +inline void Converter::convert(const A & a, B & b) { fromMsg(toMsg(a), b); } -} -} +} // namespace impl +} // namespace tf2 -#endif //TF2_IMPL_CONVERT_H +#endif // TF2__IMPL__CONVERT_H_ diff --git a/tf2/include/tf2/impl/utils.h b/tf2/include/tf2/impl/utils.h index 5b8599c78..bfeabf922 100644 --- a/tf2/include/tf2/impl/utils.h +++ b/tf2/include/tf2/impl/utils.h @@ -12,56 +12,63 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TF2_IMPL_UTILS_H -#define TF2_IMPL_UTILS_H +#ifndef TF2__IMPL__UTILS_H_ +#define TF2__IMPL__UTILS_H_ #include #include #include -namespace tf2 { -namespace impl { +namespace tf2 +{ +namespace impl +{ /** Function needed for the generalization of toQuaternion * \param q a tf2::Quaternion * \return a copy of the same quaternion */ inline -tf2::Quaternion toQuaternion(const tf2::Quaternion& q) { - return q; - } +tf2::Quaternion toQuaternion(const tf2::Quaternion & q) +{ + return q; +} /** Function needed for the generalization of toQuaternion * \param q a geometry_msgs::msg::Quaternion * \return a copy of the same quaternion as a tf2::Quaternion */ inline -tf2::Quaternion toQuaternion(const geometry_msgs::msg::Quaternion& q) { - tf2::Quaternion res; - fromMsg(q, res); - return res; - } +tf2::Quaternion toQuaternion(const geometry_msgs::msg::Quaternion & q) +{ + tf2::Quaternion res; + fromMsg(q, res); + return res; +} /** Function needed for the generalization of toQuaternion * \param q a geometry_msgs::msg::QuaternionStamped * \return a copy of the same quaternion as a tf2::Quaternion */ inline -tf2::Quaternion toQuaternion(const geometry_msgs::msg::QuaternionStamped& q) { - tf2::Quaternion res; - fromMsg(q.quaternion, res); - return res; - } +tf2::Quaternion toQuaternion(const geometry_msgs::msg::QuaternionStamped & q) +{ + tf2::Quaternion res; + fromMsg(q.quaternion, res); + return res; +} /** Function needed for the generalization of toQuaternion * \param t some tf2::Stamped object * \return a copy of the same quaternion as a tf2::Quaternion */ template - tf2::Quaternion toQuaternion(const tf2::Stamped& t) { - geometry_msgs::msg::QuaternionStamped q = toMsg, geometry_msgs::msg::QuaternionStamped>(t); - return toQuaternion(q); - } +tf2::Quaternion toQuaternion(const tf2::Stamped & t) +{ + geometry_msgs::msg::QuaternionStamped q = toMsg, + geometry_msgs::msg::QuaternionStamped>(t); + return toQuaternion(q); +} /** Generic version of toQuaternion. It tries to convert the argument * to a geometry_msgs::msg::Quaternion @@ -69,10 +76,11 @@ template * \return a copy of the same quaternion as a tf2::Quaternion */ template - tf2::Quaternion toQuaternion(const T& t) { - geometry_msgs::msg::Quaternion q = toMsg(t); - return toQuaternion(q); - } +tf2::Quaternion toQuaternion(const T & t) +{ + geometry_msgs::msg::Quaternion q = toMsg(t); + return toQuaternion(q); +} /** The code below is blantantly copied from urdfdom_headers * only the normalization has been added. @@ -84,7 +92,7 @@ template * \param roll the computed roll */ inline -void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll) +void getEulerYPR(const tf2::Quaternion & q, double & yaw, double & pitch, double & roll) { const double pi_2 = 1.57079632679489661923; double sqw; @@ -98,19 +106,20 @@ void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &r 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 */ + // normalization added from urdfom_headers + double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw); if (sarg <= -0.99999) { - pitch = -0.5*pi_2; - roll = 0; - yaw = -2 * atan2(q.y(), q.x()); + pitch = -0.5 * pi_2; + roll = 0; + yaw = -2 * atan2(q.y(), q.x()); } else if (sarg >= 0.99999) { - pitch = 0.5*pi_2; - roll = 0; - yaw = 2 * atan2(q.y(), q.x()); + pitch = 0.5 * pi_2; + 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); + 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); } } @@ -121,7 +130,7 @@ void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &r * \return the computed yaw */ inline -double getYaw(const tf2::Quaternion& q) +double getYaw(const tf2::Quaternion & q) { double yaw; @@ -136,19 +145,19 @@ double getYaw(const tf2::Quaternion& q) 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 */ + // normalization added from urdfom_headers + double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw); if (sarg <= -0.99999) { - yaw = -2 * atan2(q.y(), q.x()); + yaw = -2 * atan2(q.y(), q.x()); } else if (sarg >= 0.99999) { - yaw = 2 * atan2(q.y(), q.x()); + yaw = 2 * atan2(q.y(), q.x()); } else { - yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); + yaw = atan2(2 * (q.x() * q.y() + q.w() * q.z()), sqw + sqx - sqy - sqz); } return yaw; } -} -} - -#endif //TF2_IMPL_UTILS_H +} // namespace impl +} // namespace tf2 +#endif // TF2__IMPL__UTILS_H_ diff --git a/tf2/include/tf2/time.h b/tf2/include/tf2/time.h index a35eb3b50..4d41a5119 100644 --- a/tf2/include/tf2/time.h +++ b/tf2/include/tf2/time.h @@ -1,97 +1,93 @@ -/* - * Copyright (c) 2015-2016, 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. - */ - - #ifndef TF2_TIME_H - #define TF2_TIME_H +// Copyright 2015-2016, 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 Open Source Robotics Foundation 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TF2__TIME_H_ +#define TF2__TIME_H_ #include #include -#include #include -#include -#include -#include +#include "tf2/visibility_control.h" namespace tf2 { - using Duration = std::chrono::nanoseconds; - using TimePoint = std::chrono::time_point; - +using Duration = std::chrono::nanoseconds; +using TimePoint = std::chrono::time_point; - using IDuration = std::chrono::duration; - // This is the zero time in ROS - static const TimePoint TimePointZero = TimePoint(IDuration::zero()); +using IDuration = std::chrono::duration; +// This is the zero time in ROS +static const TimePoint TimePointZero = TimePoint(IDuration::zero()); - inline TimePoint get_now() - { - return std::chrono::system_clock::now(); - } +inline TimePoint get_now() +{ + return std::chrono::system_clock::now(); +} - inline Duration durationFromSec(double t_sec) - { - int32_t sec, nsec; - sec = static_cast(floor(t_sec)); - nsec = static_cast(std::round((t_sec - sec) * 1e9)); - // avoid rounding errors - sec += (nsec / 1000000000l); - nsec %= 1000000000l; - return std::chrono::seconds(sec) + std::chrono::nanoseconds(nsec); - } +inline Duration durationFromSec(double t_sec) +{ + int32_t sec, nsec; + sec = static_cast(floor(t_sec)); + nsec = static_cast(std::round((t_sec - sec) * 1e9)); + // avoid rounding errors + sec += (nsec / 1000000000l); + nsec %= 1000000000l; + return std::chrono::seconds(sec) + std::chrono::nanoseconds(nsec); +} - inline TimePoint timeFromSec(double t_sec) - { - return tf2::TimePoint(durationFromSec(t_sec)); - } +inline TimePoint timeFromSec(double t_sec) +{ + return tf2::TimePoint(durationFromSec(t_sec)); +} - inline double durationToSec(const tf2::Duration& input){ - int64_t count = input.count(); +inline double durationToSec(const tf2::Duration & input) +{ + int64_t count = input.count(); - // scale the nanoseconds separately for improved accuracy - int32_t sec, nsec; - nsec = static_cast(count % 1000000000l); - sec = static_cast((count - nsec) / 1000000000l); + // scale the nanoseconds separately for improved accuracy + int32_t sec, nsec; + nsec = static_cast(count % 1000000000l); + sec = static_cast((count - nsec) / 1000000000l); - double sec_double, nsec_double; - nsec_double = 1e-9 * static_cast(nsec); - sec_double = static_cast(sec); - return sec_double + nsec_double; - } + double sec_double, nsec_double; + nsec_double = 1e-9 * static_cast(nsec); + sec_double = static_cast(sec); + return sec_double + nsec_double; +} - inline double timeToSec(const TimePoint& timepoint) - { - return durationToSec(Duration(timepoint.time_since_epoch())); - } +inline double timeToSec(const TimePoint & timepoint) +{ + return durationToSec(Duration(timepoint.time_since_epoch())); +} - TF2_PUBLIC - std::string displayTimePoint(const TimePoint& stamp); +TF2_PUBLIC +std::string displayTimePoint(const TimePoint & stamp); -} +} // namespace tf2 -#endif // TF2_TIME_H +#endif // TF2__TIME_H_ diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index dc03c5a65..c9be7d523 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -1,49 +1,48 @@ -/* - * 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. - */ - +// Copyright 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 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /** \author Tully Foote */ -#ifndef TF2_TIME_CACHE_H -#define TF2_TIME_CACHE_H - -#include "transform_storage.h" +#ifndef TF2__TIME_CACHE_H_ +#define TF2__TIME_CACHE_H_ +#include #include #include #include +#include +#include -#include +#include "tf2/visibility_control.h" +#include "tf2/transform_storage.h" namespace tf2 { - -typedef std::pair P_TimeAndFrameID; +typedef std::pair P_TimeAndFrameID; class TimeCacheInterface { @@ -51,21 +50,25 @@ class TimeCacheInterface TF2_PUBLIC virtual ~TimeCacheInterface() = default; - /** \brief Access data from the cache */ + /** \brief Access data from the cache + * returns false if data unavailable (should be thrown as lookup exception) + */ TF2_PUBLIC - virtual bool getData(TimePoint time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception + virtual bool getData( + tf2::TimePoint time, tf2::TransformStorage & data_out, + std::string * error_str = 0) = 0; /** \brief Insert data into the cache */ TF2_PUBLIC - virtual bool insertData(const TransformStorage& new_data)=0; + virtual bool insertData(const tf2::TransformStorage & new_data) = 0; /** @brief Clear the list of stored values */ TF2_PUBLIC - virtual void clearList()=0; + virtual void clearList() = 0; /** \brief Retrieve the parent at a specific time */ TF2_PUBLIC - virtual CompactFrameID getParent(TimePoint time, std::string* error_str) = 0; + virtual CompactFrameID getParent(tf2::TimePoint 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. @@ -73,24 +76,24 @@ class TimeCacheInterface TF2_PUBLIC virtual P_TimeAndFrameID getLatestTimeAndParent() = 0; - /// Debugging information methods /** @brief Get the length of the stored list */ TF2_PUBLIC - virtual unsigned int getListLength()=0; + virtual unsigned int getListLength() = 0; /** @brief Get the latest timestamp cached */ TF2_PUBLIC - virtual TimePoint getLatestTimestamp()=0; + virtual tf2::TimePoint getLatestTimestamp() = 0; /** @brief Get the oldest timestamp cached */ TF2_PUBLIC - virtual TimePoint getOldestTimestamp()=0; + virtual tf2::TimePoint getOldestTimestamp() = 0; }; using TimeCacheInterfacePtr = std::shared_ptr; -constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10); //!< default value of 10 seconds storage +/// default value of 10 seconds storage +constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10); /** \brief A class to keep a sorted linked list in time * This builds and maintains a list of timestamped @@ -98,26 +101,29 @@ constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::second * data out as a function of time. */ class TimeCache : public TimeCacheInterface { - public: - TF2_PUBLIC - static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below. +public: + /// Number of nano-seconds to not interpolate below. TF2_PUBLIC - 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 int MIN_INTERPOLATION_DISTANCE = 5; + /// Maximum length of linked list, to make sure not to be able to use unlimited memory. TF2_PUBLIC - TimeCache(tf2::Duration max_storage_time = TIMECACHE_DEFAULT_MAX_STORAGE_TIME); - + static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; + TF2_PUBLIC + explicit TimeCache(tf2::Duration max_storage_time = TIMECACHE_DEFAULT_MAX_STORAGE_TIME); /// Virtual methods TF2_PUBLIC - virtual bool getData(TimePoint time, TransformStorage & data_out, std::string* error_str = 0); + virtual bool getData( + tf2::TimePoint time, tf2::TransformStorage & data_out, + std::string * error_str = 0); TF2_PUBLIC - virtual bool insertData(const TransformStorage& new_data); + virtual bool insertData(const tf2::TransformStorage & new_data); TF2_PUBLIC virtual void clearList(); TF2_PUBLIC - virtual CompactFrameID getParent(TimePoint time, std::string* error_str); + virtual tf2::CompactFrameID getParent(tf2::TimePoint time, std::string * error_str); TF2_PUBLIC virtual P_TimeAndFrameID getLatestTimeAndParent(); @@ -128,7 +134,6 @@ class TimeCache : public TimeCacheInterface virtual TimePoint getLatestTimestamp(); TF2_PUBLIC virtual TimePoint getOldestTimestamp(); - private: typedef std::list L_TransformStorage; @@ -137,36 +142,35 @@ class TimeCache : public TimeCacheInterface tf2::Duration max_storage_time_; - /// A helper function for getData - //Assumes storage is already locked for it - inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, TimePoint target_time, std::string* error_str); - - inline void interpolate(const TransformStorage& one, const TransformStorage& two, TimePoint time, TransformStorage& output); + // A helper function for getData + // Assumes storage is already locked for it + inline uint8_t findClosest( + tf2::TransformStorage * & one, TransformStorage * & two, + tf2::TimePoint target_time, std::string * error_str); + inline void interpolate( + const tf2::TransformStorage & one, const tf2::TransformStorage & two, + tf2::TimePoint time, tf2::TransformStorage & output); void pruneList(); - - - }; class StaticCache : public TimeCacheInterface { - public: +public: /// Virtual methods - TF2_PUBLIC - virtual bool getData(TimePoint time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception + virtual bool getData(TimePoint time, TransformStorage & data_out, std::string * error_str = 0); + // returns false if data unavailable (should be thrown as lookup exception TF2_PUBLIC - virtual bool insertData(const TransformStorage& new_data); + virtual bool insertData(const TransformStorage & new_data); TF2_PUBLIC virtual void clearList(); TF2_PUBLIC - virtual CompactFrameID getParent(TimePoint time, std::string* error_str); + virtual CompactFrameID getParent(TimePoint time, std::string * error_str); TF2_PUBLIC virtual P_TimeAndFrameID getLatestTimeAndParent(); - /// Debugging information methods TF2_PUBLIC virtual unsigned int getListLength(); @@ -174,12 +178,9 @@ class StaticCache : public TimeCacheInterface virtual TimePoint getLatestTimestamp(); TF2_PUBLIC virtual TimePoint getOldestTimestamp(); - private: - TransformStorage storage_; + TransformStorage storage_; }; - -} - -#endif // TF2_TIME_CACHE_H +} // namespace tf2 +#endif // TF2__TIME_CACHE_H_ diff --git a/tf2/include/tf2/transform_datatypes.h b/tf2/include/tf2/transform_datatypes.h index 75c55a1a0..0dfcd348f 100644 --- a/tf2/include/tf2/transform_datatypes.h +++ b/tf2/include/tf2/transform_datatypes.h @@ -1,69 +1,77 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /** \author Tully Foote */ -#ifndef TF2_TRANSFORM_DATATYPES_H -#define TF2_TRANSFORM_DATATYPES_H +#ifndef TF2__TRANSFORM_DATATYPES_H_ +#define TF2__TRANSFORM_DATATYPES_H_ #include #include -#include + +#include "tf2/time.h" namespace tf2 { /** \brief The data type which will be cross compatable with geometry_msgs * This is the tf2 datatype equivilant of a MessageStamped */ -template -class Stamped : public T{ - public: - TimePoint stamp_; ///< The timestamp associated with this data - std::string frame_id_; ///< The frame_id associated this data +template +class Stamped : public T +{ +public: + TimePoint 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 + Stamped() + : frame_id_("NO_ID_STAMPED_DEFAULT_CONSTRUCTION") + { + } /** Full constructor */ - Stamped(const T& input, const TimePoint& timestamp, const std::string & frame_id) : - T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ; - + Stamped(const T & input, const TimePoint & timestamp, const std::string & frame_id) + : T(input), stamp_(timestamp), frame_id_(frame_id) + { + } + /** Copy Constructor */ - Stamped(const Stamped& s): - T (s), + Stamped(const Stamped & s) + : T(s), stamp_(s.stamp_), - frame_id_(s.frame_id_) {} - + frame_id_(s.frame_id_) + { + } + /** Set the data element */ - void setData(const T& input){*static_cast(this) = input;}; + void setData(const T & input) {*static_cast(this) = input;} - Stamped& operator=(const Stamped& s) + Stamped & operator=(const Stamped & s) { T::operator=(s); this->stamp_ = s.stamp_; @@ -73,11 +81,13 @@ class Stamped : public T{ }; /** \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); +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); } +} // namespace tf2 -} -#endif //TF2_TRANSFORM_DATATYPES_H +#endif // TF2__TRANSFORM_DATATYPES_H_ diff --git a/tf2/include/tf2/transform_storage.h b/tf2/include/tf2/transform_storage.h index 0f342ca8e..34d87e6b1 100644 --- a/tf2/include/tf2/transform_storage.h +++ b/tf2/include/tf2/transform_storage.h @@ -1,48 +1,42 @@ -/* - * 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. - */ - +// Copyright 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 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /** \author Tully Foote */ -#ifndef TF2_TRANSFORM_STORAGE_H -#define TF2_TRANSFORM_STORAGE_H - -#include -#include +#ifndef TF2__TRANSFORM_STORAGE_H_ +#define TF2__TRANSFORM_STORAGE_H_ +#include "tf2/LinearMath/Vector3.h" +#include "tf2/LinearMath/Quaternion.h" #include "tf2/time.h" -#include +#include "tf2/visibility_control.h" namespace tf2 { - - - typedef uint32_t CompactFrameID; /** \brief Storage for transforms and their parent */ @@ -52,17 +46,18 @@ class TransformStorage TF2_PUBLIC TransformStorage(); TF2_PUBLIC - TransformStorage(const TimePoint& stamp, const Quaternion& q, const Vector3& t, CompactFrameID frame_id, - CompactFrameID child_frame_id); + TransformStorage( + const TimePoint & stamp, const Quaternion & q, const Vector3 & t, CompactFrameID frame_id, + CompactFrameID child_frame_id); TF2_PUBLIC - TransformStorage(const TransformStorage& rhs) + TransformStorage(const TransformStorage & rhs) { *this = rhs; } TF2_PUBLIC - TransformStorage& operator=(const TransformStorage& rhs) + TransformStorage & operator=(const TransformStorage & rhs) { #if 01 rotation_ = rhs.rotation_; @@ -80,7 +75,5 @@ class TransformStorage CompactFrameID frame_id_; CompactFrameID child_frame_id_; }; - -} - -#endif // TF2_TRANSFORM_STORAGE_H +} // namespace tf2 +#endif // TF2__TRANSFORM_STORAGE_H_ diff --git a/tf2/include/tf2/utils.h b/tf2/include/tf2/utils.h index 0ce39a5d0..09a0e2d0f 100644 --- a/tf2/include/tf2/utils.h +++ b/tf2/include/tf2/utils.h @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TF2_UTILS_H -#define TF2_UTILS_H +#ifndef TF2__UTILS_H_ +#define TF2__UTILS_H_ #include #include #include #include -namespace tf2 { +namespace tf2 +{ /** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h * \param a the object to get data from (it represents a rotation/quaternion) @@ -28,12 +29,12 @@ namespace tf2 { * \param pitch pitch * \param roll roll */ -template - void getEulerYPR(const A& a, double& yaw, double& pitch, double& roll) - { - tf2::Quaternion q = impl::toQuaternion(a); - impl::getEulerYPR(q, yaw, pitch, roll); - } +template +void getEulerYPR(const A & a, double & yaw, double & pitch, double & roll) +{ + tf2::Quaternion q = impl::toQuaternion(a); + impl::getEulerYPR(q, yaw, pitch, roll); +} /** Return the yaw of anything that can be converted to a tf2::Quaternion * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h @@ -42,26 +43,24 @@ template * \param a the object to get data from (it represents a rotation/quaternion) * \param yaw yaw */ -template - double getYaw(const A& a) - { - tf2::Quaternion q = impl::toQuaternion(a); - return impl::getYaw(q); - } +template +double getYaw(const A & a) +{ + tf2::Quaternion q = impl::toQuaternion(a); + return impl::getYaw(q); +} /** Return the identity for any type that can be converted to a tf2::Transform * \return an object of class A that is an identity transform */ -template - A getTransformIdentity() - { - tf2::Transform t; - t.setIdentity(); - A a; - convert(t, a); - return a; - } - +template +A getTransformIdentity() +{ + tf2::Transform t; + t.setIdentity(); + A a; + convert(t, a); + return a; } - -#endif //TF2_UTILS_H +} // namespace tf2 +#endif // TF2__UTILS_H_ diff --git a/tf2/include/tf2/visibility_control.h b/tf2/include/tf2/visibility_control.h index 0f75bcee3..cb4611891 100644 --- a/tf2/include/tf2/visibility_control.h +++ b/tf2/include/tf2/visibility_control.h @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2017 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. - */ +// Copyright 2017, 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 Open Source Robotics Foundation 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #ifndef TF2__VISIBILITY_CONTROL_H_ #define TF2__VISIBILITY_CONTROL_H_ diff --git a/tf2/mainpage.dox b/tf2/mainpage.dox index 2cba60ce8..551d4837e 100644 --- a/tf2/mainpage.dox +++ b/tf2/mainpage.dox @@ -10,10 +10,10 @@ There is also a Python wrapper with the same API that class this library using C \section codeapi Code API -The main interface is through the tf2::BufferCore interface. +The main interface is through the tf2::BufferCore interface. -It uses the exceptions in exceptions.h and the Stamped datatype -in transform_datatypes.h. +It uses the exceptions in exceptions.h and the Stamped datatype +in transform_datatypes.h. \section conversions Conversion Interface @@ -33,4 +33,6 @@ Some packages that implement this interface: More documentation for the conversion interface is available on the ROS Wiki. +Some tutorials are available on https://index.ros.org/. + */ diff --git a/tf2/package.xml b/tf2/package.xml index 560ae13f6..755f922e9 100644 --- a/tf2/package.xml +++ b/tf2/package.xml @@ -26,9 +26,8 @@ rcutils ament_cmake_gtest - + ament_lint_common ament_cmake diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 25bbcb05f..143bd00ed 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -1,54 +1,67 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER 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 + +#include +#include +#include +#include +#include + #include "tf2/buffer_core.h" #include "tf2/time_cache.h" #include "tf2/exceptions.h" -#include -#include +#include "console_bridge/console.h" #include "tf2/LinearMath/Transform.h" namespace tf2 { // Tolerance for acceptable quaternion normalization -static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3; +constexpr static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3; /** \brief convert Transform msg to Transform */ -void transformMsgToTF2(const geometry_msgs::msg::Transform& msg, tf2::Transform& tf2) -{tf2 = tf2::Transform(tf2::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z));} +void transformMsgToTF2(const geometry_msgs::msg::Transform & msg, tf2::Transform & tf2) +{ + tf2 = + tf2::Transform( + tf2::Quaternion( + msg.rotation.x, msg.rotation.y, msg.rotation.z, + msg.rotation.w), + tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z)); +} /** \brief convert Transform to Transform msg*/ -void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::msg::Transform& msg) +void transformTF2ToMsg(const tf2::Transform & tf2, geometry_msgs::msg::Transform & msg) { msg.translation.x = tf2.getOrigin().x(); msg.translation.y = tf2.getOrigin().y(); @@ -60,7 +73,10 @@ void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::msg::Transform& } /** \brief convert Transform to Transform msg*/ -void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::msg::TransformStamped& msg, builtin_interfaces::msg::Time stamp, const std::string& frame_id, const std::string& child_frame_id) +void transformTF2ToMsg( + const tf2::Transform & tf2, geometry_msgs::msg::TransformStamped & msg, + builtin_interfaces::msg::Time stamp, const std::string & frame_id, + const std::string & child_frame_id) { transformTF2ToMsg(tf2, msg.transform); msg.header.stamp = stamp; @@ -68,7 +84,9 @@ void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::msg::TransformS msg.child_frame_id = child_frame_id; } -void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::msg::Transform& msg) +void transformTF2ToMsg( + const tf2::Quaternion & orient, const tf2::Vector3 & pos, + geometry_msgs::msg::Transform & msg) { msg.translation.x = pos.x(); msg.translation.y = pos.y(); @@ -79,7 +97,11 @@ void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, g msg.rotation.w = orient.w(); } -void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::msg::TransformStamped& msg, builtin_interfaces::msg::Time stamp, const std::string& frame_id, const std::string& child_frame_id) +void transformTF2ToMsg( + const tf2::Quaternion & orient, const tf2::Vector3 & pos, + geometry_msgs::msg::TransformStamped & msg, + builtin_interfaces::msg::Time stamp, const std::string & frame_id, + const std::string & child_frame_id) { transformTF2ToMsg(orient, pos, msg.transform); msg.header.stamp = stamp; @@ -87,7 +109,7 @@ void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, g msg.child_frame_id = child_frame_id; } -void setIdentity(geometry_msgs::msg::Transform& tx) +void setIdentity(geometry_msgs::msg::Transform & tx) { tx.translation.x = 0; tx.translation.y = 0; @@ -98,38 +120,39 @@ void setIdentity(geometry_msgs::msg::Transform& tx) tx.rotation.w = 1; } -bool startsWithSlash(const std::string& frame_id) +bool startsWithSlash(const std::string & frame_id) { - if (frame_id.size() > 0) - if (frame_id[0] == '/') + if (frame_id.size() > 0) { + if (frame_id[0] == '/') { return true; + } + } return false; } -std::string stripSlash(const std::string& in) +std::string stripSlash(const std::string & in) { std::string out = in; - if (startsWithSlash(in)) - out.erase(0,1); + if (startsWithSlash(in)) { + out.erase(0, 1); + } return out; } -namespace { +namespace +{ void fillOrWarnMessageForInvalidFrame( - const char* function_name_arg, - const std::string& frame_id, - std::string* error_msg, - const char* rationale) + const char * function_name_arg, + const std::string & frame_id, + std::string * error_msg, + const char * rationale) { std::string s = "Invalid frame ID \"" + frame_id + - "\" passed to " + function_name_arg + " - " + rationale; - if (error_msg != nullptr) - { + "\" passed to " + function_name_arg + " - " + rationale; + if (error_msg != nullptr) { *error_msg = s; - } - else - { + } else { CONSOLE_BRIDGE_logWarn("%s", s.c_str()); } } @@ -137,19 +160,17 @@ void fillOrWarnMessageForInvalidFrame( } // anonymous namespace CompactFrameID BufferCore::validateFrameId( - const char* function_name_arg, - const std::string& frame_id, - std::string* error_msg) const + const char * function_name_arg, + const std::string & frame_id, + std::string * error_msg) const { - if (frame_id.empty()) - { + if (frame_id.empty()) { fillOrWarnMessageForInvalidFrame( function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot be empty"); return 0; } - if (startsWithSlash(frame_id)) - { + if (startsWithSlash(frame_id)) { fillOrWarnMessageForInvalidFrame( function_name_arg, frame_id, error_msg, "in tf2 frame_ids cannot start with a '/'"); return 0; @@ -163,173 +184,187 @@ CompactFrameID BufferCore::validateFrameId( return id; } -CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const +CompactFrameID BufferCore::validateFrameId( + const char * function_name_arg, + const std::string & frame_id) const { - if (frame_id.empty()) - { - std::string error_msg = "Invalid argument \"" + frame_id + "\" passed to " + function_name_arg + " - in tf2 frame_ids cannot be empty"; + if (frame_id.empty()) { + std::string error_msg = "Invalid argument \"" + frame_id + "\" passed to " + function_name_arg + + " - in tf2 frame_ids cannot be empty"; throw tf2::InvalidArgumentException(error_msg.c_str()); } - if (startsWithSlash(frame_id)) - { - std::string error_msg = "Invalid argument \"" + frame_id + "\" passed to " + function_name_arg + " - in tf2 frame_ids cannot start with a '/'"; + if (startsWithSlash(frame_id)) { + std::string error_msg = "Invalid argument \"" + frame_id + "\" passed to " + function_name_arg + + " - in tf2 frame_ids cannot start with a '/'"; throw tf2::InvalidArgumentException(error_msg.c_str()); } CompactFrameID id = lookupFrameNumber(frame_id); - if (id == 0) - { - std::string error_msg = "\"" + frame_id + "\" passed to " + function_name_arg + " does not exist. "; + if (id == 0) { + std::string error_msg = "\"" + frame_id + "\" passed to " + function_name_arg + + " does not exist. "; throw tf2::LookupException(error_msg.c_str()); } - + return id; } BufferCore::BufferCore(tf2::Duration cache_time) -: cache_time_(cache_time) -, transformable_callbacks_counter_(0) -, transformable_requests_counter_(0) -, using_dedicated_thread_(false) +: cache_time_(cache_time), + transformable_callbacks_counter_(0), + transformable_requests_counter_(0), + using_dedicated_thread_(false) { frameIDs_["NO_PARENT"] = 0; frames_.push_back(TimeCacheInterfacePtr()); frameIDs_reverse.push_back("NO_PARENT"); } -BufferCore::~BufferCore() -{ - -} +BufferCore::~BufferCore() {} void BufferCore::clear() { - //old_tf_.clear(); - - std::unique_lock lock(frame_mutex_); - if ( frames_.size() > 1 ) - { - for (std::vector::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it) + if (frames_.size() > 1) { + for (std::vector::iterator cache_it = frames_.begin() + 1; + cache_it != frames_.end(); ++cache_it) { - if (*cache_it) + if (*cache_it) { (*cache_it)->clearList(); + } } } - } -bool BufferCore::setTransform(const geometry_msgs::msg::TransformStamped& transform, const std::string & authority, bool is_static) +bool BufferCore::setTransform( + const geometry_msgs::msg::TransformStamped & transform, + const std::string & authority, bool is_static) { - tf2::Transform tf2_transform(tf2::Quaternion(transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z, - transform.transform.rotation.w), - tf2::Vector3(transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z)); + tf2::Transform tf2_transform(tf2::Quaternion( + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w), + tf2::Vector3( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z)); TimePoint time_point(std::chrono::nanoseconds(transform.header.stamp.nanosec) + - std::chrono::duration_cast(std::chrono::seconds(transform.header.stamp.sec))); - return setTransformImpl(tf2_transform, transform.header.frame_id, transform.child_frame_id, - time_point, authority, is_static); + std::chrono::duration_cast( + std::chrono::seconds( + transform.header.stamp.sec))); + return setTransformImpl( + tf2_transform, transform.header.frame_id, transform.child_frame_id, + time_point, authority, is_static); } -bool BufferCore::setTransformImpl(const tf2::Transform& transform_in, const std::string frame_id, - const std::string child_frame_id, const TimePoint stamp, - const std::string& authority, bool is_static) +bool BufferCore::setTransformImpl( + const tf2::Transform & transform_in, const std::string frame_id, + const std::string child_frame_id, const TimePoint stamp, + const std::string & authority, bool is_static) { - - /////BACKEARDS COMPATABILITY + // BACKWARDS 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"); - }*/ + printf("Warning old setTransform Failed but was not caught\n"); + }*/ /////// New implementation std::string stripped_frame_id = stripSlash(frame_id); std::string stripped_child_frame_id = stripSlash(child_frame_id); - bool error_exists = false; - if (stripped_child_frame_id == stripped_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()); + if (stripped_child_frame_id == stripped_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()); + 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_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()); + if (stripped_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(transform_in.getOrigin().x()) || std::isnan(transform_in.getOrigin().y()) || std::isnan(transform_in.getOrigin().z())|| - std::isnan(transform_in.getRotation().x()) || std::isnan(transform_in.getRotation().y()) || std::isnan(transform_in.getRotation().z()) || std::isnan(transform_in.getRotation().w())) + if (std::isnan(transform_in.getOrigin().x()) || std::isnan(transform_in.getOrigin().y()) || + std::isnan(transform_in.getOrigin().z()) || + std::isnan(transform_in.getRotation().x()) || std::isnan(transform_in.getRotation().y()) || + std::isnan(transform_in.getRotation().z()) || std::isnan(transform_in.getRotation().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(), - transform_in.getOrigin().x(), transform_in.getOrigin().y(), transform_in.getOrigin().z(), - transform_in.getRotation().x(), transform_in.getRotation().y(), transform_in.getRotation().z(), transform_in.getRotation().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(), + transform_in.getOrigin().x(), transform_in.getOrigin().y(), transform_in.getOrigin().z(), + transform_in.getRotation().x(), transform_in.getRotation().y(), + transform_in.getRotation().z(), transform_in.getRotation().w() + ); error_exists = true; } - bool valid = std::abs((transform_in.getRotation().w() * transform_in.getRotation().w() - + transform_in.getRotation().x() * transform_in.getRotation().x() - + transform_in.getRotation().y() * transform_in.getRotation().y() - + transform_in.getRotation().z() * transform_in.getRotation().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(), - transform_in.getRotation().x(), transform_in.getRotation().y(), transform_in.getRotation().z(), transform_in.getRotation().w()); + bool valid = std::abs( + (transform_in.getRotation().w() * transform_in.getRotation().w() + + transform_in.getRotation().x() * transform_in.getRotation().x() + + transform_in.getRotation().y() * transform_in.getRotation().y() + + transform_in.getRotation().z() * transform_in.getRotation().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(), + transform_in.getRotation().x(), transform_in.getRotation().y(), + transform_in.getRotation().z(), transform_in.getRotation().w()); error_exists = true; } - if (error_exists) + if (error_exists) { return false; - + } + { std::unique_lock lock(frame_mutex_); CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped_child_frame_id); TimeCacheInterfacePtr frame = getFrame(frame_number); - if (frame == NULL) - { + if (frame == NULL) { frame = allocateFrame(frame_number, is_static); - } - else - { + } else { // Overwrite TimeCacheInterface type with a current input - const TimeCache* time_cache_ptr = dynamic_cast(frame.get()); - const StaticCache* static_cache_ptr = dynamic_cast(frame.get()); - if (time_cache_ptr && is_static) - { + const TimeCache * time_cache_ptr = dynamic_cast(frame.get()); + const StaticCache * static_cache_ptr = dynamic_cast(frame.get()); + if (time_cache_ptr && is_static) { frame = allocateFrame(frame_number, is_static); - } - else if (static_cache_ptr && !is_static) - { + } else if (static_cache_ptr && !is_static) { frame = allocateFrame(frame_number, is_static); } } - if (frame->insertData(TransformStorage(stamp, transform_in.getRotation(), transform_in.getOrigin(), lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) + if (frame->insertData( + TransformStorage( + stamp, transform_in.getRotation(), + transform_in.getOrigin(), lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) { frame_authority_[frame_number] = authority; - } - else - { + } else { std::string stamp_str = displayTimePoint(stamp); - CONSOLE_BRIDGE_logWarn("TF_OLD_DATA ignoring data from the past for frame %s at time %s according to authority %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained", stripped_child_frame_id.c_str(), stamp_str.c_str(), authority.c_str()); + CONSOLE_BRIDGE_logWarn( + "TF_OLD_DATA ignoring data from the past for frame %s at time %s according to authority" + " %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained", + stripped_child_frame_id.c_str(), stamp_str.c_str(), authority.c_str()); return false; } } @@ -359,34 +394,36 @@ enum WalkEnding FullPath, }; -// TODO for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI +// TODO(anyone): for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI template -tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const +tf2::TF2Error BufferCore::walkToTopParent( + F & f, TimePoint time, CompactFrameID target_id, + CompactFrameID source_id, + std::string * error_string) const { return walkToTopParent(f, time, target_id, source_id, error_string, NULL); } template -tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID target_id, - CompactFrameID source_id, std::string* error_string, std::vector - *frame_chain) const +tf2::TF2Error BufferCore::walkToTopParent( + F & f, TimePoint time, CompactFrameID target_id, + CompactFrameID source_id, std::string * error_string, std::vector + * frame_chain) const { - if (frame_chain) + if (frame_chain) { frame_chain->clear(); + } // Short circuit if zero length transform to allow lookups on non existant links - if (source_id == target_id) - { + if (source_id == target_id) { f.finalize(Identity, time); return tf2::TF2Error::NO_ERROR; } - //If getting the latest get the latest common time - if (time == TimePointZero) - { + // If getting the latest get the latest common time + if (time == TimePointZero) { tf2::TF2Error retval = getLatestCommonTime(target_id, source_id, time, error_string); - if (retval != tf2::TF2Error::NO_ERROR) - { + if (retval != tf2::TF2Error::NO_ERROR) { return retval; } } @@ -399,22 +436,20 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t std::string extrapolation_error_string; bool extrapolation_might_have_occurred = false; - while (frame != 0) - { + while (frame != 0) { TimeCacheInterfacePtr cache = getFrame(frame); - if (frame_chain) + if (frame_chain) { frame_chain->push_back(frame); + } - if (!cache) - { + if (!cache) { // There will be no cache for the very root of the tree top_parent = frame; break; } CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string); - if (parent == 0) - { + if (parent == 0) { // Just break out here... there may still be a path from source -> target top_parent = frame; extrapolation_might_have_occurred = true; @@ -422,8 +457,7 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t } // Early out... target frame is a direct parent of the source frame - if (frame == target_id) - { + if (frame == target_id) { f.finalize(TargetParentOfSource, time); return tf2::TF2Error::NO_ERROR; } @@ -434,13 +468,11 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t frame = parent; ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { + 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; + ss << "The tf tree is invalid because it contains a loop." << std::endl << + allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2::TF2Error::LOOKUP_ERROR; @@ -452,24 +484,22 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t depth = 0; std::vector reverse_frame_chain; - while (frame != top_parent) - { + while (frame != top_parent) { TimeCacheInterfacePtr cache = getFrame(frame); - if (frame_chain) + if (frame_chain) { reverse_frame_chain.push_back(frame); + } - if (!cache) - { + if (!cache) { break; } CompactFrameID parent = f.gather(cache, time, error_string); - if (parent == 0) - { - if (error_string) - { + if (parent == 0) { + if (error_string) { std::stringstream ss; - ss << *error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; + ss << *error_string << ", when looking up transform from frame [" << lookupFrameString( + source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; *error_string = ss.str(); } @@ -477,11 +507,9 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t } // Early out... source frame is a direct parent of the target frame - if (frame == source_id) - { + if (frame == source_id) { f.finalize(SourceParentOfTarget, time); - if (frame_chain) - { + if (frame_chain) { frame_chain->swap(reverse_frame_chain); } return tf2::TF2Error::NO_ERROR; @@ -492,57 +520,48 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t frame = parent; ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { + 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; + ss << "The tf tree is invalid because it contains a loop." << std::endl << + allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2::TF2Error::LOOKUP_ERROR; } } - if (frame != top_parent) - { - if (extrapolation_might_have_occurred) - { - if (error_string) - { + if (frame != top_parent) { + if (extrapolation_might_have_occurred) { + if (error_string) { std::stringstream ss; - ss << extrapolation_error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; + ss << extrapolation_error_string << ", when looking up transform from frame [" << + lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; *error_string = ss.str(); } - return tf2::TF2Error::EXTRAPOLATION_ERROR; - } - createConnectivityErrorString(source_id, target_id, error_string); return tf2::TF2Error::CONNECTIVITY_ERROR; } f.finalize(FullPath, time); - if (frame_chain) - { + if (frame_chain) { // Pruning: Compare the chains starting at the parent (end) until they differ - int m = (int)reverse_frame_chain.size()-1; - int n = (int)frame_chain->size()-1; - for (; m >= 0 && n >= 0; --m, --n) - { - if ((*frame_chain)[n] != reverse_frame_chain[m]) + int m = static_cast(reverse_frame_chain.size() - 1); + int n = static_cast(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) - frame_chain->erase(frame_chain->begin() + (n-1), frame_chain->end()); + if (n > 0) { + frame_chain->erase(frame_chain->begin() + (n - 1), frame_chain->end()); + } - if (m < reverse_frame_chain.size()) - { - for (int i = m; i >= 0; --i) - { + if (m < reverse_frame_chain.size()) { + for (int i = m; i >= 0; --i) { frame_chain->push_back(reverse_frame_chain[i]); } } @@ -551,24 +570,21 @@ tf2::TF2Error BufferCore::walkToTopParent(F& f, TimePoint time, CompactFrameID t return tf2::TF2Error::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) + : 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, TimePoint time, std::string* error_string) + CompactFrameID gather(TimeCacheInterfacePtr cache, TimePoint time, std::string * error_string) { - if (!cache->getData(time, st, error_string)) - { + if (!cache->getData(time, st, error_string)) { return 0; } @@ -577,13 +593,10 @@ struct TransformAccum void accum(bool source) { - if (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 - { + } else { target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; target_to_top_quat = st.rotation_ * target_to_top_quat; } @@ -591,32 +604,31 @@ struct TransformAccum void finalize(WalkEnding end, TimePoint _time) { - switch (end) - { - case Identity: - break; - case TargetParentOfSource: - result_vec = source_to_top_vec; - result_quat = source_to_top_quat; - break; - case SourceParentOfTarget: - { - tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); - tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); - result_vec = inv_target_vec; - result_quat = inv_target_quat; + switch (end) { + case Identity: break; - } - case FullPath: - { - tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); - tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); + case TargetParentOfSource: + result_vec = source_to_top_vec; + result_quat = source_to_top_quat; + break; + case SourceParentOfTarget: + { + tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); + tf2::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: + { + tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); + tf2::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; - }; + 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; } @@ -632,9 +644,10 @@ struct TransformAccum tf2::Vector3 result_vec; }; -geometry_msgs::msg::TransformStamped - BufferCore::lookupTransform(const std::string& target_frame, const std::string& source_frame, - const TimePoint& time) const +geometry_msgs::msg::TransformStamped +BufferCore::lookupTransform( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time) const { tf2::Transform transform; TimePoint time_out; @@ -647,8 +660,10 @@ geometry_msgs::msg::TransformStamped msg.transform.rotation.y = transform.getRotation().y(); msg.transform.rotation.z = transform.getRotation().z(); msg.transform.rotation.w = transform.getRotation().w(); - std::chrono::nanoseconds ns = std::chrono::duration_cast(time_out.time_since_epoch()); - std::chrono::seconds s = std::chrono::duration_cast(time_out.time_since_epoch()); + std::chrono::nanoseconds ns = std::chrono::duration_cast( + time_out.time_since_epoch()); + std::chrono::seconds s = std::chrono::duration_cast( + time_out.time_since_epoch()); msg.header.stamp.sec = (int32_t)s.count(); msg.header.stamp.nanosec = (uint32_t)(ns.count() % 1000000000ull); msg.header.frame_id = target_frame; @@ -658,14 +673,16 @@ geometry_msgs::msg::TransformStamped } geometry_msgs::msg::TransformStamped - BufferCore::lookupTransform(const std::string& target_frame, const TimePoint& target_time, - const std::string& source_frame, const TimePoint& source_time, - const std::string& fixed_frame) const +BufferCore::lookupTransform( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame) const { tf2::Transform transform; TimePoint time_out; - lookupTransformImpl(target_frame, target_time, source_frame, source_time, - fixed_frame, transform, time_out); + lookupTransformImpl( + target_frame, target_time, source_frame, source_time, + fixed_frame, transform, time_out); geometry_msgs::msg::TransformStamped msg; msg.transform.translation.x = transform.getOrigin().x(); msg.transform.translation.y = transform.getOrigin().y(); @@ -674,8 +691,10 @@ geometry_msgs::msg::TransformStamped msg.transform.rotation.y = transform.getRotation().y(); msg.transform.rotation.z = transform.getRotation().z(); msg.transform.rotation.w = transform.getRotation().w(); - std::chrono::nanoseconds ns = std::chrono::duration_cast(time_out.time_since_epoch()); - std::chrono::seconds s = std::chrono::duration_cast(time_out.time_since_epoch()); + std::chrono::nanoseconds ns = std::chrono::duration_cast( + time_out.time_since_epoch()); + std::chrono::seconds s = std::chrono::duration_cast( + time_out.time_since_epoch()); msg.header.stamp.sec = (int32_t)s.count(); msg.header.stamp.nanosec = (uint32_t)(ns.count() % 1000000000ull); msg.header.frame_id = target_frame; @@ -684,28 +703,28 @@ geometry_msgs::msg::TransformStamped return msg; } - -void BufferCore::lookupTransformImpl(const std::string& target_frame, - const std::string& source_frame, - const TimePoint& time, tf2::Transform& transform, - TimePoint& time_out) const +void BufferCore::lookupTransformImpl( + const std::string & target_frame, + const std::string & source_frame, + const TimePoint & time, tf2::Transform & transform, + TimePoint & time_out) const { std::unique_lock lock(frame_mutex_); if (target_frame == source_frame) { transform.setIdentity(); - if (time == TimePointZero) - { + if (time == TimePointZero) { CompactFrameID target_id = lookupFrameNumber(target_frame); TimeCacheInterfacePtr cache = getFrame(target_id); - if (cache) + if (cache) { time_out = cache->getLatestTimestamp(); - else + } else { time_out = time; - } - else + } + } else { time_out = time; + } return; } @@ -716,19 +735,17 @@ void BufferCore::lookupTransformImpl(const std::string& target_frame, std::string error_string; TransformAccum accum; tf2::TF2Error retval = walkToTopParent(accum, time, target_id, source_id, &error_string); - if (retval != tf2::TF2Error::NO_ERROR) - { - switch (retval) - { - case tf2::TF2Error::CONNECTIVITY_ERROR: - throw ConnectivityException(error_string); - case tf2::TF2Error::EXTRAPOLATION_ERROR: - throw ExtrapolationException(error_string); - case tf2::TF2Error::LOOKUP_ERROR: - throw LookupException(error_string); - default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); - assert(0); + if (retval != tf2::TF2Error::NO_ERROR) { + switch (retval) { + case tf2::TF2Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf2::TF2Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf2::TF2Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); } } @@ -737,13 +754,14 @@ void BufferCore::lookupTransformImpl(const std::string& target_frame, transform.setRotation(accum.result_quat); } - -void BufferCore::lookupTransformImpl(const std::string& target_frame, - const TimePoint& target_time, - const std::string& source_frame, - const TimePoint& source_time, - const std::string& fixed_frame, tf2::Transform& transform, - TimePoint& time_out) const + +void BufferCore::lookupTransformImpl( + const std::string & target_frame, + const TimePoint & target_time, + const std::string & source_frame, + const TimePoint & source_time, + const std::string & fixed_frame, tf2::Transform & transform, + TimePoint & time_out) const { validateFrameId("lookupTransform argument target_frame", target_frame); validateFrameId("lookupTransform argument source_frame", source_frame); @@ -754,21 +772,20 @@ void BufferCore::lookupTransformImpl(const std::string& target_frame, lookupTransformImpl(fixed_frame, source_frame, source_time, tf1, time_out); lookupTransformImpl(target_frame, fixed_frame, target_time, tf2, time_out); - transform = tf2*tf1; + transform = tf2 * tf1; } - /* -geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, - const std::string& observation_frame, - const tf2::TimePoint& time, +geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, + const std::string& observation_frame, + const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const { try { geometry_msgs::Twist t; - old_tf_.lookupTwist(tracking_frame, observation_frame, + old_tf_.lookupTwist(tracking_frame, observation_frame, time, averaging_interval, t); return t; } @@ -790,12 +807,12 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, } } -geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, - const std::string& observation_frame, +geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, + const std::string& observation_frame, const std::string& reference_frame, - const tf2::Point & reference_point, - const std::string& reference_point_frame, - const tf2::TimePoint& time, + const tf2::Point & reference_point, + const std::string& reference_point_frame, + const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const { try{ @@ -825,7 +842,7 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, struct CanTransformAccum { - CompactFrameID gather(TimeCacheInterfacePtr cache, TimePoint time, std::string* error_string) + CompactFrameID gather(TimeCacheInterfacePtr cache, TimePoint time, std::string * error_string) { return cache->getParent(time, error_string); } @@ -841,85 +858,81 @@ struct CanTransformAccum TransformStorage st; }; -bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, - const TimePoint& time, std::string* error_msg) const +bool BufferCore::canTransformNoLock( + CompactFrameID target_id, CompactFrameID source_id, + const TimePoint & time, std::string * error_msg) const { - if (target_id == 0 || source_id == 0) - { - if (error_msg) - { + if (target_id == 0 || source_id == 0) { + if (error_msg) { *error_msg = "Source or target frame is not yet defined"; } return false; } - if (target_id == source_id) - { + if (target_id == source_id) { return true; } CanTransformAccum accum; - if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2::TF2Error::NO_ERROR) - { + if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2::TF2Error::NO_ERROR) { return true; } return false; } -bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, - const TimePoint& time, std::string* error_msg) const +bool BufferCore::canTransformInternal( + CompactFrameID target_id, CompactFrameID source_id, + const TimePoint & time, std::string * error_msg) const { std::unique_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 TimePoint& time, std::string* error_msg) const +bool BufferCore::canTransform( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time, std::string * error_msg) const { // Short circuit if target_frame == source_frame - if (target_frame == source_frame) + if (target_frame == source_frame) { return true; + } CompactFrameID target_id = validateFrameId( "canTransform argument target_frame", target_frame, error_msg); - if (target_id == 0) - { + if (target_id == 0) { return false; } CompactFrameID source_id = validateFrameId( "canTransform argument source_frame", source_frame, error_msg); - if (source_id == 0) - { + if (source_id == 0) { return false; } return canTransformInternal(target_id, source_id, time, error_msg); } -bool BufferCore::canTransform(const std::string& target_frame, const TimePoint& target_time, - const std::string& source_frame, const TimePoint& source_time, - const std::string& fixed_frame, std::string* error_msg) const +bool BufferCore::canTransform( + const std::string & target_frame, const TimePoint & target_time, + const std::string & source_frame, const TimePoint & source_time, + const std::string & fixed_frame, std::string * error_msg) const { CompactFrameID target_id = validateFrameId( "canTransform argument target_frame", target_frame, error_msg); - if (target_id == 0) - { + if (target_id == 0) { return false; } CompactFrameID source_id = validateFrameId( "canTransform argument source_frame", source_frame, error_msg); - if (source_id == 0) - { + if (source_id == 0) { return false; } CompactFrameID fixed_id = validateFrameId( "canTransform argument fixed_frame", fixed_frame, error_msg); - if (fixed_id == 0) - { + if (fixed_id == 0) { return false; } - + return canTransformInternal(target_id, fixed_id, target_time, error_msg) && canTransformInternal(fixed_id, source_id, source_time, error_msg); @@ -928,65 +941,63 @@ bool BufferCore::canTransform(const std::string& target_frame, const TimePoint& tf2::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const { - if (frame_id >= frames_.size()) + if (frame_id >= frames_.size()) { return TimeCacheInterfacePtr(); - else - { + } else { return frames_[frame_id]; } } -CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const +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()) - { + if (map_it == frameIDs_.end()) { retval = CompactFrameID(0); - } - else + } else { retval = map_it->second; + } return retval; } -CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str) +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()) - { + if (map_it == frameIDs_.end()) { retval = CompactFrameID(frames_.size()); - frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration + // Just a place holder for iteration + frames_.push_back(TimeCacheInterfacePtr()); frameIDs_[frameid_str] = retval; frameIDs_reverse.push_back(frameid_str); - } - else + } else { retval = frameIDs_[frameid_str]; - + } return retval; } -const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const +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 tf2::LookupException(ss.str()); - } - else - return frameIDs_reverse[frame_id_num]; + if (frame_id_num >= frameIDs_reverse.size()) { + std::stringstream ss; + ss << "Reverse lookup of frame id " << frame_id_num << " failed!"; + throw tf2::LookupException(ss.str()); + } else { + return frameIDs_reverse[frame_id_num]; + } } -void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const +void BufferCore::createConnectivityErrorString( + CompactFrameID source_frame, + CompactFrameID target_frame, std::string * out) const { - if (!out) - { + 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."); + *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::vector BufferCore::getAllFrameNames() const @@ -1008,22 +1019,20 @@ std::string BufferCore::allFramesAsStringNoLock() const 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 ++) - { + // regular transforms + for (unsigned int counter = 1; counter < frames_.size(); counter++) { TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter)); - if (frame_ptr == NULL) + if (frame_ptr == NULL) { continue; + } CompactFrameID frame_id_num; - if( frame_ptr->getData(TimePointZero, temp)) + if (frame_ptr->getData(TimePointZero, temp)) { frame_id_num = temp.frame_id_; - else - { + } else { frame_id_num = 0; } - mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <getLatestTimestamp(); - else + } else { time = TimePointZero; + } return tf2::TF2Error::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 + // 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; TimePoint common_time = TimePoint::max(); - while (frame != 0) - { + while (frame != 0) { TimeCacheInterfacePtr cache = getFrame(frame); - if (!cache) - { + if (!cache) { // There will be no cache for the very root of the tree break; } P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); - if (latest.second == 0) - { + if (latest.second == 0) { // Just break out here... there may still be a path from source -> target break; } - if (latest.first != TimePointZero) - { + if (latest.first != TimePointZero) { common_time = std::min(latest.first, common_time); } @@ -1095,59 +1102,53 @@ tf2::TF2Error BufferCore::getLatestCommonTime(CompactFrameID target_id, CompactF frame = latest.second; // Early out... target frame is a direct parent of the source frame - if (frame == target_id) - { + if (frame == target_id) { time = common_time; - if (time == TimePoint::max()) - { + if (time == TimePoint::max()) { time = TimePointZero; } return tf2::TF2Error::NO_ERROR; } ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { + 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; + ss << "The tf tree is invalid because it contains a loop." << std::endl << + allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2::TF2Error::LOOKUP_ERROR; } } - // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent + // 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 = TimePoint::max(); CompactFrameID common_parent = 0; - while (true) - { + while (true) { TimeCacheInterfacePtr cache = getFrame(frame); - if (!cache) - { + if (!cache) { break; } P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); - if (latest.second == 0) - { + if (latest.second == 0) { break; } - if (latest.first != TimePointZero) - { + if (latest.first != TimePointZero) { 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 - { + 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; } @@ -1155,32 +1156,27 @@ tf2::TF2Error BufferCore::getLatestCommonTime(CompactFrameID target_id, CompactF frame = latest.second; // Early out... source frame is a direct parent of the target frame - if (frame == source_id) - { + if (frame == source_id) { time = common_time; - if (time == TimePoint::max()) - { + if (time == TimePoint::max()) { time = TimePointZero; } return tf2::TF2Error::NO_ERROR; } ++depth; - if (depth > MAX_GRAPH_DEPTH) - { - if (error_string) - { + 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; + ss << "The tf tree is invalid because it contains a loop." << std::endl << + allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2::TF2Error::LOOKUP_ERROR; } } - if (common_parent == 0) - { + if (common_parent == 0) { createConnectivityErrorString(source_id, target_id, error_string); return tf2::TF2Error::CONNECTIVITY_ERROR; } @@ -1189,22 +1185,18 @@ tf2::TF2Error BufferCore::getLatestCommonTime(CompactFrameID target_id, CompactF { std::vector::iterator it = lct_cache.begin(); std::vector::iterator end = lct_cache.end(); - for (; it != end; ++it) - { - if (it->first != TimePointZero) - { + for (; it != end; ++it) { + if (it->first != TimePointZero) { common_time = std::min(common_time, it->first); } - if (it->second == common_parent) - { + if (it->second == common_parent) { break; } } } - if (common_time == TimePoint::max()) - { + if (common_time == TimePoint::max()) { common_time = TimePointZero; } @@ -1219,25 +1211,23 @@ std::string BufferCore::allFramesAsYAML(TimePoint current_time) const TransformStorage temp; - if (frames_.size() ==1) - mstream <<"[]"; + if (frames_.size() == 1) { + mstream << "[]"; + } mstream.precision(3); - mstream.setf(std::ios::fixed,std::ios::floatfield); + 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 - { + // one referenced for 0 is no frame + for (unsigned int counter = 1; counter < frames_.size(); counter++) { CompactFrameID cfid = CompactFrameID(counter); CompactFrameID frame_id_num; TimeCacheInterfacePtr cache = getFrame(cfid); - if (!cache) - { + if (!cache) { continue; } - if(!cache->getData(TimePointZero, temp)) - { + if (!cache->getData(TimePointZero, temp)) { continue; } @@ -1253,23 +1243,29 @@ std::string BufferCore::allFramesAsYAML(TimePoint current_time) const tf2::Duration dur2 = tf2::Duration(std::chrono::microseconds(100)); double rate; - if (dur1 > dur2) - rate = (cache->getListLength() * 1e9) / std::chrono::duration_cast(dur1).count(); - else - rate = (cache->getListLength() * 1e9) / std::chrono::duration_cast(dur2).count(); + if (dur1 > dur2) { + rate = (cache->getListLength() * 1e9) / std::chrono::duration_cast( + dur1).count(); + } else { + rate = (cache->getListLength() * 1e9) / std::chrono::duration_cast( + dur2).count(); + } - mstream << std::fixed; //fixed point notation - mstream.precision(3); //3 decimal places + 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: " << displayTimePoint(cache->getLatestTimestamp()) << std::endl; + mstream << " most_recent_transform: " << displayTimePoint(cache->getLatestTimestamp()) << + std::endl; mstream << " oldest_transform: " << displayTimePoint(cache->getOldestTimestamp()) << std::endl; - if ( current_time !=TimePointZero ) { - mstream << " transform_delay: " << durationToSec(current_time - cache->getLatestTimestamp()) << std::endl; + if (current_time != TimePointZero) { + mstream << " transform_delay: " << + durationToSec(current_time - cache->getLatestTimestamp()) << std::endl; } - mstream << " buffer_length: " << durationToSec(cache->getLatestTimestamp() - cache->getOldestTimestamp()) << std::endl; + mstream << " buffer_length: " << durationToSec( + cache->getLatestTimestamp() - cache->getOldestTimestamp()) << std::endl; } return mstream.str(); @@ -1280,12 +1276,11 @@ std::string BufferCore::allFramesAsYAML() const return this->allFramesAsYAML(TimePointZero); } -TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb) +TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback & cb) { std::unique_lock lock(transformable_callbacks_mutex_); TransformableCallbackHandle handle = ++transformable_callbacks_counter_; - while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second) - { + while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second) { handle = ++transformable_callbacks_counter_; } @@ -1294,11 +1289,11 @@ TransformableCallbackHandle BufferCore::addTransformableCallback(const Transform struct BufferCore::RemoveRequestByCallback { - RemoveRequestByCallback(TransformableCallbackHandle handle) + explicit RemoveRequestByCallback(TransformableCallbackHandle handle) : handle_(handle) {} - bool operator()(const TransformableRequest& req) + bool operator()(const TransformableRequest & req) { return req.cb_handle == handle_; } @@ -1315,16 +1310,21 @@ void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle) { std::unique_lock lock(transformable_requests_mutex_); - V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle)); + V_TransformableRequest::iterator it = std::remove_if( + transformable_requests_.begin(), transformable_requests_.end(), + RemoveRequestByCallback(handle)); transformable_requests_.erase(it, transformable_requests_.end()); } } -TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, TimePoint time) +TransformableRequestHandle BufferCore::addTransformableRequest( + TransformableCallbackHandle handle, + const std::string & target_frame, + const std::string & source_frame, + TimePoint time) { // shortcut if target == source - if (target_frame == source_frame) - { + if (target_frame == source_frame) { return 0; } @@ -1333,20 +1333,17 @@ TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCall 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)) - { + 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) - { + if (req.target_id && req.source_id) { TimePoint latest_time; - // TODO: This is incorrect, but better than nothing. Really we want the latest time for + // TODO(anyone): 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 != TimePointZero) && (time + cache_time_ < latest_time)) - { + if ((latest_time != TimePointZero) && (time + cache_time_ < latest_time)) { return 0xffffffffffffffffULL; } } @@ -1354,18 +1351,15 @@ TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCall req.cb_handle = handle; req.time = time; req.request_handle = ++transformable_requests_counter_; - if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL) - { + if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL) { req.request_handle = 1; } - if (req.target_id == 0) - { + if (req.target_id == 0) { req.target_string = target_frame; } - if (req.source_id == 0) - { + if (req.source_id == 0) { req.source_string = source_frame; } @@ -1377,11 +1371,11 @@ TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCall struct BufferCore::RemoveRequestByID { - RemoveRequestByID(TransformableRequestHandle handle) + explicit RemoveRequestByID(TransformableRequestHandle handle) : handle_((TransformableCallbackHandle)handle) {} - bool operator()(const TransformableRequest& req) + bool operator()(const TransformableRequest & req) { return req.request_handle == handle_; } @@ -1392,40 +1386,41 @@ struct BufferCore::RemoveRequestByID void BufferCore::cancelTransformableRequest(TransformableRequestHandle handle) { std::unique_lock lock(transformable_requests_mutex_); - V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle)); + V_TransformableRequest::iterator it = std::remove_if( + transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle)); - if (it != transformable_requests_.end()) - { + if (it != transformable_requests_.end()) { transformable_requests_.erase(it, transformable_requests_.end()); } } - - // backwards compability for tf methods -bool BufferCore::_frameExists(const std::string& frame_id_str) const +bool BufferCore::_frameExists(const std::string & frame_id_str) const { std::unique_lock lock(frame_mutex_); return frameIDs_.count(frame_id_str) != 0; } -bool BufferCore::_getParent(const std::string& frame_id, TimePoint time, std::string& parent) const +bool BufferCore::_getParent( + const std::string & frame_id, TimePoint time, + std::string & parent) const { - std::unique_lock lock(frame_mutex_); CompactFrameID frame_number = lookupFrameNumber(frame_id); TimeCacheInterfacePtr frame = getFrame(frame_number); - if (! frame) + if (!frame) { return false; - + } + CompactFrameID parent_id = frame->getParent(time, NULL); - if (parent_id == 0) + if (parent_id == 0) { return false; + } parent = lookupFrameString(parent_id); return true; -}; +} void BufferCore::_getFrameStrings(std::vector & vec) const { @@ -1435,68 +1430,56 @@ void BufferCore::_getFrameStrings(std::vector & vec) const TransformStorage temp; - // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) - for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++) - { + for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter++) { vec.push_back(frameIDs_reverse[counter]); } - return; } - - - void BufferCore::testTransformableRequests() { std::unique_lock lock(transformable_requests_mutex_); V_TransformableRequest::iterator it = transformable_requests_.begin(); - for (; it != transformable_requests_.end();) - { - TransformableRequest& req = *it; + 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) - { + if (req.target_id == 0) { req.target_id = lookupFrameNumber(req.target_string); } - if (req.source_id == 0) - { + if (req.source_id == 0) { req.source_id = lookupFrameNumber(req.source_string); } TimePoint latest_time; bool do_cb = false; TransformableResult result = TransformAvailable; - // TODO: This is incorrect, but better than nothing. Really we want the latest time for + // TODO(anyone): 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 != TimePointZero) && (req.time + cache_time_ < latest_time)) - { + if ((latest_time != TimePointZero) && (req.time + cache_time_ < latest_time)) { do_cb = true; result = TransformFailure; - } - else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) - { + } else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) { do_cb = true; result = TransformAvailable; } - if (do_cb) - { + if (do_cb) { { std::unique_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 (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(); + if (transformable_requests_.size() > 1) { + transformable_requests_[it - + transformable_requests_.begin()] = transformable_requests_.back(); } transformable_requests_.erase(transformable_requests_.end() - 1); @@ -1505,9 +1488,7 @@ void BufferCore::testTransformableRequests() if (0u == transformable_requests_.size()) { it = transformable_requests_.end(); } - } - else - { + } else { ++it; } } @@ -1526,80 +1507,87 @@ std::string BufferCore::_allFramesAsDot(TimePoint current_time) const TransformStorage temp; if (frames_.size() == 1) { - mstream <<"\"no tf data recieved\""; + 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 - { + mstream.setf(std::ios::fixed, std::ios::floatfield); + // one referenced for 0 is no frame + for (unsigned int counter = 1; counter < frames_.size(); counter++) { unsigned int frame_id_num; TimeCacheInterfacePtr counter_frame = getFrame(counter); if (!counter_frame) { continue; } - if(!counter_frame->getData(TimePointZero, temp)) { + if (!counter_frame->getData(TimePointZero, 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()) + if (it != frame_authority_.end()) { authority = it->second; + } tf2::Duration dur1 = counter_frame->getLatestTimestamp() - counter_frame->getOldestTimestamp(); tf2::Duration dur2 = std::chrono::microseconds(100); double rate; - if (dur1 > dur2) - rate = (counter_frame->getListLength() * 1e9) / std::chrono::duration_cast(dur1).count(); - else - rate = (counter_frame->getListLength() * 1e9) / std::chrono::duration_cast(dur2).count(); - - 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: " << displayTimePoint(counter_frame->getLatestTimestamp()) <<" "; - if (current_time != TimePointZero) - mstream << "( "<< durationToSec(current_time - counter_frame->getLatestTimestamp()) << " 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: " << durationToSec(counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()) << " sec\\n" - <<"\"];" < dur2) { + rate = (counter_frame->getListLength() * 1e9) / + std::chrono::duration_cast(dur1).count(); + } else { + rate = (counter_frame->getListLength() * 1e9) / + std::chrono::duration_cast(dur2).count(); + } + + mstream << std::fixed; // fixed point notation + mstream.precision(3); // 3 decimal places + mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> " << + "\"" << frameIDs_reverse[counter] << "\"" << "[label=\"" << + "Broadcaster: " << authority << "\\n" << + "Average rate: " << rate << " Hz\\n" << + "Most recent transform: " << displayTimePoint(counter_frame->getLatestTimestamp()) << " "; + if (current_time != TimePointZero) { + mstream << "( " << durationToSec(current_time - counter_frame->getLatestTimestamp()) << + " sec old)"; + } + mstream << "\\n" << + "Buffer length: " << durationToSec( + counter_frame->getLatestTimestamp() - counter_frame->getOldestTimestamp()) << " sec\\n" << + "\"];" << std::endl; + } + + // one referenced for 0 is no frame + for (unsigned int counter = 1; counter < frames_.size(); counter++) { unsigned int frame_id_num; TimeCacheInterfacePtr counter_frame = getFrame(counter); if (!counter_frame) { if (current_time != TimePointZero) { - mstream << "edge [style=invis];" <" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; + mstream << "edge [style=invis];" << std::endl; + mstream << + " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n" + << + "\"Recorded at time: " << displayTimePoint(current_time) << + "\"[ shape=plaintext ] ;\n " << + "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; } continue; } if (counter_frame->getData(TimePointZero, temp)) { frame_id_num = temp.frame_id_; } else { - frame_id_num = 0; + frame_id_num = 0; } - if(frameIDs_reverse[frame_id_num]=="NO_PARENT") - { - mstream << "edge [style=invis];" <" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; } } @@ -1612,11 +1600,15 @@ std::string BufferCore::_allFramesAsDot() const return _allFramesAsDot(TimePointZero); } -void BufferCore::_chainAsVector(const std::string & target_frame, TimePoint target_time, const std::string & source_frame, TimePoint source_time, const std::string& fixed_frame, std::vector& output) const +void BufferCore::_chainAsVector( + const std::string & target_frame, TimePoint target_time, + const std::string & source_frame, TimePoint source_time, + const std::string & fixed_frame, + std::vector & output) const { std::string error_string; - output.clear(); //empty vector + output.clear(); // empty vector std::stringstream mstream; std::unique_lock lock(frame_mutex_); @@ -1629,32 +1621,12 @@ void BufferCore::_chainAsVector(const std::string & target_frame, TimePoint targ CompactFrameID target_id = lookupFrameNumber(target_frame); std::vector source_frame_chain; - tf2::TF2Error retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain); - - if (retval != tf2::TF2Error::NO_ERROR) - { - switch (retval) - { - case tf2::TF2Error::CONNECTIVITY_ERROR: - throw ConnectivityException(error_string); - case tf2::TF2Error::EXTRAPOLATION_ERROR: - throw ExtrapolationException(error_string); - case tf2::TF2Error::LOOKUP_ERROR: - throw LookupException(error_string); - default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); - assert(0); - } - } - if (source_time != target_time) - { - std::vector target_frame_chain; - retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain); + tf2::TF2Error retval = walkToTopParent( + accum, source_time, fixed_id, source_id, &error_string, + &source_frame_chain); - if (retval != tf2::TF2Error::NO_ERROR) - { - switch (retval) - { + if (retval != tf2::TF2Error::NO_ERROR) { + switch (retval) { case tf2::TF2Error::CONNECTIVITY_ERROR: throw ConnectivityException(error_string); case tf2::TF2Error::EXTRAPOLATION_ERROR: @@ -1664,34 +1636,49 @@ void BufferCore::_chainAsVector(const std::string & target_frame, TimePoint targ default: CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); assert(0); + } + } + if (source_time != target_time) { + std::vector target_frame_chain; + retval = walkToTopParent( + accum, target_time, target_id, fixed_id, &error_string, + &target_frame_chain); + + if (retval != tf2::TF2Error::NO_ERROR) { + switch (retval) { + case tf2::TF2Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf2::TF2Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf2::TF2Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); } } - int m = (int)target_frame_chain.size()-1; - int n = (int)source_frame_chain.size()-1; - for (; m >= 0 && n >= 0; --m, --n) - { - if (source_frame_chain[n] != target_frame_chain[m]) + int m = static_cast(target_frame_chain.size() - 1); + int n = static_cast(source_frame_chain.size() - 19); + for (; m >= 0 && n >= 0; --m, --n) { + if (source_frame_chain[n] != target_frame_chain[m]) { break; + } } // Erase all duplicate items from frame_chain - if (n > 0) - source_frame_chain.erase(source_frame_chain.begin() + (n-1), source_frame_chain.end()); + if (n > 0) { + source_frame_chain.erase(source_frame_chain.begin() + (n - 1), source_frame_chain.end()); + } - if (m < target_frame_chain.size()) - { - for (int i = 0; i <= m; ++i) - { + if (m < target_frame_chain.size()) { + for (int i = 0; i <= m; ++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) - { + for (unsigned int i = 0; i < source_frame_chain.size(); ++i) { output.push_back(lookupFrameString(source_frame_chain[i])); - } + } } - - -} // namespace tf2 +} // namespace tf2 diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 2983b0afc..0cce966ee 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -1,55 +1,60 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER 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 + +#include +#include + #include "tf2/time_cache.h" #include "tf2/exceptions.h" -#include -#include -#include -#include +#include "tf2/LinearMath/Vector3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Transform.h" -namespace tf2 { +namespace tf2 +{ TransformStorage::TransformStorage() { } -TransformStorage::TransformStorage(const TimePoint& stamp, const Quaternion& q, const Vector3& t, - CompactFrameID frame_id, CompactFrameID child_frame_id) -: stamp_(stamp) -, frame_id_(frame_id) -, child_frame_id_(child_frame_id) -, rotation_(q) -, translation_(t) +TransformStorage::TransformStorage( + const TimePoint & stamp, const Quaternion & q, const Vector3 & t, + CompactFrameID frame_id, CompactFrameID child_frame_id) +: stamp_(stamp), + frame_id_(frame_id), + child_frame_id_(child_frame_id), + rotation_(q), + translation_(t) { } @@ -57,65 +62,64 @@ TimeCache::TimeCache(tf2::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(TimePoint t0, TimePoint t1, std::string* error_str) +// Avoid ODR collisions https://github.com/ros/geometry2/issues/175 +namespace cache +{ +// hoisting these into separate functions causes an ~8% speedup. +// Removing calling them altogether adds another ~10% +void createExtrapolationException1(TimePoint t0, TimePoint t1, std::string * error_str) { - if (error_str) - { + if (error_str) { std::stringstream ss; - ss << "Lookup would require extrapolation at time " << displayTimePoint(t0) << ", but only time " << displayTimePoint(t1) << " is in the buffer"; + ss << "Lookup would require extrapolation at time " << displayTimePoint(t0) << + ", but only time " << displayTimePoint(t1) << " is in the buffer"; *error_str = ss.str(); } } -void createExtrapolationException2(TimePoint t0, TimePoint t1, std::string* error_str) +void createExtrapolationException2(TimePoint t0, TimePoint t1, std::string * error_str) { - if (error_str) - { + if (error_str) { std::stringstream ss; - ss << "Lookup would require extrapolation into the future. Requested time " << displayTimePoint(t0) << " but the latest data is at time " << displayTimePoint(t1); + ss << "Lookup would require extrapolation into the future. Requested time " << + displayTimePoint(t0) << " but the latest data is at time " << displayTimePoint(t1); *error_str = ss.str(); } } -void createExtrapolationException3(TimePoint t0, TimePoint t1, std::string* error_str) +void createExtrapolationException3(TimePoint t0, TimePoint t1, std::string * error_str) { - if (error_str) - { + if (error_str) { std::stringstream ss; - ss << "Lookup would require extrapolation into the past. Requested time " << displayTimePoint(t0) << " but the earliest data is at time " << displayTimePoint(t1); + ss << "Lookup would require extrapolation into the past. Requested time " << displayTimePoint( + t0) << " but the earliest data is at time " << displayTimePoint(t1); *error_str = ss.str(); } } -} // namespace cache +} // namespace cache -uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, TimePoint target_time, std::string* error_str) +uint8_t TimeCache::findClosest( + TransformStorage * & one, TransformStorage * & two, + TimePoint target_time, std::string * error_str) { - //No values stored - if (storage_.empty()) - { + // No values stored + if (storage_.empty()) { return 0; } - //If time == 0 return the latest - if (target_time == TimePointZero) - { + // If time == 0 return the latest + if (target_time == TimePointZero) { one = &storage_.front(); return 1; } // One value stored - if (++storage_.begin() == storage_.end()) - { - TransformStorage& ts = *storage_.begin(); - if (ts.stamp_ == target_time) - { + if (++storage_.begin() == storage_.end()) { + TransformStorage & ts = *storage_.begin(); + if (ts.stamp_ == target_time) { one = &ts; return 1; - } - else - { + } else { cache::createExtrapolationException1(target_time, ts.stamp_, error_str); return 0; } @@ -124,132 +128,116 @@ uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, T TimePoint latest_time = (*storage_.begin()).stamp_; TimePoint earliest_time = (*(storage_.rbegin())).stamp_; - if (target_time == latest_time) - { + if (target_time == latest_time) { one = &(*storage_.begin()); return 1; - } - else if (target_time == earliest_time) - { + } 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_str); - return 0; - } - else if (target_time < earliest_time) - { - cache::createExtrapolationException3(target_time, earliest_time, error_str); - return 0; + } else { // Catch cases that would require extrapolation + if (target_time > latest_time) { + cache::createExtrapolationException2(target_time, latest_time, error_str); + return 0; + } else { + if (target_time < earliest_time) { + cache::createExtrapolationException3(target_time, earliest_time, error_str); + return 0; + } + } } - //At least 2 values stored - //Find the first value less than the target value + // At least 2 values stored + // Find the first value less than the target value L_TransformStorage::iterator storage_it = storage_.begin(); - while(storage_it != storage_.end()) - { - if (storage_it->stamp_ <= target_time) + while (storage_it != storage_.end()) { + if (storage_it->stamp_ <= target_time) { break; + } storage_it++; } - //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) - one = &*(storage_it); //Older - two = &*(--storage_it); //Newer + // 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, TimePoint time, TransformStorage& output) +void TimeCache::interpolate( + const TransformStorage & one, const TransformStorage & two, + TimePoint time, TransformStorage & output) { // Check for zero distance case - if( two.stamp_ == one.stamp_ ) - { + if (two.stamp_ == one.stamp_) { output = two; return; } - //Calculate the ratio - tf2Scalar ratio = double((time - one.stamp_).count()) / double((two.stamp_ - one.stamp_).count()); + // Calculate the ratio + tf2Scalar ratio = static_cast((time - one.stamp_).count()) / + static_cast((two.stamp_ - one.stamp_).count()); - //Interpolate translation + // Interpolate translation output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); - //Interpolate rotation - output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); + // Interpolate rotation + output.rotation_ = slerp(one.rotation_, two.rotation_, ratio); output.stamp_ = one.stamp_; output.frame_id_ = one.frame_id_; output.child_frame_id_ = one.child_frame_id_; } -bool TimeCache::getData(TimePoint time, TransformStorage & data_out, std::string* error_str) //returns false if data not available +bool TimeCache::getData( + TimePoint time, TransformStorage & data_out, + std::string * error_str) { - TransformStorage* p_temp_1; - TransformStorage* p_temp_2; + // 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_str); - if (num_nodes == 0) - { + if (num_nodes == 0) { return false; - } - else if (num_nodes == 1) - { + } 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_) - { + } 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 - { + } else { data_out = *p_temp_1; } - } - else - { + } else { assert(0); } - return true; } -CompactFrameID TimeCache::getParent(TimePoint time, std::string* error_str) +CompactFrameID TimeCache::getParent(TimePoint time, std::string * error_str) { - TransformStorage* p_temp_1; - TransformStorage* p_temp_2; + TransformStorage * p_temp_1; + TransformStorage * p_temp_2; int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); - if (num_nodes == 0) - { + if (num_nodes == 0) { return 0; } return p_temp_1->frame_id_; } -bool TimeCache::insertData(const TransformStorage& new_data) +bool TimeCache::insertData(const TransformStorage & new_data) { L_TransformStorage::iterator storage_it = storage_.begin(); - if(storage_it != storage_.end()) - { - if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) - { + if (storage_it != storage_.end()) { + if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) { return false; } } - - while(storage_it != storage_.end()) - { - if (storage_it->stamp_ <= new_data.stamp_) + while (storage_it != storage_.end()) { + if (storage_it->stamp_ <= new_data.stamp_) { break; + } storage_it++; } storage_.insert(storage_it, new_data); @@ -270,35 +258,38 @@ unsigned int TimeCache::getListLength() P_TimeAndFrameID TimeCache::getLatestTimeAndParent() { - if (storage_.empty()) - { + if (storage_.empty()) { return std::make_pair(TimePoint(), 0); } - const TransformStorage& ts = storage_.front(); + const TransformStorage & ts = storage_.front(); return std::make_pair(ts.stamp_, ts.frame_id_); } TimePoint TimeCache::getLatestTimestamp() -{ - if (storage_.empty()) return TimePoint(); //empty list case +{ + // empty list case + if (storage_.empty()) { + return TimePoint(); + } return storage_.front().stamp_; } TimePoint TimeCache::getOldestTimestamp() -{ - if (storage_.empty()) return TimePoint(); //empty list case +{ + // empty list case + if (storage_.empty()) { + return TimePoint(); + } return storage_.back().stamp_; } void TimeCache::pruneList() { TimePoint latest_time = storage_.begin()->stamp_; - - while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) - { + + while (!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) { storage_.pop_back(); } - -} // namespace tf2 } +} // namespace tf2 diff --git a/tf2/src/static_cache.cpp b/tf2/src/static_cache.cpp index 1434a374f..caf1b083f 100644 --- a/tf2/src/static_cache.cpp +++ b/tf2/src/static_cache.cpp @@ -1,78 +1,76 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER 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 +#include + #include "tf2/time_cache.h" #include "tf2/exceptions.h" #include "tf2/LinearMath/Transform.h" - -using namespace tf2; - -bool StaticCache::getData(TimePoint time, TransformStorage & data_out, std::string* error_str) //returns false if data not available +bool tf2::StaticCache::getData( + tf2::TimePoint time, + tf2::TransformStorage & data_out, std::string * error_str) { data_out = storage_; data_out.stamp_ = time; return true; -}; +} -bool StaticCache::insertData(const TransformStorage& new_data) +bool tf2::StaticCache::insertData(const tf2::TransformStorage & new_data) { storage_ = new_data; return true; -}; - - - +} -void StaticCache::clearList() { return; }; +void tf2::StaticCache::clearList() {} -unsigned int StaticCache::getListLength() { return 1; }; +unsigned tf2::StaticCache::getListLength() {return 1;} -CompactFrameID StaticCache::getParent(TimePoint time, std::string* error_str) +tf2::CompactFrameID tf2::StaticCache::getParent(tf2::TimePoint time, std::string * error_str) { return storage_.frame_id_; } -P_TimeAndFrameID StaticCache::getLatestTimeAndParent() +tf2::P_TimeAndFrameID tf2::StaticCache::getLatestTimeAndParent() { return std::make_pair(TimePoint(), storage_.frame_id_); } -TimePoint StaticCache::getLatestTimestamp() -{ - return TimePoint(); -}; +tf2::TimePoint tf2::StaticCache::getLatestTimestamp() +{ + return tf2::TimePoint(); +} -TimePoint StaticCache::getOldestTimestamp() -{ - return TimePoint(); -}; +tf2::TimePoint tf2::StaticCache::getOldestTimestamp() +{ + return tf2::TimePoint(); +} diff --git a/tf2/src/time.cpp b/tf2/src/time.cpp index 673495582..3b1bbc3f4 100644 --- a/tf2/src/time.cpp +++ b/tf2/src/time.cpp @@ -1,31 +1,30 @@ -/* - * Copyright (c) 2016, 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. - */ +// Copyright 2008, 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 Open Source Robotics Foundation 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 HOLDER 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 */ @@ -36,7 +35,7 @@ #include "rcutils/strerror.h" #include "tf2/time.h" -std::string tf2::displayTimePoint(const tf2::TimePoint& stamp) +std::string tf2::displayTimePoint(const tf2::TimePoint & stamp) { const char * format_str = "%.6f"; double current_time = tf2::timeToSec(stamp); diff --git a/tf2/test/cache_unittest.cpp b/tf2/test/cache_unittest.cpp index 7209978d3..5e26ce108 100644 --- a/tf2/test/cache_unittest.cpp +++ b/tf2/test/cache_unittest.cpp @@ -1,35 +1,38 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER 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 +#include +#include #include "tf2/time_cache.h" #include "tf2/LinearMath/Quaternion.h" @@ -40,29 +43,24 @@ unsigned int step = 0; void seed_rand() { values.clear(); - for (unsigned int i = 0; i < 2000; i++) - { - int pseudo_rand = (int)std::floor((double)i * 3.141592653589793); - values.push_back(( pseudo_rand % 100)/50.0 - 1.0); - //printf("Seeding with %f\n", values.back()); + for (unsigned int i = 0; i < 2000; i++) { + int pseudo_rand = static_cast(std::floor(static_cast(i * 3.141592653589793))); + values.push_back(( pseudo_rand % 100) / 50.0 - 1.0); } -}; - +} -double get_rand() -{ - if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first"); - if (step >= values.size()) +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 + } else { step++; + } return values[step]; } -using namespace tf2; - - -void setIdentity(TransformStorage& stor) +void setIdentity(tf2::TransformStorage & stor) { stor.translation_.setValue(0.0, 0.0, 0.0); stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); @@ -71,99 +69,86 @@ void setIdentity(TransformStorage& stor) TEST(TimeCache, Repeatability) { unsigned int runs = 100; - - tf2::TimeCache cache; - TransformStorage stor; + tf2::TimeCache cache; + + tf2::TransformStorage stor; setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { + + for (uint64_t i = 1; i < runs; i++) { stor.frame_id_ = tf2::CompactFrameID(i); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(i)); - + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(i)); + cache.insertData(stor); } - for ( uint64_t i = 1; i < runs ; i++ ) - - { - cache.getData(TimePoint(std::chrono::nanoseconds(i)), stor); + for (uint64_t i = 1; i < runs; i++) { + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(i)), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, TimePoint(std::chrono::nanoseconds(i))); + EXPECT_EQ(stor.stamp_, tf2::TimePoint(std::chrono::nanoseconds(i))); } - } TEST(TimeCache, RepeatabilityReverseInsertOrder) { unsigned int runs = 100; - - tf2::TimeCache cache; - TransformStorage stor; + tf2::TimeCache cache; + + tf2::TransformStorage stor; setIdentity(stor); - - for ( int i = runs -1; i >= 0 ; i-- ) - { + + for (int i = runs - 1; i >= 0; i--) { stor.frame_id_ = i; - stor.stamp_ = TimePoint(std::chrono::nanoseconds(i)); - + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(i)); + cache.insertData(stor); } - for ( uint64_t i = 1; i < runs ; i++ ) - - { - cache.getData(TimePoint(std::chrono::nanoseconds(i)), stor); + for (uint64_t i = 1; i < runs; i++) { + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(i)), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, TimePoint(std::chrono::nanoseconds(i))); + EXPECT_EQ(stor.stamp_, tf2::TimePoint(std::chrono::nanoseconds(i))); } - } TEST(TimeCache, ZeroAtFront) { uint64_t runs = 100; - tf2::TimeCache cache; + tf2::TimeCache cache; - TransformStorage stor; + tf2::TransformStorage stor; setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { + + for (uint64_t i = 1; i < runs; i++) { stor.frame_id_ = tf2::CompactFrameID(i); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(i)); - + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(i)); + cache.insertData(stor); } stor.frame_id_ = tf2::CompactFrameID(runs); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(runs)); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(runs)); cache.insertData(stor); - for ( uint64_t i = 1; i < runs ; i++ ) - - { - cache.getData(TimePoint(std::chrono::nanoseconds(i)), stor); + for (uint64_t i = 1; i < runs; i++) { + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(i)), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, TimePoint(std::chrono::nanoseconds(i))); + EXPECT_EQ(stor.stamp_, tf2::TimePoint(std::chrono::nanoseconds(i))); } - cache.getData(TimePoint(), stor); + cache.getData(tf2::TimePoint(), stor); EXPECT_EQ(stor.frame_id_, runs); - EXPECT_EQ(stor.stamp_, TimePoint(std::chrono::nanoseconds(runs))); + EXPECT_EQ(stor.stamp_, tf2::TimePoint(std::chrono::nanoseconds(runs))); stor.frame_id_ = tf2::CompactFrameID(runs); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(runs+1)); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(runs + 1)); cache.insertData(stor); - - //Make sure we get a different value now that a new values is added at the front - cache.getData(TimePoint(), stor); + // Make sure we get a different value now that a new values is added at the front + cache.getData(tf2::TimePoint(), stor); EXPECT_EQ(stor.frame_id_, runs); - EXPECT_EQ(stor.stamp_, TimePoint(std::chrono::nanoseconds(runs+1))); - + EXPECT_EQ(stor.stamp_, tf2::TimePoint(std::chrono::nanoseconds(runs + 1))); } TEST(TimeCache, CartesianInterpolation) @@ -171,52 +156,44 @@ TEST(TimeCache, CartesianInterpolation) uint64_t runs = 100; double epsilon = 2e-6; seed_rand(); - - tf2::TimeCache cache; + + tf2::TimeCache cache; std::vector xvalues(2); std::vector yvalues(2); std::vector zvalues(2); uint64_t offset = 200; - TransformStorage stor; + tf2::TransformStorage stor; setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - for (uint64_t step = 0; step < 2 ; step++) - { + 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_ = TimePoint(std::chrono::nanoseconds(step * 100 + offset)); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(step * 100 + offset)); cache.insertData(stor); } - - for (int pos = 0; pos < 100 ; pos ++) - { - cache.getData(TimePoint(std::chrono::nanoseconds(offset + pos)), stor); + + for (int pos = 0; pos < 100; pos++) { + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(offset + pos)), stor); 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, + // 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_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); + 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 */ @@ -232,24 +209,22 @@ TEST(TimeCache, ReparentingInterpolationProtection) std::vector yvalues(2); std::vector zvalues(2); - TransformStorage stor; + tf2::TransformStorage stor; setIdentity(stor); - for (uint64_t step = 0; step < 2 ; step++) - { + 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_ = tf2::CompactFrameID(step + 4); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(step * 100 + offset)); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(step * 100 + offset)); cache.insertData(stor); } - - for (int pos = 0; pos < 100 ; pos ++) - { - EXPECT_TRUE(cache.getData(TimePoint(std::chrono::nanoseconds(offset + pos)), stor)); + + for (int pos = 0; pos < 100; pos++) { + EXPECT_TRUE(cache.getData(tf2::TimePoint(std::chrono::nanoseconds(offset + pos)), stor)); double x_out = stor.translation_.x(); double y_out = stor.translation_.y(); double z_out = stor.translation_.z(); @@ -261,35 +236,30 @@ TEST(TimeCache, ReparentingInterpolationProtection) TEST(Bullet, Slerp) { - uint64_t runs = 100; seed_rand(); tf2::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()); - - - tf2::Quaternion q3 = slerp(q1,q2,0.5); - + 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()); + tf2::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(); - - tf2::TimeCache cache; + + tf2::TimeCache cache; std::vector yawvalues(2); std::vector pitchvalues(2); std::vector rollvalues(2); @@ -297,61 +267,52 @@ TEST(TimeCache, AngularInterpolation) std::vector quats(2); - TransformStorage stor; + tf2::TransformStorage stor; setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - for (uint64_t step = 0; step < 2 ; step++) - { + 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(); + 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_ = TimePoint(std::chrono::nanoseconds(offset + (step * 100))); //step = 0 or 1 + // step = 0 or 1 + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(offset + (step * 100))); cache.insertData(stor); } - - for (int pos = 0; pos < 100 ; pos ++) - { - EXPECT_TRUE(cache.getData(TimePoint(std::chrono::nanoseconds(offset + pos)), stor)); //get the transform for the position - tf2::Quaternion quat (stor.rotation_); - - //Generate a ground truth quaternion directly calling slerp - tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); - - //Make sure the transformed one and the direct call match + + for (int pos = 0; pos < 100; pos++) { + // get the transform for the position + EXPECT_TRUE(cache.getData(tf2::TimePoint(std::chrono::nanoseconds(offset + pos)), stor)); + tf2::Quaternion quat(stor.rotation_); + + // Generate a ground truth quaternion directly calling slerp + tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos / 100.0); + + // Make sure the transformed one and the direct call match EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); - } - cache.clearList(); } - - } TEST(TimeCache, DuplicateEntries) { + tf2::TimeCache cache; - TimeCache cache; - - TransformStorage stor; + tf2::TransformStorage stor; setIdentity(stor); stor.frame_id_ = 3; - stor.stamp_ = TimePoint(std::chrono::nanoseconds(1)); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(1)); cache.insertData(stor); cache.insertData(stor); + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(1)), stor); - cache.getData(TimePoint(std::chrono::nanoseconds(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())); @@ -361,7 +322,8 @@ TEST(TimeCache, DuplicateEntries) EXPECT_TRUE(!std::isnan(stor.rotation_.w())); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/tf2/test/simple_tf2_core.cpp b/tf2/test/simple_tf2_core.cpp index fab0c2097..505fc7175 100644 --- a/tf2/test/simple_tf2_core.cpp +++ b/tf2/test/simple_tf2_core.cpp @@ -1,35 +1,38 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER 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 +#include + +#include "tf2/buffer_core.h" #include "tf2/LinearMath/Vector3.h" #include "tf2/exceptions.h" #include "tf2/time.h" @@ -39,7 +42,6 @@ TEST(tf2, setTransformFail) tf2::BufferCore tfc; geometry_msgs::msg::TransformStamped st; EXPECT_FALSE(tfc.setTransform(st, "authority1")); - } TEST(tf2, setTransformValid) @@ -53,7 +55,6 @@ TEST(tf2, setTransformValid) st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); - } TEST(tf2, setTransformValidWithCallback) @@ -72,7 +73,8 @@ TEST(tf2, setTransformValidWithCallback) bool transform_available = false; auto cb_handle = buffer.addTransformableCallback( - [&received_request_handle, &received_target_frame, &received_source_frame, &received_time_point, &transform_available]( + [&received_request_handle, &received_target_frame, &received_source_frame, &received_time_point, + &transform_available]( tf2::TransformableRequestHandle request_handle, const std::string & target_frame, const std::string & source_frame, @@ -89,7 +91,7 @@ TEST(tf2, setTransformValidWithCallback) tf2::TransformableRequestHandle request_handle = buffer.addTransformableRequest( cb_handle, target_frame, source_frame, time_point); ASSERT_NE(request_handle, 0u); - + geometry_msgs::msg::TransformStamped transform_msg; transform_msg.header.frame_id = target_frame; transform_msg.header.stamp = builtin_interfaces::msg::Time(); @@ -117,21 +119,22 @@ TEST(tf2, setTransformInvalidQuaternion) st.child_frame_id = "child"; st.transform.rotation.w = 0; EXPECT_FALSE(tfc.setTransform(st, "authority1")); - } TEST(tf2_lookupTransform, LookupException_Nothing_Exists) { tf2::BufferCore tfc; - EXPECT_THROW(tfc.lookupTransform("a", "b", tf2::TimePoint(std::chrono::seconds(1))), tf2::LookupException); - + EXPECT_THROW( + tfc.lookupTransform( + "a", "b", tf2::TimePoint( + std::chrono::seconds( + 1))), tf2::LookupException); } TEST(tf2_canTransform, Nothing_Exists) { tf2::BufferCore tfc; EXPECT_FALSE(tfc.canTransform("a", "b", tf2::TimePoint(std::chrono::seconds(1)))); - } TEST(tf2_lookupTransform, LookupException_One_Exists) @@ -145,8 +148,11 @@ TEST(tf2_lookupTransform, LookupException_One_Exists) st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); - EXPECT_THROW(tfc.lookupTransform("foo", "bar", tf2::TimePoint(std::chrono::seconds(1))), tf2::LookupException); - + EXPECT_THROW( + tfc.lookupTransform( + "foo", "bar", tf2::TimePoint( + std::chrono::seconds( + 1))), tf2::LookupException); } TEST(tf2_canTransform, One_Exists) @@ -210,7 +216,8 @@ TEST(tf2_time, To_From_Sec) // Exact representation of a time. tf2::TimePoint t1 = tf2::get_now(); - // Approximate representation of the time in seconds as a double (floating point error introduced). + // Approximate representation of the time in seconds as a double (floating point + // error introduced). double t1_sec = tf2::timeToSec(t1); // Time point from the t1_sec approximation. @@ -236,14 +243,14 @@ TEST(tf2_time, To_From_Duration) 0.0, }; - for (double expected_diff_sec : values ) - { + for (double expected_diff_sec : values) { tf2::TimePoint t2 = tf2::timeFromSec(tf2::timeToSec(t1) + expected_diff_sec); // Check durationToSec. tf2::Duration duration1 = t2 - t1; - // Approximate representation of the duration in seconds as a double (floating point error introduced). + // Approximate representation of the duration in seconds as a double (floating point + // error introduced). double duration1_sec = tf2::durationToSec(duration1); // Check that the difference due to duration_sec being approximate is small. @@ -262,8 +269,8 @@ TEST(tf2_time, To_From_Duration) } } - -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/tf2/test/speed_test.cpp b/tf2/test/speed_test.cpp index 0686ee96a..021b743d6 100644 --- a/tf2/test/speed_test.cpp +++ b/tf2/test/speed_test.cpp @@ -1,44 +1,41 @@ -/* - * 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 +// Copyright 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 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 HOLDER 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 "console_bridge/console.h" +#include "tf2/buffer_core.h" +#include "geometry_msgs/msg/transform_stamped.hpp" -int main(int argc, char** argv) +int main(int argc, char ** argv) { uint32_t num_levels = 10; - if (argc > 1) - { + if (argc > 1) { num_levels = std::stoi(argv[1]); } @@ -53,10 +50,8 @@ int main(int argc, char** argv) t.header.stamp = builtin_interfaces::msg::Time(2); bc.setTransform(t, "me"); - for (uint32_t i = 1; i < num_levels/2; ++i) - { - for (uint32_t j = 1; j < 3; ++j) - { + for (uint32_t i = 1; i < num_levels / 2; ++i) { + for (uint32_t j = 1; j < 3; ++j) { std::stringstream parent_ss; parent_ss << (i - 1); std::stringstream child_ss; @@ -71,17 +66,15 @@ int main(int argc, char** argv) t.header.frame_id = "root"; std::stringstream ss; - ss << num_levels/2; + ss << num_levels / 2; t.header.stamp = builtin_interfaces::msg::Time(1); t.child_frame_id = ss.str(); bc.setTransform(t, "me"); t.header.stamp = builtin_interfaces::msg::Time(2); bc.setTransform(t, "me"); - for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) - { - for (uint32_t j = 1; j < 3; ++j) - { + for (uint32_t i = num_levels / 2 + 1; i < num_levels; ++i) { + for (uint32_t j = 1; j < 3; ++j) { std::stringstream parent_ss; parent_ss << (i - 1); std::stringstream child_ss; @@ -94,10 +87,8 @@ int main(int argc, char** argv) } } - //logInfo_STREAM(bc.allFramesAsYAML()); - std::string v_frame0 = std::to_string(num_levels - 1); - std::string v_frame1 = std::to_string(num_levels/2 - 1); + std::string v_frame1 = std::to_string(num_levels / 2 - 1); logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str()); geometry_msgs::msg::TransformStamped out_t; @@ -107,112 +98,113 @@ int main(int argc, char** argv) #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, tf2::TimePoint()); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, tf2::TimePoint(std::chrono::seconds(1))); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(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, tf2::TimePoint(std::chrono::milliseconds(1500))); + for (uint32_t i = 0; i < count; ++i) { + out_t = + bc.lookupTransform(v_frame1, v_frame0, tf2::TimePoint(std::chrono::milliseconds(1500))); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, tf2::TimePoint(std::chrono::seconds(2))); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, tf2::TimePoint()); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, tf2::TimePoint(std::chrono::seconds(1))); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, tf2::TimePoint(std::chrono::milliseconds(1500))); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); - for (uint32_t i = 0; i < count; ++i) - { + for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, tf2::TimePoint(std::chrono::seconds(2))); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; - //ROS_INFO_STREAM(out_t); - logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + logInform( + "canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), + dur.toSec() / static_cast(count)); } #endif } diff --git a/tf2/test/static_cache_test.cpp b/tf2/test/static_cache_test.cpp index 6d0e8aa29..51fd74822 100644 --- a/tf2/test/static_cache_test.cpp +++ b/tf2/test/static_cache_test.cpp @@ -1,31 +1,30 @@ -/* - * 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. - */ +// Copyright 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 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 HOLDER 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 @@ -33,10 +32,7 @@ #include -using namespace tf2; - - -void setIdentity(TransformStorage& stor) +void setIdentity(tf2::TransformStorage & stor) { stor.translation_.setValue(0.0, 0.0, 0.0); stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); @@ -45,45 +41,40 @@ void setIdentity(TransformStorage& stor) TEST(StaticCache, Repeatability) { unsigned int runs = 100; - - tf2::StaticCache cache; - TransformStorage stor; + tf2::StaticCache cache; + + tf2::TransformStorage stor; setIdentity(stor); - - for ( uint64_t i = 1; i < runs ; i++ ) - { - stor.frame_id_ = CompactFrameID(i); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(i)); - + + for (uint64_t i = 1; i < runs; i++) { + stor.frame_id_ = tf2::CompactFrameID(i); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(i)); + cache.insertData(stor); - - cache.getData(TimePoint(std::chrono::nanoseconds(i)), stor); + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(i)), stor); EXPECT_EQ(stor.frame_id_, i); - EXPECT_EQ(stor.stamp_, TimePoint(std::chrono::nanoseconds(i))); - + EXPECT_EQ(stor.stamp_, tf2::TimePoint(std::chrono::nanoseconds(i))); } } TEST(StaticCache, DuplicateEntries) { - tf2::StaticCache cache; - TransformStorage stor; + tf2::TransformStorage stor; setIdentity(stor); - stor.frame_id_ = CompactFrameID(3); - stor.stamp_ = TimePoint(std::chrono::nanoseconds(1)); + stor.frame_id_ = tf2::CompactFrameID(3); + stor.stamp_ = tf2::TimePoint(std::chrono::nanoseconds(1)); cache.insertData(stor); cache.insertData(stor); + cache.getData(tf2::TimePoint(std::chrono::nanoseconds(1)), stor); - cache.getData(TimePoint(std::chrono::nanoseconds(1)), stor); - - //printf(" stor is %f\n", stor.transform.translation.x); + // 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())); @@ -93,7 +84,8 @@ TEST(StaticCache, DuplicateEntries) EXPECT_TRUE(!std::isnan(stor.rotation_.w())); } -int main(int argc, char **argv){ +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/tf2/test/test_time.cpp b/tf2/test/test_time.cpp index 7b1d2979e..ba25c128a 100644 --- a/tf2/test/test_time.cpp +++ b/tf2/test/test_time.cpp @@ -27,8 +27,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include +#include #include "tf2/time.h" using namespace std::literals::chrono_literals;