52 #include <tf/time_cache.h>
73 TransformStorage::TransformStorage()
83 CompactFrameID frame_id,
84 CompactFrameID child_frame_id)
85 : rotation_(data.getRotation())
86 , translation_(data.getOrigin())
89 , child_frame_id_(child_frame_id)
113 : max_storage_time_(max_storage_time)
129 if (asprintf(&tmp,
"Lookup would require extrapolation at time %li.%li, "
150 if (asprintf(&tmp,
"Lookup would require extrapolation into the future. "
151 "Requested time %s, but the latest data is at time %s",
152 t0.
str(), t1.
str()) != -1)
171 if (asprintf(&tmp,
"Lookup would require extrapolation into the past. "
172 "Requested time %s, but the latest data is at time %s",
173 t0.
str(), t1.
str()) != -1)
184 TimeCache::find_closest(TransformStorage*& one, TransformStorage*& two,
188 if (storage_.empty()) {
194 one = &storage_.front();
199 if (++storage_.begin() == storage_.end()) {
200 TransformStorage& ts = *storage_.begin();
201 if (ts.stamp == target_time) {
205 create_extrapolation_exception1(target_time, ts.
stamp, error_str);
213 if (target_time == latest_time) {
214 one = &(*storage_.begin());
216 }
else if (target_time == earliest_time) {
217 one = &(*storage_.rbegin());
219 }
else if (target_time > latest_time) {
221 create_extrapolation_exception2(target_time, latest_time, error_str);
223 }
else if (target_time < earliest_time) {
224 create_extrapolation_exception3(target_time, earliest_time, error_str);
230 L_TransformStorage::iterator storage_it = storage_.begin();
231 while(storage_it != storage_.end()) {
232 if (storage_it->stamp <= target_time)
break;
237 one = &*(storage_it);
238 two = &*(--storage_it);
244 TimeCache::interpolate(
const TransformStorage& one,
245 const TransformStorage& two,
249 if( two.stamp == one.stamp ) {
255 (time.
in_sec() - one.stamp.in_sec()) /
256 (two.stamp.in_sec() - one.stamp.in_sec());
259 output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
262 output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
264 output.stamp = one.stamp;
265 output.frame_id_ = one.frame_id_;
266 output.child_frame_id_ = one.child_frame_id_;
277 std::string* error_str)
282 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
283 if (num_nodes == 0) {
285 }
else if (num_nodes == 1) {
286 data_out = *p_temp_1;
287 }
else if (num_nodes == 2) {
289 interpolate(*p_temp_1, *p_temp_2, time, data_out);
291 data_out = *p_temp_1;
309 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
310 if (num_nodes == 0) {
325 L_TransformStorage::iterator storage_it = storage_.begin();
327 if(storage_it != storage_.end()) {
328 if (storage_it->stamp > new_data.
stamp + max_storage_time_) {
334 while(storage_it != storage_.end()) {
335 if (storage_it->stamp <= new_data.
stamp)
339 storage_.insert(storage_it, new_data);
358 return storage_.size();
367 if (storage_.empty()) {
382 return storage_.front().stamp;
392 return storage_.back().stamp;
397 TimeCache::prune_list()
401 while(!storage_.empty() &&
402 storage_.back().stamp + max_storage_time_ < latest_time)
bool insert_data(const TransformStorage &new_data)
Insert data.
void clear_list()
Clear storage.
bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
A class for handling time.
P_TimeAndFrameID get_latest_time_and_parent() const
Get latest time and parent frame number.
long get_sec() const
Get seconds.
bool is_zero() const
Check if time is zero.
double in_sec() const
Convet time to seconds.
TimeCache(float max_storage_time=10.0)
Constructor.
fawkes::Time get_latest_timestamp() const
Get latest timestamp from cache.
unsigned int get_list_length() const
Debugging information methods.
long get_nsec() const
Get nanoseconds.
fawkes::Time get_oldest_timestamp() const
Get oldest timestamp from cache.
const char * str(bool utc=false) const
Output function.
Time & stamp()
Set this time to the current time.