00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00032 #ifndef TF2_BUFFER_CORE_H
00033 #define TF2_BUFFER_CORE_H
00034
00035 #include "transform_storage.h"
00036
00037 #include <string>
00038
00039 #include "ros/duration.h"
00040 #include "ros/time.h"
00041
00042 #include "geometry_msgs/TransformStamped.h"
00043
00045
00046
00047 #include <boost/unordered_map.hpp>
00048 #include <boost/thread/mutex.hpp>
00049 #include <boost/function.hpp>
00050
00051 namespace tf2
00052 {
00053
00054 typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
00055 typedef uint32_t TransformableCallbackHandle;
00056 typedef uint64_t TransformableRequestHandle;
00057
00058 class TimeCacheInterface;
00059
00060 enum TransformableResult
00061 {
00062 TransformAvailable,
00063 TransformFailure,
00064 };
00065
00084 class BufferCore
00085 {
00086 public:
00087
00088 static const double DEFAULT_CACHE_TIME = 10.0;
00089 static const uint32_t MAX_GRAPH_DEPTH = 1000UL;
00090 static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL;
00091
00097 BufferCore(ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
00098 virtual ~BufferCore(void);
00099
00101 void clear();
00102
00109 bool setTransform(const geometry_msgs::TransformStamped& transform, const std::string & authority, bool is_static = false);
00110
00111
00112
00122 geometry_msgs::TransformStamped
00123 lookupTransform(const std::string& target_frame, const std::string& source_frame,
00124 const ros::Time& time) const;
00125
00138 geometry_msgs::TransformStamped
00139 lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00140 const std::string& source_frame, const ros::Time& source_time,
00141 const std::string& fixed_frame) const;
00142
00163
00164
00165
00166
00167
00168
00181
00182
00183
00184
00185
00193 bool canTransform(const std::string& target_frame, const std::string& source_frame,
00194 const ros::Time& time, std::string* error_msg = NULL) const;
00195
00205 bool canTransform(const std::string& target_frame, const ros::Time& target_time,
00206 const std::string& source_frame, const ros::Time& source_time,
00207 const std::string& fixed_frame, std::string* error_msg = NULL) const;
00208
00212 std::string allFramesAsYAML() const;
00213
00217 std::string allFramesAsString() const;
00218
00219 typedef boost::function<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
00220 ros::Time time, TransformableResult result)> TransformableCallback;
00221
00223 TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb);
00225 void removeTransformableCallback(TransformableCallbackHandle handle);
00227 TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time);
00229 void cancelTransformableRequest(TransformableRequestHandle handle);
00230
00231 private:
00232
00233
00237 std::string allFramesAsStringNoLock() const;
00238
00239
00240
00241
00244 typedef std::vector<TimeCacheInterface*> V_TimeCacheInterface;
00245 V_TimeCacheInterface frames_;
00246
00248 mutable boost::mutex frame_mutex_;
00249
00251 typedef boost::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
00252 M_StringToCompactFrameID frameIDs_;
00254 std::vector<std::string> frameIDs_reverse;
00256 std::map<CompactFrameID, std::string> frame_authority_;
00257
00258
00260 ros::Duration cache_time_;
00261
00262 mutable std::vector<P_TimeAndFrameID> lct_cache_;
00263
00264 typedef boost::unordered_map<TransformableCallbackHandle, TransformableCallback> M_TransformableCallback;
00265 M_TransformableCallback transformable_callbacks_;
00266 uint32_t transformable_callbacks_counter_;
00267 boost::mutex transformable_callbacks_mutex_;
00268
00269 struct TransformableRequest
00270 {
00271 ros::Time time;
00272 TransformableRequestHandle request_handle;
00273 TransformableCallbackHandle cb_handle;
00274 CompactFrameID target_id;
00275 CompactFrameID source_id;
00276 std::string target_string;
00277 std::string source_string;
00278 };
00279 typedef std::vector<TransformableRequest> V_TransformableRequest;
00280 V_TransformableRequest transformable_requests_;
00281 boost::mutex transformable_requests_mutex_;
00282 uint64_t transformable_requests_counter_;
00283
00284 struct RemoveRequestByCallback;
00285 struct RemoveRequestByID;
00286
00287
00288
00295 TimeCacheInterface* getFrame(CompactFrameID c_frame_id) const;
00296
00297 TimeCacheInterface* allocateFrame(CompactFrameID cfid, bool is_static);
00298
00299
00300 bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const;
00301 CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const;
00302
00304 CompactFrameID lookupFrameNumber(const std::string& frameid_str) const;
00305
00307 CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str);
00308
00310 const std::string& lookupFrameString(CompactFrameID frame_id_num) const;
00311
00312 void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
00313
00316 int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const;
00317
00318 template<typename F>
00319 int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
00320
00321 void testTransformableRequests();
00322 bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
00323 const ros::Time& time, std::string* error_msg) const;
00324 bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
00325 const ros::Time& time, std::string* error_msg) const;
00326
00328
00329
00330 };
00331
00332 }
00333 #endif //TF2_CORE_H