TEST(TimeCache, DuplicateEntries)
{
TimeCache cache;
TransformStorage stor;
setIdentity(stor);
stor.frame_id_ = 3;
stor.stamp_ = ros::Time().fromNSec(1);
cache.insertData(stor);
cache.insertData(stor);
cache.getData(ros::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()));
}
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()));
}
bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority)
{
StampedTransform mapped_transform((tf::Transform)transform, transform.stamp_, transform.frame_id_, transform.child_frame_id_);
mapped_transform.child_frame_id_ = assert_resolved(tf_prefix_, transform.child_frame_id_);
mapped_transform.frame_id_ = assert_resolved(tf_prefix_, transform.frame_id_);
bool error_exists = false;
if (mapped_transform.child_frame_id_ == mapped_transform.frame_id_)
{
fprintf(stderr,"TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same\n", authority.c_str(), mapped_transform.child_frame_id_.c_str());
error_exists = true;
}
if (mapped_transform.child_frame_id_ == "/")//empty frame id will be mapped to "/"
{
fprintf(stderr,"TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set\n", authority.c_str());
error_exists = true;
}
if (mapped_transform.frame_id_ == "/")//empty parent id will be mapped to "/"
{
fprintf(stderr,"TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set\n", mapped_transform.child_frame_id_.c_str(), authority.c_str());
error_exists = true;
}
if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w()))
{
fprintf(stderr,"TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
mapped_transform.child_frame_id_.c_str(), authority.c_str(),
mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
);
error_exists = true;
}
if (error_exists)
return false;
{
boost::recursive_mutex::scoped_lock lock(frame_mutex_);
CompactFrameID frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id_);
TimeCache* frame = getFrame(frame_number);
if (frame == NULL)
{
frames_[frame_number] = new TimeCache(cache_time);
frame = frames_[frame_number];
}
if (frame->insertData(TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.frame_id_), frame_number)))
{
frame_authority_[frame_number] = authority;
}
else
{
fprintf(stderr,"TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at\n", mapped_transform.child_frame_id_.c_str(), mapped_transform.stamp_.toSec(), authority.c_str());
return false;
}
}
{
boost::mutex::scoped_lock lock(transforms_changed_mutex_);
transforms_changed_();
}
return true;
};