From a20555e2032fb4530351039efe20396016e09ea6 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 18 Jun 2020 18:00:59 +0200 Subject: [PATCH 1/8] Use snprintf instead of stringstream to increase performance of lookupTransform() in error cases. --- tf2/include/tf2/time_cache.h | 2 -- tf2/src/buffer_core.cpp | 14 ++++++++------ tf2/src/cache.cpp | 26 +++++++++++--------------- 3 files changed, 19 insertions(+), 23 deletions(-) diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index fe729c39d..14311129b 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -36,8 +36,6 @@ #include -#include - #include #include diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index c9bcb07d4..1ef898f91 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -414,9 +414,10 @@ int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, { if (error_string) { - std::stringstream ss; - ss << *error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; - *error_string = ss.str(); + // optimize performance by not using stringstream + char str[1000]; + snprintf(str, 1000, "%s, when looking up transform from frame [%s] to frame [%s]", error_string->c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); + *error_string = str; } return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR; @@ -460,9 +461,10 @@ int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, { if (error_string) { - std::stringstream ss; - ss << extrapolation_error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; - *error_string = ss.str(); + // optimize performance by not using stringstream + char str[1000]; + snprintf(str, 1000, "%s, when looking up transform from frame [%s] to frame [%s]", extrapolation_error_string.c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); + *error_string = str; } return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR; diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index b1a695a85..180630126 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -66,9 +66,9 @@ void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* erro { if (error_str) { - std::stringstream ss; - ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer"; - *error_str = ss.str(); + char str[116]; // Text without formatting strings has 76, each timestamp has up to 20 + snprintf(str, 116, "Lookup would require extrapolation at time %.09f, but only time %.09f is in the buffer", t0.toSec(), t1.toSec()); + *error_str = str; } } @@ -76,9 +76,9 @@ void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* erro { if (error_str) { - std::stringstream ss; - ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1; - *error_str = ss.str(); + char str[141]; // Text without formatting strings has 100, each timestamp has up to 20 + snprintf(str, 141, "Lookup would require extrapolation into the future. Requested time %.09f but the latest data is at time %.09f", t0.toSec(), t1.toSec()); + *error_str = str; } } @@ -86,9 +86,9 @@ void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* erro { if (error_str) { - std::stringstream ss; - ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1; - *error_str = ss.str(); + char str[141]; // Text without formatting strings has 100, each timestamp has up to 20 + snprintf(str, 141, "Lookup would require extrapolation into the past. Requested time %.09f but the earliest data is at time %.09f", t0.toSec(), t1.toSec()); + *error_str = str; } } } // namespace cache @@ -252,9 +252,7 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ { if (error_str) { - std::stringstream ss; - ss << "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; - *error_str = ss.str(); + *error_str = "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; } return false; } @@ -271,9 +269,7 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ { if (error_str) { - std::stringstream ss; - ss << "TF_REPEATED_DATA ignoring data with redundant timestamp"; - *error_str = ss.str(); + *error_str = "TF_REPEATED_DATA ignoring data with redundant timestamp"; } return false; } From 1c613fb49e0bf15e0adfd8eca58481970fea845e Mon Sep 17 00:00:00 2001 From: Jochen Sprickerhof Date: Mon, 20 Jul 2020 10:01:18 +0200 Subject: [PATCH 2/8] Use list instead of set to make build reproducible (#473) --- tf2_geometry_msgs/setup.py | 2 +- tf2_sensor_msgs/setup.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_geometry_msgs/setup.py b/tf2_geometry_msgs/setup.py index e0232f987..689f124b6 100644 --- a/tf2_geometry_msgs/setup.py +++ b/tf2_geometry_msgs/setup.py @@ -6,7 +6,7 @@ d = generate_distutils_setup( packages=['tf2_geometry_msgs'], package_dir={'': 'src'}, - requires={'rospy','geometry_msgs','tf2_ros','orocos_kdl'} + requires=['rospy','geometry_msgs','tf2_ros','orocos_kdl'] ) setup(**d) diff --git a/tf2_sensor_msgs/setup.py b/tf2_sensor_msgs/setup.py index bf9fc1e6d..c6c730de9 100644 --- a/tf2_sensor_msgs/setup.py +++ b/tf2_sensor_msgs/setup.py @@ -6,7 +6,7 @@ d = generate_distutils_setup( packages=['tf2_sensor_msgs'], package_dir={'': 'src'}, - requires={'rospy','sensor_msgs','tf2_ros','orocos_kdl'} + requires=['rospy','sensor_msgs','tf2_ros','orocos_kdl'] ) setup(**d) From 7d95bb259cff8f7b914266b04b6621b7f92f92e4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 24 Aug 2020 18:30:54 +0200 Subject: [PATCH 3/8] Cherry-picking various commits from Melodic (#471) * Revert "rework Eigen functions namespace hack" (#436) * Fixed warnings in message_filter.h (#434) the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall * Fix ambiguous call for tf2::convert on MSVC (#444) * rework ambiguous call on MSVC. Co-authored-by: Tully Foote Co-authored-by: moooeeeep Co-authored-by: Sean Yen --- test_tf2/CMakeLists.txt | 2 +- test_tf2/package.xml | 2 + test_tf2/test/test_convert.cpp | 13 +++ tf2/include/tf2/convert.h | 68 ++++++++++++++- tf2/include/tf2/impl/convert.h | 12 +++ tf2/include/tf2/transform_functions.h | 103 ----------------------- tf2_eigen/include/tf2_eigen/tf2_eigen.h | 65 +++++++++++++- tf2_ros/include/tf2_ros/message_filter.h | 4 +- 8 files changed, 159 insertions(+), 110 deletions(-) delete mode 100644 tf2/include/tf2/transform_functions.h diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index b99e10e21..9b9fa8440 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(test_tf2) -find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs) +find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs tf2_eigen) find_package(Boost REQUIRED COMPONENTS thread) find_package(orocos_kdl REQUIRED) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index b88f6f39e..1b3d56abe 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs tf2_kdl tf2_msgs + tf2_eigen rosconsole roscpp @@ -34,6 +35,7 @@ tf2_geometry_msgs tf2_kdl tf2_msgs + tf2_eigen rosunit rosbash diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index d5fb2629e..a8b0dfe71 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include TEST(tf2Convert, kdlToBullet) @@ -97,6 +98,18 @@ TEST(tf2Convert, kdlBulletROSConversions) EXPECT_NEAR(b1.z(), b4.z(), epsilon); } +TEST(tf2Convert, ConvertTf2Quaternion) +{ + tf2::Quaternion tq(1,2,3,4); + Eigen::Quaterniond eq; + tf2::convert(tq, eq); + + EXPECT_EQ(tq.w(), eq.w()); + EXPECT_EQ(tq.x(), eq.x()); + EXPECT_EQ(tq.y(), eq.y()); + EXPECT_EQ(tq.z(), eq.z()); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 5c6d55fa7..36efd5be8 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -32,11 +32,76 @@ #ifndef TF2_CONVERT_H #define TF2_CONVERT_H -#include + +#include +#include +#include #include 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. + * + * This method needs to be implemented by client library developers + */ +template + void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform); + +/**\brief Get the timestamp from data + * \param t The data input. + * \return The timestamp associated with the data. The lifetime of the returned + * reference is bound to the lifetime of the argument. + */ +template + const ros::Time& getTimestamp(const T& t); + +/**\brief Get the frame_id from data + * \param t The data input. + * \return The frame_id associated with the data. The lifetime of the returned + * reference is bound to the lifetime of the argument. + */ +template + const std::string& getFrameId(const T& t); + + + +/* An implementation for Stamped

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

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

datatypes */ +template + const 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 + * 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); + +/** 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); + /** 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 @@ -59,6 +124,7 @@ template a2 = a1; } + } #endif //TF2_CONVERT_H diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 6ccca3128..6b68ccdd4 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -55,21 +55,33 @@ template <> template inline void Converter::convert(const A& a, B& b) { +#ifdef _MSC_VER + tf2::fromMsg(a, b); +#else fromMsg(a, b); +#endif } template <> template inline void Converter::convert(const A& a, B& b) { +#ifdef _MSC_VER + b = tf2::toMsg(a); +#else b = toMsg(a); +#endif } template <> template inline void Converter::convert(const A& a, B& b) { +#ifdef _MSC_VER + tf2::fromMsg(tf2::toMsg(a), b); +#else fromMsg(toMsg(a), b); +#endif } } diff --git a/tf2/include/tf2/transform_functions.h b/tf2/include/tf2/transform_functions.h deleted file mode 100644 index 7b49ad37a..000000000 --- a/tf2/include/tf2/transform_functions.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (c) 2013, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Tully Foote */ - -#ifndef TF2_TRANSFORM_FUNCTIONS_H -#define TF2_TRANSFORM_FUNCTIONS_H - -#include -#include -#include - -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. - * - * This method needs to be implemented by client library developers - */ -template - void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform); - -/**\brief Get the timestamp from data - * \param t The data input. - * \return The timestamp associated with the data. The lifetime of the returned - * reference is bound to the lifetime of the argument. - */ -template - const ros::Time& getTimestamp(const T& t); - -/**\brief Get the frame_id from data - * \param t The data input. - * \return The frame_id associated with the data. The lifetime of the returned - * reference is bound to the lifetime of the argument. - */ -template - const std::string& getFrameId(const T& t); - -/* An implementation for Stamped

datatypes */ -template - const ros::Time& getTimestamp(const tf2::Stamped

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

datatypes */ -template - const 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 - * 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); - -/** 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); - -} - -#endif //TF2_TRANSFORM_FUNCTIONS_H diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index f2bc67120..00c789ba7 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -29,7 +29,7 @@ #ifndef TF2_EIGEN_H #define TF2_EIGEN_H -#include +#include #include #include #include @@ -520,7 +520,66 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped + +namespace Eigen { +// This is needed to make the usage of the following conversion functions usable in tf2::convert(). +// According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or +// in an associated namespace of one of its arguments. The stamped versions of this conversion +// functions work because they have tf2::Stamped as an argument which is the same namespace as +// which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is +// defined in tf2, so it take the following definitions in Eigen namespace to make them usable in +// tf2::convert(). + +inline +geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { + return tf2::toMsg(in); +} + +inline +geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Point toMsg(const Eigen::Vector3d& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { + tf2::fromMsg(msg, out); +} + +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { + tf2::fromMsg(msg, out); +} + +} // namespace #endif // TF2_EIGEN_H diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index 9cb0d3dae..d74104297 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -456,8 +456,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5)); } - void transformable(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame, - ros::Time time, tf2::TransformableResult result) + void transformable(tf2::TransformableRequestHandle request_handle, const std::string& /* target_frame */, const std::string& /* source_frame */, + ros::Time /* time */, tf2::TransformableResult result) { namespace mt = ros::message_traits; From 87a48fc805e6659ede3090f624f9d6cb79a90188 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Mon, 24 Aug 2020 09:32:29 -0700 Subject: [PATCH 4/8] Output time difference of extrapolation exceptions (#477) --- tf2/src/cache.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index b1a695a85..be5c15cda 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -77,7 +77,9 @@ void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* erro if (error_str) { std::stringstream ss; - ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1; + ros::Duration tdiff = t0 - t1; + ss << "Lookup would require extrapolation " << tdiff << "s into the future. Requested time " + << t0 << " but the latest data is at time " << t1; *error_str = ss.str(); } } @@ -87,7 +89,9 @@ void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* erro if (error_str) { std::stringstream ss; - ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1; + ros::Duration tdiff = t1 - t0; + ss << "Lookup would require extrapolation " << tdiff << "s into the past. Requested time " + << t0 << " but the earliest data is at time " << t1; *error_str = ss.str(); } } From 936018a936f0f34c80f0a7d76852f2227a3c890a Mon Sep 17 00:00:00 2001 From: Sean Yen Date: Mon, 24 Aug 2020 09:34:00 -0700 Subject: [PATCH 5/8] avoid name collision b/t tf2_py and tf2 (#478) --- tf2_py/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_py/CMakeLists.txt b/tf2_py/CMakeLists.txt index 18dbf2c8c..bb53f9d35 100644 --- a/tf2_py/CMakeLists.txt +++ b/tf2_py/CMakeLists.txt @@ -96,7 +96,7 @@ add_dependencies(tf2_py ${catkin_EXPORTED_TARGETS}) if(WIN32) # use .pyd extension on Windows - set_target_properties(tf2_py PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".pyd") + set_target_properties(tf2_py PROPERTIES OUTPUT_NAME "_tf2" SUFFIX ".pyd") else() set_target_properties(tf2_py PROPERTIES COMPILE_FLAGS "-g -Wno-missing-field-initializers") set_target_properties(tf2_py PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") From ad04943f23608ab757389ce57d04f110df1c692b Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 24 Aug 2020 18:36:53 +0200 Subject: [PATCH 6/8] Use correct frame service name in docstrings. (#476) Replaces the deprecated names {tf_frames, view_frames} -> tf2_frames --- tf2_ros/include/tf2_ros/buffer.h | 4 ++-- tf2_ros/src/tf2_ros/buffer.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 88ac1be79..835dfa2f6 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -45,7 +45,7 @@ namespace tf2_ros /** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type. * * Inherits tf2_ros::BufferInterface and tf2::BufferCore. - * Stores known frames and offers a ROS service, "tf_frames", which responds to client requests + * Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests * with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames. */ class Buffer: public BufferInterface, public tf2::BufferCore @@ -57,7 +57,7 @@ namespace tf2_ros /** * @brief Constructor for a Buffer object * @param cache_time How long to keep a history of transforms - * @param debug Whether to advertise the view_frames service that exposes debugging information from the buffer + * @param debug Whether to advertise the tf2_frames service that exposes debugging information from the buffer * @return */ Buffer(ros::Duration cache_time = ros::Duration(BufferCore::DEFAULT_CACHE_TIME), bool debug = false); diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 3d14ac8e8..30013c0a4 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -39,7 +39,7 @@ class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): Inherits from :class:`tf2_ros.buffer_interface.BufferInterface` and :class:`tf2.BufferCore`. - Stores known frames and offers a ROS service, "tf_frames", which responds to client requests + Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests with a response containing a :class:`tf2_msgs.FrameGraph` representing the relationship of known frames. """ From 34a4154c25621a4f2b9a714859fa57678708bf3b Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 24 Aug 2020 18:59:30 +0200 Subject: [PATCH 7/8] Do not waste time constructing error string if nobody is interested in it in canTransform(). (#469) --- tf2/src/buffer_core.cpp | 2 +- tf2/test/speed_test.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index c9bcb07d4..9df5daee5 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -358,7 +358,7 @@ int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, break; } - CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string); + CompactFrameID parent = f.gather(cache, time, error_string ? &extrapolation_error_string : NULL); if (parent == 0) { // Just break out here... there may still be a path from source -> target diff --git a/tf2/test/speed_test.cpp b/tf2/test/speed_test.cpp index c63ca18c7..c7987fa96 100644 --- a/tf2/test/speed_test.cpp +++ b/tf2/test/speed_test.cpp @@ -222,4 +222,31 @@ int main(int argc, char** argv) CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + bc.canTransform(v_frame1, v_frame0, ros::Time(3)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + std::string str; + for (uint32_t i = 0; i < count; ++i) + { + bc.canTransform(v_frame1, v_frame0, ros::Time(3), &str); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif } From 217d436f653c253c94838dae70c8b40dddc67c90 Mon Sep 17 00:00:00 2001 From: Fabian Freihube Date: Tue, 25 Aug 2020 21:47:15 +0200 Subject: [PATCH 8/8] test_tf2: remove unused tf dependency (#464) Co-authored-by: Tully Foote --- test_tf2/CMakeLists.txt | 2 +- test_tf2/package.xml | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 9b9fa8440..b57dd70e0 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(test_tf2) -find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs tf2_eigen) +find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_ros) find_package(Boost REQUIRED COMPONENTS thread) find_package(orocos_kdl REQUIRED) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index 1b3d56abe..86d907623 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -16,7 +16,6 @@ rosconsole roscpp rostest - tf tf2 tf2_bullet tf2_ros @@ -28,7 +27,6 @@ rosconsole roscpp rostest - tf tf2 tf2_bullet tf2_ros