52 #include <tf/time_cache.h> 89 , child_frame_id(child_frame_id)
159 : max_storage_time_(max_storage_time)
176 if (asprintf(&tmp,
"Lookup would require extrapolation at time %li.%li, " 197 if (asprintf(&tmp,
"Lookup would require extrapolation into the future. " 198 "Requested time %li.%li, but the latest data is at time %li.%li",
218 if (asprintf(&tmp,
"Lookup would require extrapolation into the past. " 219 "Requested time %li.%li, but the latest data is at time %li.%li",
235 if (storage_.empty()) {
236 if (error_str) *error_str =
"Transform cache storage is empty";
242 one = &storage_.front();
247 if (++storage_.begin() == storage_.end()) {
249 if (ts.
stamp == target_time) {
253 create_extrapolation_exception1(target_time, ts.
stamp, error_str);
261 if (target_time == latest_time) {
262 one = &(*storage_.begin());
264 }
else if (target_time == earliest_time) {
265 one = &(*storage_.rbegin());
267 }
else if (target_time > latest_time) {
269 create_extrapolation_exception2(target_time, latest_time, error_str);
271 }
else if (target_time < earliest_time) {
272 create_extrapolation_exception3(target_time, earliest_time, error_str);
278 L_TransformStorage::iterator storage_it = storage_.begin();
279 while(storage_it != storage_.end()) {
280 if (storage_it->stamp <= target_time)
break;
285 one = &*(storage_it);
286 two = &*(--storage_it);
317 TimeCacheInterfacePtr
321 if (look_back_until.
is_zero()) {
322 copy->storage_ = storage_;
324 L_TransformStorage::const_iterator storage_it = storage_.begin();
325 for (storage_it = storage_.begin(); storage_it != storage_.end(); ++storage_it) {
326 if (storage_it->stamp <= look_back_until)
break;
327 copy->storage_.push_back(*storage_it);
330 return std::shared_ptr<TimeCacheInterface>(copy);
336 std::string* error_str)
341 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
342 if (num_nodes == 0) {
344 }
else if (num_nodes == 1) {
345 data_out = *p_temp_1;
346 }
else if (num_nodes == 2) {
348 interpolate(*p_temp_1, *p_temp_2, time, data_out);
350 data_out = *p_temp_1;
363 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
364 if (num_nodes == 0) {
375 L_TransformStorage::iterator storage_it = storage_.begin();
377 if(storage_it != storage_.end()) {
378 if (storage_it->stamp > new_data.
stamp + max_storage_time_) {
384 while(storage_it != storage_.end()) {
385 if (storage_it->stamp <= new_data.
stamp)
389 storage_.insert(storage_it, new_data);
404 return storage_.size();
423 if (storage_.empty()) {
435 return storage_.front().stamp;
442 return storage_.back().stamp;
447 TimeCache::prune_list()
451 while(!storage_.empty() &&
452 storage_.back().stamp + max_storage_time_ < latest_time)
virtual L_TransformStorage get_storage_copy() const
Get copy of storage elements.
virtual bool insert_data(const TransformStorage &new_data)
Insert data.
double in_sec() const
Convet time to seconds.
virtual void clear_list()
Clear storage.
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
virtual const L_TransformStorage & get_storage() const
Get storage list.
Fawkes library namespace.
A class for handling time.
virtual fawkes::Time get_latest_timestamp() const
Get latest timestamp from cache.
virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until=fawkes::Time(0, 0)) const
Create a copy of this time cache.
std::list< TransformStorage > L_TransformStorage
List of stored transforms.
Time based transform cache.
long get_nsec() const
Get nanoseconds.
bool is_zero() const
Check if time is zero.
virtual unsigned int get_list_length() const
Debugging information methods.
TimeCache(float max_storage_time=DEFAULT_MAX_STORAGE_TIME)
Constructor.
long get_sec() const
Get seconds.
virtual P_TimeAndFrameID get_latest_time_and_parent()
Get latest time and parent frame number.
long get_usec() const
Get microseconds.
virtual fawkes::Time get_oldest_timestamp() const
Get oldest timestamp from cache.
Time & stamp()
Set this time to the current time.