tf2: simple_tf2_core.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <gtest/gtest.h>
00031 #include <tf2/buffer_core.h>
00032 #include <sys/time.h>
00033 #include <ros/ros.h>
00034 #include "tf2/LinearMath/Vector3.h"
00035 #include "tf2/exceptions.h"
00036 
00037 TEST(tf2, setTransformFail)
00038 {
00039   tf2::BufferCore tfc;
00040   geometry_msgs::TransformStamped st;
00041   EXPECT_FALSE(tfc.setTransform(st, "authority1"));
00042 
00043 }
00044 
00045 TEST(tf2, setTransformValid)
00046 {
00047   tf2::BufferCore tfc;
00048   geometry_msgs::TransformStamped st;
00049   st.header.frame_id = "foo";
00050   st.header.stamp = ros::Time(1.0);
00051   st.child_frame_id = "child";
00052   st.transform.rotation.w = 1;
00053   EXPECT_TRUE(tfc.setTransform(st, "authority1"));
00054 
00055 }
00056 
00057 TEST(tf2, setTransformInvalidQuaternion)
00058 {
00059   tf2::BufferCore tfc;
00060   geometry_msgs::TransformStamped st;
00061   st.header.frame_id = "foo";
00062   st.header.stamp = ros::Time(1.0);
00063   st.child_frame_id = "child";
00064   st.transform.rotation.w = 0;
00065   EXPECT_FALSE(tfc.setTransform(st, "authority1"));
00066 
00067 }
00068 
00069 TEST(tf2_lookupTransform, LookupException_Nothing_Exists)
00070 {
00071   tf2::BufferCore tfc;
00072   EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf2::LookupException);
00073 
00074 }
00075 
00076 TEST(tf2_canTransform, Nothing_Exists)
00077 {
00078   tf2::BufferCore tfc;
00079   EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0)));
00080 
00081   std::string error_msg = std::string();
00082   EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg));
00083   ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist.");
00084 
00085 }
00086 
00087 TEST(tf2_lookupTransform, LookupException_One_Exists)
00088 {
00089   tf2::BufferCore tfc;
00090   geometry_msgs::TransformStamped st;
00091   st.header.frame_id = "foo";
00092   st.header.stamp = ros::Time(1.0);
00093   st.child_frame_id = "child";
00094   st.transform.rotation.w = 1;
00095   EXPECT_TRUE(tfc.setTransform(st, "authority1"));
00096   EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf2::LookupException);
00097 
00098 }
00099 
00100 TEST(tf2_canTransform, One_Exists)
00101 {
00102   tf2::BufferCore tfc;
00103   geometry_msgs::TransformStamped st;
00104   st.header.frame_id = "foo";
00105   st.header.stamp = ros::Time(1.0);
00106   st.child_frame_id = "child";
00107   st.transform.rotation.w = 1;
00108   EXPECT_TRUE(tfc.setTransform(st, "authority1"));
00109   EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0)));
00110 }
00111 
00112 TEST(tf2_chainAsVector, chain_v_configuration)
00113 {
00114   tf2::BufferCore mBC;
00115   tf2::TestBufferCore tBC;
00116 
00117   geometry_msgs::TransformStamped st;
00118   st.header.stamp = ros::Time(0);
00119   st.transform.rotation.w = 1;
00120 
00121   st.header.frame_id = "a";
00122   st.child_frame_id = "b";
00123   mBC.setTransform(st, "authority1");
00124 
00125   st.header.frame_id = "b";
00126   st.child_frame_id = "c";
00127   mBC.setTransform(st, "authority1");
00128 
00129   st.header.frame_id = "a";
00130   st.child_frame_id = "d";
00131   mBC.setTransform(st, "authority1");
00132 
00133   st.header.frame_id = "d";
00134   st.child_frame_id = "e";
00135   mBC.setTransform(st, "authority1");
00136 
00137   std::vector<std::string> chain;
00138   
00139   
00140   mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain);
00141   EXPECT_EQ( 0, chain.size());
00142 
00143   mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain);
00144   EXPECT_EQ( 3, chain.size());
00145   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
00146   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
00147   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
00148 
00149   mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain);
00150   EXPECT_EQ( 3, chain.size());
00151   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
00152   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
00153   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
00154 
00155   mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain);
00156   EXPECT_EQ( 3, chain.size());
00157   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
00158   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
00159   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
00160 
00161   mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain);
00162   EXPECT_EQ( 3, chain.size());
00163   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
00164   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
00165   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
00166   
00167   mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain);
00168 
00169   EXPECT_EQ( 5, chain.size());
00170   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
00171   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
00172   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
00173   if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
00174   if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
00175 
00176   mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain);
00177 
00178   EXPECT_EQ( 5, chain.size());
00179   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
00180   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
00181   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
00182   if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
00183   if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
00184 
00185   mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain);
00186 
00187   EXPECT_EQ( 5, chain.size());
00188   if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
00189   if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" );
00190   if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
00191   if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
00192   if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
00193 }
00194 
00195 TEST(tf2_walkToTopParent, walk_i_configuration)
00196 {
00197   tf2::BufferCore mBC;
00198   tf2::TestBufferCore tBC;
00199 
00200   geometry_msgs::TransformStamped st;
00201   st.header.stamp = ros::Time(0);
00202   st.transform.rotation.w = 1;
00203 
00204   st.header.frame_id = "a";
00205   st.child_frame_id = "b";
00206   mBC.setTransform(st, "authority1");
00207 
00208   st.header.frame_id = "b";
00209   st.child_frame_id = "c";
00210   mBC.setTransform(st, "authority1");
00211 
00212   st.header.frame_id = "c";
00213   st.child_frame_id = "d";
00214   mBC.setTransform(st, "authority1");
00215 
00216   st.header.frame_id = "d";
00217   st.child_frame_id = "e";
00218   mBC.setTransform(st, "authority1");
00219 
00220   std::vector<tf2::CompactFrameID> id_chain;
00221   tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
00222 
00223   EXPECT_EQ(5, id_chain.size() );
00224   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
00225   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
00226   if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
00227   if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
00228   if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4]));
00229 
00230   id_chain.clear();
00231   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
00232 
00233   EXPECT_EQ(5, id_chain.size() );
00234   if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
00235   if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
00236   if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2]));
00237   if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
00238   if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
00239 
00240 }
00241 
00242 TEST(tf2_walkToTopParent, walk_v_configuration)
00243 {
00244   tf2::BufferCore mBC;
00245   tf2::TestBufferCore tBC;
00246 
00247   geometry_msgs::TransformStamped st;
00248   st.header.stamp = ros::Time(0);
00249   st.transform.rotation.w = 1;
00250 
00251   // st.header.frame_id = "aaa";
00252   // st.child_frame_id = "aa";
00253   // mBC.setTransform(st, "authority1");
00254   // 
00255   // st.header.frame_id = "aa";
00256   // st.child_frame_id = "a";
00257   // mBC.setTransform(st, "authority1");  
00258 
00259   st.header.frame_id = "a";
00260   st.child_frame_id = "b";
00261   mBC.setTransform(st, "authority1");
00262 
00263   st.header.frame_id = "b";
00264   st.child_frame_id = "c";
00265   mBC.setTransform(st, "authority1");
00266 
00267   st.header.frame_id = "a";
00268   st.child_frame_id = "d";
00269   mBC.setTransform(st, "authority1");
00270 
00271   st.header.frame_id = "d";
00272   st.child_frame_id = "e";
00273   mBC.setTransform(st, "authority1");
00274 
00275   std::vector<tf2::CompactFrameID> id_chain;
00276   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
00277 
00278   EXPECT_EQ(5, id_chain.size());
00279   if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0]));
00280   if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1]));
00281   if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
00282   if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
00283   if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
00284 
00285   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
00286   EXPECT_EQ(5, id_chain.size());
00287   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
00288   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
00289   if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
00290   if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
00291   if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4]));
00292 
00293   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
00294   EXPECT_EQ( id_chain.size(), 3 );
00295   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
00296   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
00297   if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
00298 
00299   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
00300   EXPECT_EQ( id_chain.size(), 3 );
00301   if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
00302   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
00303   if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2]));
00304 
00305   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
00306   EXPECT_EQ( id_chain.size(), 2 );
00307   if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0]));
00308   if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1]));
00309 
00310   tBC._walkToTopParent(mBC,  ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
00311   EXPECT_EQ( id_chain.size(), 2 );
00312   if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
00313   if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
00314 }
00315 
00316 
00317 int main(int argc, char **argv){
00318   testing::InitGoogleTest(&argc, argv);
00319   ros::Time::init(); //needed for ros::TIme::now()
00320   return RUN_ALL_TESTS();
00321 }