52 #ifndef _LIBS_TF_BUFFER_CORE_H_
53 #define _LIBS_TF_BUFFER_CORE_H_
55 #include <tf/transform_storage.h>
57 #include <utils/time/time.h>
63 #include <unordered_map>
69 class TimeCacheInterface;
70 typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
76 CONNECTIVITY_ERROR = 2,
77 EXTRAPOLATION_ERROR = 3,
78 INVALID_ARGUMENT_ERROR = 4,
119 const std::string & authority,
120 bool is_static =
false);
124 const std::string & source_frame,
130 const std::string & source_frame,
132 const std::string & fixed_frame,
136 const std::string & source_frame,
138 std::string * error_msg = NULL)
const;
142 const std::string & source_frame,
144 const std::string & fixed_frame,
145 std::string * error_msg = NULL)
const;
191 TimeCacheInterfacePtr
get_frame(CompactFrameID c_frame_id)
const;
193 TimeCacheInterfacePtr
allocate_frame(CompactFrameID cfid,
bool is_static);
195 bool warn_frame_id(
const char *function_name_arg,
const std::string &frame_id)
const;
197 const std::string &frame_id)
const;
209 CompactFrameID target_frame,
210 std::string * out)
const;
213 CompactFrameID source_frame,
215 std::string * error_string)
const;
217 template <
typename F>
220 CompactFrameID target_id,
221 CompactFrameID source_id,
222 std::string * error_string)
const;
224 template <
typename F>
227 CompactFrameID target_id,
228 CompactFrameID source_id,
229 std::string * error_string,
230 std::vector<CompactFrameID> *frame_chain)
const;
233 CompactFrameID source_id,
235 std::string * error_msg)
const;
237 CompactFrameID source_id,
239 std::string * error_msg)
const;
A class for handling time.
A Class which provides coordinate transforms between any two frames in a system.
float cache_time_
How long to cache transform history.
bool can_transform_no_lock(CompactFrameID target_id, CompactFrameID source_id, const fawkes::Time &time, std::string *error_msg) const
Test if a transform is possible.
int walk_to_top_parent(F &f, fawkes::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const
Traverse transform tree: walk from frame to top-parent of both.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
static const uint32_t MAX_GRAPH_DEPTH
Maximum number of times to recurse before assuming the tree has a loop.
const std::string & lookup_frame_string(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
float get_cache_length()
Get the duration over which this transformer will cache.
void clear()
Clear all data.
int get_latest_common_time(CompactFrameID target_frame, CompactFrameID source_frame, fawkes::Time &time, std::string *error_string) const
Get latest common time of two frames.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
bool can_transform_internal(CompactFrameID target_id, CompactFrameID source_id, const fawkes::Time &time, std::string *error_msg) const
Test if a transform is possible.
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Create error string.
bool warn_frame_id(const char *function_name_arg, const std::string &frame_id) const
Warn if an illegal frame_id was passed.
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
std::string all_frames_as_string() const
A way to see what frames have been cached.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
CompactFrameID validate_frame_id(const char *function_name_arg, const std::string &frame_id) const
Check if frame ID is valid and return compact ID.
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
std::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
A map from string frame ids to CompactFrameID.
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
std::string all_frames_as_YAML() const
Get latest frames as YAML.
BufferCore(float cache_time=DEFAULT_CACHE_TIME)
Constructor.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
std::vector< TimeCacheInterfacePtr > V_TimeCacheInterface
Vector data type for frame caches.
std::string all_frames_as_string_no_lock() const
A way to see what frames have been cached.
TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static)
Allocate a new frame cache.
Fawkes library namespace.