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()));
}
// i optimized this function so the CDTimes don't need to use the timeConvert() function
// with all the + operations. If the data is buffered, it should return very quickly.
// This only ensures that one frame is available at bufferPointer, but that should be
// enough for most things...
inline void FileInterface::seek(const CDTime& cdt)
throw(Exception)
{
using namespace std;
seekTime = cdt;
if (seekTime >= pregapTime)
seekTime -= pregapLength;
if (seekTime < CDLength)
{
if (cacheMode == newMode)
{
// check in the global cache first
if (cache.find(seekTime, holdout))
{
return;
}
}
// see if its in the file cache.
// in both of these cases, bufferPointer should point to the data
// requested
if ( (seekTime >= bufferPos) &&
( (seekTime.getAbsoluteFrame() + 1) <= (bufferEnd.getAbsoluteFrame()) ) )
{
bufferPointer =
fileBuffer + (seekTime.getAbsoluteByte() - bufferPos.getAbsoluteByte());
}
else
{
seekUnbuffered(seekTime);
}
// insert the new data into the cache
if (cacheMode == newMode)
{
holdout = bufferPointer;
cache.insert(seekTime, holdout);
}
}
else
{
Exception e("Seek past end of disc\r\n");
THROW(e);
}
}
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()));
}
int Transformer::getLatestCommonTime(CompactFrameID target_id, CompactFrameID source_id, Time & time, std::string * error_string) const
{
if (source_id == target_id)
{
//Set time to latest timestamp of frameid in case of target and source frame id are the same
time = now();
return NO_ERROR;
}
std::vector<P_TimeAndFrameID> lct_cache;
// Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
// in the target is a direct parent
CompactFrameID frame = source_id;
P_TimeAndFrameID temp;
uint32_t depth = 0;
Time common_time = TIME_MAX;
while (frame != 0)
{
TimeCache* cache = getFrame(frame);
if (!cache)
{
// There will be no cache for the very root of the tree
break;
}
P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
if (latest.second == 0)
{
// Just break out here... there may still be a path from source -> target
break;
}
if (!latest.first.isZero())
{
common_time = std::min(latest.first, common_time);
}
lct_cache.push_back(latest);
frame = latest.second;
// Early out... target frame is a direct parent of the source frame
if (frame == target_id)
{
time = common_time;
if (time == TIME_MAX)
{
time = Time();
}
return NO_ERROR;
}
++depth;
if (depth > MAX_GRAPH_DEPTH)
{
if (error_string)
{
std::stringstream ss;
ss<<"The tf tree is invalid because it contains a loop." << std::endl
<< allFramesAsString() << std::endl;
*error_string = ss.str();
}
return LOOKUP_ERROR;
}
}
// Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
frame = target_id;
depth = 0;
common_time = TIME_MAX;
CompactFrameID common_parent = 0;
while (true)
{
TimeCache* cache = getFrame(frame);
if (!cache)
{
break;
}
P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
if (latest.second == 0)
{
break;
}
if (!latest.first.isZero())
{
common_time = std::min(latest.first, common_time);
}
std::vector<P_TimeAndFrameID>::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second));
if (it != lct_cache.end()) // found a common parent
{
common_parent = it->second;
break;
}
frame = latest.second;
// Early out... source frame is a direct parent of the target frame
if (frame == source_id)
{
time = common_time;
if (time == TIME_MAX)
{
time = Time();
}
return NO_ERROR;
}
++depth;
if (depth > MAX_GRAPH_DEPTH)
{
if (error_string)
{
std::stringstream ss;
ss<<"The tf tree is invalid because it contains a loop." << std::endl
<< allFramesAsString() << std::endl;
*error_string = ss.str();
}
return LOOKUP_ERROR;
}
}
if (common_parent == 0)
{
createConnectivityErrorString(source_id, target_id, error_string);
return CONNECTIVITY_ERROR;
}
// Loop through the source -> root list until we hit the common parent
{
std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
for (; it != end; ++it)
{
if (!it->first.isZero())
{
common_time = std::min(common_time, it->first);
}
if (it->second == common_parent)
{
break;
}
}
}
if (common_time == TIME_MAX)
{
common_time = Time();
}
time = common_time;
return NO_ERROR;
}
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;
};
