tf.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 TF_TF_H
00033 #define TF_TF_H
00034 
00035 #include <iostream>
00036 #include <iomanip>
00037 #include <cmath>
00038 #include <vector>
00039 #include <sstream>
00040 #include <map>
00041 
00042 #include <tf/exceptions.h>
00043 #include "tf/time_cache.h"
00044 #include <boost/thread/recursive_mutex.hpp>
00045 #include <boost/unordered_map.hpp>
00046 #include <boost/signals.hpp>
00047 #include "geometry_msgs/TwistStamped.h"
00048 
00049 namespace tf
00050 {
00052 std::string resolve(const std::string& prefix, const std::string& frame_name);
00053 
00055 __attribute__((deprecated)) static inline std::string remap(const std::string& prefix, const std::string& frame_name) { return tf::resolve(prefix, frame_name);} ;
00056 
00057 enum ErrorValues { NO_ERROR = 0, LOOKUP_ERROR, CONNECTIVITY_ERROR, EXTRAPOLATION_ERROR};
00058 
00062 typedef struct
00063 {
00064   std::vector<TransformStorage > inverseTransforms;
00065   std::vector<TransformStorage > forwardTransforms;
00066 } TransformLists;
00067 
00086 class Transformer
00087 {
00088 public:
00089   /************* Constants ***********************/
00090   static const unsigned int MAX_GRAPH_DEPTH = 100UL;   
00091   static const double DEFAULT_CACHE_TIME;  
00092   static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL; 
00093 
00094 
00100   Transformer(bool interpolating = true,
00101               ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
00102   virtual ~Transformer(void);
00103 
00105   void clear();
00106 
00112   bool setTransform(const StampedTransform& transform, const std::string & authority = "default_authority");
00113 
00114   /*********** Accessors *************/
00115 
00125   void lookupTransform(const std::string& target_frame, const std::string& source_frame,
00126                        const ros::Time& time, StampedTransform& transform) const;
00138   void lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00139                        const std::string& source_frame, const ros::Time& source_time,
00140                        const std::string& fixed_frame, StampedTransform& transform) const;
00141 
00162   void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00163                    const tf::Point & reference_point, const std::string& reference_point_frame, 
00164                    const ros::Time& time, const ros::Duration& averaging_interval, 
00165                    geometry_msgs::Twist& twist) const;
00166 
00177   void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, 
00178                    const ros::Time& time, const ros::Duration& averaging_interval,
00179                    geometry_msgs::Twist& twist) const;
00180 
00189   bool waitForTransform(const std::string& target_frame, const std::string& source_frame,
00190                         const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
00191                         std::string* error_msg = NULL) const;
00198   bool canTransform(const std::string& target_frame, const std::string& source_frame,
00199                     const ros::Time& time,
00200                     std::string* error_msg = NULL) const;
00201 
00210   bool canTransform(const std::string& target_frame, const ros::Time& target_time,
00211                     const std::string& source_frame, const ros::Time& source_time,
00212                     const std::string& fixed_frame,
00213                     std::string* error_msg = NULL) const;
00214 
00225   bool waitForTransform(const std::string& target_frame, const ros::Time& target_time,
00226                     const std::string& source_frame, const ros::Time& source_time,
00227                     const std::string& fixed_frame,
00228                     const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
00229                     std::string* error_msg = NULL) const;
00230 
00233   int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const;
00234 
00237   void transformQuaternion(const std::string& target_frame, const Stamped<tf::Quaternion>& stamped_in, Stamped<tf::Quaternion>& stamped_out) const;
00240   void transformVector(const std::string& target_frame, const Stamped<tf::Vector3>& stamped_in, Stamped<tf::Vector3>& stamped_out) const;
00243   void transformPoint(const std::string& target_frame, const Stamped<tf::Point>& stamped_in, Stamped<tf::Point>& stamped_out) const;
00246   void transformPose(const std::string& target_frame, const Stamped<tf::Pose>& stamped_in, Stamped<tf::Pose>& stamped_out) const;
00247 
00250   void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00251                            const Stamped<tf::Quaternion>& stamped_in,
00252                            const std::string& fixed_frame,
00253                            Stamped<tf::Quaternion>& stamped_out) const;
00256   void transformVector(const std::string& target_frame, const ros::Time& target_time,
00257                        const Stamped<tf::Vector3>& stamped_in,
00258                        const std::string& fixed_frame,
00259                        Stamped<tf::Vector3>& stamped_out) const;
00262   void transformPoint(const std::string& target_frame, const ros::Time& target_time,
00263                       const Stamped<tf::Point>& stamped_in,
00264                       const std::string& fixed_frame,
00265                       Stamped<tf::Point>& stamped_out) const;
00268   void transformPose(const std::string& target_frame, const ros::Time& target_time,
00269                      const Stamped<tf::Pose>& stamped_in,
00270                      const std::string& fixed_frame,
00271                      Stamped<tf::Pose>& stamped_out) const;
00272 
00277   //std::string chainAsString(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame) const;
00278 
00283   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;
00284 
00288   std::string allFramesAsString() const;
00289 
00293   std::string allFramesAsDot() const;
00294 
00296   void getFrameStrings(std::vector<std::string>& ids) const;
00297 
00302   bool getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
00303 
00306   bool frameExists(const std::string& frame_id_str) const;
00307 
00311   void setExtrapolationLimit(const ros::Duration& distance);
00312 
00314   ros::Duration getCacheLength() { return cache_time;}
00315 
00323   boost::signals::connection addTransformsChangedListener(boost::function<void(void)> callback);
00324   void removeTransformsChangedListener(boost::signals::connection c);
00325 
00329   std::string getTFPrefix() const { return tf_prefix_;};
00330 
00331   //Declare that it is safe to call waitForTransform
00332   void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
00333   // Get the state of using_dedicated_thread_
00334   bool isUsingDedicatedThread() { return using_dedicated_thread_;};
00335 
00336 protected:
00337 
00349   /******************** Internal Storage ****************/
00350 
00352   typedef boost::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
00353   M_StringToCompactFrameID frameIDs_;
00355   std::vector<std::string> frameIDs_reverse;
00357   std::map<CompactFrameID, std::string> frame_authority_;
00358 
00360   ros::Duration cache_time_;
00361 
00364   std::vector<TimeCache*> frames_;
00365 
00367   mutable boost::recursive_mutex frame_mutex_;
00368 
00370   ros::Duration cache_time;
00371 
00373   bool interpolating;
00374 
00376   ros::Duration max_extrapolation_distance_;
00377 
00378 
00380   std::string tf_prefix_;
00381 
00382   typedef boost::signal<void(void)> TransformsChangedSignal;
00384   TransformsChangedSignal transforms_changed_;
00385   boost::mutex transforms_changed_mutex_;
00386 
00387   //Whether it is safe to use waitForTransform.  This is basically stating that tf is multithreaded.  
00388   bool using_dedicated_thread_;
00389   
00390  public:
00391   // A flag to allow falling back to wall time
00392   bool fall_back_to_wall_time_;
00393   
00394  protected:
00396   ros::Time now() const { 
00397     if (!fall_back_to_wall_time_) 
00398       return ros::Time::now() ; 
00399     else {
00400       ros::WallTime wt = ros::WallTime::now();
00401       return ros::Time(wt.sec, wt.nsec);
00402     };
00403   }
00404 
00405   // Allows broadcaster to check ok() before wait for transform
00406   // Always returns true in base class 
00407   virtual bool ok() const;
00408    
00409   /************************* Internal Functions ****************************/
00410 
00417   TimeCache* getFrame(unsigned int frame_number) const;
00418 
00420   CompactFrameID lookupFrameNumber(const std::string& frameid_str) const
00421   {
00422     unsigned int retval = 0;
00423     boost::recursive_mutex::scoped_lock(frame_mutex_);
00424     M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
00425     if (map_it == frameIDs_.end())
00426     {
00427       std::stringstream ss;
00428       ss << "Frame id " << frameid_str << " does not exist! Frames (" << frameIDs_.size() << "): " << allFramesAsString();
00429       throw tf::LookupException(ss.str());
00430     }
00431     else
00432       retval = map_it->second;
00433     return retval;
00434   };
00435 
00437   CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str)
00438   {
00439     unsigned int retval = 0;
00440     boost::recursive_mutex::scoped_lock(frame_mutex_);
00441     M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
00442     if (map_it == frameIDs_.end())
00443     {
00444       retval = frames_.size();
00445       frameIDs_[frameid_str] = retval;
00446       frames_.push_back( new TimeCache(cache_time));
00447       frameIDs_reverse.push_back(frameid_str);
00448     }
00449     else
00450       retval = frameIDs_[frameid_str];
00451     return retval;
00452   };
00454   std::string lookupFrameString(unsigned int frame_id_num) const
00455   {
00456     if (frame_id_num >= frameIDs_reverse.size())
00457     {
00458       std::stringstream ss;
00459       ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
00460       throw LookupException(ss.str());
00461     }
00462     else
00463       return frameIDs_reverse[frame_id_num];
00464 
00465   };
00466 
00467   /*
00468   bool test_extrapolation_one_value(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const;
00469   bool test_extrapolation_past(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const;
00470   bool test_extrapolation_future(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const;
00471   bool test_extrapolation(const ros::Time& target_time, const TransformLists& t_lists, std::string * error_string) const;
00472         */
00473 
00474  private:
00477   int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const;
00478 
00479   template<typename F>
00480   int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
00481 
00482   bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
00483                     const ros::Time& time, std::string* error_msg) const;
00484   bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
00485                       const ros::Time& time, std::string* error_msg) const;
00486 
00487   void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
00488 };
00489 
00490 
00492 inline void assertQuaternionValid(const tf::Quaternion & q)
00493 {
00494   if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
00495   {
00496     std::stringstream ss;
00497     ss << "Quaternion malformed, magnitude: " << q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() << " should be 1.0" <<std::endl;
00498     throw tf::InvalidArgument(ss.str());
00499   }  //  ROS_ASSERT(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z*q.z() + q.w()*q.w() - 1 < 0.01));
00500 };
00501 
00503 inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
00504 {
00505   if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
00506   {
00507     std::stringstream ss;
00508     ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" <<std::endl;
00509     throw tf::InvalidArgument(ss.str());
00510   }  //  ROS_ASSERT(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z*q.z() + q.w()*q.w() - 1 < 0.01));
00511 };
00512 }
00513 #endif //TF_TF_H


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Oct 6 2014 00:12:04