buffer_core.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00032 #ifndef TF2_BUFFER_CORE_H
00033 #define TF2_BUFFER_CORE_H
00034 
00035 #include "transform_storage.h"
00036 
00037 #include <boost/signals2.hpp>
00038 
00039 #include <string>
00040 
00041 #include "ros/duration.h"
00042 #include "ros/time.h"
00043 //#include "geometry_msgs/TwistStamped.h"
00044 #include "geometry_msgs/TransformStamped.h"
00045 
00047 //#include "tf/tf.h"
00048 
00049 #include <boost/unordered_map.hpp>
00050 #include <boost/thread/mutex.hpp>
00051 #include <boost/function.hpp>
00052 #include <boost/shared_ptr.hpp>
00053 
00054 namespace tf2
00055 {
00056 
00057 typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
00058 typedef uint32_t TransformableCallbackHandle;
00059 typedef uint64_t TransformableRequestHandle;
00060 
00061 class TimeCacheInterface;
00062 typedef boost::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
00063 
00064 enum TransformableResult
00065 {
00066   TransformAvailable,
00067   TransformFailure,
00068 };
00069 
00088 class BufferCore
00089 {
00090 public:
00091   /************* Constants ***********************/
00092   static const int DEFAULT_CACHE_TIME = 10;  
00093   static const uint32_t MAX_GRAPH_DEPTH = 1000UL;  
00094 
00100   BufferCore(ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
00101   virtual ~BufferCore(void);
00102 
00104   void clear();
00105 
00112   bool setTransform(const geometry_msgs::TransformStamped& transform, const std::string & authority, bool is_static = false);
00113 
00114   /*********** Accessors *************/
00115 
00125   geometry_msgs::TransformStamped 
00126     lookupTransform(const std::string& target_frame, const std::string& source_frame,
00127                     const ros::Time& time) const;
00128 
00141   geometry_msgs::TransformStamped
00142     lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00143                     const std::string& source_frame, const ros::Time& source_time,
00144                     const std::string& fixed_frame) const;
00145   
00146   /* \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point
00147    * \param tracking_frame The frame to track
00148    * \param observation_frame The frame from which to measure the twist
00149    * \param reference_frame The reference frame in which to express the twist
00150    * \param reference_point The reference point with which to express the twist
00151    * \param reference_point_frame The frame_id in which the reference point is expressed
00152    * \param time The time at which to get the velocity
00153    * \param duration The period over which to average
00154    * \return twist The twist output
00155    * 
00156    * This will compute the average velocity on the interval 
00157    * (time - duration/2, time+duration/2). If that is too close to the most
00158    * recent reading, in which case it will shift the interval up to
00159    * duration/2 to prevent extrapolation.
00160    *
00161    * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
00162    * tf2::ExtrapolationException, tf2::InvalidArgumentException
00163    * 
00164    * New in geometry 1.1
00165    */
00166   /*
00167   geometry_msgs::Twist
00168     lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00169                 const tf::Point & reference_point, const std::string& reference_point_frame, 
00170                 const ros::Time& time, const ros::Duration& averaging_interval) const;
00171   */
00172   /* \brief lookup the twist of the tracking frame with respect to the observational frame
00173    * 
00174    * This is a simplified version of
00175    * lookupTwist with it assumed that the reference point is the
00176    * origin of the tracking frame, and the reference frame is the
00177    * observation frame.  
00178    *
00179    * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
00180    * tf2::ExtrapolationException, tf2::InvalidArgumentException
00181    * 
00182    * New in geometry 1.1
00183    */
00184   /*
00185   geometry_msgs::Twist
00186     lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, 
00187                 const ros::Time& time, const ros::Duration& averaging_interval) const;
00188   */
00196   bool canTransform(const std::string& target_frame, const std::string& source_frame,
00197                     const ros::Time& time, std::string* error_msg = NULL) const;
00198   
00208   bool canTransform(const std::string& target_frame, const ros::Time& target_time,
00209                     const std::string& source_frame, const ros::Time& source_time,
00210                     const std::string& fixed_frame, std::string* error_msg = NULL) const;
00211 
00215   std::string allFramesAsYAML(double current_time) const;
00216 
00219   std::string allFramesAsYAML() const;
00220 
00224   std::string allFramesAsString() const;
00225   
00226   typedef boost::function<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
00227                                ros::Time time, TransformableResult result)> TransformableCallback;
00228 
00230   TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb);
00232   void removeTransformableCallback(TransformableCallbackHandle handle);
00234   TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time);
00236   void cancelTransformableRequest(TransformableRequestHandle handle);
00237 
00238 
00239 
00240 
00241   // Tell the buffer that there are multiple threads serviciing it. 
00242   // This is useful for derived classes to know if they can block or not. 
00243   void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
00244   // Get the state of using_dedicated_thread_
00245   bool isUsingDedicatedThread() const { return using_dedicated_thread_;};
00246   
00247 
00248 
00249 
00250   /* Backwards compatability section for tf::Transformer you should not use these
00251    */
00252 
00260   boost::signals2::connection _addTransformsChangedListener(boost::function<void(void)> callback);
00261   void _removeTransformsChangedListener(boost::signals2::connection c);
00262 
00263 
00266   bool _frameExists(const std::string& frame_id_str) const;
00267 
00272   bool _getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
00273 
00275   void _getFrameStrings(std::vector<std::string>& ids) const;
00276 
00277 
00278   CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const { 
00279     return lookupFrameNumber(frameid_str); 
00280   }
00281   CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) {
00282     return lookupOrInsertFrameNumber(frameid_str); 
00283   }
00284 
00285   int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const {
00286     boost::mutex::scoped_lock lock(frame_mutex_);
00287     return getLatestCommonTime(target_frame, source_frame, time, error_string);
00288   }
00289 
00290   CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const {
00291     return validateFrameId(function_name_arg, frame_id);
00292   }
00293 
00295   ros::Duration getCacheLength() { return cache_time_;}
00296 
00300   std::string _allFramesAsDot(double current_time) const;
00301   std::string _allFramesAsDot() const;
00302 
00306   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;
00307 
00308 private:
00309 
00313   std::string allFramesAsStringNoLock() const;  
00314 
00315 
00316   /******************** Internal Storage ****************/
00317   
00320   typedef std::vector<TimeCacheInterfacePtr> V_TimeCacheInterface;
00321   V_TimeCacheInterface frames_;
00322   
00324   mutable boost::mutex frame_mutex_;
00325 
00327   typedef boost::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
00328   M_StringToCompactFrameID frameIDs_;
00330   std::vector<std::string> frameIDs_reverse;
00332   std::map<CompactFrameID, std::string> frame_authority_;
00333 
00334 
00336   ros::Duration cache_time_;
00337 
00338   typedef boost::unordered_map<TransformableCallbackHandle, TransformableCallback> M_TransformableCallback;
00339   M_TransformableCallback transformable_callbacks_;
00340   uint32_t transformable_callbacks_counter_;
00341   boost::mutex transformable_callbacks_mutex_;
00342 
00343   struct TransformableRequest
00344   {
00345     ros::Time time;
00346     TransformableRequestHandle request_handle;
00347     TransformableCallbackHandle cb_handle;
00348     CompactFrameID target_id;
00349     CompactFrameID source_id;
00350     std::string target_string;
00351     std::string source_string;
00352   };
00353   typedef std::vector<TransformableRequest> V_TransformableRequest;
00354   V_TransformableRequest transformable_requests_;
00355   boost::mutex transformable_requests_mutex_;
00356   uint64_t transformable_requests_counter_;
00357 
00358   struct RemoveRequestByCallback;
00359   struct RemoveRequestByID;
00360 
00361   // Backwards compatability for tf message_filter
00362   typedef boost::signals2::signal<void(void)> TransformsChangedSignal;
00364   TransformsChangedSignal _transforms_changed_;
00365 
00366 
00367   /************************* Internal Functions ****************************/
00368 
00375   TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const;
00376 
00377   TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static);
00378 
00379 
00380   bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const;
00381   CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const;
00382 
00384   CompactFrameID lookupFrameNumber(const std::string& frameid_str) const;
00385 
00387   CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str);
00388 
00390   const std::string& lookupFrameString(CompactFrameID frame_id_num) const;
00391 
00392   void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
00393 
00396   int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const;
00397 
00398   template<typename F>
00399   int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
00400 
00403   template<typename F>
00404   int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
00405 
00406   void testTransformableRequests();
00407   bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
00408                     const ros::Time& time, std::string* error_msg) const;
00409   bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
00410                       const ros::Time& time, std::string* error_msg) const;
00411 
00412 
00413   //Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.)
00414   bool using_dedicated_thread_;
00415 
00416 public:
00417   friend class TestBufferCore; // For unit testing
00418 
00419 };
00420 
00422 class TestBufferCore
00423 {
00424 public:
00425   int _walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
00426   const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const
00427   {
00428     return buffer.lookupFrameString(frame_id_num);
00429   }
00430 };
00431 };
00432 
00433 #endif //TF2_CORE_H


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jun 6 2019 20:22:55