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