first commit

test/cache_unittest.cpp

@@ -0,0 +1,414 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+#include <tf3/time_cache.h>
+#include "tf3/LinearMath/Quaternion.h"
+#include <stdexcept>
+
+#include <robot_geometry_msgs/TransformStamped.h>
+
+#include <cmath>
+
+std::vector<double> values;
+unsigned int step = 0;
+
+void seed_rand()
+{
+  values.clear();
+  for (unsigned int i = 0; i < 1000; i++)
+  {
+    int pseudo_rand = std::floor(i * M_PI);
+    values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
+    //printf("Seeding with %f\n", values.back());
+  }
+};
+
+
+double get_rand() 
+{ 
+  if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first");
+  if (step >= values.size()) 
+    step = 0;
+  else
+    step++;
+  return values[step];
+}
+
+using namespace tf3;
+
+
+void setIdentity(TransformStorage& stor)
+{
+  stor.translation_.setValue(0.0, 0.0, 0.0);
+  stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
+}
+
+TEST(TimeCache, Repeatability)
+{
+  unsigned int runs = 100;
+  
+  tf3::TimeCache  cache;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  
+  for ( uint64_t i = 1; i < runs ; i++ )
+  {
+    stor.frame_id_ = i;
+    stor.stamp_ = robot::Time().fromNSec(i);
+    
+    cache.insertData(stor);
+  }
+
+  for ( uint64_t i = 1; i < runs ; i++ )
+
+  {
+    cache.getData(robot::Time().fromNSec(i), stor);
+    EXPECT_EQ(stor.frame_id_, i);
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
+  }
+  
+}
+
+TEST(TimeCache, RepeatabilityReverseInsertOrder)
+{
+  unsigned int runs = 100;
+  
+  tf3::TimeCache  cache;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  
+  for ( int i = runs -1; i >= 0 ; i-- )
+  {
+    stor.frame_id_ = i;
+    stor.stamp_ = robot::Time().fromNSec(i);
+    
+    cache.insertData(stor);
+  }
+  for ( uint64_t i = 1; i < runs ; i++ )
+
+  {
+    cache.getData(robot::Time().fromNSec(i), stor);
+    EXPECT_EQ(stor.frame_id_, i);
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
+  }
+  
+}
+
+#if 0    // jfaust: this doesn't seem to actually be testing random insertion?
+TEST(TimeCache, RepeatabilityRandomInsertOrder)
+{
+
+  seed_rand();
+  
+  tf3::TimeCache  cache;
+  double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
+  std::vector<double> values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); 
+  unsigned int runs = values.size();
+
+  TransformStorage stor;
+  setIdentity(stor);
+  for ( uint64_t i = 0; i <runs ; i++ )
+  {
+    values[i] = 10.0 * get_rand();
+    std::stringstream ss;
+    ss << values[i];
+    stor.header.frame_id = ss.str();
+    stor.frame_id_ = i;
+    stor.stamp_ = robot::Time().fromNSec(i);
+    
+    cache.insertData(stor);
+  }
+  for ( uint64_t i = 1; i < runs ; i++ )
+
+  {
+    cache.getData(robot::Time().fromNSec(i), stor);
+    EXPECT_EQ(stor.frame_id_, i);
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
+    std::stringstream ss;
+    ss << values[i];
+    EXPECT_EQ(stor.header.frame_id, ss.str());
+  }
+  
+}
+#endif
+
+TEST(TimeCache, ZeroAtFront)
+{
+  uint64_t runs = 100;
+
+  tf3::TimeCache  cache;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  
+  for ( uint64_t i = 1; i < runs ; i++ )
+  {
+    stor.frame_id_ = i;
+    stor.stamp_ = robot::Time().fromNSec(i);
+    
+    cache.insertData(stor);
+  }
+
+  stor.frame_id_ = runs;
+  stor.stamp_ = robot::Time().fromNSec(runs);
+  cache.insertData(stor);
+
+  for ( uint64_t i = 1; i < runs ; i++ )
+
+  {
+    cache.getData(robot::Time().fromNSec(i), stor);
+    EXPECT_EQ(stor.frame_id_, i);
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
+  }
+
+  cache.getData(robot::Time(), stor);
+  EXPECT_EQ(stor.frame_id_, runs);
+  EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs));
+
+  stor.frame_id_ = runs;
+  stor.stamp_ = robot::Time().fromNSec(runs+1);
+  cache.insertData(stor);
+
+
+  //Make sure we get a different value now that a new values is added at the front
+  cache.getData(robot::Time(), stor);
+  EXPECT_EQ(stor.frame_id_, runs);
+  EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs+1));
+  
+}
+
+TEST(TimeCache, CartesianInterpolation)
+{
+  uint64_t runs = 100;
+  double epsilon = 2e-6;
+  seed_rand();
+  
+  tf3::TimeCache  cache;
+  std::vector<double> xvalues(2);
+  std::vector<double> yvalues(2);
+  std::vector<double> zvalues(2);
+
+  uint64_t offset = 200;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  
+  for ( uint64_t i = 1; i < runs ; i++ )
+  {
+
+    for (uint64_t step = 0; step < 2 ; step++)
+    {
+      xvalues[step] = 10.0 * get_rand();
+      yvalues[step] = 10.0 * get_rand();
+      zvalues[step] = 10.0 * get_rand();
+    
+      stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
+      stor.frame_id_ = 2;
+      stor.stamp_ = robot::Time().fromNSec(step * 100 + offset);
+      cache.insertData(stor);
+    }
+    
+    for (int pos = 0; pos < 100 ; pos ++)
+    {
+      uint64_t time = offset + pos;
+      cache.getData(robot::Time().fromNSec(time), stor);
+      uint64_t time_out = stor.stamp_.toNSec();
+      double x_out = stor.translation_.x();
+      double y_out = stor.translation_.y();
+      double z_out = stor.translation_.z();
+      //      printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out, 
+      //       xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.,
+      //       yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0,
+      //       zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0);
+      EXPECT_EQ(time, time_out);
+      EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
+      EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
+      EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
+    }
+    
+
+    cache.clearList();
+  }
+
+  
+}
+
+/** \brief Make sure we dont' interpolate across reparented data */
+TEST(TimeCache, ReparentingInterpolationProtection)
+{
+  double epsilon = 1e-6;
+  uint64_t offset = 555;
+
+  seed_rand();
+
+  tf3::TimeCache cache;
+  std::vector<double> xvalues(2);
+  std::vector<double> yvalues(2);
+  std::vector<double> zvalues(2);
+
+  TransformStorage stor;
+  setIdentity(stor);
+
+  for (uint64_t step = 0; step < 2 ; step++)
+  {
+    xvalues[step] = 10.0 * get_rand();
+    yvalues[step] = 10.0 * get_rand();
+    zvalues[step] = 10.0 * get_rand();
+
+    stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
+    stor.frame_id_ = step + 4;
+    stor.stamp_ = robot::Time().fromNSec(step * 100 + offset);
+    cache.insertData(stor);
+  }
+  
+  for (int pos = 0; pos < 100 ; pos ++)
+  {
+    EXPECT_TRUE(cache.getData(robot::Time().fromNSec(offset + pos), stor));
+    double x_out = stor.translation_.x();
+    double y_out = stor.translation_.y();
+    double z_out = stor.translation_.z();
+    EXPECT_NEAR(xvalues[0], x_out, epsilon);
+    EXPECT_NEAR(yvalues[0], y_out, epsilon);
+    EXPECT_NEAR(zvalues[0], z_out, epsilon);
+  }
+}
+
+TEST(Bullet, Slerp)
+{
+
+  uint64_t runs = 100;
+  seed_rand();
+
+  tf3::Quaternion q1, q2;
+  q1.setEuler(0,0,0);
+  
+  for (uint64_t i = 0 ; i < runs ; i++)
+  {
+    q2.setEuler(1.0 * get_rand(),
+                1.0 * get_rand(),
+                1.0 * get_rand());
+    
+    
+    tf3::Quaternion q3 = slerp(q1,q2,0.5);
+    
+    EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
+  }
+
+}
+
+
+TEST(TimeCache, AngularInterpolation)
+{
+  uint64_t runs = 100;
+  double epsilon = 1e-6;
+  seed_rand();
+  
+  tf3::TimeCache  cache;
+  std::vector<double> yawvalues(2);
+  std::vector<double> pitchvalues(2);
+  std::vector<double> rollvalues(2);
+  uint64_t offset = 200;
+
+  std::vector<tf3::Quaternion> quats(2);
+
+  TransformStorage stor;
+  setIdentity(stor);
+  
+  for ( uint64_t i = 1; i < runs ; i++ )
+  {
+
+    for (uint64_t step = 0; step < 2 ; step++)
+    {
+      yawvalues[step] = 10.0 * get_rand() / 100.0;
+      pitchvalues[step] = 0;//10.0 * get_rand();
+      rollvalues[step] = 0;//10.0 * get_rand();
+      quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
+      stor.rotation_ = quats[step];
+      stor.frame_id_ = 3;
+      stor.stamp_ = robot::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
+      cache.insertData(stor);
+    }
+    
+    for (int pos = 0; pos < 100 ; pos ++)
+    {
+      uint64_t time = offset + pos;
+      cache.getData(robot::Time().fromNSec(time), stor);
+      uint64_t time_out = stor.stamp_.toNSec();
+      tf3::Quaternion quat (stor.rotation_);
+
+      //Generate a ground truth quaternion directly calling slerp
+      tf3::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
+      
+      //Make sure the transformed one and the direct call match
+      EXPECT_EQ(time, time_out);
+      EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
+            
+    }
+    
+    cache.clearList();
+  }
+
+  
+}
+
+TEST(TimeCache, DuplicateEntries)
+{
+
+  TimeCache cache;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  stor.frame_id_ = 3;
+  stor.stamp_ = robot::Time().fromNSec(1);
+
+  cache.insertData(stor);
+
+  cache.insertData(stor);
+
+
+  cache.getData(robot::Time().fromNSec(1), stor);
+  
+  //printf(" stor is %f\n", stor.translation_.x());
+  EXPECT_TRUE(!std::isnan(stor.translation_.x()));
+  EXPECT_TRUE(!std::isnan(stor.translation_.y()));
+  EXPECT_TRUE(!std::isnan(stor.translation_.z()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
+}
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+

test/simple_tf3_core.cpp

@@ -0,0 +1,320 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+#include <tf3/buffer_core.h>
+#include <ros/time.h>
+#include "tf3/LinearMath/Vector3.h"
+#include "tf3/exceptions.h"
+
+TEST(tf3, setTransformFail)
+{
+  tf3::BufferCore tfc;
+  robot_geometry_msgs::TransformStamped st;
+  EXPECT_FALSE(tfc.setTransform(st, "authority1"));
+
+}
+
+TEST(tf3, setTransformValid)
+{
+  tf3::BufferCore tfc;
+  robot_geometry_msgs::TransformStamped st;
+  st.header.frame_id = "foo";
+  st.header.stamp = robot::Time(1.0);
+  st.child_frame_id = "child";
+  st.transform.rotation.w = 1;
+  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
+
+}
+
+TEST(tf3, setTransformInvalidQuaternion)
+{
+  tf3::BufferCore tfc;
+  robot_geometry_msgs::TransformStamped st;
+  st.header.frame_id = "foo";
+  st.header.stamp = robot::Time(1.0);
+  st.child_frame_id = "child";
+  st.transform.rotation.w = 0;
+  EXPECT_FALSE(tfc.setTransform(st, "authority1"));
+
+}
+
+TEST(tf3_lookupTransform, LookupException_Nothing_Exists)
+{
+  tf3::BufferCore tfc;
+  EXPECT_THROW(tfc.lookupTransform("a", "b", robot::Time().fromSec(1.0)), tf3::LookupException);
+
+}
+
+TEST(tf3_canTransform, Nothing_Exists)
+{
+  tf3::BufferCore tfc;
+  EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0)));
+
+  std::string error_msg = std::string();
+  EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0), &error_msg));
+  ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist.");
+
+}
+
+TEST(tf3_lookupTransform, LookupException_One_Exists)
+{
+  tf3::BufferCore tfc;
+  robot_geometry_msgs::TransformStamped st;
+  st.header.frame_id = "foo";
+  st.header.stamp = robot::Time(1.0);
+  st.child_frame_id = "child";
+  st.transform.rotation.w = 1;
+  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
+  EXPECT_THROW(tfc.lookupTransform("foo", "bar", robot::Time().fromSec(1.0)), tf3::LookupException);
+
+}
+
+TEST(tf3_canTransform, One_Exists)
+{
+  tf3::BufferCore tfc;
+  robot_geometry_msgs::TransformStamped st;
+  st.header.frame_id = "foo";
+  st.header.stamp = robot::Time(1.0);
+  st.child_frame_id = "child";
+  st.transform.rotation.w = 1;
+  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
+  EXPECT_FALSE(tfc.canTransform("foo", "bar", robot::Time().fromSec(1.0)));
+}
+
+TEST(tf3_chainAsVector, chain_v_configuration)
+{
+  tf3::BufferCore mBC;
+  tf3::TestBufferCore tBC;
+
+  robot_geometry_msgs::TransformStamped st;
+  st.header.stamp = robot::Time(0);
+  st.transform.rotation.w = 1;
+
+  st.header.frame_id = "a";
+  st.child_frame_id = "b";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "b";
+  st.child_frame_id = "c";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "a";
+  st.child_frame_id = "d";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "d";
+  st.child_frame_id = "e";
+  mBC.setTransform(st, "authority1");
+
+  std::vector<std::string> chain;
+  
+  
+  mBC._chainAsVector( "c", robot::Time(), "c", robot::Time(), "c", chain);
+  EXPECT_EQ( 0, chain.size());
+
+  mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "c", chain);
+  EXPECT_EQ( 3, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
+
+  mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "c", chain);
+  EXPECT_EQ( 3, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
+
+  mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "a", chain);
+  EXPECT_EQ( 3, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
+
+  mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "a", chain);
+  EXPECT_EQ( 3, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
+  
+  mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "c", chain);
+
+  EXPECT_EQ( 5, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
+  if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
+  if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
+
+  mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "a", chain);
+
+  EXPECT_EQ( 5, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
+  if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
+  if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
+
+  mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "e", chain);
+
+  EXPECT_EQ( 5, chain.size());
+  if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
+  if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
+  if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
+  if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
+  if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
+}
+
+TEST(tf3_walkToTopParent, walk_i_configuration)
+{
+  tf3::BufferCore mBC;
+  tf3::TestBufferCore tBC;
+
+  robot_geometry_msgs::TransformStamped st;
+  st.header.stamp = robot::Time(0);
+  st.transform.rotation.w = 1;
+
+  st.header.frame_id = "a";
+  st.child_frame_id = "b";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "b";
+  st.child_frame_id = "c";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "c";
+  st.child_frame_id = "d";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "d";
+  st.child_frame_id = "e";
+  mBC.setTransform(st, "authority1");
+
+  std::vector<tf3::CompactFrameID> id_chain;
+  tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+
+  EXPECT_EQ(5, id_chain.size() );
+  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
+  if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
+  if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
+  if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4]));
+
+  id_chain.clear();
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
+
+  EXPECT_EQ(5, id_chain.size() );
+  if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
+  if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
+  if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
+  if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
+
+}
+
+TEST(tf3_walkToTopParent, walk_v_configuration)
+{
+  tf3::BufferCore mBC;
+  tf3::TestBufferCore tBC;
+
+  robot_geometry_msgs::TransformStamped st;
+  st.header.stamp = robot::Time(0);
+  st.transform.rotation.w = 1;
+
+  // st.header.frame_id = "aaa";
+  // st.child_frame_id = "aa";
+  // mBC.setTransform(st, "authority1");
+  // 
+  // st.header.frame_id = "aa";
+  // st.child_frame_id = "a";
+  // mBC.setTransform(st, "authority1");  
+
+  st.header.frame_id = "a";
+  st.child_frame_id = "b";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "b";
+  st.child_frame_id = "c";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "a";
+  st.child_frame_id = "d";
+  mBC.setTransform(st, "authority1");
+
+  st.header.frame_id = "d";
+  st.child_frame_id = "e";
+  mBC.setTransform(st, "authority1");
+
+  std::vector<tf3::CompactFrameID> id_chain;
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
+
+  EXPECT_EQ(5, id_chain.size());
+  if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
+  if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
+  if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
+  if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
+
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  EXPECT_EQ(5, id_chain.size());
+  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
+  if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
+  if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
+  if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4]));
+
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  EXPECT_EQ( id_chain.size(), 3 );
+  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
+  if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
+
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
+  EXPECT_EQ( id_chain.size(), 3 );
+  if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
+  if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2]));
+
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
+  EXPECT_EQ( id_chain.size(), 2 );
+  if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1]));
+
+  tBC._walkToTopParent(mBC,  robot::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
+  EXPECT_EQ( id_chain.size(), 2 );
+  if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
+  if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
+}
+
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  robot::Time::init(); //needed for robot::TIme::now()
+  return RUN_ALL_TESTS();
+}

test/speed_test.cpp

@@ -0,0 +1,252 @@
+/*
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tf3/buffer_core.h>
+
+#include <ros/time.h>
+#include <console_bridge/console.h>
+
+#include <boost/lexical_cast.hpp>
+
+int main(int argc, char** argv)
+{
+  uint32_t num_levels = 10;
+  if (argc > 1)
+  {
+    num_levels = boost::lexical_cast<uint32_t>(argv[1]);
+  }
+  double time_interval = 1.0;
+  if (argc > 2)
+  {
+    time_interval = boost::lexical_cast<double>(argv[2]);
+  }
+
+  console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
+
+  tf3::BufferCore bc;
+  robot_geometry_msgs::TransformStamped t;
+  t.header.stamp = robot::Time(1);
+  t.header.frame_id = "root";
+  t.child_frame_id = "0";
+  t.transform.translation.x = 1;
+  t.transform.rotation.w = 1.0;
+  bc.setTransform(t, "me");
+  t.header.stamp = robot::Time(2);
+  bc.setTransform(t, "me");
+
+  for (uint32_t i = 1; i < num_levels / 2; ++i)
+  {
+    for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
+    {
+      std::stringstream parent_ss;
+      parent_ss << (i - 1);
+      std::stringstream child_ss;
+      child_ss << i;
+
+      t.header.stamp = robot::Time(j);
+      t.header.frame_id = parent_ss.str();
+      t.child_frame_id = child_ss.str();
+      bc.setTransform(t, "me");
+    }
+  }
+
+  t.header.frame_id = "root";
+  std::stringstream ss;
+  ss << num_levels/2;
+  t.header.stamp = robot::Time(1);
+  t.child_frame_id = ss.str();
+  bc.setTransform(t, "me");
+  t.header.stamp = robot::Time(2);
+  bc.setTransform(t, "me");
+
+  for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
+  {
+    for (double j = time_interval; j < 2.0 + time_interval; j += time_interval)
+    {
+      std::stringstream parent_ss;
+      parent_ss << (i - 1);
+      std::stringstream child_ss;
+      child_ss << i;
+
+      t.header.stamp = robot::Time(j);
+      t.header.frame_id = parent_ss.str();
+      t.child_frame_id = child_ss.str();
+      bc.setTransform(t, "me");
+    }
+  }
+
+  //logInfo_STREAM(bc.allFramesAsYAML());
+
+  std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
+  std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
+  CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str());
+  robot_geometry_msgs::TransformStamped out_t;
+
+  const uint32_t count = 1000000;
+  CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(0));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1.5));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(2));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      bc.canTransform(v_frame1, v_frame0, robot::Time(0));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      bc.canTransform(v_frame1, v_frame0, robot::Time(1));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      bc.canTransform(v_frame1, v_frame0, robot::Time(1.5));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      bc.canTransform(v_frame1, v_frame0, robot::Time(2));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::WallDuration dur = end - start;
+    //ROS_INFO_STREAM(out_t);
+    CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
+  }
+#endif
+
+#if 01
+  {
+    robot::WallTime start = robot::WallTime::now();
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      bc.canTransform(v_frame1, v_frame0, robot::Time(3));
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::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
+  {
+    robot::WallTime start = robot::WallTime::now();
+    std::string str;
+    for (uint32_t i = 0; i < count; ++i)
+    {
+      bc.canTransform(v_frame1, v_frame0, robot::Time(3), &str);
+    }
+    robot::WallTime end = robot::WallTime::now();
+    robot::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
+}

test/static_cache_test.cpp

@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+#include <tf3/time_cache.h>
+#include <stdexcept>
+
+#include <robot_geometry_msgs/TransformStamped.h>
+
+#include <cmath>
+
+using namespace tf3;
+
+
+void setIdentity(TransformStorage& stor)
+{
+  stor.translation_.setValue(0.0, 0.0, 0.0);
+  stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
+}
+
+TEST(StaticCache, Repeatability)
+{
+  unsigned int runs = 100;
+  
+  tf3::StaticCache  cache;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  
+  for ( uint64_t i = 1; i < runs ; i++ )
+  {
+    stor.frame_id_ = CompactFrameID(i);
+    stor.stamp_ = robot::Time().fromNSec(i);
+    
+    cache.insertData(stor);
+
+    
+    cache.getData(robot::Time().fromNSec(i), stor);
+    EXPECT_EQ(stor.frame_id_, i);
+    EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
+    
+  }
+}
+
+TEST(StaticCache, DuplicateEntries)
+{
+
+  tf3::StaticCache cache;
+
+  TransformStorage stor;
+  setIdentity(stor);
+  stor.frame_id_ = CompactFrameID(3);
+  stor.stamp_ = robot::Time().fromNSec(1);
+
+  cache.insertData(stor);
+
+  cache.insertData(stor);
+
+
+  cache.getData(robot::Time().fromNSec(1), stor);
+  
+  //printf(" stor is %f\n", stor.transform.translation.x);
+  EXPECT_TRUE(!std::isnan(stor.translation_.x()));
+  EXPECT_TRUE(!std::isnan(stor.translation_.y()));
+  EXPECT_TRUE(!std::isnan(stor.translation_.z()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
+  EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
+}
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/test_transform_datatypes.cpp

@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2020, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <gtest/gtest.h>
+
+#include "tf3/transform_datatypes.h"
+
+#include <string>
+
+
+TEST(Stamped, assignment)
+{
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> second("baz", robot::Time(0), "my_frame_id");
+
+  EXPECT_NE(second, first);
+  second = first;
+  EXPECT_EQ(second, first);
+}
+
+TEST(Stamped, setData)
+{
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> second("baz", robot::Time(0), "my_frame_id");
+
+  EXPECT_NE(second, first);
+  second.setData("foobar");
+  EXPECT_EQ(second, first);
+}
+
+TEST(Stamped, copy_constructor)
+{
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> second(first);
+
+  EXPECT_EQ(second, first);
+}
+
+TEST(Stamped, default_constructor)
+{
+  tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
+  tf3::Stamped<std::string> second;
+
+  EXPECT_NE(second, first);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  robot::Time::init();
+  return RUN_ALL_TESTS();
+}