TEST(TimeCache, RepeatabilityReverseInsertOrder)
{
unsigned int runs = 100;
seed_rand();
tf::TimeCache cache;
std::vector<double> values(runs);
StampedTransform t;
t.setIdentity();
for ( int i = runs -1; i >= 0 ; i-- )
{
values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
t.stamp_ = ros::Time().fromNSec(i);
TransformStorage stor(t, i, 0);
cache.insertData(stor);
}
TransformStorage out;
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), out);
EXPECT_EQ(out.frame_id_, i);
EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
}
}
TEST(TimeCache, RepeatabilityRandomInsertOrder)
{
seed_rand();
tf::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();
StampedTransform t;
t.setIdentity();
for ( uint64_t i = 0; i <runs ; i++ )
{
values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
t.stamp_ = ros::Time().fromNSec(i);
TransformStorage stor(t, i, 0);
cache.insertData(stor);
}
TransformStorage out;
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), out);
EXPECT_EQ(out.frame_id_, i);
EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i));
}
}
TEST(TimeCache, AngularInterpolation)
{
uint64_t runs = 100;
double epsilon = 1e-6;
seed_rand();
tf::TimeCache cache;
std::vector<double> yawvalues(2);
std::vector<double> pitchvalues(2);
std::vector<double> rollvalues(2);
uint64_t offset = 200;
std::vector<btQuaternion> quats(2);
StampedTransform t;
t.setIdentity();
for ( uint64_t i = 1; i < runs ; i++ )
{
for (uint64_t step = 0; step < 2 ; step++)
{
yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0;
pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
t.setRotation(quats[step]);
t.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
TransformStorage stor(t, 3, 0);
cache.insertData(stor);
}
TransformStorage out;
for (int pos = 0; pos < 100 ; pos ++)
{
cache.getData(ros::Time().fromNSec(offset + pos), out); //get the transform for the position
btQuaternion quat = out.rotation_; //get the quaternion out of the transform
//Generate a ground truth quaternion directly calling slerp
btQuaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
//Make sure the transformed one and the direct call match
EXPECT_NEAR(0, angle(ground_truth, quat), epsilon);
}
cache.clearList();
}
}
TEST(TimeCache, ZeroAtFront)
{
uint64_t runs = 100;
seed_rand();
tf::TimeCache cache;
std::vector<double> values(runs);
StampedTransform t;
t.setIdentity();
for ( uint64_t i = 0; i <runs ; i++ )
{
values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
t.stamp_ = ros::Time().fromNSec(i);
TransformStorage stor(t, i, 0);
cache.insertData(stor);
}
t.stamp_ = ros::Time().fromNSec(runs);
TransformStorage stor(t, runs, 0);
cache.insertData(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
}
cache.getData(ros::Time(), stor);
EXPECT_EQ(stor.frame_id_, runs);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
t.stamp_ = ros::Time().fromNSec(runs+1);
TransformStorage stor2(t, runs+1, 0);
cache.insertData(stor2);
//Make sure we get a different value now that a new values is added at the front
cache.getData(ros::Time(), stor);
EXPECT_EQ(stor.frame_id_, runs+1);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
}
TEST(TimeCache, CartesianInterpolation)
{
uint64_t runs = 100;
double epsilon = 1e-6;
seed_rand();
tf::TimeCache cache;
std::vector<double> xvalues(2);
std::vector<double> yvalues(2);
std::vector<double> zvalues(2);
uint64_t offset = 200;
StampedTransform t;
t.setIdentity();
for ( uint64_t i = 1; i < runs ; i++ )
{
for (uint64_t step = 0; step < 2 ; step++)
{
xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
t.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
t.stamp_ = ros::Time().fromNSec(step * 100 + offset);
TransformStorage stor(t, 2, 0);
cache.insertData(stor);
}
TransformStorage out;
for (int pos = 0; pos < 100 ; pos++)
{
cache.getData(ros::Time().fromNSec(offset + pos), out);
double x_out = out.translation_.x();
double y_out = out.translation_.y();
double z_out = out.translation_.z();
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();
}
}
void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
const Time& time, StampedTransform& transform) const
{
std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame);
std::string mapped_src = assert_resolved(tf_prefix_, source_frame);
if (mapped_tgt == mapped_src) {
transform.setIdentity();
transform.child_frame_id_ = mapped_src;
transform.frame_id_ = mapped_tgt;
transform.stamp_ = now();
return;
}
boost::recursive_mutex::scoped_lock lock(frame_mutex_);
CompactFrameID target_id = lookupFrameNumber(mapped_tgt);
CompactFrameID source_id = lookupFrameNumber(mapped_src);
std::string error_string;
TransformAccum accum;
int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
if (retval != NO_ERROR)
{
switch (retval)
{
case CONNECTIVITY_ERROR:
throw ConnectivityException(error_string);
case EXTRAPOLATION_ERROR:
throw ExtrapolationException(error_string);
case LOOKUP_ERROR:
throw LookupException(error_string);
default:
fprintf(stderr,"Unknown error code: %d\n", retval);
break;
}
}
transform.setOrigin(accum.result_vec);
transform.setRotation(accum.result_quat);
transform.child_frame_id_ = mapped_src;
transform.frame_id_ = mapped_tgt;
transform.stamp_ = accum.time;
};
TEST(TimeCache, DuplicateEntries)
{
TimeCache cache;
StampedTransform t;
t.setIdentity();
t.stamp_ = ros::Time().fromNSec(1);
TransformStorage stor(t, 3, 0);
cache.insertData(stor);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(1), stor);
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()));
}
