38 #include <geometry_msgs/TransformStamped.h> 53 const geometry_msgs::Quaternion& o = data.transform.rotation;
55 const geometry_msgs::Vector3& v = data.transform.translation;
60 : max_storage_time_(max_storage_time)
70 ss <<
"Lookup would require extrapolation at time " << t0 <<
", but only time " << t1 <<
" is in the buffer";
71 *error_str = ss.str();
80 ss <<
"Lookup would require extrapolation into the future. Requested time " << t0 <<
" but the latest data is at time " << t1;
81 *error_str = ss.str();
90 ss <<
"Lookup would require extrapolation into the past. Requested time " << t0 <<
" but the earliest data is at time " << t1;
91 *error_str = ss.str();
120 if (ts.
stamp_ == target_time)
135 if (target_time == latest_time)
140 else if (target_time == earliest_time)
146 else if (target_time > latest_time)
151 else if (target_time < earliest_time)
159 L_TransformStorage::iterator storage_it;
161 storage_target_time.
stamp_ = target_time;
163 storage_it = std::lower_bound(
166 storage_target_time, std::greater<TransformStorage>());
169 one = &*(storage_it);
170 two = &*(--storage_it);
203 int num_nodes =
findClosest(p_temp_1, p_temp_2, time, error_str);
208 else if (num_nodes == 1)
210 data_out = *p_temp_1;
212 else if (num_nodes == 2)
220 data_out = *p_temp_1;
236 int num_nodes =
findClosest(p_temp_1, p_temp_2, time, error_str);
247 L_TransformStorage::iterator storage_it =
storage_.begin();
260 if (storage_it->stamp_ <= new_data.
stamp_)
264 storage_.insert(storage_it, new_data);
ros::Duration max_storage_time_
bool operator>(const TransformStorage &lhs, const TransformStorage &rhs)
void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string *error_str)
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
virtual ros::Time getLatestTimestamp()
Get the latest timestamp cached.
L_TransformStorage storage_
std_msgs::Header * header(M &m)
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string *error_str)
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
uint8_t findClosest(TransformStorage *&one, TransformStorage *&two, ros::Time target_time, std::string *error_str)
A helper function for getData.
virtual ros::Time getOldestTimestamp()
Get the oldest timestamp cached.
void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string *error_str)
virtual unsigned int getListLength()
Debugging information methods.
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
virtual P_TimeAndFrameID getLatestTimeAndParent()
Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no...
virtual void clearList()
Clear the list of stored values.
TimeCache(ros::Duration max_storage_time=ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME))
virtual CompactFrameID getParent(ros::Time time, std::string *error_str)
Retrieve the parent at a specific time.
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
TF2SIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tf2Scalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.