321 lines
12 KiB
C++
321 lines
12 KiB
C++
/*
|
|
* 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();
|
|
}
|