tf.cpp
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 #include "tf/tf.h"
00033 #include <sys/time.h>
00034 #include "ros/assert.h"
00035 #include "ros/ros.h"
00036 #include "angles/angles.h"
00037 
00038 using namespace tf;
00039 
00040 // Must provide storage for non-integral static const class members.
00041 // Otherwise you get undefined symbol errors on OS X (why not on Linux?).
00042 // Thanks to Rob for pointing out the right way to do this.
00043 // In C++0x this must be initialized here #5401
00044 const double tf::Transformer::DEFAULT_CACHE_TIME = 10.0;
00045 
00046 
00047 enum WalkEnding
00048 {
00049   Identity,
00050   TargetParentOfSource,
00051   SourceParentOfTarget,
00052   FullPath,
00053 };
00054 
00055 struct CanTransformAccum
00056 {
00057   CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string)
00058   {
00059     return cache->getParent(time, error_string);
00060   }
00061 
00062   void accum(bool source)
00063   {
00064   }
00065 
00066   void finalize(WalkEnding end, ros::Time _time)
00067   {
00068   }
00069 
00070   TransformStorage st;
00071 };
00072 
00073 struct TransformAccum
00074 {
00075   TransformAccum()
00076   : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
00077   , source_to_top_vec(0.0, 0.0, 0.0)
00078   , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
00079   , target_to_top_vec(0.0, 0.0, 0.0)
00080   , result_quat(0.0, 0.0, 0.0, 1.0)
00081   , result_vec(0.0, 0.0, 0.0)
00082   {
00083   }
00084 
00085   CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string)
00086   {
00087     if (!cache->getData(time, st, error_string))
00088     {
00089       return 0;
00090     }
00091 
00092     return st.frame_id_;
00093   }
00094 
00095   void accum(bool source)
00096   {
00097     if (source)
00098     {
00099       source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
00100       source_to_top_quat = st.rotation_ * source_to_top_quat;
00101     }
00102     else
00103     {
00104       target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
00105       target_to_top_quat = st.rotation_ * target_to_top_quat;
00106     }
00107   }
00108 
00109   void finalize(WalkEnding end, ros::Time _time)
00110   {
00111     switch (end)
00112     {
00113     case Identity:
00114       break;
00115     case TargetParentOfSource:
00116       result_vec = source_to_top_vec;
00117       result_quat = source_to_top_quat;
00118       break;
00119     case SourceParentOfTarget:
00120       {
00121         tf::Quaternion inv_target_quat = target_to_top_quat.inverse();
00122         tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00123         result_vec = inv_target_vec;
00124         result_quat = inv_target_quat;
00125         break;
00126       }
00127     case FullPath:
00128       {
00129         tf::Quaternion inv_target_quat = target_to_top_quat.inverse();
00130         tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00131 
00132         result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
00133         result_quat = inv_target_quat * source_to_top_quat;
00134       }
00135       break;
00136     };
00137 
00138     time = _time;
00139   }
00140 
00141   TransformStorage st;
00142   ros::Time time;
00143   tf::Quaternion source_to_top_quat;
00144   tf::Vector3 source_to_top_vec;
00145   tf::Quaternion target_to_top_quat;
00146   tf::Vector3 target_to_top_vec;
00147 
00148   tf::Quaternion result_quat;
00149   tf::Vector3 result_vec;
00150 };
00151 
00152 
00153 std::string assert_resolved(const std::string& prefix, const std::string& frame_id)
00154 {
00155   if (frame_id.size() > 0)
00156     if (frame_id[0] != '/')
00157       ROS_DEBUG("TF operating on not fully resolved frame id %s, resolving using local prefix %s", frame_id.c_str(), prefix.c_str());
00158   return tf::resolve(prefix, frame_id);
00159 };
00160 
00161 std::string tf::resolve(const std::string& prefix, const std::string& frame_name)
00162 {
00163   //  printf ("resolveping prefix:%s with frame_name:%s\n", prefix.c_str(), frame_name.c_str());
00164   if (frame_name.size() > 0)
00165     if (frame_name[0] == '/')
00166     {
00167       return frame_name;
00168     }
00169   if (prefix.size() > 0)
00170   {
00171     if (prefix[0] == '/')
00172     {
00173       std::string composite = prefix;
00174       composite.append("/");
00175       composite.append(frame_name);
00176       return composite;
00177     }
00178     else
00179     {
00180       std::string composite;
00181       composite = "/";
00182       composite.append(prefix);
00183       composite.append("/");
00184       composite.append(frame_name);
00185       return composite;
00186     }
00187 
00188   }
00189   else
00190  {
00191     std::string composite;
00192     composite = "/";
00193     composite.append(frame_name);
00194     return composite;
00195   }
00196 };
00197 
00198 
00199 
00200 Transformer::Transformer(bool interpolating,
00201                                 ros::Duration cache_time):
00202   cache_time(cache_time),
00203   interpolating (interpolating), 
00204   using_dedicated_thread_(false),
00205   fall_back_to_wall_time_(false)
00206 {
00207   max_extrapolation_distance_.fromNSec(DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
00208   frameIDs_["NO_PARENT"] = 0;
00209   frames_.push_back(NULL);// new TimeCache(interpolating, cache_time, max_extrapolation_distance));//unused but needed for iteration over all elements
00210   frameIDs_reverse.push_back("NO_PARENT");
00211 
00212   return;
00213 }
00214 
00215 Transformer::~Transformer()
00216 {
00217   /* deallocate all frames */
00218   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00219   for (std::vector<TimeCache*>::iterator  cache_it = frames_.begin(); cache_it != frames_.end(); ++cache_it)
00220   {
00221     delete (*cache_it);
00222   }
00223 
00224 };
00225 
00226 
00227 void Transformer::clear()
00228 {
00229   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00230   if ( frames_.size() > 1 )
00231   {
00232     for (std::vector< TimeCache*>::iterator  cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
00233     {
00234       (*cache_it)->clearList();
00235     }
00236   }
00237 }
00238 
00239 
00240 template<typename F>
00241 int Transformer::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const
00242 {
00243   // Short circuit if zero length transform to allow lookups on non existant links
00244   if (source_id == target_id)
00245   {
00246     f.finalize(Identity, time);
00247     return NO_ERROR;
00248   }
00249 
00250   //If getting the latest get the latest common time
00251   if (time == ros::Time())
00252   {
00253     int retval = getLatestCommonTime(target_id, source_id, time, error_string);
00254     if (retval != NO_ERROR)
00255     {
00256       return retval;
00257     }
00258   }
00259 
00260   // Walk the tree to its root from the source frame, accumulating the transform
00261   CompactFrameID frame = source_id;
00262   CompactFrameID top_parent = frame;
00263   uint32_t depth = 0;
00264   while (frame != 0)
00265   {
00266     TimeCache* cache = getFrame(frame);
00267 
00268     if (!cache)
00269     {
00270       // There will be no cache for the very root of the tree
00271       top_parent = frame;
00272       break;
00273     }
00274 
00275     CompactFrameID parent = f.gather(cache, time, 0);
00276     if (parent == 0)
00277     {
00278       // Just break out here... there may still be a path from source -> target
00279       top_parent = frame;
00280       break;
00281     }
00282 
00283     // Early out... target frame is a direct parent of the source frame
00284     if (frame == target_id)
00285     {
00286       f.finalize(TargetParentOfSource, time);
00287       return NO_ERROR;
00288     }
00289 
00290     f.accum(true);
00291 
00292     top_parent = frame;
00293     frame = parent;
00294 
00295     ++depth;
00296     if (depth > MAX_GRAPH_DEPTH)
00297     {
00298       if (error_string)
00299       {
00300         std::stringstream ss;
00301         ss << "The tf tree is invalid because it contains a loop." << std::endl
00302            << allFramesAsString() << std::endl;
00303         *error_string = ss.str();
00304       }
00305       return LOOKUP_ERROR;
00306     }
00307   }
00308 
00309   // Now walk to the top parent from the target frame, accumulating its transform
00310   frame = target_id;
00311   depth = 0;
00312   while (frame != top_parent)
00313   {
00314     TimeCache* cache = getFrame(frame);
00315 
00316     if (!cache)
00317     {
00318       break;
00319     }
00320 
00321     CompactFrameID parent = f.gather(cache, time, error_string);
00322     if (parent == 0)
00323     {
00324       if (error_string)
00325       {
00326         std::stringstream ss;
00327         ss << *error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]";
00328         *error_string = ss.str();
00329       }
00330 
00331       return EXTRAPOLATION_ERROR;
00332     }
00333 
00334     // Early out... source frame is a direct parent of the target frame
00335     if (frame == source_id)
00336     {
00337       f.finalize(SourceParentOfTarget, time);
00338       return NO_ERROR;
00339     }
00340 
00341     f.accum(false);
00342 
00343     frame = parent;
00344 
00345     ++depth;
00346     if (depth > MAX_GRAPH_DEPTH)
00347     {
00348       if (error_string)
00349       {
00350         std::stringstream ss;
00351         ss << "The tf tree is invalid because it contains a loop." << std::endl
00352            << allFramesAsString() << std::endl;
00353         *error_string = ss.str();
00354       }
00355       return LOOKUP_ERROR;
00356     }
00357   }
00358 
00359   if (frame != top_parent)
00360   {
00361     createConnectivityErrorString(source_id, target_id, error_string);
00362     return CONNECTIVITY_ERROR;
00363   }
00364 
00365   f.finalize(FullPath, time);
00366 
00367   return NO_ERROR;
00368 }
00369 
00370 
00371 
00372 bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority)
00373 {
00374 
00375   StampedTransform mapped_transform((tf::Transform)transform, transform.stamp_, transform.frame_id_, transform.child_frame_id_);
00376   mapped_transform.child_frame_id_ = assert_resolved(tf_prefix_, transform.child_frame_id_);
00377   mapped_transform.frame_id_ = assert_resolved(tf_prefix_, transform.frame_id_);
00378 
00379  
00380   bool error_exists = false;
00381   if (mapped_transform.child_frame_id_ == mapped_transform.frame_id_)
00382   {
00383     ROS_ERROR("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());
00384     error_exists = true;
00385   }
00386 
00387   if (mapped_transform.child_frame_id_ == "/")//empty frame id will be mapped to "/"
00388   {
00389     ROS_ERROR("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
00390     error_exists = true;
00391   }
00392 
00393   if (mapped_transform.frame_id_ == "/")//empty parent id will be mapped to "/"
00394   {
00395     ROS_ERROR("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());
00396     error_exists = true;
00397   }
00398 
00399   if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
00400       std::isnan(mapped_transform.getRotation().x()) ||       std::isnan(mapped_transform.getRotation().y()) ||       std::isnan(mapped_transform.getRotation().z()) ||       std::isnan(mapped_transform.getRotation().w()))
00401   {
00402     ROS_ERROR("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)",
00403               mapped_transform.child_frame_id_.c_str(), authority.c_str(),
00404               mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
00405               mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
00406               );
00407     error_exists = true;
00408   }
00409 
00410   if (error_exists)
00411     return false;
00412 
00413   {
00414     boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00415     CompactFrameID frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id_);
00416     TimeCache* frame = getFrame(frame_number);
00417     if (frame == NULL)
00418     {
00419         frames_[frame_number] = new TimeCache(cache_time);
00420         frame = frames_[frame_number];
00421     }
00422 
00423     if (frame->insertData(TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.frame_id_), frame_number)))
00424     {
00425       frame_authority_[frame_number] = authority;
00426     }
00427     else
00428     {
00429       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());
00430       return false;
00431     }
00432   }
00433 
00434   {
00435     boost::mutex::scoped_lock lock(transforms_changed_mutex_);
00436     transforms_changed_();
00437   }
00438 
00439   return true;
00440 };
00441 
00442 
00443 void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
00444                      const ros::Time& time, StampedTransform& transform) const
00445 {
00446           std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame);
00447           std::string mapped_src = assert_resolved(tf_prefix_, source_frame);
00448 
00449           if (mapped_tgt == mapped_src) {
00450                   transform.setIdentity();
00451                   transform.child_frame_id_ = mapped_src;
00452                   transform.frame_id_       = mapped_tgt;
00453                   transform.stamp_          = now();
00454                   return;
00455           }
00456 
00457           boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00458 
00459           CompactFrameID target_id = lookupFrameNumber(mapped_tgt);
00460           CompactFrameID source_id = lookupFrameNumber(mapped_src);
00461 
00462           std::string error_string;
00463           TransformAccum accum;
00464           int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
00465           if (retval != NO_ERROR)
00466           {
00467             switch (retval)
00468             {
00469             case CONNECTIVITY_ERROR:
00470               throw ConnectivityException(error_string);
00471             case EXTRAPOLATION_ERROR:
00472               throw ExtrapolationException(error_string);
00473             case LOOKUP_ERROR:
00474               throw LookupException(error_string);
00475             default:
00476               ROS_ERROR("Unknown error code: %d", retval);
00477               ROS_BREAK();
00478             }
00479           }
00480 
00481           transform.setOrigin(accum.result_vec);
00482           transform.setRotation(accum.result_quat);
00483           transform.child_frame_id_ = mapped_src;
00484           transform.frame_id_       = mapped_tgt;
00485           transform.stamp_          = accum.time;
00486 };
00487 
00488 
00489 void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00490                      const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
00491 {
00492   tf::StampedTransform temp1, temp2;
00493   lookupTransform(fixed_frame, source_frame, source_time, temp1);
00494   lookupTransform(target_frame, fixed_frame, target_time, temp2);
00495   transform.setData( temp2 * temp1);
00496   transform.stamp_ = temp2.stamp_;
00497   transform.frame_id_ = target_frame;
00498   transform.child_frame_id_ = source_frame;
00499 
00500 };
00501 
00502 
00503 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
00504                               const ros::Time& time, const ros::Duration& averaging_interval, 
00505                               geometry_msgs::Twist& twist) const
00506 {
00507   lookupTwist(tracking_frame, observation_frame, observation_frame, tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist);
00508 };
00509 // ref point is origin of tracking_frame, ref_frame = obs_frame
00510 
00511 
00512 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00513                  const tf::Point & reference_point, const std::string& reference_point_frame, 
00514                  const ros::Time& time, const ros::Duration& averaging_interval, 
00515                  geometry_msgs::Twist& twist) const
00516 {
00517   ros::Time latest_time, target_time;
00518   getLatestCommonTime(observation_frame, tracking_frame, latest_time, NULL); 
00519 
00520   if (ros::Time() == time)
00521     target_time = latest_time;
00522   else
00523     target_time = time;
00524 
00525   ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time);
00526   
00527   ros::Time start_time = std::max(ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval;  // don't collide with zero
00528   ros::Duration corrected_averaging_interval = end_time - start_time; //correct for the possiblity that start time was truncated above.
00529   StampedTransform start, end;
00530   lookupTransform(observation_frame, tracking_frame, start_time, start);
00531   lookupTransform(observation_frame, tracking_frame, end_time, end);
00532 
00533 
00534   tf::Matrix3x3 temp = start.getBasis().inverse() * end.getBasis();
00535   tf::Quaternion quat_temp;
00536   temp.getRotation(quat_temp);
00537   tf::Vector3 o = start.getBasis() * quat_temp.getAxis();
00538   tfScalar ang = quat_temp.getAngle();
00539   
00540   double delta_x = end.getOrigin().getX() - start.getOrigin().getX();
00541   double delta_y = end.getOrigin().getY() - start.getOrigin().getY();
00542   double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ();
00543 
00544 
00545   tf::Vector3 twist_vel ((delta_x)/corrected_averaging_interval.toSec(), 
00546                        (delta_y)/corrected_averaging_interval.toSec(),
00547                        (delta_z)/corrected_averaging_interval.toSec());
00548   tf::Vector3 twist_rot = o * (ang / corrected_averaging_interval.toSec());
00549 
00550 
00551   // This is a twist w/ reference frame in observation_frame  and reference point is in the tracking_frame at the origin (at start_time)
00552 
00553 
00554   //correct for the position of the reference frame
00555   tf::StampedTransform inverse;
00556   lookupTransform(reference_frame,tracking_frame,  target_time, inverse);
00557   tf::Vector3 out_rot = inverse.getBasis() * twist_rot;
00558   tf::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot);
00559 
00560 
00561   //Rereference the twist about a new reference point
00562   // Start by computing the original reference point in the reference frame:
00563   tf::Stamped<tf::Point> rp_orig(tf::Point(0,0,0), target_time, tracking_frame);
00564   transformPoint(reference_frame, rp_orig, rp_orig);
00565   // convert the requrested reference point into the right frame
00566   tf::Stamped<tf::Point> rp_desired(reference_point, target_time, reference_point_frame);
00567   transformPoint(reference_frame, rp_desired, rp_desired);
00568   // compute the delta
00569   tf::Point delta = rp_desired - rp_orig;
00570   // Correct for the change in reference point 
00571   out_vel = out_vel + out_rot * delta;
00572   // out_rot unchanged   
00573 
00574   /*
00575     printf("KDL: Rotation %f %f %f, Translation:%f %f %f\n", 
00576          out_rot.x(),out_rot.y(),out_rot.z(),
00577          out_vel.x(),out_vel.y(),out_vel.z());
00578   */   
00579 
00580   twist.linear.x =  out_vel.x();
00581   twist.linear.y =  out_vel.y();
00582   twist.linear.z =  out_vel.z();
00583   twist.angular.x =  out_rot.x();
00584   twist.angular.y =  out_rot.y();
00585   twist.angular.z =  out_rot.z();
00586 
00587 };
00588 
00589 bool Transformer::waitForTransform(const std::string& target_frame, const std::string& source_frame,
00590                                    const ros::Time& time,
00591                                    const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
00592                                    std::string* error_msg) const
00593 {
00594   if (!using_dedicated_thread_)
00595   {
00596     std::string error_string = "Do not call waitForTransform unless you are using another thread for populating data. Without a dedicated thread it will always timeout.  If you have a seperate thread servicing tf messages, call setUsingDedicatedThread(true)";
00597     ROS_ERROR("%s",error_string.c_str());
00598     
00599     if (error_msg) 
00600       *error_msg = error_string;
00601     return false;
00602   }
00603   ros::Time start_time = now();
00604   std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame);
00605   std::string mapped_src = assert_resolved(tf_prefix_, source_frame);
00606 
00607   while (ok() && (now() - start_time) < timeout)
00608   {
00609           if (frameExists(mapped_tgt) && frameExists(mapped_src) && (canTransform(mapped_tgt, mapped_src, time, error_msg)))
00610                   return true;
00611 
00612           usleep(polling_sleep_duration.sec * 1000000 + polling_sleep_duration.nsec / 1000); //hack to avoid calling ros::Time::now() in Duration.sleep
00613   }
00614   return false;
00615 }
00616 
00617 bool Transformer::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
00618                     const ros::Time& time, std::string* error_msg) const
00619 {
00620   if (target_id == 0 || source_id == 0)
00621   {
00622     return false;
00623   }
00624 
00625   CanTransformAccum accum;
00626   if (walkToTopParent(accum, time, target_id, source_id, error_msg) == NO_ERROR)
00627   {
00628     return true;
00629   }
00630 
00631   return false;
00632 }
00633 
00634 bool Transformer::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
00635                                   const ros::Time& time, std::string* error_msg) const
00636 {
00637   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00638   return canTransformNoLock(target_id, source_id, time, error_msg);
00639 }
00640 
00641 bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame,
00642                            const ros::Time& time, std::string* error_msg) const
00643 {
00644         std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame);
00645         std::string mapped_src = assert_resolved(tf_prefix_, source_frame);
00646 
00647         if (mapped_tgt == mapped_src)
00648                 return true;
00649 
00650         boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00651 
00652   if (!frameExists(mapped_tgt) || !frameExists(mapped_src))
00653           return false;
00654 
00655   CompactFrameID target_id = lookupFrameNumber(mapped_tgt);
00656   CompactFrameID source_id = lookupFrameNumber(mapped_src);
00657 
00658   return canTransformNoLock(target_id, source_id, time, error_msg);
00659 }
00660 
00661 
00662 
00663 bool Transformer::canTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00664                                const ros::Time& source_time, const std::string& fixed_frame,
00665                                std::string* error_msg) const
00666 {
00667   return canTransform(target_frame, fixed_frame, target_time) && canTransform(fixed_frame, source_frame, source_time, error_msg);
00668 };
00669 
00670 bool Transformer::waitForTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00671                                    const ros::Time& source_time, const std::string& fixed_frame,
00672                                    const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
00673                                    std::string* error_msg) const
00674 {
00675   return waitForTransform(target_frame, fixed_frame, target_time, timeout, polling_sleep_duration, error_msg) && waitForTransform(fixed_frame, source_frame, source_time, timeout, polling_sleep_duration, error_msg);
00676 };
00677 
00678 
00679 bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
00680 {
00681   std::string mapped_frame_id = assert_resolved(tf_prefix_, frame_id);
00682   tf::TimeCache* cache;
00683   try
00684   {
00685     cache = getFrame(lookupFrameNumber(mapped_frame_id));
00686   }
00687   catch  (tf::LookupException &ex)
00688   {
00689     ROS_ERROR("Transformer::getParent: %s",ex.what());
00690     return false;
00691   }
00692 
00693   TransformStorage temp;
00694   if (! cache->getData(time, temp)) {
00695     ROS_DEBUG("Transformer::getParent: No data for parent of %s", mapped_frame_id.c_str());
00696     return false;
00697   }
00698   if (temp.frame_id_ == 0) {
00699     ROS_DEBUG("Transformer::getParent: No parent for %s", mapped_frame_id.c_str());
00700     return false;
00701   }
00702 
00703   parent = lookupFrameString(temp.frame_id_);
00704   return true;
00705 };
00706 
00707 
00708 bool Transformer::frameExists(const std::string& frame_id_str) const
00709 {
00710   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
00711   std::string frame_id_resolveped = assert_resolved(tf_prefix_, frame_id_str);
00712   
00713   return frameIDs_.count(frame_id_resolveped);
00714 }
00715 
00716 void Transformer::setExtrapolationLimit(const ros::Duration& distance)
00717 {
00718   max_extrapolation_distance_ = distance;
00719 }
00720 
00721 void Transformer::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const
00722 {
00723   if (!out)
00724   {
00725     return;
00726   }
00727   *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+
00728                      lookupFrameString(source_frame)+"' because they are not part of the same tree."+
00729                      "Tf has two or more unconnected trees.");
00730 }
00731 
00732 struct TimeAndFrameIDFrameComparator
00733 {
00734   TimeAndFrameIDFrameComparator(CompactFrameID id)
00735   : id(id)
00736   {}
00737 
00738   bool operator()(const P_TimeAndFrameID& rhs) const
00739   {
00740     return rhs.second == id;
00741   }
00742 
00743   CompactFrameID id;
00744 };
00745 
00746 int Transformer::getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const
00747 {
00748           std::string mapped_tgt = assert_resolved(tf_prefix_, target_frame);
00749           std::string mapped_src = assert_resolved(tf_prefix_, source_frame);
00750 
00751           if (!frameExists(mapped_tgt) || !frameExists(mapped_src)) {
00752                   time = ros::Time();
00753                   return LOOKUP_ERROR;
00754           }
00755 
00756           CompactFrameID source_id = lookupFrameNumber(mapped_src);
00757           CompactFrameID target_id = lookupFrameNumber(mapped_tgt);
00758           return getLatestCommonTime(source_id, target_id, time, error_string);
00759 }
00760 
00761 
00762 int Transformer::getLatestCommonTime(CompactFrameID target_id, CompactFrameID source_id, ros::Time & time, std::string * error_string) const
00763 {
00764   if (source_id == target_id)
00765   {
00766     //Set time to latest timestamp of frameid in case of target and source frame id are the same
00767     time = now();
00768     return NO_ERROR;
00769   }
00770 
00771   std::vector<P_TimeAndFrameID> lct_cache;
00772 
00773   // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
00774   // in the target is a direct parent
00775   CompactFrameID frame = source_id;
00776   P_TimeAndFrameID temp;
00777   uint32_t depth = 0;
00778   ros::Time common_time = ros::TIME_MAX;
00779   while (frame != 0)
00780   {
00781     TimeCache* cache = getFrame(frame);
00782 
00783     if (!cache)
00784     {
00785       // There will be no cache for the very root of the tree
00786       break;
00787     }
00788 
00789     P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
00790 
00791     if (latest.second == 0)
00792     {
00793       // Just break out here... there may still be a path from source -> target
00794       break;
00795     }
00796 
00797     if (!latest.first.isZero())
00798     {
00799       common_time = std::min(latest.first, common_time);
00800     }
00801 
00802     lct_cache.push_back(latest);
00803 
00804     frame = latest.second;
00805 
00806     // Early out... target frame is a direct parent of the source frame
00807     if (frame == target_id)
00808     {
00809       time = common_time;
00810       if (time == ros::TIME_MAX)
00811       {
00812         time = ros::Time();
00813       }
00814       return NO_ERROR;
00815     }
00816 
00817     ++depth;
00818     if (depth > MAX_GRAPH_DEPTH)
00819     {
00820       if (error_string)
00821       {
00822         std::stringstream ss;
00823         ss<<"The tf tree is invalid because it contains a loop." << std::endl
00824           << allFramesAsString() << std::endl;
00825         *error_string = ss.str();
00826       }
00827       return LOOKUP_ERROR;
00828     }
00829   }
00830 
00831   // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
00832   frame = target_id;
00833   depth = 0;
00834   common_time = ros::TIME_MAX;
00835   CompactFrameID common_parent = 0;
00836   while (true)
00837   {
00838     TimeCache* cache = getFrame(frame);
00839 
00840     if (!cache)
00841     {
00842       break;
00843     }
00844 
00845     P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
00846 
00847     if (latest.second == 0)
00848     {
00849       break;
00850     }
00851 
00852     if (!latest.first.isZero())
00853     {
00854       common_time = std::min(latest.first, common_time);
00855     }
00856 
00857     std::vector<P_TimeAndFrameID>::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second));
00858     if (it != lct_cache.end()) // found a common parent
00859     {
00860       common_parent = it->second;
00861       break;
00862     }
00863 
00864     frame = latest.second;
00865 
00866     // Early out... source frame is a direct parent of the target frame
00867     if (frame == source_id)
00868     {
00869       time = common_time;
00870       if (time == ros::TIME_MAX)
00871       {
00872         time = ros::Time();
00873       }
00874       return NO_ERROR;
00875     }
00876 
00877     ++depth;
00878     if (depth > MAX_GRAPH_DEPTH)
00879     {
00880       if (error_string)
00881       {
00882         std::stringstream ss;
00883         ss<<"The tf tree is invalid because it contains a loop." << std::endl
00884           << allFramesAsString() << std::endl;
00885         *error_string = ss.str();
00886       }
00887       return LOOKUP_ERROR;
00888     }
00889   }
00890 
00891   if (common_parent == 0)
00892   {
00893     createConnectivityErrorString(source_id, target_id, error_string);
00894     return CONNECTIVITY_ERROR;
00895   }
00896 
00897   // Loop through the source -> root list until we hit the common parent
00898   {
00899     std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
00900     std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
00901     for (; it != end; ++it)
00902     {
00903       if (!it->first.isZero())
00904       {
00905         common_time = std::min(common_time, it->first);
00906       }
00907 
00908       if (it->second == common_parent)
00909       {
00910         break;
00911       }
00912     }
00913   }
00914 
00915   if (common_time == ros::TIME_MAX)
00916   {
00917     common_time = ros::Time();
00918   }
00919 
00920   time = common_time;
00921   return NO_ERROR;
00922 }
00923 
00924 
00925 
00926 /*
00927 int Transformer::lookupLists(unsigned int target_frame, ros::Time time, unsigned int source_frame, TransformLists& lists, std::string * error_string) const
00928 {
00930 
00931   //Clear lists before operating
00932   lists.forwardTransforms.clear();
00933   lists.inverseTransforms.clear();
00934   //  TransformLists mTfLs;
00935   if (target_frame == source_frame)
00936     return 0;  //Don't do anythign if we're not going anywhere
00937 
00938   TransformStorage temp;
00939 
00940   unsigned int frame = source_frame;
00941   unsigned int counter = 0;  //A counter to keep track of how deep we've descended
00942   unsigned int last_inverse;
00943   if (getFrame(frame) == NULL) //Test if source frame exists this will throw a lookup error if it does not (inside the loop it will be caught)
00944   {
00945     if (error_string) *error_string = "Source frame '"+lookupFrameString(frame)+"' does not exist is tf tree.";
00946     return LOOKUP_ERROR;//throw LookupException("Frame didn't exist");
00947   }
00948   while (true)
00949     {
00950       //      printf("getting data from %d:%s \n", frame, lookupFrameString(frame).c_str());
00951 
00952       TimeCache* pointer = getFrame(frame);
00953       ROS_ASSERT(pointer);
00954 
00955       if (! pointer->getData(time, temp))
00956       {
00957         last_inverse = frame;
00958         // this is thrown when there is no data
00959         break;
00960       }
00961 
00962       //break if parent is NO_PARENT (0)
00963       if (frame == 0)
00964       {
00965         last_inverse = frame;
00966         break;
00967       }
00968       lists.inverseTransforms.push_back(temp);
00969 
00970       frame = temp.frame_id_num_;
00971 
00972 
00973       // Check if we've gone too deep.  A loop in the tree would cause this
00974       if (counter++ > MAX_GRAPH_DEPTH)
00975       {
00976         if (error_string)
00977         {
00978           std::stringstream ss;
00979           ss<<"The tf tree is invalid because it contains a loop." << std::endl
00980             << allFramesAsString() << std::endl;
00981           *error_string =ss.str();
00982         }
00983         return LOOKUP_ERROR;
00984         //        throw(LookupException(ss.str()));
00985       }
00986     }
00987 
00988   frame = target_frame;
00989   counter = 0;
00990   unsigned int last_forward;
00991   if (getFrame(frame) == NULL)
00992   {
00993     if (error_string) *error_string = "Target frame '"+lookupFrameString(frame)+"' does not exist is tf tree.";
00994     return LOOKUP_ERROR;
00995   }//throw LookupException("fixme");; //Test if source frame exists this will throw a lookup error if it does not (inside the loop it will be caught)
00996   while (true)
00997     {
00998 
00999       TimeCache* pointer = getFrame(frame);
01000       ROS_ASSERT(pointer);
01001 
01002 
01003       if(!  pointer->getData(time, temp))
01004       {
01005         last_forward = frame;
01006         break;
01007       }
01008 
01009       //break if parent is NO_PARENT (0)
01010       if (frame == 0)
01011       {
01012         last_forward = frame;
01013         break;
01014       }
01015       //      std::cout << "pushing back" << temp.frame_id_ << std::endl;
01016       lists.forwardTransforms.push_back(temp);
01017       frame = temp.frame_id_num_;
01018 
01019       // Check if we've gone too deep.  A loop in the tree would cause this
01020       if (counter++ > MAX_GRAPH_DEPTH){
01021         if (error_string)
01022         {
01023           std::stringstream ss;
01024           ss<<"The tf tree is invalid because it contains a loop." << std::endl
01025             << allFramesAsString() << std::endl;
01026           *error_string = ss.str();
01027         }
01028         return LOOKUP_ERROR;//throw(LookupException(ss.str()));
01029       }
01030     }
01031 
01032   std::string connectivity_error("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+
01033                                  lookupFrameString(source_frame)+"' because they are not part of the same tree."+
01034                                  "Tf has two or more unconnected trees.");
01035   // Check the zero length cases
01036   if (lists.inverseTransforms.size() == 0)
01037   {
01038     if (lists.forwardTransforms.size() == 0) //If it's going to itself it's already been caught
01039     {
01040       if (error_string) *error_string = connectivity_error;
01041       return CONNECTIVITY_ERROR;
01042     }
01043 
01044     if (last_forward != source_frame)  //\todo match with case A
01045     {
01046       if (error_string) *error_string = connectivity_error;
01047       return CONNECTIVITY_ERROR;
01048     }
01049     else return 0;
01050   }
01051 
01052   if (lists.forwardTransforms.size() == 0)
01053   {
01054     if (lists.inverseTransforms.size() == 0)  //If it's going to itself it's already been caught
01055     {//\todo remove THis is the same as case D
01056       if (error_string) *error_string = connectivity_error;
01057       return CONNECTIVITY_ERROR;
01058     }
01059 
01060     try
01061     {
01062       if (lookupFrameNumber(lists.inverseTransforms.back().frame_id_) != target_frame)
01063       {
01064         if (error_string) *error_string = connectivity_error;
01065         return CONNECTIVITY_ERROR;
01066     }
01067     else return 0;
01068     }
01069     catch (tf::LookupException & ex)
01070     {
01071       if (error_string) *error_string = ex.what();
01072       return LOOKUP_ERROR;
01073     }
01074   }
01075 
01076 
01077   // Make sure the end of the search shares a parent.
01078   if (last_forward != last_inverse)
01079   {
01080     if (error_string) *error_string = connectivity_error;
01081     return CONNECTIVITY_ERROR;
01082   }
01083   // Make sure that we don't have a no parent at the top
01084   try
01085   {
01086     if (lookupFrameNumber(lists.inverseTransforms.back().child_frame_id_) == 0 || lookupFrameNumber( lists.forwardTransforms.back().child_frame_id_) == 0)
01087     {
01088       //if (error_string) *error_string = "NO_PARENT at top of tree";
01089       if (error_string) *error_string = connectivity_error;
01090       return CONNECTIVITY_ERROR;
01091     }
01092 
01093     while (lookupFrameNumber(lists.inverseTransforms.back().child_frame_id_) == lookupFrameNumber(lists.forwardTransforms.back().child_frame_id_))
01094     {
01095       lists.inverseTransforms.pop_back();
01096       lists.forwardTransforms.pop_back();
01097 
01098       // Make sure we don't go beyond the beginning of the list.
01099       // (The while statement above doesn't fail if you hit the beginning of the list,
01100       // which happens in the zero distance case.)
01101       if (lists.inverseTransforms.size() == 0 || lists.forwardTransforms.size() == 0)
01102         break;
01103     }
01104   }
01105   catch (tf::LookupException & ex)
01106   {
01107     if (error_string) *error_string = ex.what();
01108     return LOOKUP_ERROR;
01109   }
01110   return 0;
01111 
01112   }
01113   */
01114 
01115 /*
01116 bool Transformer::test_extrapolation_one_value(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const
01117 {
01118   std::stringstream ss;
01119   ss << std::fixed;
01120   ss.precision(3);
01121 
01122   if (tr.mode_ == ONE_VALUE)
01123   {
01124     if (tr.stamp_ - target_time > max_extrapolation_distance_ || target_time - tr.stamp_ > max_extrapolation_distance_)
01125     {
01126       if (error_string) {
01127         ss << "You requested a transform at time " << (target_time).toSec() 
01128            << ",\n but the tf buffer only contains a single transform " 
01129            << "at time " << tr.stamp_.toSec() << ".\n";
01130         if ( max_extrapolation_distance_ > ros::Duration(0))
01131         {
01132           ss << "The tf extrapollation distance is set to " 
01133              << (max_extrapolation_distance_).toSec() <<" seconds.\n";
01134         }
01135         *error_string = ss.str();
01136       }
01137       return true;
01138     }
01139   }
01140   return false;
01141 }
01142 
01143 
01144 bool Transformer::test_extrapolation_past(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const
01145 {
01146   std::stringstream ss;
01147   ss << std::fixed;
01148   ss.precision(3);
01149 
01150   if (tr.mode_ == EXTRAPOLATE_BACK &&  tr.stamp_ - target_time > max_extrapolation_distance_)
01151   {
01152     if (error_string) {
01153       ss << "You requested a transform that is " << (now() - target_time).toSec() << " seconds in the past, \n"
01154          << "but the tf buffer only has a history of " << (now() - tr.stamp_).toSec()  << " seconds.\n";
01155       if ( max_extrapolation_distance_ > ros::Duration(0))
01156       {
01157         ss << "The tf extrapollation distance is set to " 
01158            << (max_extrapolation_distance_).toSec() <<" seconds.\n";
01159       }
01160       *error_string = ss.str();
01161     }
01162     return true;
01163   }
01164   return false;
01165 }
01166 
01167 
01168 bool Transformer::test_extrapolation_future(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const
01169 {
01170   std::stringstream ss;
01171   ss << std::fixed;
01172   ss.precision(3);
01173 
01174   if( tr.mode_ == EXTRAPOLATE_FORWARD && target_time - tr.stamp_ > max_extrapolation_distance_)
01175   {
01176     if (error_string){
01177       ss << "You requested a transform that is " << (now() - target_time).toSec()*1000 << " miliseconds in the past, \n"
01178          << "but the most recent transform in the tf buffer is " << (now() - tr.stamp_).toSec()*1000 << " miliseconds old.\n";
01179       if ( max_extrapolation_distance_ > ros::Duration(0))
01180       {
01181         ss << "The tf extrapollation distance is set to " 
01182            << (max_extrapolation_distance_).toSec() <<" seconds.\n";
01183       }
01184       *error_string = ss.str();
01185     }
01186     return true;
01187   }
01188   return false;
01189 }
01190 
01191 
01192 bool Transformer::test_extrapolation(const ros::Time& target_time, const TransformLists& lists, std::string * error_string) const
01193 {
01194   std::stringstream ss;
01195   ss << std::fixed;
01196   ss.precision(3);
01197   for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
01198   {
01199     if (test_extrapolation_one_value(target_time, lists.inverseTransforms[i], error_string)) return true;
01200     if (test_extrapolation_past(target_time, lists.inverseTransforms[i], error_string)) return true;
01201     if (test_extrapolation_future(target_time, lists.inverseTransforms[i], error_string)) return true;
01202   }
01203 
01204   for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
01205   {
01206     if (test_extrapolation_one_value(target_time, lists.forwardTransforms[i], error_string)) return true;
01207     if (test_extrapolation_past(target_time, lists.forwardTransforms[i], error_string)) return true;
01208     if (test_extrapolation_future(target_time, lists.forwardTransforms[i], error_string)) return true;
01209   }
01210 
01211   return false;
01212 }
01213 */
01214 
01215 
01216 
01217 /*
01218 std::string Transformer::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
01219 {
01220   std::string error_string;
01221   std::stringstream mstream;
01222   TransformLists lists;
01224   try
01225   {
01226     lookupLists(lookupFrameNumber(target_frame), target_time, lookupFrameNumber(source_frame), lists, &error_string);
01227   }
01228   catch (tf::LookupException &ex)
01229   {
01230     mstream << ex.what();
01231     return mstream.str();
01232   }
01233   mstream << "Inverse Transforms:" <<std::endl;
01234   for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
01235     {
01236       mstream << lists.inverseTransforms[i].child_frame_id_<<", ";
01237     }
01238   mstream << std::endl;
01239 
01240   mstream << "Forward Transforms: "<<std::endl ;
01241   for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
01242     {
01243       mstream << lists.forwardTransforms[i].child_frame_id_<<", ";
01244     }
01245   mstream << std::endl;
01246   return mstream.str();
01247 }
01248 */
01249 
01250 //@todo - Fix this to work with new data structures
01251 void Transformer::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
01252 {
01253   std::string error_string;
01254 
01255   output.clear(); //empty vector
01256 
01257   std::stringstream mstream;
01258   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
01259 
01260   TransformStorage temp;
01261 
01263   for (unsigned int counter = 1; counter < frames_.size(); counter ++)
01264   {
01265     TimeCache* frame_ptr = getFrame(CompactFrameID(counter));
01266     if (frame_ptr == NULL)
01267       continue;
01268     CompactFrameID frame_id_num;
01269     if (frame_ptr->getData(ros::Time(), temp))
01270         frame_id_num = temp.frame_id_;
01271       else
01272       {
01273         frame_id_num = 0;
01274       }
01275       output.push_back(frameIDs_reverse[frame_id_num]);
01276   }
01277 }
01278 
01279 std::string Transformer::allFramesAsString() const
01280 {
01281   std::stringstream mstream;
01282   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
01283 
01284   TransformStorage temp;
01285 
01287   for (unsigned int counter = 1; counter < frames_.size(); counter ++)
01288   {
01289     TimeCache* frame_ptr = getFrame(CompactFrameID(counter));
01290     if (frame_ptr == NULL)
01291       continue;
01292     CompactFrameID frame_id_num;
01293     if(  frame_ptr->getData(ros::Time(), temp))
01294       frame_id_num = temp.frame_id_;
01295     else
01296     {
01297       frame_id_num = 0;
01298     }
01299     mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <<std::endl;
01300   }
01301 
01302   return mstream.str();
01303 }
01304 
01305 std::string Transformer::allFramesAsDot() const
01306 {
01307   std::stringstream mstream;
01308   mstream << "digraph G {" << std::endl;
01309   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
01310 
01311   TransformStorage temp;
01312 
01313   ros::Time current_time = now();
01314 
01315   if (frames_.size() ==1)
01316     mstream <<"\"no tf data recieved\"";
01317 
01318   mstream.precision(3);
01319   mstream.setf(std::ios::fixed,std::ios::floatfield);
01320     
01321    //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
01322   for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
01323   {
01324     unsigned int frame_id_num;
01325     if(  getFrame(counter)->getData(ros::Time(), temp))
01326       frame_id_num = temp.frame_id_;
01327     else
01328     {
01329       frame_id_num = 0;
01330     }
01331     if (frame_id_num != 0)
01332     {
01333       std::string authority = "no recorded authority";
01334       std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
01335       if (it != frame_authority_.end())
01336         authority = it->second;
01337 
01338       double rate = getFrame(counter)->getListLength() / std::max((getFrame(counter)->getLatestTimestamp().toSec() -
01339                                                                    getFrame(counter)->getOldestTimestamp().toSec() ), 0.0001);
01340 
01341       mstream << std::fixed; //fixed point notation
01342       mstream.precision(3); //3 decimal places
01343       mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
01344               << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
01345         //<< "Time: " << current_time.toSec() << "\\n"
01346               << "Broadcaster: " << authority << "\\n"
01347               << "Average rate: " << rate << " Hz\\n"
01348               << "Most recent transform: " << (current_time - getFrame(counter)->getLatestTimestamp()).toSec() << " sec old \\n"
01349         //    << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n"
01350         //    << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n"
01351         //    << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n"
01352               << "Buffer length: " << (getFrame(counter)->getLatestTimestamp()-getFrame(counter)->getOldestTimestamp()).toSec() << " sec\\n"
01353               <<"\"];" <<std::endl;
01354     }
01355   }
01356   
01357   for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
01358   {
01359     unsigned int frame_id_num;
01360     if(  getFrame(counter)->getData(ros::Time(), temp))
01361       frame_id_num = temp.frame_id_;
01362     else
01363       {
01364         frame_id_num = 0;
01365       }
01366 
01367     if(frameIDs_reverse[frame_id_num]=="NO_PARENT")
01368     {
01369       mstream << "edge [style=invis];" <<std::endl;
01370       mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n"
01371               << "\"Recorded at time: " << current_time.toSec() << "\"[ shape=plaintext ] ;\n "
01372               << "}" << "->" << "\"" << frameIDs_reverse[counter]<<"\";" <<std::endl;
01373     }
01374   }
01375   mstream << "}";
01376   return mstream.str();
01377 }
01378 
01379 
01380 bool Transformer::ok() const { return true; }
01381 
01382 void Transformer::getFrameStrings(std::vector<std::string> & vec) const
01383 {
01384   vec.clear();
01385 
01386   boost::recursive_mutex::scoped_lock lock(frame_mutex_);
01387 
01388   TransformStorage temp;
01389 
01390   //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
01391   for (unsigned int counter = 1; counter < frames_.size(); counter ++)
01392   {
01393     vec.push_back(frameIDs_reverse[counter]);
01394   }
01395   return;
01396 }
01397 
01398 tf::TimeCache* Transformer::getFrame(unsigned int frame_id) const
01399 {
01400   if (frame_id == 0) 
01401     return NULL;
01402   else
01403     return frames_[frame_id];
01404 };
01405 
01406 
01407 void Transformer::transformQuaternion(const std::string& target_frame, const Stamped<Quaternion>& stamped_in, Stamped<Quaternion>& stamped_out) const
01408 {
01409   tf::assertQuaternionValid(stamped_in);
01410 
01411   StampedTransform transform;
01412   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01413 
01414   stamped_out.setData( transform * stamped_in);
01415   stamped_out.stamp_ = transform.stamp_;
01416   stamped_out.frame_id_ = target_frame;
01417 };
01418 
01419 
01420 void Transformer::transformVector(const std::string& target_frame,
01421                                   const Stamped<tf::Vector3>& stamped_in,
01422                                   Stamped<tf::Vector3>& stamped_out) const
01423 {
01424   StampedTransform transform;
01425   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01426 
01428   tf::Vector3 end = stamped_in;
01429   tf::Vector3 origin = tf::Vector3(0,0,0);
01430   tf::Vector3 output = (transform * end) - (transform * origin);
01431   stamped_out.setData( output);
01432 
01433   stamped_out.stamp_ = transform.stamp_;
01434   stamped_out.frame_id_ = target_frame;
01435 };
01436 
01437 
01438 void Transformer::transformPoint(const std::string& target_frame, const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const
01439 {
01440   StampedTransform transform;
01441   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01442 
01443   stamped_out.setData(transform * stamped_in);
01444   stamped_out.stamp_ = transform.stamp_;
01445   stamped_out.frame_id_ = target_frame;
01446 };
01447 
01448 void Transformer::transformPose(const std::string& target_frame, const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const
01449 {
01450   StampedTransform transform;
01451   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01452 
01453   stamped_out.setData(transform * stamped_in);
01454   stamped_out.stamp_ = transform.stamp_;
01455   stamped_out.frame_id_ = target_frame;
01456 };
01457 
01458 
01459 void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
01460                                       const Stamped<Quaternion>& stamped_in,
01461                                       const std::string& fixed_frame,
01462                                       Stamped<Quaternion>& stamped_out) const
01463 {
01464   tf::assertQuaternionValid(stamped_in);
01465   StampedTransform transform;
01466   lookupTransform(target_frame, target_time,
01467                   stamped_in.frame_id_,stamped_in.stamp_,
01468                   fixed_frame, transform);
01469 
01470   stamped_out.setData( transform * stamped_in);
01471   stamped_out.stamp_ = transform.stamp_;
01472   stamped_out.frame_id_ = target_frame;
01473 };
01474 
01475 
01476 void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time,
01477                                   const Stamped<Vector3>& stamped_in,
01478                                   const std::string& fixed_frame,
01479                                   Stamped<Vector3>& stamped_out) const
01480 {
01481   StampedTransform transform;
01482   lookupTransform(target_frame, target_time,
01483                   stamped_in.frame_id_,stamped_in.stamp_,
01484                   fixed_frame, transform);
01485 
01487   tf::Vector3 end = stamped_in;
01488   tf::Vector3 origin = tf::Vector3(0,0,0);
01489   tf::Vector3 output = (transform * end) - (transform * origin);
01490   stamped_out.setData( output);
01491 
01492   stamped_out.stamp_ = transform.stamp_;
01493   stamped_out.frame_id_ = target_frame;
01494 };
01495 
01496 
01497 void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time,
01498                                  const Stamped<Point>& stamped_in,
01499                                  const std::string& fixed_frame,
01500                                  Stamped<Point>& stamped_out) const
01501 {
01502   StampedTransform transform;
01503   lookupTransform(target_frame, target_time,
01504                   stamped_in.frame_id_,stamped_in.stamp_,
01505                   fixed_frame, transform);
01506 
01507   stamped_out.setData(transform * stamped_in);
01508   stamped_out.stamp_ = transform.stamp_;
01509   stamped_out.frame_id_ = target_frame;
01510 };
01511 
01512 void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
01513                                 const Stamped<Pose>& stamped_in,
01514                                 const std::string& fixed_frame,
01515                                 Stamped<Pose>& stamped_out) const
01516 {
01517   StampedTransform transform;
01518   lookupTransform(target_frame, target_time,
01519                   stamped_in.frame_id_,stamped_in.stamp_,
01520                   fixed_frame, transform);
01521 
01522   stamped_out.setData(transform * stamped_in);
01523   stamped_out.stamp_ = transform.stamp_;
01524   stamped_out.frame_id_ = target_frame;
01525 };
01526 
01527 boost::signals::connection Transformer::addTransformsChangedListener(boost::function<void(void)> callback)
01528 {
01529   boost::mutex::scoped_lock lock(transforms_changed_mutex_);
01530   return transforms_changed_.connect(callback);
01531 }
01532 
01533 void Transformer::removeTransformsChangedListener(boost::signals::connection c)
01534 {
01535   boost::mutex::scoped_lock lock(transforms_changed_mutex_);
01536   c.disconnect();
01537 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 22 2013 11:29:01