52 #ifndef __LIBS_TF_BUFFER_CORE_H_ 53 #define __LIBS_TF_BUFFER_CORE_H_ 56 #include <tf/transform_storage.h> 58 #include <utils/time/time.h> 65 #include <unordered_map> 74 class TimeCacheInterface;
75 typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
81 CONNECTIVITY_ERROR = 2,
82 EXTRAPOLATION_ERROR = 3,
83 INVALID_ARGUMENT_ERROR = 4,
119 BufferCore(
float cache_time = DEFAULT_CACHE_TIME);
125 const std::string & authority,
126 bool is_static =
false);
131 const std::string& source_frame,
137 const std::string& source_frame,
139 const std::string& fixed_frame,
142 bool can_transform(
const std::string& target_frame,
const std::string& source_frame,
143 const fawkes::Time& time, std::string* error_msg = NULL)
const;
146 const std::string& source_frame,
const fawkes::Time& source_time,
147 const std::string& fixed_frame, std::string* error_msg = NULL)
const;
191 TimeCacheInterfacePtr
get_frame(CompactFrameID c_frame_id)
const;
193 TimeCacheInterfacePtr
allocate_frame(CompactFrameID cfid,
bool is_static);
196 bool warn_frame_id(
const char* function_name_arg,
const std::string& frame_id)
const;
197 CompactFrameID
validate_frame_id(
const char* function_name_arg,
const std::string& frame_id)
const;
217 int walk_to_top_parent(F& f,
fawkes::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain)
const;
220 const fawkes::Time& time, std::string* error_msg)
const;
222 const fawkes::Time& time, std::string* error_msg)
const;
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
std::vector< TimeCacheInterfacePtr > V_TimeCacheInterface
Vector data type for frame caches.
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.
std::string all_frames_as_string() const
A way to see what frames have been cached.
Fawkes library namespace.
float get_cache_length()
Get the duration over which this transformer will cache.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
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 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.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
void clear()
Clear all data.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
bool warn_frame_id(const char *function_name_arg, const std::string &frame_id) const
Warn if an illegal frame_id was passed.
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.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
A class for handling time.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
std::string all_frames_as_YAML() const
Get latest frames as YAML.
A Class which provides coordinate transforms between any two frames in a system.
std::string all_frames_as_string_no_lock() const
A way to see what frames have been cached.
void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Create error string.
std::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
A map from string frame ids to CompactFrameID.
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
const std::string & lookup_frame_string(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
float cache_time_
How long to cache transform history.
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static)
Allocate a new frame cache.
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.
BufferCore(float cache_time=DEFAULT_CACHE_TIME)
assuming the tree has a loop
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.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
static const uint32_t MAX_GRAPH_DEPTH
Maximum number of times to recurse before.