Fawkes API  Fawkes Development Version
transformer.cpp
00001 /***************************************************************************
00002  *  transformer.cpp - Fawkes tf transformer (based on ROS tf)
00003  *
00004  *  Created: Thu Oct 20 10:56:25 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 #include <tf/transformer.h>
00053 #include <tf/time_cache.h>
00054 #include <tf/exceptions.h>
00055 
00056 #include <core/threading/mutex_locker.h>
00057 #include <iostream>
00058 #include <sstream>
00059 #include <algorithm>
00060 
00061 namespace fawkes {
00062   namespace tf {
00063 #if 0 /* just to make Emacs auto-indent happy */
00064   }
00065 }
00066 #endif
00067 
00068 /** @class Transformer <tf/transformer.h>
00069  * Coordinate transforms between any two frames in a system.
00070  *
00071  * This class provides a simple interface to allow recording and
00072  * lookup of relationships between arbitrary frames of the system.
00073  *
00074  * TF assumes that there is a tree of coordinate frame transforms
00075  * which define the relationship between all coordinate frames.  For
00076  * example your typical robot would have a transform from global to
00077  * real world.  And then from base to hand, and from base to head.
00078  * But Base to Hand really is composed of base to shoulder to elbow to
00079  * wrist to hand.  TF is designed to take care of all the intermediate
00080  * steps for you.
00081  *
00082  * Internal Representation TF will store frames with the parameters
00083  * necessary for generating the transform into that frame from it's
00084  * parent and a reference to the parent frame.  Frames are designated
00085  * using an std::string 0 is a frame without a parent (the top of a
00086  * tree) The positions of frames over time must be pushed in.
00087  *
00088  * All function calls which pass frame ids can potentially throw the
00089  * exception fawkes::tf::LookupException
00090  *
00091  * 
00092  * @fn bool Transformer::is_enabled() const
00093  * Check if transformer is enabled.
00094  * @return true if enabled, false otherwise
00095  *
00096  * @var static const unsigned int Transformer::MAX_GRAPH_DEPTH
00097  * Maximum number of times to recurse before assuming the tree
00098  * has a loop
00099  *
00100  * @param static const int64_t Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE
00101  * The default amount of time to extrapolate.
00102  */
00103 
00104 /** The default amount of time to extrapolate. */
00105 const float Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0.f;
00106 
00107 /// Flag to advise accumulator finalization.
00108 enum WalkEnding
00109 {
00110   Identity,     ///< identity
00111   TargetParentOfSource, ///< Target is parent of source
00112   SourceParentOfTarget, ///< Source is parent of target
00113   FullPath,     ///< Full path between source and target.
00114 };
00115 
00116 /// Internal error values
00117 enum ErrorValues {
00118   NO_ERROR = 0, ///< No error occured.
00119   LOOKUP_ERROR, ///< Frame ID lookup error
00120   CONNECTIVITY_ERROR, ///< No connection between frames found
00121   EXTRAPOLATION_ERROR   ///< Extrapolation required out of limits.
00122 };
00123 
00124 /** Accumulator to test for transformability.
00125  * Operations are basically noops.
00126  */
00127 struct CanTransformAccum
00128 {
00129   /** Gather frame number.
00130    * @param cache cache
00131    * @param time time
00132    * @param error_string string containing error message in case of failure
00133    * @return parent frame number
00134    */
00135   CompactFrameID
00136   gather(TimeCache* cache, fawkes::Time time, std::string* error_string)
00137   {
00138     return cache->get_parent(time, error_string);
00139   }
00140 
00141   /** Accumulate.
00142    * @param source true if looking from source
00143    */
00144   void accum(bool source)
00145   {
00146   }
00147 
00148   /** Finalize accumulation.
00149    * @param end flag how the walk ended
00150    * @param _time time
00151    */
00152   void finalize(WalkEnding end, fawkes::Time _time)
00153   {
00154   }
00155 
00156   /// Transform storage to use.
00157   TransformStorage st;
00158 };
00159 
00160 /** Accumulator to accumulate transforms between two frames.
00161  */
00162 struct TransformAccum
00163 {
00164   /** Constructor. */
00165   TransformAccum()
00166   : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
00167   , source_to_top_vec(0.0, 0.0, 0.0)
00168   , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
00169   , target_to_top_vec(0.0, 0.0, 0.0)
00170   , result_quat(0.0, 0.0, 0.0, 1.0)
00171   , result_vec(0.0, 0.0, 0.0)
00172   {
00173   }
00174 
00175   /** Gather frame number.
00176    * @param cache cache
00177    * @param time time
00178    * @param error_string string containing error message in case of failure
00179    * @return parent frame number
00180    */
00181   CompactFrameID
00182   gather(TimeCache* cache, fawkes::Time time, std::string* error_string)
00183   {
00184     if (!cache->get_data(time, st, error_string))
00185     {
00186       return 0;
00187     }
00188 
00189     return st.frame_id_;
00190   }
00191 
00192   /** Accumulate.
00193    * @param source true if looking from source
00194    */
00195   void accum(bool source)
00196   {
00197     if (source)
00198     {
00199       source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
00200       source_to_top_quat = st.rotation_ * source_to_top_quat;
00201     }
00202     else
00203     {
00204       target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
00205       target_to_top_quat = st.rotation_ * target_to_top_quat;
00206     }
00207   }
00208 
00209   /** Finalize accumulation.
00210    * @param end flag how the walk ended
00211    * @param _time time
00212    */
00213   void finalize(WalkEnding end, fawkes::Time _time)
00214   {
00215     switch (end)
00216     {
00217     case Identity:
00218       break;
00219     case TargetParentOfSource:
00220       result_vec = source_to_top_vec;
00221       result_quat = source_to_top_quat;
00222       break;
00223     case SourceParentOfTarget:
00224       {
00225         btQuaternion inv_target_quat = target_to_top_quat.inverse();
00226         btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00227         result_vec = inv_target_vec;
00228         result_quat = inv_target_quat;
00229         break;
00230       }
00231     case FullPath:
00232       {
00233         btQuaternion inv_target_quat = target_to_top_quat.inverse();
00234         btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00235 
00236         result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
00237         result_quat = inv_target_quat * source_to_top_quat;
00238       }
00239       break;
00240     };
00241 
00242     time = _time;
00243   }
00244 
00245   /// Transform storage.
00246   TransformStorage st;
00247   /// time stamp
00248   fawkes::Time time;
00249   /// Source to top accumulated quaternion
00250   btQuaternion source_to_top_quat;
00251   /// Source to top accumulated vector
00252   btVector3 source_to_top_vec;
00253   /// Target to top accumulated quaternion
00254   btQuaternion target_to_top_quat;
00255   /// Target to top accumulated vector
00256   btVector3 target_to_top_vec;
00257 
00258   /// After finalize contains result quaternion.
00259   btQuaternion result_quat;
00260   /// After finalize contains result vector.
00261   btVector3 result_vec;
00262 };
00263 
00264 /** Constructor.
00265  * @param cache_time time in seconds to cache incoming transforms
00266  */
00267 Transformer::Transformer(float cache_time)
00268   : cache_time_(cache_time),
00269     fall_back_to_wall_time_(false)
00270 {
00271   max_extrapolation_distance_ = DEFAULT_MAX_EXTRAPOLATION_DISTANCE;
00272   frameIDs_["NO_PARENT"] = 0;
00273   //unused but needed for iteration over all elements
00274   frames_.push_back(NULL);
00275   frameIDs_reverse.push_back("NO_PARENT");
00276 
00277   frame_mutex_ = new Mutex();
00278 }
00279 
00280 
00281 /** Destructor. */
00282 Transformer::~Transformer()
00283 {
00284   // deallocate all frames
00285   MutexLocker lock(frame_mutex_);
00286   std::vector<TimeCache*>::iterator cache_it;
00287   for (cache_it = frames_.begin(); cache_it != frames_.end(); ++cache_it)
00288   {
00289     delete (*cache_it);
00290   }
00291 
00292 };
00293 
00294 
00295 /** Set transformer enabled or disabled.
00296  * @param enabled true to enable, false to disable
00297  */
00298 void
00299 Transformer::set_enabled(bool enabled)
00300 {
00301   enabled_ = enabled;
00302 }
00303 
00304 /** Clear cached transforms. */
00305 void
00306 Transformer::clear()
00307 {
00308   MutexLocker lock(frame_mutex_);
00309   if ( frames_.size() > 1 )
00310   {
00311     std::vector< TimeCache*>::iterator cache_it;
00312     for (cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
00313     {
00314       (*cache_it)->clear_list();
00315     }
00316   }
00317 }
00318 
00319 
00320 /** Check if frame exists.
00321  * @param frame_id_str frame ID
00322  * @result true if frame exists, false otherwise
00323  */
00324 bool
00325 Transformer::frame_exists(const std::string& frame_id_str) const
00326 {
00327   MutexLocker lock(frame_mutex_);
00328   std::string frame_id_resolveped = /*assert_resolved(tf_prefix_,*/ frame_id_str;
00329   
00330   return frameIDs_.count(frame_id_resolveped);
00331 }
00332 
00333 void
00334 Transformer::create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const
00335 {
00336   if (!out) return;
00337 
00338   *out = std::string("Could not find a connection between '"+lookup_frame_string(target_frame)+"' and '"+
00339                      lookup_frame_string(source_frame)+"' because they are not part of the same tree."+
00340                      "Tf has two or more unconnected trees.");
00341 }
00342 
00343 
00344 /** Walk from frame to top-parent of both.
00345  * @param f accumulator
00346  * @param time timestamp
00347  * @param target_id frame number of target
00348  * @param source_id frame number of source
00349  * @param error_string accumulated error string
00350  * @return error flag from ErrorValues
00351  */
00352 template<typename F>
00353 int
00354 Transformer::walk_to_top_parent(F& f, fawkes::Time time,
00355                                 CompactFrameID target_id, CompactFrameID source_id,
00356                                 std::string* error_string) const
00357 {
00358   // Short circuit if zero length transform to allow lookups on non existant links
00359   if (source_id == target_id)
00360   {
00361     f.finalize(Identity, time);
00362     return NO_ERROR;
00363   }
00364 
00365   //If getting the latest get the latest common time
00366   if (time == fawkes::Time(0,0))
00367   {
00368     int retval = get_latest_common_time(target_id, source_id, time, error_string);
00369     if (retval != NO_ERROR)
00370     {
00371       return retval;
00372     }
00373   }
00374 
00375   // Walk the tree to its root from the source frame, accumulating the transform
00376   CompactFrameID frame = source_id;
00377   CompactFrameID top_parent = frame;
00378   uint32_t depth = 0;
00379   while (frame != 0)
00380   {
00381     TimeCache* cache = get_frame(frame);
00382 
00383     if (!cache)
00384     {
00385       // There will be no cache for the very root of the tree
00386       top_parent = frame;
00387       break;
00388     }
00389 
00390     CompactFrameID parent = f.gather(cache, time, 0);
00391     if (parent == 0)
00392     {
00393       // Just break out here... there may still be a path from source -> target
00394       top_parent = frame;
00395       break;
00396     }
00397 
00398     // Early out... target frame is a direct parent of the source frame
00399     if (frame == target_id)
00400     {
00401       f.finalize(TargetParentOfSource, time);
00402       return NO_ERROR;
00403     }
00404 
00405     f.accum(true);
00406 
00407     top_parent = frame;
00408     frame = parent;
00409 
00410     ++depth;
00411     if (depth > MAX_GRAPH_DEPTH)
00412     {
00413       if (error_string)
00414       {
00415         std::stringstream ss;
00416         //ss << "The tf tree is invalid because it contains a loop." << std::endl
00417         //   << allFramesAsString() << std::endl;
00418         *error_string = ss.str();
00419       }
00420       return LOOKUP_ERROR;
00421     }
00422   }
00423 
00424   // Now walk to the top parent from the target frame, accumulating its transform
00425   frame = target_id;
00426   depth = 0;
00427   while (frame != top_parent)
00428   {
00429     TimeCache* cache = get_frame(frame);
00430 
00431     if (!cache)
00432     {
00433       break;
00434     }
00435 
00436     CompactFrameID parent = f.gather(cache, time, error_string);
00437     if (parent == 0)
00438     {
00439       if (error_string)
00440       {
00441         std::stringstream ss;
00442         ss << "Looking up transform from frame [" << lookup_frame_string(source_id) <<
00443           "] to frame [" << lookup_frame_string(target_id) << "] failed";
00444         *error_string = ss.str();
00445       }
00446 
00447       return EXTRAPOLATION_ERROR;
00448     }
00449 
00450     // Early out... source frame is a direct parent of the target frame
00451     if (frame == source_id)
00452     {
00453       f.finalize(SourceParentOfTarget, time);
00454       return NO_ERROR;
00455     }
00456 
00457     f.accum(false);
00458 
00459     frame = parent;
00460 
00461     ++depth;
00462     if (depth > MAX_GRAPH_DEPTH)
00463     {
00464       if (error_string)
00465       {
00466         std::stringstream ss;
00467         //ss << "The tf tree is invalid because it contains a loop." << std::endl
00468         //   << allFramesAsString() << std::endl;
00469         *error_string = "The tf tree is invalid because it contains a loop.";
00470       }
00471       return LOOKUP_ERROR;
00472     }
00473   }
00474 
00475   if (frame != top_parent)
00476   {
00477     create_connectivity_error_string(source_id, target_id, error_string);
00478     return CONNECTIVITY_ERROR;
00479   }
00480 
00481   f.finalize(FullPath, time);
00482 
00483   return NO_ERROR;
00484 }
00485 
00486 /** Time and frame ID comparator class. */
00487 struct TimeAndFrameIDFrameComparator
00488 {
00489   /** Constructor.
00490    * @param id frame number to look for
00491    */
00492   TimeAndFrameIDFrameComparator(CompactFrameID id)
00493   : id(id)
00494   {}
00495 
00496   /** Application operator.
00497    * @param rhs right-hand side to compare to
00498    * @return true if the frame numbers are equal, false otherwise
00499    */
00500   bool operator()(const P_TimeAndFrameID& rhs) const
00501   {
00502     return rhs.second == id;
00503   }
00504 
00505   /// Frame number to look for.
00506   CompactFrameID id;
00507 };
00508 
00509 
00510 /** Get latest common time of two frames.
00511  * @param target_id target frame number
00512  * @param source_id source frame number
00513  * @param time upon return contains latest common timestamp
00514  * @param error_string upon error contains accumulated error message.
00515  * @return value from ErrorValues
00516  */
00517 int
00518 Transformer::get_latest_common_time(CompactFrameID target_id, CompactFrameID source_id,
00519                                  fawkes::Time & time, std::string *error_string) const
00520 {
00521   if (source_id == target_id)
00522   {
00523     //Set time to latest timestamp of frameid in case of target and source frame id are the same
00524     time.stamp();
00525     return NO_ERROR;
00526   }
00527 
00528   std::vector<P_TimeAndFrameID> lct_cache;
00529 
00530   // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
00531   // in the target is a direct parent
00532   CompactFrameID frame = source_id;
00533   P_TimeAndFrameID temp;
00534   uint32_t depth = 0;
00535   fawkes::Time common_time = fawkes::TIME_MAX;
00536   while (frame != 0)
00537   {
00538     TimeCache* cache = get_frame(frame);
00539 
00540     if (!cache) {
00541       // There will be no cache for the very root of the tree
00542       break;
00543     }
00544 
00545     P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
00546 
00547     if (latest.second == 0) {
00548       // Just break out here... there may still be a path from source -> target
00549       break;
00550     }
00551 
00552     if (!latest.first.is_zero()) {
00553       common_time = std::min(latest.first, common_time);
00554     }
00555 
00556     lct_cache.push_back(latest);
00557 
00558     frame = latest.second;
00559 
00560     // Early out... target frame is a direct parent of the source frame
00561     if (frame == target_id) {
00562       time = common_time;
00563       if (time == fawkes::TIME_MAX) {
00564         time = fawkes::Time();
00565       }
00566       return NO_ERROR;
00567     }
00568 
00569     ++depth;
00570     if (depth > MAX_GRAPH_DEPTH) {
00571       if (error_string) {
00572         /*
00573         std::stringstream ss;
00574         ss<<"The tf tree is invalid because it contains a loop." << std::endl
00575           << allFramesAsString() << std::endl;
00576         *error_string = ss.str();
00577         */
00578       }
00579       return LOOKUP_ERROR;
00580     }
00581   }
00582 
00583   // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
00584   frame = target_id;
00585   depth = 0;
00586   common_time = fawkes::TIME_MAX;
00587   CompactFrameID common_parent = 0;
00588   while (true)
00589   {
00590     TimeCache* cache = get_frame(frame);
00591 
00592     if (!cache) {
00593       break;
00594     }
00595 
00596     P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
00597 
00598     if (latest.second == 0) {
00599       break;
00600     }
00601 
00602     if (!latest.first.is_zero()) {
00603       common_time = std::min(latest.first, common_time);
00604     }
00605 
00606     std::vector<P_TimeAndFrameID>::iterator it =
00607       std::find_if(lct_cache.begin(), lct_cache.end(),
00608                    TimeAndFrameIDFrameComparator(latest.second));
00609 
00610     if (it != lct_cache.end()) { // found a common parent
00611       common_parent = it->second;
00612       break;
00613     }
00614 
00615     frame = latest.second;
00616 
00617     // Early out... source frame is a direct parent of the target frame
00618     if (frame == source_id) {
00619       time = common_time;
00620       if (time == fawkes::TIME_MAX) {
00621         time = fawkes::Time();
00622       }
00623       return NO_ERROR;
00624     }
00625 
00626     ++depth;
00627     if (depth > MAX_GRAPH_DEPTH) {
00628       if (error_string) {
00629         std::stringstream ss;
00630         //ss<<"The tf tree is invalid because it contains a loop." << std::endl
00631         //  << allFramesAsString() << std::endl;
00632         *error_string = ss.str();
00633       }
00634       return LOOKUP_ERROR;
00635     }
00636   }
00637 
00638   if (common_parent == 0) {
00639     create_connectivity_error_string(source_id, target_id, error_string);
00640     return CONNECTIVITY_ERROR;
00641   }
00642 
00643   // Loop through the source -> root list until we hit the common parent
00644   {
00645     std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
00646     std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
00647     for (; it != end; ++it) {
00648       if (!it->first.is_zero()) {
00649         common_time = std::min(common_time, it->first);
00650       }
00651 
00652       if (it->second == common_parent) {
00653         break;
00654       }
00655     }
00656   }
00657 
00658   if (common_time == fawkes::TIME_MAX) {
00659     common_time = fawkes::Time();
00660   }
00661 
00662   time = common_time;
00663   return NO_ERROR;
00664 }
00665 
00666 /** Get latest common time of two frames.
00667  * @param source_frame source frame ID
00668  * @param target_frame frame target frame ID
00669  * @param time upon return contains latest common timestamp
00670  * @param error_string upon error contains accumulated error message.
00671  * @return value from ErrorValues
00672  */
00673 int
00674 Transformer::get_latest_common_time(const std::string &source_frame,
00675                                     const std::string &target_frame,
00676                                     fawkes::Time& time,
00677                                     std::string* error_string) const
00678 {
00679   std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame;
00680   std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame;
00681 
00682   if (!frame_exists(mapped_tgt) || !frame_exists(mapped_src)) {
00683     time = fawkes::Time();
00684     return LOOKUP_ERROR;
00685   }
00686 
00687   CompactFrameID source_id = lookup_frame_number(mapped_src);
00688   CompactFrameID target_id = lookup_frame_number(mapped_tgt);
00689   return get_latest_common_time(source_id, target_id, time, error_string);
00690 }
00691 
00692 /** Set a transform.
00693  * @param transform transform to set
00694  * @param authority authority from which the transform was received.
00695  * @return true on success, false otherwise
00696  */
00697 bool
00698 Transformer::set_transform(const StampedTransform &transform, const std::string &authority)
00699 {
00700   StampedTransform mapped_transform((btTransform)transform, transform.stamp,
00701                                     transform.frame_id, transform.child_frame_id);
00702   mapped_transform.child_frame_id = /*assert_resolved(tf_prefix_, */ transform.child_frame_id;
00703   mapped_transform.frame_id = /*assert_resolved(tf_prefix_,*/ transform.frame_id;
00704 
00705  
00706   bool error_exists = false;
00707   if (mapped_transform.child_frame_id == mapped_transform.frame_id)
00708   {
00709     printf("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id  \"%s\" because they are the same",  authority.c_str(), mapped_transform.child_frame_id.c_str());
00710     error_exists = true;
00711   }
00712 
00713   if (mapped_transform.child_frame_id == "/")//empty frame id will be mapped to "/"
00714   {
00715     printf("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
00716     error_exists = true;
00717   }
00718 
00719   if (mapped_transform.frame_id == "/")//empty parent id will be mapped to "/"
00720   {
00721     printf("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\"  from authority \"%s\" because frame_id not set", mapped_transform.child_frame_id.c_str(), authority.c_str());
00722     error_exists = true;
00723   }
00724 
00725   if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
00726       std::isnan(mapped_transform.getRotation().x()) ||       std::isnan(mapped_transform.getRotation().y()) ||       std::isnan(mapped_transform.getRotation().z()) ||       std::isnan(mapped_transform.getRotation().w()))
00727   {
00728     printf("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
00729            mapped_transform.child_frame_id.c_str(), authority.c_str(),
00730            mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
00731            mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
00732               );
00733     error_exists = true;
00734   }
00735 
00736   if (error_exists)
00737     return false;
00738 
00739   {
00740     MutexLocker lock(frame_mutex_);
00741     CompactFrameID frame_number = lookup_or_insert_frame_number(mapped_transform.child_frame_id);
00742     TimeCache* frame = get_frame(frame_number);
00743     if (frame == NULL) {
00744       frames_[frame_number] = new TimeCache(cache_time_);
00745       frame = frames_[frame_number];
00746     }
00747 
00748     if (frame->insert_data(TransformStorage(mapped_transform, lookup_or_insert_frame_number(mapped_transform.frame_id), frame_number)))
00749     {
00750       frame_authority_[frame_number] = authority;
00751     } else {
00752       //ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", mapped_transform.child_frame_id.c_str(), mapped_transform.stamp.toSec(), authority.c_str());
00753       return false;
00754     }
00755   }
00756 
00757   return true;
00758 };
00759 
00760 
00761 /** An accessor to get a frame.
00762  * This is an internal function which will get the pointer to the
00763  * frame associated with the frame id
00764  * @param frame_number The frameID of the desired reference frame
00765  * @return time cache for given frame number
00766  * @exception LookupException thrown if lookup fails
00767  */
00768 TimeCache *
00769 Transformer::get_frame(unsigned int frame_number) const
00770 {
00771   if (frame_number == 0) /// @todo check larger values too
00772     return NULL;
00773   else
00774     return frames_[frame_number];
00775 }
00776 
00777 
00778 /** An accessor to get access to the frame cache.
00779  * @param frame_id frame ID
00780  * @return time cache for given frame ID
00781  * @exception LookupException thrown if lookup fails
00782  */
00783 const TimeCache *
00784 Transformer::get_frame_cache(const std::string &frame_id) const
00785 {
00786   CompactFrameID frame_number = lookup_frame_number(frame_id);
00787   if (frame_number == 0) {
00788     throw LookupException("Failed to lookup frame %s", frame_id.c_str());
00789   } else {
00790     return frames_[frame_number];
00791   }
00792 }
00793 
00794 
00795 /** String to number for frame lookup.
00796  * @param frameid_str frame ID
00797  * @return frame number
00798  * @exception LookupException if frame ID is unknown
00799  */
00800 CompactFrameID
00801 Transformer::lookup_frame_number(const std::string &frameid_str) const
00802 {
00803   unsigned int retval = 0;
00804   //MutexLocker lock(frame_mutex_);
00805   M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
00806   if (map_it == frameIDs_.end())
00807   {
00808     throw LookupException("Frame id %s does not exist! Frames (%zu): %s",
00809                           frameid_str.c_str(), frameIDs_.size(), "" /*allFramesAsString()*/);
00810   }
00811   else
00812     retval = map_it->second;
00813   return retval;
00814 };
00815 
00816 
00817 /** String to number for frame lookup with dynamic allocation of new
00818  * frames.
00819  * @param frameid_str frame ID
00820  * @return frame number, if none existed for the given frame ID a new
00821  * one is created
00822  */
00823 CompactFrameID
00824 Transformer::lookup_or_insert_frame_number(const std::string &frameid_str)
00825 {
00826   unsigned int retval = 0;
00827   //MutexLocker lock(frame_mutex_);
00828   M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
00829   if (map_it == frameIDs_.end())
00830   {
00831     retval = frames_.size();
00832     frameIDs_[frameid_str] = retval;
00833     frames_.push_back( new TimeCache(cache_time_));
00834     frameIDs_reverse.push_back(frameid_str);
00835   }
00836   else
00837     retval = frameIDs_[frameid_str];
00838   return retval;
00839 };
00840 
00841 /** Get frame ID given its number.
00842  * @param frame_num frame number
00843  * @return frame ID string
00844  * @exception LookupException thrown if number invalid
00845  */
00846 std::string
00847 Transformer::lookup_frame_string(unsigned int frame_num) const
00848 {
00849   if (frame_num >= frameIDs_reverse.size()) {
00850     throw LookupException("Reverse lookup of frame id %u failed!", frame_num);
00851   }
00852   else return frameIDs_reverse[frame_num];
00853 }
00854 
00855 
00856 /** Lookup transform.
00857  * @param target_frame target frame ID
00858  * @param source_frame source frame ID
00859  * @param time time for which to get the transform, set to (0,0) to get latest
00860  * common time frame
00861  * @param transform upon return contains the transform
00862  * @exception ConnectivityException thrown if no connection between
00863  * the source and target frame could be found in the tree.
00864  * @exception ExtrapolationException returning a value would have
00865  * required extrapolation beyond current limits.
00866  * @exception LookupException at least one of the two given frames is
00867  * unknown
00868  */
00869 void Transformer::lookup_transform(const std::string& target_frame,
00870                                    const std::string& source_frame,
00871                                    const fawkes::Time& time,
00872                                    StampedTransform& transform) const
00873 {
00874   if (! enabled_) {
00875     throw DisabledException("Transformer has been disabled");
00876   }
00877 
00878   std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame;
00879   std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame;
00880 
00881   if (mapped_tgt == mapped_src) {
00882     transform.setIdentity();
00883     transform.child_frame_id = mapped_src;
00884     transform.frame_id       = mapped_tgt;
00885     transform.stamp          = fawkes::Time();
00886     return;
00887   }
00888 
00889   MutexLocker lock(frame_mutex_);
00890 
00891   CompactFrameID target_id = lookup_frame_number(mapped_tgt);
00892   CompactFrameID source_id = lookup_frame_number(mapped_src);
00893 
00894   std::string error_string;
00895   TransformAccum accum;
00896   int rv = walk_to_top_parent(accum, time, target_id, source_id, &error_string);
00897   if (rv != NO_ERROR)
00898   {
00899     switch (rv)
00900     {
00901     case CONNECTIVITY_ERROR:
00902       throw ConnectivityException("Connectivity: %s", error_string.c_str());
00903     case EXTRAPOLATION_ERROR:
00904       throw ExtrapolationException("Extrapolation: %s", error_string.c_str());
00905     case LOOKUP_ERROR:
00906       throw LookupException("Lookup: %s", error_string.c_str());
00907     default:
00908       throw Exception("lookup_transform: unknown error code: %d", rv);
00909       break;
00910     }
00911   }
00912 
00913   transform.setOrigin(accum.result_vec);
00914   transform.setRotation(accum.result_quat);
00915   transform.child_frame_id = mapped_src;
00916   transform.frame_id       = mapped_tgt;
00917   transform.stamp          = accum.time;
00918 }
00919 
00920 
00921 /** Lookup transform at latest common time.
00922  * @param target_frame target frame ID
00923  * @param source_frame source frame ID
00924  * @param transform upon return contains the transform
00925  * @exception ConnectivityException thrown if no connection between
00926  * the source and target frame could be found in the tree.
00927  * @exception ExtrapolationException returning a value would have
00928  * required extrapolation beyond current limits.
00929  * @exception LookupException at least one of the two given frames is
00930  * unknown
00931  */
00932 void Transformer::lookup_transform(const std::string& target_frame,
00933                                    const std::string& source_frame,
00934                                    StampedTransform& transform) const
00935 {
00936   lookup_transform(target_frame, source_frame, fawkes::Time(0,0), transform);
00937 }
00938 
00939 
00940 /** Lookup transform assuming a fixed frame.
00941  * This will lookup a transformation from source to target, assuming
00942  * that there is a fixed frame, by first finding the transform of the
00943  * source frame in the fixed frame, and then a transformation from the
00944  * fixed frame to the target frame.
00945  * @param target_frame target frame ID
00946  * @param target_time time for the target frame
00947  * @param source_frame source frame ID
00948  * @param source_time time in the source frame
00949  * @param fixed_frame ID of fixed frame
00950  * @param transform upon return contains the transform
00951  * @exception ConnectivityException thrown if no connection between
00952  * the source and target frame could be found in the tree.
00953  * @exception ExtrapolationException returning a value would have
00954  * required extrapolation beyond current limits.
00955  * @exception LookupException at least one of the two given frames is
00956  * unknown
00957  */
00958 void
00959 Transformer::lookup_transform(const std::string& target_frame,
00960                               const fawkes::Time& target_time,
00961                               const std::string& source_frame,
00962                               const fawkes::Time& source_time,
00963                               const std::string& fixed_frame,
00964                               StampedTransform& transform) const
00965 {
00966   StampedTransform temp1, temp2;
00967   lookup_transform(fixed_frame, source_frame, source_time, temp1);
00968   lookup_transform(target_frame, fixed_frame, target_time, temp2);
00969   transform.set_data(temp2 * temp1);
00970   transform.stamp = temp2.stamp;
00971   transform.frame_id = target_frame;
00972   transform.child_frame_id = source_frame;
00973 }
00974 
00975 /** Test if a transform is possible.
00976  * @param target_frame The frame into which to transform
00977  * @param source_frame The frame from which to transform
00978  * @param time The time at which to transform
00979  * @return true if the transformation can be calculated, false otherwise
00980  */
00981 bool
00982 Transformer::can_transform(const std::string& target_frame,
00983                            const std::string& source_frame,
00984                            const fawkes::Time& time) const
00985 {
00986   std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame;
00987   std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame;
00988 
00989   if (mapped_tgt == mapped_src)  return true;
00990 
00991   if (!frame_exists(mapped_tgt) || !frame_exists(mapped_src))  return false;
00992 
00993   MutexLocker lock(frame_mutex_);
00994   CompactFrameID target_id = lookup_frame_number(mapped_tgt);
00995   CompactFrameID source_id = lookup_frame_number(mapped_src);
00996 
00997   return can_transform_no_lock(target_id, source_id, time);
00998 }
00999 
01000 
01001 /** Test if a transform is possible.
01002  * @param target_frame The frame into which to transform
01003  * @param target_time The time into which to transform
01004  * @param source_frame The frame from which to transform
01005  * @param source_time The time from which to transform
01006  * @param fixed_frame The frame in which to treat the transform as
01007  * constant in time
01008  * @return true if the transformation can be calculated, false otherwise
01009  */
01010 bool
01011 Transformer::can_transform(const std::string& target_frame,
01012                            const fawkes::Time& target_time,
01013                            const std::string& source_frame,
01014                            const fawkes::Time& source_time,
01015                            const std::string& fixed_frame) const
01016 {
01017   return
01018     can_transform(target_frame, fixed_frame, target_time) &&
01019     can_transform(fixed_frame, source_frame, source_time);
01020 }
01021 
01022 
01023 /** Test if a transform is possible.
01024  * Internal check that does not lock the frame mutex.
01025  * @param target_id The frame number into which to transform
01026  * @param source_id The frame number from which to transform
01027  * @param time The time at which to transform
01028  * @return true if the transformation can be calculated, false otherwise
01029  */
01030 bool
01031 Transformer::can_transform_no_lock(CompactFrameID target_id,
01032                                    CompactFrameID source_id,
01033                                    const fawkes::Time& time) const
01034 {
01035   if (target_id == 0 || source_id == 0)  return false;
01036 
01037   CanTransformAccum accum;
01038   if (walk_to_top_parent(accum, time, target_id, source_id, NULL) == NO_ERROR)
01039     return true;
01040   else
01041     return false;
01042 }
01043 
01044 
01045 /** Transform a stamped Quaternion into the target frame.
01046  * This transforms the quaternion relative to the frame set in the
01047  * stamped quaternion into the target frame.
01048  * @param target_frame frame into which to transform
01049  * @param stamped_in stamped quaternion, defines source frame and time
01050  * @param stamped_out stamped output quaternion in target_frame
01051  * @exception ConnectivityException thrown if no connection between
01052  * the source and target frame could be found in the tree.
01053  * @exception ExtrapolationException returning a value would have
01054  * required extrapolation beyond current limits.
01055  * @exception LookupException at least one of the two given frames is
01056  * unknown
01057  * @exception InvalidArgument thrown if the Quaternion is invalid, most
01058  * likely an uninitialized Quaternion (0,0,0,0).
01059  */
01060 void
01061 Transformer::transform_quaternion(const std::string& target_frame,
01062                                   const Stamped<Quaternion>& stamped_in,
01063                                   Stamped<Quaternion>& stamped_out) const
01064 {
01065   assert_quaternion_valid(stamped_in);
01066 
01067   StampedTransform transform;
01068   lookup_transform(target_frame, stamped_in.frame_id,
01069                    stamped_in.stamp, transform);
01070 
01071   stamped_out.set_data(transform * stamped_in);
01072   stamped_out.stamp = transform.stamp;
01073   stamped_out.frame_id = target_frame;
01074 }
01075 
01076 /** Transform a stamped vector into the target frame.
01077  * This transforms the vector given relative to the frame set in the
01078  * stamped vector into the target frame.
01079  * @param target_frame frame into which to transform
01080  * @param stamped_in stamped vector, defines source frame and time
01081  * @param stamped_out stamped output vector in target_frame
01082  * @exception ConnectivityException thrown if no connection between
01083  * the source and target frame could be found in the tree.
01084  * @exception ExtrapolationException returning a value would have
01085  * required extrapolation beyond current limits.
01086  * @exception LookupException at least one of the two given frames is
01087  * unknown
01088  */
01089 void 
01090 Transformer::transform_vector(const std::string& target_frame,
01091                               const Stamped<Vector3>& stamped_in,
01092                               Stamped<Vector3>& stamped_out) const
01093 {
01094   StampedTransform transform;
01095   lookup_transform(target_frame, stamped_in.frame_id,
01096                    stamped_in.stamp, transform);
01097 
01098   // may not be most efficient
01099   Vector3 end    = stamped_in;
01100   Vector3 origin = Vector3(0,0,0);
01101   Vector3 output = (transform * end) - (transform * origin);
01102   stamped_out.set_data(output);
01103 
01104   stamped_out.stamp = transform.stamp;
01105   stamped_out.frame_id = target_frame;
01106 }
01107 
01108 /** Transform a stamped point into the target frame.
01109  * This transforms the point given relative to the frame set in the
01110  * stamped point into the target frame.
01111  * @param target_frame frame into which to transform
01112  * @param stamped_in stamped point, defines source frame and time
01113  * @param stamped_out stamped output point in target_frame
01114  * @exception ConnectivityException thrown if no connection between
01115  * the source and target frame could be found in the tree.
01116  * @exception ExtrapolationException returning a value would have
01117  * required extrapolation beyond current limits.
01118  * @exception LookupException at least one of the two given frames is
01119  * unknown
01120  */
01121 void 
01122 Transformer::transform_point(const std::string& target_frame,
01123                              const Stamped<Point>& stamped_in,
01124                              Stamped<Point>& stamped_out) const
01125 {
01126   StampedTransform transform;
01127   lookup_transform(target_frame, stamped_in.frame_id,
01128                    stamped_in.stamp, transform);
01129 
01130   stamped_out.set_data(transform * stamped_in);
01131   stamped_out.stamp = transform.stamp;
01132   stamped_out.frame_id = target_frame;
01133 
01134 }
01135 
01136 /** Transform a stamped pose into the target frame.
01137  * This transforms the pose given relative to the frame set in the
01138  * stamped vector into the target frame.
01139  * @param target_frame frame into which to transform
01140  * @param stamped_in stamped pose, defines source frame and time
01141  * @param stamped_out stamped output pose in target_frame
01142  * @exception ConnectivityException thrown if no connection between
01143  * the source and target frame could be found in the tree.
01144  * @exception ExtrapolationException returning a value would have
01145  * required extrapolation beyond current limits.
01146  * @exception LookupException at least one of the two given frames is
01147  * unknown
01148  */
01149 void 
01150 Transformer::transform_pose(const std::string& target_frame,
01151                             const Stamped<Pose>& stamped_in,
01152                             Stamped<Pose>& stamped_out) const
01153 {
01154   StampedTransform transform;
01155   lookup_transform(target_frame, stamped_in.frame_id,
01156                    stamped_in.stamp, transform);
01157 
01158   stamped_out.set_data(transform * stamped_in);
01159   stamped_out.stamp = transform.stamp;
01160   stamped_out.frame_id = target_frame;
01161 }
01162 
01163 
01164 /** Transform a stamped Quaternion into the target frame assuming a fixed frame.
01165  * This transforms the quaternion relative to the frame set in the
01166  * stamped quaternion into the target frame.
01167  * This will transform the quaternion from source to target, assuming
01168  * that there is a fixed frame, by first finding the transform of the
01169  * source frame in the fixed frame, and then a transformation from the
01170  * fixed frame into the target frame.
01171  * @param target_frame frame into which to transform
01172  * @param target_time desired time in the target frame
01173  * @param stamped_in stamped quaternion, defines source frame and time
01174  * @param fixed_frame ID of fixed frame
01175  * @param stamped_out stamped output quaternion in target_frame
01176  * @exception ConnectivityException thrown if no connection between
01177  * the source and target frame could be found in the tree.
01178  * @exception ExtrapolationException returning a value would have
01179  * required extrapolation beyond current limits.
01180  * @exception LookupException at least one of the two given frames is
01181  * unknown
01182  * @exception InvalidArgument thrown if the Quaternion is invalid, most
01183  * likely an uninitialized Quaternion (0,0,0,0).
01184  */
01185 void 
01186 Transformer::transform_quaternion(const std::string& target_frame,
01187                                   const fawkes::Time& target_time,
01188                                   const Stamped<Quaternion>& stamped_in,
01189                                   const std::string& fixed_frame,
01190                                   Stamped<Quaternion>& stamped_out) const
01191 {
01192   assert_quaternion_valid(stamped_in);
01193   StampedTransform transform;
01194   lookup_transform(target_frame, target_time,
01195                    stamped_in.frame_id, stamped_in.stamp,
01196                    fixed_frame, transform);
01197 
01198   stamped_out.set_data(transform * stamped_in);
01199   stamped_out.stamp = transform.stamp;
01200   stamped_out.frame_id = target_frame;
01201 }
01202 
01203 /** Transform a stamped vector into the target frame assuming a fixed frame.
01204  * This transforms the vector relative to the frame set in the
01205  * stamped quaternion into the target frame.
01206  * This will transform the vector from source to target, assuming
01207  * that there is a fixed frame, by first finding the transform of the
01208  * source frame in the fixed frame, and then a transformation from the
01209  * fixed frame into the target frame.
01210  * @param target_frame frame into which to transform
01211  * @param target_time desired time in the target frame
01212  * @param stamped_in stamped vector, defines source frame and time
01213  * @param fixed_frame ID of fixed frame
01214  * @param stamped_out stamped output vector in target_frame
01215  * @exception ConnectivityException thrown if no connection between
01216  * the source and target frame could be found in the tree.
01217  * @exception ExtrapolationException returning a value would have
01218  * required extrapolation beyond current limits.
01219  * @exception LookupException at least one of the two given frames is
01220  * unknown
01221  */
01222 void 
01223 Transformer::transform_vector(const std::string& target_frame,
01224                               const fawkes::Time& target_time,
01225                               const Stamped<Vector3>& stamped_in,
01226                               const std::string& fixed_frame,
01227                               Stamped<Vector3>& stamped_out) const
01228 {
01229   StampedTransform transform;
01230   lookup_transform(target_frame, target_time,
01231                    stamped_in.frame_id,stamped_in.stamp,
01232                    fixed_frame, transform);
01233 
01234   // may not be most efficient
01235   Vector3 end = stamped_in;
01236   Vector3 origin(0,0,0);
01237   Vector3 output = (transform * end) - (transform * origin);
01238   stamped_out.set_data( output);
01239 
01240   stamped_out.stamp = transform.stamp;
01241   stamped_out.frame_id = target_frame;
01242 
01243 }
01244 
01245 /** Transform a stamped point into the target frame assuming a fixed frame.
01246  * This transforms the point relative to the frame set in the
01247  * stamped quaternion into the target frame.
01248  * This will transform the point from source to target, assuming
01249  * that there is a fixed frame, by first finding the transform of the
01250  * source frame in the fixed frame, and then a transformation from the
01251  * fixed frame into the target frame.
01252  * @param target_frame frame into which to transform
01253  * @param target_time desired time in the target frame
01254  * @param stamped_in stamped point, defines source frame and time
01255  * @param fixed_frame ID of fixed frame
01256  * @param stamped_out stamped output point in target_frame
01257  * @exception ConnectivityException thrown if no connection between
01258  * the source and target frame could be found in the tree.
01259  * @exception ExtrapolationException returning a value would have
01260  * required extrapolation beyond current limits.
01261  * @exception LookupException at least one of the two given frames is
01262  * unknown
01263  */
01264 void 
01265 Transformer::transform_point(const std::string& target_frame,
01266                              const fawkes::Time& target_time,
01267                              const Stamped<Point>& stamped_in,
01268                              const std::string& fixed_frame,
01269                              Stamped<Point>& stamped_out) const
01270 {
01271   StampedTransform transform;
01272   lookup_transform(target_frame, target_time,
01273                    stamped_in.frame_id, stamped_in.stamp,
01274                    fixed_frame, transform);
01275 
01276   stamped_out.set_data(transform * stamped_in);
01277   stamped_out.stamp = transform.stamp;
01278   stamped_out.frame_id = target_frame;
01279 }
01280 
01281 /** Transform a stamped pose into the target frame assuming a fixed frame.
01282  * This transforms the pose relative to the frame set in the
01283  * stamped quaternion into the target frame.
01284  * This will transform the pose from source to target, assuming
01285  * that there is a fixed frame, by first finding the transform of the
01286  * source frame in the fixed frame, and then a transformation from the
01287  * fixed frame into the target frame.
01288  * @param target_frame frame into which to transform
01289  * @param target_time desired time in the target frame
01290  * @param stamped_in stamped pose, defines source frame and time
01291  * @param fixed_frame ID of fixed frame
01292  * @param stamped_out stamped output pose in target_frame
01293  * @exception ConnectivityException thrown if no connection between
01294  * the source and target frame could be found in the tree.
01295  * @exception ExtrapolationException returning a value would have
01296  * required extrapolation beyond current limits.
01297  * @exception LookupException at least one of the two given frames is
01298  * unknown
01299  */
01300 void 
01301 Transformer::transform_pose(const std::string& target_frame,
01302                             const fawkes::Time& target_time,
01303                             const Stamped<Pose>& stamped_in,
01304                             const std::string& fixed_frame,
01305                             Stamped<Pose>& stamped_out) const
01306 {
01307   StampedTransform transform;
01308   lookup_transform(target_frame, target_time,
01309                    stamped_in.frame_id, stamped_in.stamp,
01310                    fixed_frame, transform);
01311 
01312   stamped_out.set_data(transform * stamped_in);
01313   stamped_out.stamp = transform.stamp;
01314   stamped_out.frame_id = target_frame;
01315 }
01316 
01317 } // end namespace tf
01318 } // end namespace fawkes