52 #include <tf/time_cache.h>
79 CompactFrameID frame_id,
80 CompactFrameID child_frame_id)
81 : rotation(data.getRotation()),
82 translation(data.getOrigin()),
85 child_frame_id(child_frame_id)
176 "Lookup would require extrapolation at time %li.%li, "
177 "but only time %li.%li is in the buffer",
200 "Lookup would require extrapolation into the future. "
201 "Requested time %li.%li, but the latest data is at time %li.%li",
224 "Lookup would require extrapolation into the past. "
225 "Requested time %li.%li, but the latest data is at time %li.%li",
240 TimeCache::find_closest(TransformStorage *&one,
241 TransformStorage *&two,
243 std::string * error_str)
246 if (storage_.empty()) {
248 *error_str =
"Transform cache storage is empty";
254 one = &storage_.front();
259 if (++storage_.begin() == storage_.end()) {
260 TransformStorage &ts = *storage_.begin();
261 if (ts.stamp == target_time) {
265 create_extrapolation_exception1(target_time, ts.stamp, error_str);
273 if (target_time == latest_time) {
274 one = &(*storage_.begin());
276 }
else if (target_time == earliest_time) {
277 one = &(*storage_.rbegin());
279 }
else if (target_time > latest_time) {
281 create_extrapolation_exception2(target_time, latest_time, error_str);
283 }
else if (target_time < earliest_time) {
284 create_extrapolation_exception3(target_time, earliest_time, error_str);
290 L_TransformStorage::iterator storage_it = storage_.begin();
291 while (storage_it != storage_.end()) {
292 if (storage_it->stamp <= target_time)
298 one = &*(storage_it);
299 two = &*(--storage_it);
304 TimeCache::interpolate(
const TransformStorage &one,
305 const TransformStorage &two,
307 TransformStorage & output)
310 if (two.stamp == one.stamp) {
315 btScalar ratio = (time.
in_sec() - one.stamp.in_sec()) / (two.stamp.in_sec() - one.stamp.in_sec());
318 output.translation.setInterpolate3(one.translation, two.translation, ratio);
321 output.rotation = slerp(one.rotation, two.rotation, ratio);
323 output.stamp = one.stamp;
324 output.frame_id = one.frame_id;
325 output.child_frame_id = one.child_frame_id;
328 TimeCacheInterfacePtr
332 if (look_back_until.
is_zero()) {
333 copy->storage_ = storage_;
335 L_TransformStorage::const_iterator storage_it = storage_.begin();
336 for (storage_it = storage_.begin(); storage_it != storage_.end(); ++storage_it) {
337 if (storage_it->stamp <= look_back_until)
339 copy->storage_.push_back(*storage_it);
342 return std::shared_ptr<TimeCacheInterface>(copy);
348 TransformStorage *p_temp_1 = NULL;
349 TransformStorage *p_temp_2 = NULL;
351 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
352 if (num_nodes == 0) {
354 }
else if (num_nodes == 1) {
355 data_out = *p_temp_1;
356 }
else if (num_nodes == 2) {
357 if (p_temp_1->frame_id == p_temp_2->frame_id) {
358 interpolate(*p_temp_1, *p_temp_2, time, data_out);
360 data_out = *p_temp_1;
370 TransformStorage *p_temp_1 = NULL;
371 TransformStorage *p_temp_2 = NULL;
373 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
374 if (num_nodes == 0) {
378 return p_temp_1->frame_id;
384 L_TransformStorage::iterator storage_it = storage_.begin();
386 if (storage_it != storage_.end()) {
387 if (storage_it->stamp > new_data.stamp + max_storage_time_) {
392 while (storage_it != storage_.end()) {
393 if (storage_it->stamp <= new_data.stamp)
397 storage_.insert(storage_it, new_data);
412 return storage_.size();
430 if (storage_.empty()) {
441 if (storage_.empty())
443 return storage_.front().stamp;
449 if (storage_.empty())
451 return storage_.back().stamp;
456 TimeCache::prune_list()
460 while (!storage_.empty() && storage_.back().stamp + max_storage_time_ < latest_time) {