Fawkes API  Fawkes Development Version
transformer.h
00001 /***************************************************************************
00002  *  transformer.h - Fawkes tf transformer (based on ROS tf)
00003  *
00004  *  Created: Tue Oct 18 17:03:47 2011
00005  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version. A runtime exception applies to
00012  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00020  */
00021 
00022 /* This code is based on ROS tf with the following copyright and license:
00023  *
00024  * Copyright (c) 2008, Willow Garage, Inc.
00025  * All rights reserved.
00026  * 
00027  * Redistribution and use in source and binary forms, with or without
00028  * modification, are permitted provided that the following conditions are met:
00029  * 
00030  *     * Redistributions of source code must retain the above copyright
00031  *       notice, this list of conditions and the following disclaimer.
00032  *     * Redistributions in binary form must reproduce the above copyright
00033  *       notice, this list of conditions and the following disclaimer in the
00034  *       documentation and/or other materials provided with the distribution.
00035  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00036  *       contributors may be used to endorse or promote products derived from
00037  *       this software without specific prior written permission.
00038  * 
00039  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00040  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00041  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00042  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00043  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00044  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00045  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00046  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00047  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00048  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00049  * POSSIBILITY OF SUCH DAMAGE.
00050  */
00051 
00052 #ifndef __LIBS_TF_TRANSFORMER_H_
00053 #define __LIBS_TF_TRANSFORMER_H_
00054 
00055 #include <tf/types.h>
00056 #include <stdint.h>
00057 
00058 #include <map>
00059 #ifdef __FreeBSD__
00060 #  include <tr1/unordered_map>
00061 #else
00062 #  include <unordered_map>
00063 #endif
00064 #include <vector>
00065 #include <string>
00066 
00067 namespace fawkes {
00068 
00069   class Mutex;
00070 
00071   namespace tf {
00072 #if 0 /* just to make Emacs auto-indent happy */
00073   }
00074 }
00075 #endif
00076 
00077 class TimeCache;
00078 
00079 class Transformer
00080 {
00081  public:
00082   static const unsigned int MAX_GRAPH_DEPTH = 100UL;
00083   static const float DEFAULT_MAX_EXTRAPOLATION_DISTANCE;
00084 
00085   Transformer(float cache_time_sec = 10.0);
00086   virtual ~Transformer(void);
00087 
00088   void clear();
00089 
00090   bool set_transform(const StampedTransform &transform,
00091                      const std::string &authority = "default_authority");
00092 
00093   bool frame_exists(const std::string& frame_id_str) const;
00094 
00095 
00096   void lookup_transform(const std::string& target_frame,
00097                         const std::string& source_frame,
00098                         const fawkes::Time& time,
00099                         StampedTransform& transform) const;
00100 
00101   void lookup_transform(const std::string& target_frame,
00102                         const std::string& source_frame,
00103                         StampedTransform& transform) const;
00104 
00105   void lookup_transform(const std::string& target_frame,
00106                         const fawkes::Time& target_time,
00107                         const std::string& source_frame,
00108                         const fawkes::Time& source_time,
00109                         const std::string& fixed_frame,
00110                         StampedTransform& transform) const;
00111 
00112   bool can_transform(const std::string& target_frame,
00113                      const std::string& source_frame,
00114                      const fawkes::Time& time) const;
00115 
00116   bool can_transform(const std::string& target_frame,
00117                      const fawkes::Time& target_time,
00118                      const std::string& source_frame,
00119                      const fawkes::Time& source_time,
00120                      const std::string& fixed_frame) const;
00121 
00122   const TimeCache *  get_frame_cache(const std::string &frame_id) const;
00123 
00124   void set_enabled(bool enabled);
00125   bool is_enabled() const { return enabled_; };
00126 
00127   int get_latest_common_time(const std::string &source_frame, const std::string &target_frame,
00128                              fawkes::Time& time, std::string* error_string = 0) const;
00129 
00130   void transform_quaternion(const std::string& target_frame,
00131                             const Stamped<Quaternion>& stamped_in,
00132                             Stamped<Quaternion>& stamped_out) const;
00133   void transform_vector(const std::string& target_frame,
00134                         const Stamped<Vector3>& stamped_in,
00135                         Stamped<Vector3>& stamped_out) const;
00136   void transform_point(const std::string& target_frame,
00137                        const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const;
00138   void transform_pose(const std::string& target_frame,
00139                       const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const;
00140 
00141   void transform_quaternion(const std::string& target_frame, const fawkes::Time& target_time,
00142                             const Stamped<Quaternion>& stamped_in,
00143                             const std::string& fixed_frame,
00144                             Stamped<Quaternion>& stamped_out) const;
00145   void transform_vector(const std::string& target_frame, const fawkes::Time& target_time,
00146                        const Stamped<Vector3>& stamped_in,
00147                        const std::string& fixed_frame,
00148                        Stamped<Vector3>& stamped_out) const;
00149   void transform_point(const std::string& target_frame, const fawkes::Time& target_time,
00150                       const Stamped<Point>& stamped_in,
00151                       const std::string& fixed_frame,
00152                       Stamped<Point>& stamped_out) const;
00153   void transform_pose(const std::string& target_frame, const fawkes::Time& target_time,
00154                      const Stamped<Pose>& stamped_in,
00155                      const std::string& fixed_frame,
00156                      Stamped<Pose>& stamped_out) const;
00157 
00158  protected: /* methods */
00159   TimeCache *  get_frame(unsigned int frame_number) const;
00160 
00161   CompactFrameID lookup_frame_number(const std::string &frameid_str) const;
00162   CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str);
00163   std::string    lookup_frame_string(unsigned int frame_id_num) const;
00164 
00165  protected:
00166   /// Flag to mark the transformer as disabled
00167   bool enabled_;
00168   /// Map from string frame ids to CompactFrameID.
00169 #ifdef __FreeBSD__
00170   typedef std::tr1::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
00171 #else
00172   typedef std::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
00173 #endif
00174   /// Map from frame IDs to frame numbers
00175   M_StringToCompactFrameID frameIDs_;
00176   /// Map from CompactFrameID frame_id_numbers to string for debugging and output.
00177   std::vector<std::string> frameIDs_reverse;
00178   /// Map to lookup the most recent authority for a given frame.
00179   std::map<CompactFrameID, std::string> frame_authority_;
00180 
00181   /** \brief The pointers to potential frames that the tree can be made of.
00182    * The frames will be dynamically allocated at run time when set the first time. */
00183   std::vector<TimeCache*> frames_;
00184 
00185   /** \brief A mutex to protect testing and allocating new frames on the above vector. */
00186   mutable Mutex *frame_mutex_;
00187 
00188   /// How long to cache transform history
00189   float cache_time_;
00190 
00191   /// whether or not to allow extrapolation
00192   float max_extrapolation_distance_;
00193 
00194   /// transform prefix to apply as necessary
00195   std::string tf_prefix_;
00196 
00197   /// Set to true to allow falling back to wall time
00198   bool fall_back_to_wall_time_;
00199 
00200  private:
00201   /**Return the latest time which is common across the spanning set.
00202    * @return zero if fails to cross */
00203   int get_latest_common_time(CompactFrameID target_frame, CompactFrameID source_frame,
00204                              fawkes::Time& time, std::string* error_string) const;
00205 
00206   bool can_transform_no_lock(CompactFrameID target_id, CompactFrameID source_id,
00207                              const fawkes::Time& time) const;
00208   void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
00209 
00210   template<typename F>
00211   int walk_to_top_parent(F& f, fawkes::Time time,
00212                          CompactFrameID target_id, CompactFrameID source_id,
00213                          std::string* error_string) const;
00214 
00215 };
00216 
00217 
00218 } // end namespace tf
00219 } // end namespace fawkes
00220 
00221 #endif