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())); }