Fawkes API
Fawkes Development Version
|
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