32 #ifndef TF2_BUFFER_CORE_H 33 #define TF2_BUFFER_CORE_H 37 #include <boost/signals2.hpp> 44 #include "geometry_msgs/TransformStamped.h" 49 #include <boost/unordered_map.hpp> 50 #include <boost/thread/mutex.hpp> 51 #include <boost/function.hpp> 52 #include <boost/shared_ptr.hpp> 112 bool setTransform(
const geometry_msgs::TransformStamped& transform,
const std::string & authority,
bool is_static =
false);
125 geometry_msgs::TransformStamped
126 lookupTransform(
const std::string& target_frame,
const std::string& source_frame,
141 geometry_msgs::TransformStamped
143 const std::string& source_frame,
const ros::Time& source_time,
144 const std::string& fixed_frame)
const;
196 bool canTransform(
const std::string& target_frame,
const std::string& source_frame,
197 const ros::Time& time, std::string* error_msg = NULL)
const;
209 const std::string& source_frame,
const ros::Time& source_time,
210 const std::string& fixed_frame, std::string* error_msg = NULL)
const;
226 typedef boost::function<void(TransformableRequestHandle request_handle,
const std::string& target_frame,
const std::string& source_frame,
234 TransformableRequestHandle
addTransformableRequest(TransformableCallbackHandle handle,
const std::string& target_frame,
const std::string& source_frame,
ros::Time time);
266 bool _frameExists(
const std::string& frame_id_str)
const;
306 void _chainAsVector(
const std::string & target_frame,
ros::Time target_time,
const std::string & source_frame,
ros::Time source_time,
const std::string & fixed_frame, std::vector<std::string>& output)
const;
380 bool warnFrameId(
const char* function_name_arg,
const std::string& frame_id)
const;
408 const ros::Time& time, std::string* error_msg)
const;
410 const ros::Time& time, std::string* error_msg)
const;
bool using_dedicated_thread_
bool _getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
Fill the parent of a frame.
A Class which provides coordinate transforms between any two frames in a system.
TransformsChangedSignal _transforms_changed_
Signal which is fired whenever new transform data has arrived, from the thread the data arrived in...
static const uint32_t MAX_GRAPH_DEPTH
Maximum graph search depth (deeper graphs will be assumed to have loops)
uint64_t transformable_requests_counter_
CompactFrameID _validateFrameId(const char *function_name_arg, const std::string &frame_id) const
TransformableCallbackHandle addTransformableCallback(const TransformableCallback &cb)
Internal use only.
int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const
Return the latest rostime which is common across the spanning set zero if fails to cross...
void setUsingDedicatedThread(bool value)
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
Get the transform between two frames by frame ID.
bool warnFrameId(const char *function_name_arg, const std::string &frame_id) const
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const
An accessor to get a frame, which will throw an exception if the frame is no there.
boost::signals2::connection _addTransformsChangedListener(boost::function< void(void)> callback)
Add a callback that happens when a new transform has arrived.
boost::mutex transformable_requests_mutex_
uint32_t TransformableCallbackHandle
std::string allFramesAsStringNoLock() const
A way to see what frames have been cached Useful for debugging. Use this call internally.
std::string allFramesAsYAML() const
M_TransformableCallback transformable_callbacks_
uint32_t transformable_callbacks_counter_
TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static)
std::vector< TimeCacheInterfacePtr > V_TimeCacheInterface
The pointers to potential frames that the tree can be made of. The frames will be dynamically allocat...
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
virtual ~BufferCore(void)
std::vector< TransformableRequest > V_TransformableRequest
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
boost::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
boost::unordered_map< TransformableCallbackHandle, TransformableCallback > M_TransformableCallback
void _getFrameStrings(std::vector< std::string > &ids) const
A way to get a std::vector of available frame ids.
void _removeTransformsChangedListener(boost::signals2::connection c)
bool _frameExists(const std::string &frame_id_str) const
Check if a frame exists in the tree.
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
void clear()
Clear all data.
ros::Duration getCacheLength()
Get the duration over which this transformer will cache.
int walkToTopParent(F &f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const
int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const
void _chainAsVector(const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
Backwards compatabilityA way to see what frames are in a chain Useful for debugging.
CompactFrameID _lookupFrameNumber(const std::string &frameid_str) const
const std::string & _lookupFrameString(BufferCore &buffer, CompactFrameID frame_id_num) const
void cancelTransformableRequest(TransformableRequestHandle handle)
Internal use only.
boost::signals2::signal< void(void)> TransformsChangedSignal
ros::Duration cache_time_
How long to cache transform history.
bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const
const std::string & lookupFrameString(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
CompactFrameID validateFrameId(const char *function_name_arg, const std::string &frame_id) const
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
std::string _allFramesAsDot() const
M_StringToCompactFrameID frameIDs_
TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string &target_frame, const std::string &source_frame, ros::Time time)
Internal use only.
V_TransformableRequest transformable_requests_
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
boost::shared_ptr< TimeCacheInterface > TimeCacheInterfacePtr
bool isUsingDedicatedThread() const
boost::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
A map from string frame ids to CompactFrameID.
void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
void removeTransformableCallback(TransformableCallbackHandle handle)
Internal use only.
boost::mutex transformable_callbacks_mutex_
V_TimeCacheInterface frames_
CompactFrameID _lookupOrInsertFrameNumber(const std::string &frameid_str)
bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const
CompactFrameID lookupOrInsertFrameNumber(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
BufferCore(ros::Duration cache_time_=ros::Duration(DEFAULT_CACHE_TIME))
CompactFrameID lookupFrameNumber(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
void testTransformableRequests()
boost::function< void(TransformableRequestHandle request_handle, const std::string &target_frame, const std::string &source_frame, ros::Time time, TransformableResult result)> TransformableCallback
uint64_t TransformableRequestHandle