36 #include <geometry_msgs/TransformStamped.h> 56 : max_storage_time_(max_storage_time)
64 *error_str =
"Unable to lookup transform, cache is empty";
73 ss <<
"Lookup would require extrapolation at time " << t0 <<
", but only time " << t1 <<
" is in the buffer";
74 *error_str = ss.str();
83 ss <<
"Lookup would require extrapolation into the future. Requested time " << t0 <<
" but the latest data is at time " << t1;
84 *error_str = ss.str();
93 ss <<
"Lookup would require extrapolation into the past. Requested time " << t0 <<
" but the earliest data is at time " << t1;
94 *error_str = ss.str();
118 if (ts.
stamp_ == target_time)
133 if (target_time == latest_time)
138 else if (target_time == earliest_time)
144 else if (target_time > latest_time)
149 else if (target_time < earliest_time)
160 L_TransformStorage::iterator storage_it =
storage_.upper_bound(tmp);
163 two = &*(storage_it);
164 one = &*(--storage_it);
197 int num_nodes =
findClosest(p_temp_1, p_temp_2, time, error_str);
202 else if (num_nodes == 1)
204 data_out = *p_temp_1;
206 else if (num_nodes == 2)
214 data_out = *p_temp_1;
230 int num_nodes =
findClosest(p_temp_1, p_temp_2, time, error_str);
251 L_TransformStorage::iterator storage_it =
storage_.find(new_data);
ros::Duration max_storage_time_
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
TimeCache(ros::Duration max_storage_time=ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME))
unsigned int getListLength()
Debugging information methods.
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
ros::Time getLatestTimestamp()
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
uint8_t findClosest(const TransformStorage *&one, const TransformStorage *&two, ros::Time target_time, std::string *error_str)
A helper function for getData.
void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string *error_str)
void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string *error_str)
TFSIMD_FORCE_INLINE void setInterpolate3(const Vector3 &v0, const Vector3 &v1, tfScalar rt)
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
bool insertData(const TransformStorage &new_data)
void createEmptyException(std::string *error_str)
CompactFrameID getParent(ros::Time time, std::string *error_str)
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
P_TimeAndFrameID getLatestTimeAndParent()
L_TransformStorage storage_
void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string *error_str)
ros::Time getOldestTimestamp()