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);
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) {
358 interpolate(*p_temp_1, *p_temp_2, time, data_out);
360 data_out = *p_temp_1;
373 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
374 if (num_nodes == 0) {
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) {
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.
virtual ~TimeCache()
Destructor.
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.