Use snprintf instead of stringstream to increase performance of lookupTransform() in error cases.

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 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)
 

test_tf2/package.xml

@@ -16,24 +16,24 @@
   <build_depend>rosconsole</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rostest</build_depend>
-  <build_depend>tf</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_bullet</build_depend>
   <build_depend>tf2_ros</build_depend>
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>tf2_kdl</build_depend>
   <build_depend>tf2_msgs</build_depend>
+  <build_depend>tf2_eigen</build_depend>
 
   <run_depend>rosconsole</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rostest</run_depend>
-  <run_depend>tf</run_depend>
   <run_depend>tf2</run_depend>
   <run_depend>tf2_bullet</run_depend>
   <run_depend>tf2_ros</run_depend>
   <run_depend>tf2_geometry_msgs</run_depend>
   <run_depend>tf2_kdl</run_depend>
   <run_depend>tf2_msgs</run_depend>
+  <run_depend>tf2_eigen</run_depend>
 
   <test_depend>rosunit</test_depend>
   <test_depend>rosbash</test_depend>

test_tf2/test/test_convert.cpp

@@ -39,6 +39,7 @@
 #include <tf2_kdl/tf2_kdl.h>
 #include <tf2_bullet/tf2_bullet.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <ros/time.h>
 
 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);

tf2/include/tf2/convert.h

@@ -32,11 +32,76 @@
 #ifndef TF2_CONVERT_H
 #define TF2_CONVERT_H
 
-#include <tf2/transform_functions.h>
+
+#include <tf2/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <tf2/impl/convert.h>
 
 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 <class T>
+  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 <class T>
+  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 <class T>
+  const std::string& getFrameId(const T& t);
+
+
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const ros::Time& getTimestamp(const tf2::Stamped<P>& t)
+  {
+    return t.stamp_;
+  }
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const std::string& getFrameId(const tf2::Stamped<P>& t)
+  {
+    return t.frame_id_;
+  }
+
+/** Function that converts from one type to a ROS message type. It has to be
+ * implemented by each data type in 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<typename A, typename B>
+  B toMsg(const A& a);
+
+/** Function that converts from a ROS message type to another type. It has to be
+ * implemented by each data type in 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<typename A, typename B>
+  void fromMsg(const A&, B& b);
+
 /** Function that converts any type to any type (messages or not).
  * Matching toMsg and from Msg conversion functions need to exist.
  * If they don't exist or do not apply (for example, if your two
@@ -59,6 +124,7 @@ template <class A>
       a2 = a1;
   }
 
+
 }
 
 #endif //TF2_CONVERT_H

tf2/include/tf2/impl/convert.h

@@ -55,21 +55,33 @@ template <>
 template <typename A, typename B>
 inline void Converter<true, false>::convert(const A& a, B& b)
 {
+#ifdef _MSC_VER
+  tf2::fromMsg(a, b);
+#else
   fromMsg(a, b);
+#endif
 }
 
 template <>
 template <typename A, typename B>
 inline void Converter<false, true>::convert(const A& a, B& b)
 {
+#ifdef _MSC_VER
+  b = tf2::toMsg(a);
+#else
   b = toMsg(a);
+#endif
 }
 
 template <>
 template <typename A, typename B>
 inline void Converter<false, false>::convert(const A& a, B& b)
 {
+#ifdef _MSC_VER
+  tf2::fromMsg(tf2::toMsg(a), b);
+#else
   fromMsg(toMsg(a), b);
+#endif
 }
 
 }

tf2/include/tf2/time_cache.h

@@ -36,8 +36,6 @@
 
 #include <deque>
 
-#include <sstream>
-
 #include <ros/message_forward.h>
 #include <ros/time.h>
 

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
@@ -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;