buffer_core.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, 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 "tf2/buffer_core.h"
00033 #include "tf2/time_cache.h"
00034 #include "tf2/exceptions.h"
00035 #include "tf2_msgs/TF2Error.h"
00036 
00037 #include <assert.h>
00038 #include <console_bridge/console.h>
00039 #include "tf2/LinearMath/Transform.h"
00040 #include <boost/foreach.hpp>
00041 
00042 namespace tf2
00043 {
00044 
00045 // Tolerance for acceptable quaternion normalization
00046 static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3;
00047 
00049 void transformMsgToTF2(const geometry_msgs::Transform& msg, tf2::Transform& tf2)
00050 {tf2 = tf2::Transform(tf2::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z));}
00051 
00053 void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::Transform& msg)
00054 {
00055   msg.translation.x = tf2.getOrigin().x();
00056   msg.translation.y = tf2.getOrigin().y();
00057   msg.translation.z = tf2.getOrigin().z();
00058   msg.rotation.x = tf2.getRotation().x();
00059   msg.rotation.y = tf2.getRotation().y();
00060   msg.rotation.z = tf2.getRotation().z();
00061   msg.rotation.w = tf2.getRotation().w();
00062 }
00063 
00065 void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id)
00066 {
00067   transformTF2ToMsg(tf2, msg.transform);
00068   msg.header.stamp = stamp;
00069   msg.header.frame_id = frame_id;
00070   msg.child_frame_id = child_frame_id;
00071 }
00072 
00073 void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::Transform& msg)
00074 {
00075   msg.translation.x = pos.x();
00076   msg.translation.y = pos.y();
00077   msg.translation.z = pos.z();
00078   msg.rotation.x = orient.x();
00079   msg.rotation.y = orient.y();
00080   msg.rotation.z = orient.z();
00081   msg.rotation.w = orient.w();
00082 }
00083 
00084 void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id)
00085 {
00086   transformTF2ToMsg(orient, pos, msg.transform);
00087   msg.header.stamp = stamp;
00088   msg.header.frame_id = frame_id;
00089   msg.child_frame_id = child_frame_id;
00090 }
00091 
00092 void setIdentity(geometry_msgs::Transform& tx)
00093 {
00094   tx.translation.x = 0;
00095   tx.translation.y = 0;
00096   tx.translation.z = 0;
00097   tx.rotation.x = 0;
00098   tx.rotation.y = 0;
00099   tx.rotation.z = 0;
00100   tx.rotation.w = 1;
00101 }
00102 
00103 bool startsWithSlash(const std::string& frame_id)
00104 {
00105   if (frame_id.size() > 0)
00106     if (frame_id[0] == '/')
00107       return true;
00108   return false;
00109 }
00110 
00111 std::string stripSlash(const std::string& in)
00112 {
00113   std::string out = in;
00114   if (startsWithSlash(in))
00115     out.erase(0,1);
00116   return out;
00117 }
00118 
00119 
00120 bool BufferCore::warnFrameId(const char* function_name_arg, const std::string& frame_id) const
00121 {
00122   if (frame_id.size() == 0)
00123   {
00124     std::stringstream ss;
00125     ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty";
00126     logWarn("%s",ss.str().c_str());
00127     return true;
00128   }
00129 
00130   if (startsWithSlash(frame_id))
00131   {
00132     std::stringstream ss;
00133     ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: ";
00134     logWarn("%s",ss.str().c_str());
00135     return true;
00136   }
00137 
00138   return false;
00139 }
00140 
00141 CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const
00142 {
00143   if (frame_id.empty())
00144   {
00145     std::stringstream ss;
00146     ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty";
00147     throw tf2::InvalidArgumentException(ss.str().c_str());
00148   }
00149 
00150   if (startsWithSlash(frame_id))
00151   {
00152     std::stringstream ss;
00153     ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: ";
00154     throw tf2::InvalidArgumentException(ss.str().c_str());
00155   }
00156 
00157   CompactFrameID id = lookupFrameNumber(frame_id);
00158   if (id == 0)
00159   {
00160     std::stringstream ss;
00161     ss << "\"" << frame_id << "\" passed to "<< function_name_arg <<" does not exist. ";
00162     throw tf2::LookupException(ss.str().c_str());
00163   }
00164   
00165   return id;
00166 }
00167 
00168 BufferCore::BufferCore(ros::Duration cache_time)
00169 : cache_time_(cache_time)
00170 , transformable_callbacks_counter_(0)
00171 , transformable_requests_counter_(0)
00172 , using_dedicated_thread_(false)
00173 {
00174   frameIDs_["NO_PARENT"] = 0;
00175   frames_.push_back(TimeCacheInterfacePtr());
00176   frameIDs_reverse.push_back("NO_PARENT");
00177 }
00178 
00179 BufferCore::~BufferCore()
00180 {
00181 
00182 }
00183 
00184 void BufferCore::clear()
00185 {
00186   //old_tf_.clear();
00187 
00188 
00189   boost::mutex::scoped_lock lock(frame_mutex_);
00190   if ( frames_.size() > 1 )
00191   {
00192     for (std::vector<TimeCacheInterfacePtr>::iterator  cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
00193     {
00194       if (*cache_it)
00195         (*cache_it)->clearList();
00196     }
00197   }
00198   
00199 }
00200 
00201 bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_in, const std::string& authority, bool is_static)
00202 {
00203 
00205   /* tf::StampedTransform tf_transform;
00206   tf::transformStampedMsgToTF(transform_in, tf_transform);
00207   if  (!old_tf_.setTransform(tf_transform, authority))
00208   {
00209     printf("Warning old setTransform Failed but was not caught\n");
00210     }*/
00211 
00213   geometry_msgs::TransformStamped stripped = transform_in;
00214   stripped.header.frame_id = stripSlash(stripped.header.frame_id);
00215   stripped.child_frame_id = stripSlash(stripped.child_frame_id);
00216 
00217 
00218   bool error_exists = false;
00219   if (stripped.child_frame_id == stripped.header.frame_id)
00220   {
00221     logError("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id  \"%s\" because they are the same",  authority.c_str(), stripped.child_frame_id.c_str());
00222     error_exists = true;
00223   }
00224 
00225   if (stripped.child_frame_id == "")
00226   {
00227     logError("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
00228     error_exists = true;
00229   }
00230 
00231   if (stripped.header.frame_id == "")
00232   {
00233     logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\"  from authority \"%s\" because frame_id not set", stripped.child_frame_id.c_str(), authority.c_str());
00234     error_exists = true;
00235   }
00236 
00237   if (std::isnan(stripped.transform.translation.x) || std::isnan(stripped.transform.translation.y) || std::isnan(stripped.transform.translation.z)||
00238       std::isnan(stripped.transform.rotation.x) ||       std::isnan(stripped.transform.rotation.y) ||       std::isnan(stripped.transform.rotation.z) ||       std::isnan(stripped.transform.rotation.w))
00239   {
00240     logError("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)",
00241               stripped.child_frame_id.c_str(), authority.c_str(),
00242               stripped.transform.translation.x, stripped.transform.translation.y, stripped.transform.translation.z,
00243               stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w
00244               );
00245     error_exists = true;
00246   }
00247 
00248   bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w
00249                         + stripped.transform.rotation.x * stripped.transform.rotation.x
00250                         + stripped.transform.rotation.y * stripped.transform.rotation.y
00251                         + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE;
00252 
00253   if (!valid) 
00254   {
00255     logError("TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)",
00256              stripped.child_frame_id.c_str(), authority.c_str(),
00257              stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w);
00258     error_exists = true;
00259   }
00260 
00261   if (error_exists)
00262     return false;
00263   
00264   {
00265     boost::mutex::scoped_lock lock(frame_mutex_);
00266     CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id);
00267     TimeCacheInterfacePtr frame = getFrame(frame_number);
00268     if (frame == NULL)
00269       frame = allocateFrame(frame_number, is_static);
00270 
00271     if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number)))
00272     {
00273       frame_authority_[frame_number] = authority;
00274     }
00275     else
00276     {
00277       logWarn("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained", stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str());
00278       return false;
00279     }
00280   }
00281 
00282   testTransformableRequests();
00283 
00284   return true;
00285 }
00286 
00287 TimeCacheInterfacePtr BufferCore::allocateFrame(CompactFrameID cfid, bool is_static)
00288 {
00289   TimeCacheInterfacePtr frame_ptr = frames_[cfid];
00290   if (is_static) {
00291     frames_[cfid] = TimeCacheInterfacePtr(new StaticCache());
00292   } else {
00293     frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_));
00294   }
00295 
00296   return frames_[cfid];
00297 }
00298 
00299 enum WalkEnding
00300 {
00301   Identity,
00302   TargetParentOfSource,
00303   SourceParentOfTarget,
00304   FullPath,
00305 };
00306 
00307 // TODO for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI
00308 template<typename F>
00309 int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const
00310 {
00311   return walkToTopParent(f, time, target_id, source_id, error_string, NULL);
00312 }
00313 
00314 template<typename F>
00315 int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id,
00316     CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID>
00317     *frame_chain) const
00318 {
00319   if (frame_chain)
00320     frame_chain->clear();
00321 
00322   // Short circuit if zero length transform to allow lookups on non existant links
00323   if (source_id == target_id)
00324   {
00325     f.finalize(Identity, time);
00326     return tf2_msgs::TF2Error::NO_ERROR;
00327   }
00328 
00329   //If getting the latest get the latest common time
00330   if (time == ros::Time())
00331   {
00332     int retval = getLatestCommonTime(target_id, source_id, time, error_string);
00333     if (retval != tf2_msgs::TF2Error::NO_ERROR)
00334     {
00335       return retval;
00336     }
00337   }
00338 
00339   // Walk the tree to its root from the source frame, accumulating the transform
00340   CompactFrameID frame = source_id;
00341   CompactFrameID top_parent = frame;
00342   uint32_t depth = 0;
00343 
00344   std::string extrapolation_error_string;
00345   bool extrapolation_might_have_occurred = false;
00346 
00347   while (frame != 0)
00348   {
00349     TimeCacheInterfacePtr cache = getFrame(frame);
00350     if (frame_chain)
00351       frame_chain->push_back(frame);
00352 
00353     if (!cache)
00354     {
00355       // There will be no cache for the very root of the tree
00356       top_parent = frame;
00357       break;
00358     }
00359 
00360     CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string);
00361     if (parent == 0)
00362     {
00363       // Just break out here... there may still be a path from source -> target
00364       top_parent = frame;
00365       extrapolation_might_have_occurred = true;
00366       break;
00367     }
00368 
00369     // Early out... target frame is a direct parent of the source frame
00370     if (frame == target_id)
00371     {
00372       f.finalize(TargetParentOfSource, time);
00373       return tf2_msgs::TF2Error::NO_ERROR;
00374     }
00375 
00376     f.accum(true);
00377 
00378     top_parent = frame;
00379     frame = parent;
00380 
00381     ++depth;
00382     if (depth > MAX_GRAPH_DEPTH)
00383     {
00384       if (error_string)
00385       {
00386         std::stringstream ss;
00387         ss << "The tf tree is invalid because it contains a loop." << std::endl
00388            << allFramesAsStringNoLock() << std::endl;
00389         *error_string = ss.str();
00390       }
00391       return tf2_msgs::TF2Error::LOOKUP_ERROR;
00392     }
00393   }
00394 
00395   // Now walk to the top parent from the target frame, accumulating its transform
00396   frame = target_id;
00397   depth = 0;
00398   std::vector<CompactFrameID> reverse_frame_chain;
00399 
00400   while (frame != top_parent)
00401   {
00402     TimeCacheInterfacePtr cache = getFrame(frame);
00403     if (frame_chain)
00404       reverse_frame_chain.push_back(frame);
00405 
00406     if (!cache)
00407     {
00408       break;
00409     }
00410 
00411     CompactFrameID parent = f.gather(cache, time, error_string);
00412     if (parent == 0)
00413     {
00414       if (error_string)
00415       {
00416         std::stringstream ss;
00417         ss << *error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]";
00418         *error_string = ss.str();
00419       }
00420 
00421       return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR;
00422     }
00423 
00424     // Early out... source frame is a direct parent of the target frame
00425     if (frame == source_id)
00426     {
00427       f.finalize(SourceParentOfTarget, time);
00428       if (frame_chain)
00429       {
00430         // Use the walk we just did
00431         frame_chain->swap(reverse_frame_chain);
00432         // Reverse it before returning because this is the reverse walk.
00433         std::reverse(frame_chain->begin(), frame_chain->end());
00434       }
00435       return tf2_msgs::TF2Error::NO_ERROR;
00436     }
00437 
00438     f.accum(false);
00439 
00440     frame = parent;
00441 
00442     ++depth;
00443     if (depth > MAX_GRAPH_DEPTH)
00444     {
00445       if (error_string)
00446       {
00447         std::stringstream ss;
00448         ss << "The tf tree is invalid because it contains a loop." << std::endl
00449            << allFramesAsStringNoLock() << std::endl;
00450         *error_string = ss.str();
00451       }
00452       return tf2_msgs::TF2Error::LOOKUP_ERROR;
00453     }
00454   }
00455 
00456   if (frame != top_parent)
00457   {
00458     if (extrapolation_might_have_occurred)
00459     {
00460       if (error_string)
00461       {
00462         std::stringstream ss;
00463         ss << extrapolation_error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]";
00464         *error_string = ss.str();
00465       }
00466 
00467       return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR;
00468       
00469     }
00470 
00471     createConnectivityErrorString(source_id, target_id, error_string);
00472     return tf2_msgs::TF2Error::CONNECTIVITY_ERROR;
00473   }
00474   else if (frame_chain){
00475     // append top_parent to reverse_frame_chain for easier matching/trimming
00476     reverse_frame_chain.push_back(frame);
00477   }
00478 
00479   f.finalize(FullPath, time);
00480   if (frame_chain)
00481   {
00482     // Pruning: Compare the chains starting at the parent (end) until they differ
00483     int m = reverse_frame_chain.size()-1;
00484     int n = frame_chain->size()-1;
00485     for (; m >= 0 && n >= 0; --m, --n)
00486     {
00487       if ((*frame_chain)[n] != reverse_frame_chain[m])
00488       {
00489         break;
00490       }
00491     }
00492     // Erase all duplicate items from frame_chain
00493     if (n > 0)
00494     {
00495       // N is offset by 1 and leave the common parent for this result
00496       frame_chain->erase(frame_chain->begin() + (n + 2), frame_chain->end());
00497     }
00498     if (m < reverse_frame_chain.size())
00499     {
00500       for (int i = m; i >= 0; --i)
00501       {
00502         frame_chain->push_back(reverse_frame_chain[i]);
00503       }
00504     }
00505   }
00506   
00507   return tf2_msgs::TF2Error::NO_ERROR;
00508 }
00509 
00510 
00511 
00512 struct TransformAccum
00513 {
00514   TransformAccum()
00515   : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
00516   , source_to_top_vec(0.0, 0.0, 0.0)
00517   , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
00518   , target_to_top_vec(0.0, 0.0, 0.0)
00519   , result_quat(0.0, 0.0, 0.0, 1.0)
00520   , result_vec(0.0, 0.0, 0.0)
00521   {
00522   }
00523 
00524   CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string)
00525   {
00526     if (!cache->getData(time, st, error_string))
00527     {
00528       return 0;
00529     }
00530 
00531     return st.frame_id_;
00532   }
00533 
00534   void accum(bool source)
00535   {
00536     if (source)
00537     {
00538       source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
00539       source_to_top_quat = st.rotation_ * source_to_top_quat;
00540     }
00541     else
00542     {
00543       target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
00544       target_to_top_quat = st.rotation_ * target_to_top_quat;
00545     }
00546   }
00547 
00548   void finalize(WalkEnding end, ros::Time _time)
00549   {
00550     switch (end)
00551     {
00552     case Identity:
00553       break;
00554     case TargetParentOfSource:
00555       result_vec = source_to_top_vec;
00556       result_quat = source_to_top_quat;
00557       break;
00558     case SourceParentOfTarget:
00559       {
00560         tf2::Quaternion inv_target_quat = target_to_top_quat.inverse();
00561         tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00562         result_vec = inv_target_vec;
00563         result_quat = inv_target_quat;
00564         break;
00565       }
00566     case FullPath:
00567       {
00568         tf2::Quaternion inv_target_quat = target_to_top_quat.inverse();
00569         tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00570 
00571         result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
00572         result_quat = inv_target_quat * source_to_top_quat;
00573       }
00574       break;
00575     };
00576 
00577     time = _time;
00578   }
00579 
00580   TransformStorage st;
00581   ros::Time time;
00582   tf2::Quaternion source_to_top_quat;
00583   tf2::Vector3 source_to_top_vec;
00584   tf2::Quaternion target_to_top_quat;
00585   tf2::Vector3 target_to_top_vec;
00586 
00587   tf2::Quaternion result_quat;
00588   tf2::Vector3 result_vec;
00589 };
00590 
00591 geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
00592                                                             const std::string& source_frame,
00593                                                             const ros::Time& time) const
00594 {
00595   boost::mutex::scoped_lock lock(frame_mutex_);
00596 
00597   if (target_frame == source_frame) {
00598     geometry_msgs::TransformStamped identity;
00599     identity.header.frame_id = target_frame;
00600     identity.child_frame_id = source_frame;
00601     identity.transform.rotation.w = 1;
00602 
00603     if (time == ros::Time())
00604     {
00605       CompactFrameID target_id = lookupFrameNumber(target_frame);
00606       TimeCacheInterfacePtr cache = getFrame(target_id);
00607       if (cache)
00608         identity.header.stamp = cache->getLatestTimestamp();
00609       else
00610         identity.header.stamp = time;
00611     }
00612     else
00613       identity.header.stamp = time;
00614 
00615     return identity;
00616   }
00617 
00618   //Identify case does not need to be validated above
00619   CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame);
00620   CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame);
00621 
00622   std::string error_string;
00623   TransformAccum accum;
00624   int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
00625   if (retval != tf2_msgs::TF2Error::NO_ERROR)
00626   {
00627     switch (retval)
00628     {
00629     case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
00630       throw ConnectivityException(error_string);
00631     case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
00632       throw ExtrapolationException(error_string);
00633     case tf2_msgs::TF2Error::LOOKUP_ERROR:
00634       throw LookupException(error_string);
00635     default:
00636       logError("Unknown error code: %d", retval);
00637       assert(0);
00638     }
00639   }
00640 
00641   geometry_msgs::TransformStamped output_transform;
00642   transformTF2ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame);
00643   return output_transform;
00644 }
00645 
00646                                                        
00647 geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, 
00648                                                         const ros::Time& target_time,
00649                                                         const std::string& source_frame,
00650                                                         const ros::Time& source_time,
00651                                                         const std::string& fixed_frame) const
00652 {
00653   validateFrameId("lookupTransform argument target_frame", target_frame);
00654   validateFrameId("lookupTransform argument source_frame", source_frame);
00655   validateFrameId("lookupTransform argument fixed_frame", fixed_frame);
00656 
00657   geometry_msgs::TransformStamped output;
00658   geometry_msgs::TransformStamped temp1 =  lookupTransform(fixed_frame, source_frame, source_time);
00659   geometry_msgs::TransformStamped temp2 =  lookupTransform(target_frame, fixed_frame, target_time);
00660   
00661   tf2::Transform tf1, tf2;
00662   transformMsgToTF2(temp1.transform, tf1);
00663   transformMsgToTF2(temp2.transform, tf2);
00664   transformTF2ToMsg(tf2*tf1, output.transform);
00665   output.header.stamp = temp2.header.stamp;
00666   output.header.frame_id = target_frame;
00667   output.child_frame_id = source_frame;
00668   return output;
00669 }
00670 
00671 
00672 
00673 /*
00674 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
00675                                           const std::string& observation_frame, 
00676                                           const ros::Time& time, 
00677                                           const ros::Duration& averaging_interval) const
00678 {
00679   try
00680   {
00681   geometry_msgs::Twist t;
00682   old_tf_.lookupTwist(tracking_frame, observation_frame, 
00683                       time, averaging_interval, t);
00684   return t;
00685   }
00686   catch (tf::LookupException& ex)
00687   {
00688     throw tf2::LookupException(ex.what());
00689   }
00690   catch (tf::ConnectivityException& ex)
00691   {
00692     throw tf2::ConnectivityException(ex.what());
00693   }
00694   catch (tf::ExtrapolationException& ex)
00695   {
00696     throw tf2::ExtrapolationException(ex.what());
00697   }
00698   catch (tf::InvalidArgument& ex)
00699   {
00700     throw tf2::InvalidArgumentException(ex.what());
00701   }
00702 }
00703 
00704 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
00705                                           const std::string& observation_frame, 
00706                                           const std::string& reference_frame,
00707                                           const tf2::Point & reference_point, 
00708                                           const std::string& reference_point_frame, 
00709                                           const ros::Time& time, 
00710                                           const ros::Duration& averaging_interval) const
00711 {
00712   try{
00713   geometry_msgs::Twist t;
00714   old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame,
00715                       time, averaging_interval, t);
00716   return t;
00717   }
00718   catch (tf::LookupException& ex)
00719   {
00720     throw tf2::LookupException(ex.what());
00721   }
00722   catch (tf::ConnectivityException& ex)
00723   {
00724     throw tf2::ConnectivityException(ex.what());
00725   }
00726   catch (tf::ExtrapolationException& ex)
00727   {
00728     throw tf2::ExtrapolationException(ex.what());
00729   }
00730   catch (tf::InvalidArgument& ex)
00731   {
00732     throw tf2::InvalidArgumentException(ex.what());
00733   }
00734 }
00735 */
00736 
00737 struct CanTransformAccum
00738 {
00739   CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string)
00740   {
00741     return cache->getParent(time, error_string);
00742   }
00743 
00744   void accum(bool source)
00745   {
00746   }
00747 
00748   void finalize(WalkEnding end, ros::Time _time)
00749   {
00750   }
00751 
00752   TransformStorage st;
00753 };
00754 
00755 bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
00756                     const ros::Time& time, std::string* error_msg) const
00757 {
00758   if (target_id == 0 || source_id == 0)
00759   {
00760     if (error_msg)
00761       {
00762         if (target_id == 0)
00763         {
00764           *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist.");
00765         }
00766         if (source_id == 0)
00767         {
00768           if (target_id == 0)
00769           {
00770               *error_msg += std::string(" ");
00771           }
00772           *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist.");
00773         }
00774       }
00775     return false;
00776   }
00777 
00778   if (target_id == source_id)
00779   {
00780     return true;
00781   }
00782 
00783   CanTransformAccum accum;
00784   if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2_msgs::TF2Error::NO_ERROR)
00785   {
00786     return true;
00787   }
00788 
00789   return false;
00790 }
00791 
00792 bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
00793                                   const ros::Time& time, std::string* error_msg) const
00794 {
00795   boost::mutex::scoped_lock lock(frame_mutex_);
00796   return canTransformNoLock(target_id, source_id, time, error_msg);
00797 }
00798 
00799 bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame,
00800                            const ros::Time& time, std::string* error_msg) const
00801 {
00802   // Short circuit if target_frame == source_frame
00803   if (target_frame == source_frame)
00804     return true;
00805 
00806   if (warnFrameId("canTransform argument target_frame", target_frame))
00807     return false;
00808   if (warnFrameId("canTransform argument source_frame", source_frame))
00809     return false;
00810 
00811   boost::mutex::scoped_lock lock(frame_mutex_);
00812 
00813   CompactFrameID target_id = lookupFrameNumber(target_frame);
00814   CompactFrameID source_id = lookupFrameNumber(source_frame);
00815 
00816   if (target_id == 0 || source_id == 0)
00817   {
00818     if (error_msg)
00819       {
00820         if (target_id == 0)
00821         {
00822           *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
00823         }
00824         if (source_id == 0)
00825         {
00826           if (target_id == 0)
00827           {
00828               *error_msg += std::string(" ");
00829           }
00830           *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
00831         }
00832       }
00833     return false;
00834   }
00835   return canTransformNoLock(target_id, source_id, time, error_msg);
00836 }
00837 
00838 bool BufferCore::canTransform(const std::string& target_frame, const ros::Time& target_time,
00839                           const std::string& source_frame, const ros::Time& source_time,
00840                           const std::string& fixed_frame, std::string* error_msg) const
00841 {
00842   if (warnFrameId("canTransform argument target_frame", target_frame))
00843     return false;
00844   if (warnFrameId("canTransform argument source_frame", source_frame))
00845     return false;
00846   if (warnFrameId("canTransform argument fixed_frame", fixed_frame))
00847     return false;
00848 
00849   boost::mutex::scoped_lock lock(frame_mutex_);
00850   CompactFrameID target_id = lookupFrameNumber(target_frame);
00851   CompactFrameID source_id = lookupFrameNumber(source_frame);
00852   CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
00853 
00854   if (target_id == 0 || source_id == 0 || fixed_id == 0)
00855   {
00856     if (error_msg)
00857       {
00858         if (target_id == 0)
00859         {
00860           *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
00861         }
00862         if (source_id == 0)
00863         {
00864           if (target_id == 0)
00865           {
00866               *error_msg += std::string(" ");
00867           }
00868           *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
00869         }
00870         if (source_id == 0)
00871         {
00872           if (target_id == 0 || source_id == 0)
00873           {
00874               *error_msg += std::string(" ");
00875           }
00876           *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist.");
00877         }
00878       }
00879     return false;
00880   }
00881   return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg);
00882 }
00883 
00884 
00885 tf2::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const
00886 {
00887   if (frame_id >= frames_.size())
00888     return TimeCacheInterfacePtr();
00889   else
00890   {
00891     return frames_[frame_id];
00892   }
00893 }
00894 
00895 CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const
00896 {
00897   CompactFrameID retval;
00898   M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
00899   if (map_it == frameIDs_.end())
00900   {
00901     retval = CompactFrameID(0);
00902   }
00903   else
00904     retval = map_it->second;
00905   return retval;
00906 }
00907 
00908 CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str)
00909 {
00910   CompactFrameID retval = 0;
00911   M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
00912   if (map_it == frameIDs_.end())
00913   {
00914     retval = CompactFrameID(frames_.size());
00915     frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration
00916     frameIDs_[frameid_str] = retval;
00917     frameIDs_reverse.push_back(frameid_str);
00918   }
00919   else
00920     retval = frameIDs_[frameid_str];
00921 
00922   return retval;
00923 }
00924 
00925 const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const
00926 {
00927     if (frame_id_num >= frameIDs_reverse.size())
00928     {
00929       std::stringstream ss;
00930       ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
00931       throw tf2::LookupException(ss.str());
00932     }
00933     else
00934       return frameIDs_reverse[frame_id_num];
00935 }
00936 
00937 void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const
00938 {
00939   if (!out)
00940   {
00941     return;
00942   }
00943   *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+
00944                      lookupFrameString(source_frame)+"' because they are not part of the same tree."+
00945                      "Tf has two or more unconnected trees.");
00946 }
00947 
00948 std::string BufferCore::allFramesAsString() const
00949 {
00950   boost::mutex::scoped_lock lock(frame_mutex_);
00951   return this->allFramesAsStringNoLock();
00952 }
00953 
00954 std::string BufferCore::allFramesAsStringNoLock() const
00955 {
00956   std::stringstream mstream;
00957 
00958   TransformStorage temp;
00959 
00960   //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
00961 
00963   for (unsigned int counter = 1; counter < frames_.size(); counter ++)
00964   {
00965     TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter));
00966     if (frame_ptr == NULL)
00967       continue;
00968     CompactFrameID frame_id_num;
00969     if(  frame_ptr->getData(ros::Time(), temp))
00970       frame_id_num = temp.frame_id_;
00971     else
00972     {
00973       frame_id_num = 0;
00974     }
00975     mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <<std::endl;
00976   }
00977 
00978   return mstream.str();
00979 }
00980 
00981 struct TimeAndFrameIDFrameComparator
00982 {
00983   TimeAndFrameIDFrameComparator(CompactFrameID id)
00984   : id(id)
00985   {}
00986 
00987   bool operator()(const P_TimeAndFrameID& rhs) const
00988   {
00989     return rhs.second == id;
00990   }
00991 
00992   CompactFrameID id;
00993 };
00994 
00995 int BufferCore::getLatestCommonTime(CompactFrameID target_id, CompactFrameID source_id, ros::Time & time, std::string * error_string) const
00996 {
00997   // Error if one of the frames don't exist.
00998   if (source_id == 0 || target_id == 0) return tf2_msgs::TF2Error::LOOKUP_ERROR;
00999 
01000   if (source_id == target_id)
01001   {
01002     TimeCacheInterfacePtr cache = getFrame(source_id);
01003     //Set time to latest timestamp of frameid in case of target and source frame id are the same
01004     if (cache)
01005       time = cache->getLatestTimestamp();
01006     else
01007       time = ros::Time();
01008     return tf2_msgs::TF2Error::NO_ERROR;
01009   }
01010 
01011   std::vector<P_TimeAndFrameID> lct_cache;
01012 
01013   // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
01014   // in the target is a direct parent
01015   CompactFrameID frame = source_id;
01016   P_TimeAndFrameID temp;
01017   uint32_t depth = 0;
01018   ros::Time common_time = ros::TIME_MAX;
01019   while (frame != 0)
01020   {
01021     TimeCacheInterfacePtr cache = getFrame(frame);
01022 
01023     if (!cache)
01024     {
01025       // There will be no cache for the very root of the tree
01026       break;
01027     }
01028 
01029     P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
01030 
01031     if (latest.second == 0)
01032     {
01033       // Just break out here... there may still be a path from source -> target
01034       break;
01035     }
01036 
01037     if (!latest.first.isZero())
01038     {
01039       common_time = std::min(latest.first, common_time);
01040     }
01041 
01042     lct_cache.push_back(latest);
01043 
01044     frame = latest.second;
01045 
01046     // Early out... target frame is a direct parent of the source frame
01047     if (frame == target_id)
01048     {
01049       time = common_time;
01050       if (time == ros::TIME_MAX)
01051       {
01052         time = ros::Time();
01053       }
01054       return tf2_msgs::TF2Error::NO_ERROR;
01055     }
01056 
01057     ++depth;
01058     if (depth > MAX_GRAPH_DEPTH)
01059     {
01060       if (error_string)
01061       {
01062         std::stringstream ss;
01063         ss<<"The tf tree is invalid because it contains a loop." << std::endl
01064           << allFramesAsStringNoLock() << std::endl;
01065         *error_string = ss.str();
01066       }
01067       return tf2_msgs::TF2Error::LOOKUP_ERROR;
01068     }
01069   }
01070 
01071   // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
01072   frame = target_id;
01073   depth = 0;
01074   common_time = ros::TIME_MAX;
01075   CompactFrameID common_parent = 0;
01076   while (true)
01077   {
01078     TimeCacheInterfacePtr cache = getFrame(frame);
01079 
01080     if (!cache)
01081     {
01082       break;
01083     }
01084 
01085     P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
01086 
01087     if (latest.second == 0)
01088     {
01089       break;
01090     }
01091 
01092     if (!latest.first.isZero())
01093     {
01094       common_time = std::min(latest.first, common_time);
01095     }
01096 
01097     std::vector<P_TimeAndFrameID>::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second));
01098     if (it != lct_cache.end()) // found a common parent
01099     {
01100       common_parent = it->second;
01101       break;
01102     }
01103 
01104     frame = latest.second;
01105 
01106     // Early out... source frame is a direct parent of the target frame
01107     if (frame == source_id)
01108     {
01109       time = common_time;
01110       if (time == ros::TIME_MAX)
01111       {
01112         time = ros::Time();
01113       }
01114       return tf2_msgs::TF2Error::NO_ERROR;
01115     }
01116 
01117     ++depth;
01118     if (depth > MAX_GRAPH_DEPTH)
01119     {
01120       if (error_string)
01121       {
01122         std::stringstream ss;
01123         ss<<"The tf tree is invalid because it contains a loop." << std::endl
01124           << allFramesAsStringNoLock() << std::endl;
01125         *error_string = ss.str();
01126       }
01127       return tf2_msgs::TF2Error::LOOKUP_ERROR;
01128     }
01129   }
01130 
01131   if (common_parent == 0)
01132   {
01133     createConnectivityErrorString(source_id, target_id, error_string);
01134     return tf2_msgs::TF2Error::CONNECTIVITY_ERROR;
01135   }
01136 
01137   // Loop through the source -> root list until we hit the common parent
01138   {
01139     std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
01140     std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
01141     for (; it != end; ++it)
01142     {
01143       if (!it->first.isZero())
01144       {
01145         common_time = std::min(common_time, it->first);
01146       }
01147 
01148       if (it->second == common_parent)
01149       {
01150         break;
01151       }
01152     }
01153   }
01154 
01155   if (common_time == ros::TIME_MAX)
01156   {
01157     common_time = ros::Time();
01158   }
01159 
01160   time = common_time;
01161   return tf2_msgs::TF2Error::NO_ERROR;
01162 }
01163 
01164 std::string BufferCore::allFramesAsYAML(double current_time) const
01165 {
01166   std::stringstream mstream;
01167   boost::mutex::scoped_lock lock(frame_mutex_);
01168 
01169   TransformStorage temp;
01170 
01171   if (frames_.size() ==1)
01172     mstream <<"{}";
01173 
01174   mstream.precision(3);
01175   mstream.setf(std::ios::fixed,std::ios::floatfield);
01176 
01177    //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
01178   for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
01179   {
01180     CompactFrameID cfid = CompactFrameID(counter);
01181     CompactFrameID frame_id_num;
01182     TimeCacheInterfacePtr cache = getFrame(cfid);
01183     if (!cache)
01184     {
01185       continue;
01186     }
01187 
01188     if(!cache->getData(ros::Time(), temp))
01189     {
01190       continue;
01191     }
01192 
01193     frame_id_num = temp.frame_id_;
01194 
01195     std::string authority = "no recorded authority";
01196     std::map<CompactFrameID, std::string>::const_iterator it = frame_authority_.find(cfid);
01197     if (it != frame_authority_.end()) {
01198       authority = it->second;
01199     }
01200 
01201     double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() -
01202                                                      cache->getOldestTimestamp().toSec() ), 0.0001);
01203 
01204     mstream << std::fixed; //fixed point notation
01205     mstream.precision(3); //3 decimal places
01206     mstream << frameIDs_reverse[cfid] << ": " << std::endl;
01207     mstream << "  parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl;
01208     mstream << "  broadcaster: '" << authority << "'" << std::endl;
01209     mstream << "  rate: " << rate << std::endl;
01210     mstream << "  most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl;
01211     mstream << "  oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl;
01212     if ( current_time > 0 ) {
01213       mstream << "  transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl;
01214     }
01215     mstream << "  buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl;
01216   }
01217 
01218   return mstream.str();
01219 }
01220 
01221 std::string BufferCore::allFramesAsYAML() const
01222 {
01223   return this->allFramesAsYAML(0.0);
01224 }
01225 
01226 TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb)
01227 {
01228   boost::mutex::scoped_lock lock(transformable_callbacks_mutex_);
01229   TransformableCallbackHandle handle = ++transformable_callbacks_counter_;
01230   while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second)
01231   {
01232     handle = ++transformable_callbacks_counter_;
01233   }
01234 
01235   return handle;
01236 }
01237 
01238 struct BufferCore::RemoveRequestByCallback
01239 {
01240   RemoveRequestByCallback(TransformableCallbackHandle handle)
01241   : handle_(handle)
01242   {}
01243 
01244   bool operator()(const TransformableRequest& req)
01245   {
01246     return req.cb_handle == handle_;
01247   }
01248 
01249   TransformableCallbackHandle handle_;
01250 };
01251 
01252 void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle)
01253 {
01254   {
01255     boost::mutex::scoped_lock lock(transformable_callbacks_mutex_);
01256     transformable_callbacks_.erase(handle);
01257   }
01258 
01259   {
01260     boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01261     V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle));
01262     transformable_requests_.erase(it, transformable_requests_.end());
01263   }
01264 }
01265 
01266 TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time)
01267 {
01268   // shortcut if target == source
01269   if (target_frame == source_frame)
01270   {
01271     return 0;
01272   }
01273 
01274   TransformableRequest req;
01275   req.target_id = lookupFrameNumber(target_frame);
01276   req.source_id = lookupFrameNumber(source_frame);
01277 
01278   // First check if the request is already transformable.  If it is, return immediately
01279   if (canTransformInternal(req.target_id, req.source_id, time, 0))
01280   {
01281     return 0;
01282   }
01283 
01284   // Might not be transformable at all, ever (if it's too far in the past)
01285   if (req.target_id && req.source_id)
01286   {
01287     ros::Time latest_time;
01288     // TODO: This is incorrect, but better than nothing.  Really we want the latest time for
01289     // any of the frames
01290     getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
01291     if (!latest_time.isZero() && time + cache_time_ < latest_time)
01292     {
01293       return 0xffffffffffffffffULL;
01294     }
01295   }
01296 
01297   req.cb_handle = handle;
01298   req.time = time;
01299   req.request_handle = ++transformable_requests_counter_;
01300   if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL)
01301   {
01302     req.request_handle = 1;
01303   }
01304 
01305   if (req.target_id == 0)
01306   {
01307     req.target_string = target_frame;
01308   }
01309 
01310   if (req.source_id == 0)
01311   {
01312     req.source_string = source_frame;
01313   }
01314 
01315   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01316   transformable_requests_.push_back(req);
01317 
01318   return req.request_handle;
01319 }
01320 
01321 struct BufferCore::RemoveRequestByID
01322 {
01323   RemoveRequestByID(TransformableRequestHandle handle)
01324   : handle_(handle)
01325   {}
01326 
01327   bool operator()(const TransformableRequest& req)
01328   {
01329     return req.request_handle == handle_;
01330   }
01331 
01332   TransformableCallbackHandle handle_;
01333 };
01334 
01335 void BufferCore::cancelTransformableRequest(TransformableRequestHandle handle)
01336 {
01337   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01338   V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle));
01339 
01340   if (it != transformable_requests_.end())
01341   {
01342     transformable_requests_.erase(it, transformable_requests_.end());
01343   }
01344 }
01345 
01346 
01347 
01348 // backwards compability for tf methods
01349 boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function<void(void)> callback)
01350 {
01351   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01352   return _transforms_changed_.connect(callback);
01353 }
01354 
01355 void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c)
01356 {
01357   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01358   c.disconnect();
01359 }
01360 
01361 
01362 bool BufferCore::_frameExists(const std::string& frame_id_str) const
01363 {
01364   boost::mutex::scoped_lock lock(frame_mutex_);
01365   return frameIDs_.count(frame_id_str);
01366 }
01367 
01368 bool BufferCore::_getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
01369 {
01370 
01371   boost::mutex::scoped_lock lock(frame_mutex_);
01372   CompactFrameID frame_number = lookupFrameNumber(frame_id);
01373   TimeCacheInterfacePtr frame = getFrame(frame_number);
01374 
01375   if (! frame)
01376     return false;
01377       
01378   CompactFrameID parent_id = frame->getParent(time, NULL);
01379   if (parent_id == 0)
01380     return false;
01381 
01382   parent = lookupFrameString(parent_id);
01383   return true;
01384 };
01385 
01386 void BufferCore::_getFrameStrings(std::vector<std::string> & vec) const
01387 {
01388   vec.clear();
01389 
01390   boost::mutex::scoped_lock lock(frame_mutex_);
01391 
01392   TransformStorage temp;
01393 
01394   //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
01395   for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++)
01396   {
01397     vec.push_back(frameIDs_reverse[counter]);
01398   }
01399   return;
01400 }
01401 
01402 
01403 
01404 
01405 void BufferCore::testTransformableRequests()
01406 {
01407   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01408   V_TransformableRequest::iterator it = transformable_requests_.begin();
01409 
01410   typedef boost::tuple<TransformableCallback&, TransformableRequestHandle, std::string,
01411                        std::string, ros::Time&, TransformableResult&> TransformableTuple;
01412   std::vector<TransformableTuple> transformables;
01413 
01414   for (; it != transformable_requests_.end();)
01415   {
01416     TransformableRequest& req = *it;
01417 
01418     // One or both of the frames may not have existed when the request was originally made.
01419     if (req.target_id == 0)
01420     {
01421       req.target_id = lookupFrameNumber(req.target_string);
01422     }
01423 
01424     if (req.source_id == 0)
01425     {
01426       req.source_id = lookupFrameNumber(req.source_string);
01427     }
01428 
01429     ros::Time latest_time;
01430     bool do_cb = false;
01431     TransformableResult result = TransformAvailable;
01432     // TODO: This is incorrect, but better than nothing.  Really we want the latest time for
01433     // any of the frames
01434     getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
01435     if (!latest_time.isZero() && req.time + cache_time_ < latest_time)
01436     {
01437       do_cb = true;
01438       result = TransformFailure;
01439     }
01440     else if (canTransformInternal(req.target_id, req.source_id, req.time, 0))
01441     {
01442       do_cb = true;
01443       result = TransformAvailable;
01444     }
01445 
01446     if (do_cb)
01447     {
01448       {
01449         boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_);
01450         M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle);
01451         if (it != transformable_callbacks_.end())
01452         {
01453           transformables.push_back(boost::make_tuple(boost::ref(it->second),
01454                                                      req.request_handle,
01455                                                      lookupFrameString(req.target_id),
01456                                                      lookupFrameString(req.source_id),
01457                                                      boost::ref(req.time),
01458                                                      boost::ref(result)));
01459         }
01460       }
01461 
01462       if (transformable_requests_.size() > 1)
01463       {
01464         transformable_requests_[it - transformable_requests_.begin()] = transformable_requests_.back();
01465       }
01466 
01467       transformable_requests_.erase(transformable_requests_.end() - 1);
01468     }
01469     else
01470     {
01471       ++it;
01472     }
01473   }
01474 
01475   // unlock before allowing possible user callbacks to avoid potential deadlock (#91)
01476   lock.unlock();
01477 
01478   BOOST_FOREACH (TransformableTuple tt, transformables)
01479   {
01480     tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>());
01481   }
01482 
01483   // Backwards compatability callback for tf
01484   _transforms_changed_();
01485 }
01486 
01487 
01488 std::string BufferCore::_allFramesAsDot(double current_time) const
01489 {
01490   std::stringstream mstream;
01491   mstream << "digraph G {" << std::endl;
01492   boost::mutex::scoped_lock lock(frame_mutex_);
01493 
01494   TransformStorage temp;
01495 
01496   if (frames_.size() == 1) {
01497     mstream <<"\"no tf data recieved\"";
01498   }
01499   mstream.precision(3);
01500   mstream.setf(std::ios::fixed,std::ios::floatfield);
01501 
01502   for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame
01503   {
01504     unsigned int frame_id_num;
01505     TimeCacheInterfacePtr counter_frame = getFrame(counter);
01506     if (!counter_frame) {
01507       continue;
01508     }
01509     if(!counter_frame->getData(ros::Time(), temp)) {
01510       continue;
01511     } else {
01512       frame_id_num = temp.frame_id_;
01513     }
01514     std::string authority = "no recorded authority";
01515     std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
01516     if (it != frame_authority_.end())
01517       authority = it->second;
01518 
01519     double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() -
01520                                                              counter_frame->getOldestTimestamp().toSec()), 0.0001);
01521 
01522     mstream << std::fixed; //fixed point notation
01523     mstream.precision(3); //3 decimal places
01524     mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
01525             << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
01526       //<< "Time: " << current_time.toSec() << "\\n"
01527             << "Broadcaster: " << authority << "\\n"
01528             << "Average rate: " << rate << " Hz\\n"
01529             << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" ";
01530     if (current_time > 0)
01531       mstream << "( "<<  current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)";
01532     mstream << "\\n"
01533       //    << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n"
01534       //    << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n"
01535       //    << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n"
01536             << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n"
01537             <<"\"];" <<std::endl;
01538   }
01539 
01540   for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
01541   {
01542     unsigned int frame_id_num;
01543     TimeCacheInterfacePtr counter_frame = getFrame(counter);
01544     if (!counter_frame) {
01545       if (current_time > 0) {
01546         mstream << "edge [style=invis];" <<std::endl;
01547         mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n"
01548                 << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "
01549                 << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
01550       }
01551       continue;
01552     }
01553     if (counter_frame->getData(ros::Time(), temp)) {
01554       frame_id_num = temp.frame_id_;
01555     } else {
01556         frame_id_num = 0;
01557     }
01558 
01559     if(frameIDs_reverse[frame_id_num]=="NO_PARENT")
01560     {
01561       mstream << "edge [style=invis];" <<std::endl;
01562       mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n";
01563       if (current_time > 0)
01564         mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n ";
01565       mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
01566     }
01567   }
01568   mstream << "}";
01569   return mstream.str();
01570 }
01571 
01572 std::string BufferCore::_allFramesAsDot() const
01573 {
01574   return _allFramesAsDot(0.0);
01575 }
01576 
01577 void BufferCore::_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
01578 {
01579   std::string error_string;
01580 
01581   output.clear(); //empty vector
01582 
01583   std::stringstream mstream;
01584   boost::mutex::scoped_lock lock(frame_mutex_);
01585 
01586   TransformAccum accum;
01587 
01588   // Get source frame/time using getFrame
01589   CompactFrameID source_id = lookupFrameNumber(source_frame);
01590   CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
01591   CompactFrameID target_id = lookupFrameNumber(target_frame);
01592 
01593   std::vector<CompactFrameID> source_frame_chain;
01594   int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain);
01595 
01596   if (retval != tf2_msgs::TF2Error::NO_ERROR)
01597   {
01598     switch (retval)
01599     {
01600     case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
01601       throw ConnectivityException(error_string);
01602     case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
01603       throw ExtrapolationException(error_string);
01604     case tf2_msgs::TF2Error::LOOKUP_ERROR:
01605       throw LookupException(error_string);
01606     default:
01607       logError("Unknown error code: %d", retval);
01608       assert(0);
01609     }
01610   }
01611 
01612   std::vector<CompactFrameID> target_frame_chain;
01613   retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain);
01614 
01615   if (retval != tf2_msgs::TF2Error::NO_ERROR)
01616   {
01617     switch (retval)
01618     {
01619     case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
01620       throw ConnectivityException(error_string);
01621     case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
01622       throw ExtrapolationException(error_string);
01623     case tf2_msgs::TF2Error::LOOKUP_ERROR:
01624       throw LookupException(error_string);
01625     default:
01626       logError("Unknown error code: %d", retval);
01627       assert(0);
01628     }
01629   }
01630   // If the two chains overlap clear the overlap
01631   if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 &&
01632       source_frame_chain.back() == target_frame_chain.front())
01633   {
01634     source_frame_chain.pop_back();
01635   }
01636   // Join the two walks
01637   for (unsigned int i = 0; i < target_frame_chain.size(); ++i)
01638   {
01639     source_frame_chain.push_back(target_frame_chain[i]);
01640   }
01641 
01642 
01643   // Write each element of source_frame_chain as string
01644   for (unsigned int i = 0; i < source_frame_chain.size(); ++i)
01645   {
01646     output.push_back(lookupFrameString(source_frame_chain[i]));
01647   }
01648 }
01649 
01650 int TestBufferCore::_walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const
01651 {
01652   TransformAccum accum;
01653   return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain);
01654 }
01655 
01656 } // namespace tf2


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jan 4 2018 03:35:29