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/unordered_map.hpp>
00045 #include <boost/signals2.hpp>
00046 #include "geometry_msgs/TwistStamped.h"
00047 
00048 #include <tf2_ros/buffer.h>
00049 
00050 namespace tf
00051 {
00053 std::string resolve(const std::string& prefix, const std::string& frame_name);
00054 
00056 std::string strip_leading_slash(const std::string& frame_name);
00057 
00059 __attribute__((deprecated)) static inline std::string remap(const std::string& prefix, const std::string& frame_name) { return tf::resolve(prefix, frame_name);} ;
00060 
00061 enum ErrorValues { NO_ERROR = 0, LOOKUP_ERROR, CONNECTIVITY_ERROR, EXTRAPOLATION_ERROR};
00062 
00066 typedef struct
00067 {
00068   std::vector<TransformStorage > inverseTransforms;
00069   std::vector<TransformStorage > forwardTransforms;
00070 } TransformLists;
00071 
00090 class Transformer
00091 {
00092 public:
00093   /************* Constants ***********************/
00094   static const unsigned int MAX_GRAPH_DEPTH = 100UL;   
00095   static const double DEFAULT_CACHE_TIME;  
00096   static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL; 
00097 
00098 
00104   Transformer(bool interpolating = true,
00105               ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
00106   virtual ~Transformer(void);
00107 
00109   void clear();
00110 
00116   bool setTransform(const StampedTransform& transform, const std::string & authority = "default_authority");
00117 
00118   /*********** Accessors *************/
00119 
00129   void lookupTransform(const std::string& target_frame, const std::string& source_frame,
00130                        const ros::Time& time, StampedTransform& transform) const;
00142   void 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, StampedTransform& transform) const;
00145 
00166   void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00167                    const tf::Point & reference_point, const std::string& reference_point_frame, 
00168                    const ros::Time& time, const ros::Duration& averaging_interval, 
00169                    geometry_msgs::Twist& twist) const;
00170 
00181   void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, 
00182                    const ros::Time& time, const ros::Duration& averaging_interval,
00183                    geometry_msgs::Twist& twist) const;
00184 
00193   bool waitForTransform(const std::string& target_frame, const std::string& source_frame,
00194                         const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
00195                         std::string* error_msg = NULL) const;
00202   bool canTransform(const std::string& target_frame, const std::string& source_frame,
00203                     const ros::Time& time,
00204                     std::string* error_msg = NULL) const;
00205 
00214   bool canTransform(const std::string& target_frame, const ros::Time& target_time,
00215                     const std::string& source_frame, const ros::Time& source_time,
00216                     const std::string& fixed_frame,
00217                     std::string* error_msg = NULL) const;
00218 
00229   bool waitForTransform(const std::string& target_frame, const ros::Time& target_time,
00230                     const std::string& source_frame, const ros::Time& source_time,
00231                     const std::string& fixed_frame,
00232                     const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
00233                     std::string* error_msg = NULL) const;
00234 
00237   int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const;
00238 
00241   void transformQuaternion(const std::string& target_frame, const Stamped<tf::Quaternion>& stamped_in, Stamped<tf::Quaternion>& stamped_out) const;
00244   void transformVector(const std::string& target_frame, const Stamped<tf::Vector3>& stamped_in, Stamped<tf::Vector3>& stamped_out) const;
00247   void transformPoint(const std::string& target_frame, const Stamped<tf::Point>& stamped_in, Stamped<tf::Point>& stamped_out) const;
00250   void transformPose(const std::string& target_frame, const Stamped<tf::Pose>& stamped_in, Stamped<tf::Pose>& stamped_out) const;
00251 
00254   void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00255                            const Stamped<tf::Quaternion>& stamped_in,
00256                            const std::string& fixed_frame,
00257                            Stamped<tf::Quaternion>& stamped_out) const;
00260   void transformVector(const std::string& target_frame, const ros::Time& target_time,
00261                        const Stamped<tf::Vector3>& stamped_in,
00262                        const std::string& fixed_frame,
00263                        Stamped<tf::Vector3>& stamped_out) const;
00266   void transformPoint(const std::string& target_frame, const ros::Time& target_time,
00267                       const Stamped<tf::Point>& stamped_in,
00268                       const std::string& fixed_frame,
00269                       Stamped<tf::Point>& stamped_out) const;
00272   void transformPose(const std::string& target_frame, const ros::Time& target_time,
00273                      const Stamped<tf::Pose>& stamped_in,
00274                      const std::string& fixed_frame,
00275                      Stamped<tf::Pose>& stamped_out) const;
00276 
00281   //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;
00282 
00287   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;
00288 
00292   std::string allFramesAsString() const;
00293 
00297   std::string allFramesAsDot(double current_time = 0) const;
00298 
00300   void getFrameStrings(std::vector<std::string>& ids) const;
00301 
00306   bool getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
00307 
00310   bool frameExists(const std::string& frame_id_str) const;
00311 
00315   void setExtrapolationLimit(const ros::Duration& distance);
00316 
00318   ros::Duration getCacheLength() { return tf2_buffer_.getCacheLength();}
00319 
00327   boost::signals2::connection addTransformsChangedListener(boost::function<void(void)> callback);
00328   void removeTransformsChangedListener(boost::signals2::connection c);
00329 
00333   std::string getTFPrefix() const { return tf_prefix_;};
00334 
00335   //Declare that it is safe to call waitForTransform
00336   void setUsingDedicatedThread(bool value) { tf2_buffer_.setUsingDedicatedThread(value);};
00337   // Get the state of using_dedicated_thread_ from the buffer
00338   bool isUsingDedicatedThread() { return tf2_buffer_.isUsingDedicatedThread();};
00339 
00340 protected:
00341 
00342   
00343 
00355   /******************** Internal Storage ****************/
00356 
00357 
00359   std::string tf_prefix_;
00360 
00361   
00362  public:
00363   // A flag to allow falling back to wall time
00364   bool fall_back_to_wall_time_;
00365   
00366  protected:
00368   ros::Time now() const { 
00369     if (!fall_back_to_wall_time_) 
00370       return ros::Time::now() ; 
00371     else {
00372       ros::WallTime wt = ros::WallTime::now();
00373       return ros::Time(wt.sec, wt.nsec);
00374     };
00375   }
00376 
00377   // Allows broadcaster to check ok() before wait for transform
00378   // Always returns true in base class 
00379   virtual bool ok() const;
00380    
00381   /************************* Internal Functions ****************************/
00382 
00383 
00384 
00385 
00386 protected:
00387   tf2_ros::Buffer tf2_buffer_;
00388 
00389 };
00390 
00391 
00393 inline void assertQuaternionValid(const tf::Quaternion & q)
00394 {
00395   if(std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w()))
00396   {
00397     std::stringstream ss;
00398     ss << "Quaternion contains a NaN" << std::endl;
00399     throw tf::InvalidArgument(ss.str());
00400   }
00401 
00402   if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
00403   {
00404     std::stringstream ss;
00405     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;
00406     throw tf::InvalidArgument(ss.str());
00407   }  //  ROS_ASSERT(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z*q.z() + q.w()*q.w() - 1 < 0.01));
00408 };
00409 
00411 inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
00412 {
00413   if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w))
00414   {
00415     std::stringstream ss;
00416     ss << "Quaternion contains a NaN" << std::endl;
00417     throw tf::InvalidArgument(ss.str());
00418   }
00419 
00420   if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
00421   {
00422     std::stringstream ss;
00423     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;
00424     throw tf::InvalidArgument(ss.str());
00425   }  //  ROS_ASSERT(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z*q.z() + q.w()*q.w() - 1 < 0.01));
00426 };
00427 }
00428 #endif //TF_TF_H


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55