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         frame_chain->swap(reverse_frame_chain);
00431       }
00432       return tf2_msgs::TF2Error::NO_ERROR;
00433     }
00434 
00435     f.accum(false);
00436 
00437     frame = parent;
00438 
00439     ++depth;
00440     if (depth > MAX_GRAPH_DEPTH)
00441     {
00442       if (error_string)
00443       {
00444         std::stringstream ss;
00445         ss << "The tf tree is invalid because it contains a loop." << std::endl
00446            << allFramesAsStringNoLock() << std::endl;
00447         *error_string = ss.str();
00448       }
00449       return tf2_msgs::TF2Error::LOOKUP_ERROR;
00450     }
00451   }
00452 
00453   if (frame != top_parent)
00454   {
00455     if (extrapolation_might_have_occurred)
00456     {
00457       if (error_string)
00458       {
00459         std::stringstream ss;
00460         ss << extrapolation_error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]";
00461         *error_string = ss.str();
00462       }
00463 
00464       return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR;
00465       
00466     }
00467 
00468     createConnectivityErrorString(source_id, target_id, error_string);
00469     return tf2_msgs::TF2Error::CONNECTIVITY_ERROR;
00470   }
00471 
00472   f.finalize(FullPath, time);
00473   if (frame_chain)
00474   {
00475     // Pruning: Compare the chains starting at the parent (end) until they differ
00476     int m = reverse_frame_chain.size()-1;
00477     int n = frame_chain->size()-1;
00478     for (; m >= 0 && n >= 0; --m, --n)
00479     {
00480       if ((*frame_chain)[n] != reverse_frame_chain[m])
00481         break;
00482     }
00483     // Erase all duplicate items from frame_chain
00484     if (n > 0)
00485       frame_chain->erase(frame_chain->begin() + (n-1), frame_chain->end());
00486 
00487     if (m < reverse_frame_chain.size())
00488     {
00489       for (int i = m; i >= 0; --i)
00490       {
00491         frame_chain->push_back(reverse_frame_chain[i]);
00492       }
00493     }
00494   }
00495   
00496   return tf2_msgs::TF2Error::NO_ERROR;
00497 }
00498 
00499 
00500 
00501 struct TransformAccum
00502 {
00503   TransformAccum()
00504   : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
00505   , source_to_top_vec(0.0, 0.0, 0.0)
00506   , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
00507   , target_to_top_vec(0.0, 0.0, 0.0)
00508   , result_quat(0.0, 0.0, 0.0, 1.0)
00509   , result_vec(0.0, 0.0, 0.0)
00510   {
00511   }
00512 
00513   CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string)
00514   {
00515     if (!cache->getData(time, st, error_string))
00516     {
00517       return 0;
00518     }
00519 
00520     return st.frame_id_;
00521   }
00522 
00523   void accum(bool source)
00524   {
00525     if (source)
00526     {
00527       source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
00528       source_to_top_quat = st.rotation_ * source_to_top_quat;
00529     }
00530     else
00531     {
00532       target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
00533       target_to_top_quat = st.rotation_ * target_to_top_quat;
00534     }
00535   }
00536 
00537   void finalize(WalkEnding end, ros::Time _time)
00538   {
00539     switch (end)
00540     {
00541     case Identity:
00542       break;
00543     case TargetParentOfSource:
00544       result_vec = source_to_top_vec;
00545       result_quat = source_to_top_quat;
00546       break;
00547     case SourceParentOfTarget:
00548       {
00549         tf2::Quaternion inv_target_quat = target_to_top_quat.inverse();
00550         tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00551         result_vec = inv_target_vec;
00552         result_quat = inv_target_quat;
00553         break;
00554       }
00555     case FullPath:
00556       {
00557         tf2::Quaternion inv_target_quat = target_to_top_quat.inverse();
00558         tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00559 
00560         result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
00561         result_quat = inv_target_quat * source_to_top_quat;
00562       }
00563       break;
00564     };
00565 
00566     time = _time;
00567   }
00568 
00569   TransformStorage st;
00570   ros::Time time;
00571   tf2::Quaternion source_to_top_quat;
00572   tf2::Vector3 source_to_top_vec;
00573   tf2::Quaternion target_to_top_quat;
00574   tf2::Vector3 target_to_top_vec;
00575 
00576   tf2::Quaternion result_quat;
00577   tf2::Vector3 result_vec;
00578 };
00579 
00580 geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
00581                                                             const std::string& source_frame,
00582                                                             const ros::Time& time) const
00583 {
00584   boost::mutex::scoped_lock lock(frame_mutex_);
00585 
00586   if (target_frame == source_frame) {
00587     geometry_msgs::TransformStamped identity;
00588     identity.header.frame_id = target_frame;
00589     identity.child_frame_id = source_frame;
00590     identity.transform.rotation.w = 1;
00591 
00592     if (time == ros::Time())
00593     {
00594       CompactFrameID target_id = lookupFrameNumber(target_frame);
00595       TimeCacheInterfacePtr cache = getFrame(target_id);
00596       if (cache)
00597         identity.header.stamp = cache->getLatestTimestamp();
00598       else
00599         identity.header.stamp = time;
00600     }
00601     else
00602       identity.header.stamp = time;
00603 
00604     return identity;
00605   }
00606 
00607   //Identify case does not need to be validated above
00608   CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame);
00609   CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame);
00610 
00611   std::string error_string;
00612   TransformAccum accum;
00613   int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
00614   if (retval != tf2_msgs::TF2Error::NO_ERROR)
00615   {
00616     switch (retval)
00617     {
00618     case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
00619       throw ConnectivityException(error_string);
00620     case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
00621       throw ExtrapolationException(error_string);
00622     case tf2_msgs::TF2Error::LOOKUP_ERROR:
00623       throw LookupException(error_string);
00624     default:
00625       logError("Unknown error code: %d", retval);
00626       assert(0);
00627     }
00628   }
00629 
00630   geometry_msgs::TransformStamped output_transform;
00631   transformTF2ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame);
00632   return output_transform;
00633 }
00634 
00635                                                        
00636 geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, 
00637                                                         const ros::Time& target_time,
00638                                                         const std::string& source_frame,
00639                                                         const ros::Time& source_time,
00640                                                         const std::string& fixed_frame) const
00641 {
00642   validateFrameId("lookupTransform argument target_frame", target_frame);
00643   validateFrameId("lookupTransform argument source_frame", source_frame);
00644   validateFrameId("lookupTransform argument fixed_frame", fixed_frame);
00645 
00646   geometry_msgs::TransformStamped output;
00647   geometry_msgs::TransformStamped temp1 =  lookupTransform(fixed_frame, source_frame, source_time);
00648   geometry_msgs::TransformStamped temp2 =  lookupTransform(target_frame, fixed_frame, target_time);
00649   
00650   tf2::Transform tf1, tf2;
00651   transformMsgToTF2(temp1.transform, tf1);
00652   transformMsgToTF2(temp2.transform, tf2);
00653   transformTF2ToMsg(tf2*tf1, output.transform);
00654   output.header.stamp = temp2.header.stamp;
00655   output.header.frame_id = target_frame;
00656   output.child_frame_id = source_frame;
00657   return output;
00658 }
00659 
00660 
00661 
00662 /*
00663 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
00664                                           const std::string& observation_frame, 
00665                                           const ros::Time& time, 
00666                                           const ros::Duration& averaging_interval) const
00667 {
00668   try
00669   {
00670   geometry_msgs::Twist t;
00671   old_tf_.lookupTwist(tracking_frame, observation_frame, 
00672                       time, averaging_interval, t);
00673   return t;
00674   }
00675   catch (tf::LookupException& ex)
00676   {
00677     throw tf2::LookupException(ex.what());
00678   }
00679   catch (tf::ConnectivityException& ex)
00680   {
00681     throw tf2::ConnectivityException(ex.what());
00682   }
00683   catch (tf::ExtrapolationException& ex)
00684   {
00685     throw tf2::ExtrapolationException(ex.what());
00686   }
00687   catch (tf::InvalidArgument& ex)
00688   {
00689     throw tf2::InvalidArgumentException(ex.what());
00690   }
00691 }
00692 
00693 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
00694                                           const std::string& observation_frame, 
00695                                           const std::string& reference_frame,
00696                                           const tf2::Point & reference_point, 
00697                                           const std::string& reference_point_frame, 
00698                                           const ros::Time& time, 
00699                                           const ros::Duration& averaging_interval) const
00700 {
00701   try{
00702   geometry_msgs::Twist t;
00703   old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame,
00704                       time, averaging_interval, t);
00705   return t;
00706   }
00707   catch (tf::LookupException& ex)
00708   {
00709     throw tf2::LookupException(ex.what());
00710   }
00711   catch (tf::ConnectivityException& ex)
00712   {
00713     throw tf2::ConnectivityException(ex.what());
00714   }
00715   catch (tf::ExtrapolationException& ex)
00716   {
00717     throw tf2::ExtrapolationException(ex.what());
00718   }
00719   catch (tf::InvalidArgument& ex)
00720   {
00721     throw tf2::InvalidArgumentException(ex.what());
00722   }
00723 }
00724 */
00725 
00726 struct CanTransformAccum
00727 {
00728   CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string)
00729   {
00730     return cache->getParent(time, error_string);
00731   }
00732 
00733   void accum(bool source)
00734   {
00735   }
00736 
00737   void finalize(WalkEnding end, ros::Time _time)
00738   {
00739   }
00740 
00741   TransformStorage st;
00742 };
00743 
00744 bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
00745                     const ros::Time& time, std::string* error_msg) const
00746 {
00747   if (target_id == 0 || source_id == 0)
00748   {
00749     if (error_msg)
00750       {
00751         if (target_id == 0)
00752         {
00753           *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist.");
00754         }
00755         if (source_id == 0)
00756         {
00757           if (target_id == 0)
00758           {
00759               *error_msg += std::string(" ");
00760           }
00761           *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist.");
00762         }
00763       }
00764     return false;
00765   }
00766 
00767   if (target_id == source_id)
00768   {
00769     return true;
00770   }
00771 
00772   CanTransformAccum accum;
00773   if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2_msgs::TF2Error::NO_ERROR)
00774   {
00775     return true;
00776   }
00777 
00778   return false;
00779 }
00780 
00781 bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
00782                                   const ros::Time& time, std::string* error_msg) const
00783 {
00784   boost::mutex::scoped_lock lock(frame_mutex_);
00785   return canTransformNoLock(target_id, source_id, time, error_msg);
00786 }
00787 
00788 bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame,
00789                            const ros::Time& time, std::string* error_msg) const
00790 {
00791   // Short circuit if target_frame == source_frame
00792   if (target_frame == source_frame)
00793     return true;
00794 
00795   if (warnFrameId("canTransform argument target_frame", target_frame))
00796     return false;
00797   if (warnFrameId("canTransform argument source_frame", source_frame))
00798     return false;
00799 
00800   boost::mutex::scoped_lock lock(frame_mutex_);
00801 
00802   CompactFrameID target_id = lookupFrameNumber(target_frame);
00803   CompactFrameID source_id = lookupFrameNumber(source_frame);
00804 
00805   if (target_id == 0 || source_id == 0)
00806   {
00807     if (error_msg)
00808       {
00809         if (target_id == 0)
00810         {
00811           *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
00812         }
00813         if (source_id == 0)
00814         {
00815           if (target_id == 0)
00816           {
00817               *error_msg += std::string(" ");
00818           }
00819           *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
00820         }
00821       }
00822     return false;
00823   }
00824   return canTransformNoLock(target_id, source_id, time, error_msg);
00825 }
00826 
00827 bool BufferCore::canTransform(const std::string& target_frame, const ros::Time& target_time,
00828                           const std::string& source_frame, const ros::Time& source_time,
00829                           const std::string& fixed_frame, std::string* error_msg) const
00830 {
00831   if (warnFrameId("canTransform argument target_frame", target_frame))
00832     return false;
00833   if (warnFrameId("canTransform argument source_frame", source_frame))
00834     return false;
00835   if (warnFrameId("canTransform argument fixed_frame", fixed_frame))
00836     return false;
00837 
00838   boost::mutex::scoped_lock lock(frame_mutex_);
00839   CompactFrameID target_id = lookupFrameNumber(target_frame);
00840   CompactFrameID source_id = lookupFrameNumber(source_frame);
00841   CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
00842 
00843   if (target_id == 0 || source_id == 0 || fixed_id == 0)
00844   {
00845     if (error_msg)
00846       {
00847         if (target_id == 0)
00848         {
00849           *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
00850         }
00851         if (source_id == 0)
00852         {
00853           if (target_id == 0)
00854           {
00855               *error_msg += std::string(" ");
00856           }
00857           *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
00858         }
00859         if (source_id == 0)
00860         {
00861           if (target_id == 0 || source_id == 0)
00862           {
00863               *error_msg += std::string(" ");
00864           }
00865           *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist.");
00866         }
00867       }
00868     return false;
00869   }
00870   return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg);
00871 }
00872 
00873 
00874 tf2::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const
00875 {
00876   if (frame_id >= frames_.size())
00877     return TimeCacheInterfacePtr();
00878   else
00879   {
00880     return frames_[frame_id];
00881   }
00882 }
00883 
00884 CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const
00885 {
00886   CompactFrameID retval;
00887   M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
00888   if (map_it == frameIDs_.end())
00889   {
00890     retval = CompactFrameID(0);
00891   }
00892   else
00893     retval = map_it->second;
00894   return retval;
00895 }
00896 
00897 CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str)
00898 {
00899   CompactFrameID retval = 0;
00900   M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
00901   if (map_it == frameIDs_.end())
00902   {
00903     retval = CompactFrameID(frames_.size());
00904     frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration
00905     frameIDs_[frameid_str] = retval;
00906     frameIDs_reverse.push_back(frameid_str);
00907   }
00908   else
00909     retval = frameIDs_[frameid_str];
00910 
00911   return retval;
00912 }
00913 
00914 const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const
00915 {
00916     if (frame_id_num >= frameIDs_reverse.size())
00917     {
00918       std::stringstream ss;
00919       ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
00920       throw tf2::LookupException(ss.str());
00921     }
00922     else
00923       return frameIDs_reverse[frame_id_num];
00924 }
00925 
00926 void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const
00927 {
00928   if (!out)
00929   {
00930     return;
00931   }
00932   *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+
00933                      lookupFrameString(source_frame)+"' because they are not part of the same tree."+
00934                      "Tf has two or more unconnected trees.");
00935 }
00936 
00937 std::string BufferCore::allFramesAsString() const
00938 {
00939   boost::mutex::scoped_lock lock(frame_mutex_);
00940   return this->allFramesAsStringNoLock();
00941 }
00942 
00943 std::string BufferCore::allFramesAsStringNoLock() const
00944 {
00945   std::stringstream mstream;
00946 
00947   TransformStorage temp;
00948 
00949   //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
00950 
00952   for (unsigned int counter = 1; counter < frames_.size(); counter ++)
00953   {
00954     TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter));
00955     if (frame_ptr == NULL)
00956       continue;
00957     CompactFrameID frame_id_num;
00958     if(  frame_ptr->getData(ros::Time(), temp))
00959       frame_id_num = temp.frame_id_;
00960     else
00961     {
00962       frame_id_num = 0;
00963     }
00964     mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <<std::endl;
00965   }
00966 
00967   return mstream.str();
00968 }
00969 
00970 struct TimeAndFrameIDFrameComparator
00971 {
00972   TimeAndFrameIDFrameComparator(CompactFrameID id)
00973   : id(id)
00974   {}
00975 
00976   bool operator()(const P_TimeAndFrameID& rhs) const
00977   {
00978     return rhs.second == id;
00979   }
00980 
00981   CompactFrameID id;
00982 };
00983 
00984 int BufferCore::getLatestCommonTime(CompactFrameID target_id, CompactFrameID source_id, ros::Time & time, std::string * error_string) const
00985 {
00986   // Error if one of the frames don't exist.
00987   if (source_id == 0 || target_id == 0) return tf2_msgs::TF2Error::LOOKUP_ERROR;
00988 
00989   if (source_id == target_id)
00990   {
00991     TimeCacheInterfacePtr cache = getFrame(source_id);
00992     //Set time to latest timestamp of frameid in case of target and source frame id are the same
00993     if (cache)
00994       time = cache->getLatestTimestamp();
00995     else
00996       time = ros::Time();
00997     return tf2_msgs::TF2Error::NO_ERROR;
00998   }
00999 
01000   std::vector<P_TimeAndFrameID> lct_cache;
01001 
01002   // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
01003   // in the target is a direct parent
01004   CompactFrameID frame = source_id;
01005   P_TimeAndFrameID temp;
01006   uint32_t depth = 0;
01007   ros::Time common_time = ros::TIME_MAX;
01008   while (frame != 0)
01009   {
01010     TimeCacheInterfacePtr cache = getFrame(frame);
01011 
01012     if (!cache)
01013     {
01014       // There will be no cache for the very root of the tree
01015       break;
01016     }
01017 
01018     P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
01019 
01020     if (latest.second == 0)
01021     {
01022       // Just break out here... there may still be a path from source -> target
01023       break;
01024     }
01025 
01026     if (!latest.first.isZero())
01027     {
01028       common_time = std::min(latest.first, common_time);
01029     }
01030 
01031     lct_cache.push_back(latest);
01032 
01033     frame = latest.second;
01034 
01035     // Early out... target frame is a direct parent of the source frame
01036     if (frame == target_id)
01037     {
01038       time = common_time;
01039       if (time == ros::TIME_MAX)
01040       {
01041         time = ros::Time();
01042       }
01043       return tf2_msgs::TF2Error::NO_ERROR;
01044     }
01045 
01046     ++depth;
01047     if (depth > MAX_GRAPH_DEPTH)
01048     {
01049       if (error_string)
01050       {
01051         std::stringstream ss;
01052         ss<<"The tf tree is invalid because it contains a loop." << std::endl
01053           << allFramesAsStringNoLock() << std::endl;
01054         *error_string = ss.str();
01055       }
01056       return tf2_msgs::TF2Error::LOOKUP_ERROR;
01057     }
01058   }
01059 
01060   // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
01061   frame = target_id;
01062   depth = 0;
01063   common_time = ros::TIME_MAX;
01064   CompactFrameID common_parent = 0;
01065   while (true)
01066   {
01067     TimeCacheInterfacePtr cache = getFrame(frame);
01068 
01069     if (!cache)
01070     {
01071       break;
01072     }
01073 
01074     P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
01075 
01076     if (latest.second == 0)
01077     {
01078       break;
01079     }
01080 
01081     if (!latest.first.isZero())
01082     {
01083       common_time = std::min(latest.first, common_time);
01084     }
01085 
01086     std::vector<P_TimeAndFrameID>::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second));
01087     if (it != lct_cache.end()) // found a common parent
01088     {
01089       common_parent = it->second;
01090       break;
01091     }
01092 
01093     frame = latest.second;
01094 
01095     // Early out... source frame is a direct parent of the target frame
01096     if (frame == source_id)
01097     {
01098       time = common_time;
01099       if (time == ros::TIME_MAX)
01100       {
01101         time = ros::Time();
01102       }
01103       return tf2_msgs::TF2Error::NO_ERROR;
01104     }
01105 
01106     ++depth;
01107     if (depth > MAX_GRAPH_DEPTH)
01108     {
01109       if (error_string)
01110       {
01111         std::stringstream ss;
01112         ss<<"The tf tree is invalid because it contains a loop." << std::endl
01113           << allFramesAsStringNoLock() << std::endl;
01114         *error_string = ss.str();
01115       }
01116       return tf2_msgs::TF2Error::LOOKUP_ERROR;
01117     }
01118   }
01119 
01120   if (common_parent == 0)
01121   {
01122     createConnectivityErrorString(source_id, target_id, error_string);
01123     return tf2_msgs::TF2Error::CONNECTIVITY_ERROR;
01124   }
01125 
01126   // Loop through the source -> root list until we hit the common parent
01127   {
01128     std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
01129     std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
01130     for (; it != end; ++it)
01131     {
01132       if (!it->first.isZero())
01133       {
01134         common_time = std::min(common_time, it->first);
01135       }
01136 
01137       if (it->second == common_parent)
01138       {
01139         break;
01140       }
01141     }
01142   }
01143 
01144   if (common_time == ros::TIME_MAX)
01145   {
01146     common_time = ros::Time();
01147   }
01148 
01149   time = common_time;
01150   return tf2_msgs::TF2Error::NO_ERROR;
01151 }
01152 
01153 std::string BufferCore::allFramesAsYAML(double current_time) const
01154 {
01155   std::stringstream mstream;
01156   boost::mutex::scoped_lock lock(frame_mutex_);
01157 
01158   TransformStorage temp;
01159 
01160   if (frames_.size() ==1)
01161     mstream <<"{}";
01162 
01163   mstream.precision(3);
01164   mstream.setf(std::ios::fixed,std::ios::floatfield);
01165 
01166    //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
01167   for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
01168   {
01169     CompactFrameID cfid = CompactFrameID(counter);
01170     CompactFrameID frame_id_num;
01171     TimeCacheInterfacePtr cache = getFrame(cfid);
01172     if (!cache)
01173     {
01174       continue;
01175     }
01176 
01177     if(!cache->getData(ros::Time(), temp))
01178     {
01179       continue;
01180     }
01181 
01182     frame_id_num = temp.frame_id_;
01183 
01184     std::string authority = "no recorded authority";
01185     std::map<CompactFrameID, std::string>::const_iterator it = frame_authority_.find(cfid);
01186     if (it != frame_authority_.end()) {
01187       authority = it->second;
01188     }
01189 
01190     double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() -
01191                                                      cache->getOldestTimestamp().toSec() ), 0.0001);
01192 
01193     mstream << std::fixed; //fixed point notation
01194     mstream.precision(3); //3 decimal places
01195     mstream << frameIDs_reverse[cfid] << ": " << std::endl;
01196     mstream << "  parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl;
01197     mstream << "  broadcaster: '" << authority << "'" << std::endl;
01198     mstream << "  rate: " << rate << std::endl;
01199     mstream << "  most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl;
01200     mstream << "  oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl;
01201     if ( current_time > 0 ) {
01202       mstream << "  transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl;
01203     }
01204     mstream << "  buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl;
01205   }
01206 
01207   return mstream.str();
01208 }
01209 
01210 std::string BufferCore::allFramesAsYAML() const
01211 {
01212   return this->allFramesAsYAML(0.0);
01213 }
01214 
01215 TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb)
01216 {
01217   boost::mutex::scoped_lock lock(transformable_callbacks_mutex_);
01218   TransformableCallbackHandle handle = ++transformable_callbacks_counter_;
01219   while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second)
01220   {
01221     handle = ++transformable_callbacks_counter_;
01222   }
01223 
01224   return handle;
01225 }
01226 
01227 struct BufferCore::RemoveRequestByCallback
01228 {
01229   RemoveRequestByCallback(TransformableCallbackHandle handle)
01230   : handle_(handle)
01231   {}
01232 
01233   bool operator()(const TransformableRequest& req)
01234   {
01235     return req.cb_handle == handle_;
01236   }
01237 
01238   TransformableCallbackHandle handle_;
01239 };
01240 
01241 void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle)
01242 {
01243   {
01244     boost::mutex::scoped_lock lock(transformable_callbacks_mutex_);
01245     transformable_callbacks_.erase(handle);
01246   }
01247 
01248   {
01249     boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01250     V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle));
01251     transformable_requests_.erase(it, transformable_requests_.end());
01252   }
01253 }
01254 
01255 TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time)
01256 {
01257   // shortcut if target == source
01258   if (target_frame == source_frame)
01259   {
01260     return 0;
01261   }
01262 
01263   TransformableRequest req;
01264   req.target_id = lookupFrameNumber(target_frame);
01265   req.source_id = lookupFrameNumber(source_frame);
01266 
01267   // First check if the request is already transformable.  If it is, return immediately
01268   if (canTransformInternal(req.target_id, req.source_id, time, 0))
01269   {
01270     return 0;
01271   }
01272 
01273   // Might not be transformable at all, ever (if it's too far in the past)
01274   if (req.target_id && req.source_id)
01275   {
01276     ros::Time latest_time;
01277     // TODO: This is incorrect, but better than nothing.  Really we want the latest time for
01278     // any of the frames
01279     getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
01280     if (!latest_time.isZero() && time + cache_time_ < latest_time)
01281     {
01282       return 0xffffffffffffffffULL;
01283     }
01284   }
01285 
01286   req.cb_handle = handle;
01287   req.time = time;
01288   req.request_handle = ++transformable_requests_counter_;
01289   if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL)
01290   {
01291     req.request_handle = 1;
01292   }
01293 
01294   if (req.target_id == 0)
01295   {
01296     req.target_string = target_frame;
01297   }
01298 
01299   if (req.source_id == 0)
01300   {
01301     req.source_string = source_frame;
01302   }
01303 
01304   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01305   transformable_requests_.push_back(req);
01306 
01307   return req.request_handle;
01308 }
01309 
01310 struct BufferCore::RemoveRequestByID
01311 {
01312   RemoveRequestByID(TransformableRequestHandle handle)
01313   : handle_(handle)
01314   {}
01315 
01316   bool operator()(const TransformableRequest& req)
01317   {
01318     return req.request_handle == handle_;
01319   }
01320 
01321   TransformableCallbackHandle handle_;
01322 };
01323 
01324 void BufferCore::cancelTransformableRequest(TransformableRequestHandle handle)
01325 {
01326   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01327   V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle));
01328 
01329   if (it != transformable_requests_.end())
01330   {
01331     transformable_requests_.erase(it, transformable_requests_.end());
01332   }
01333 }
01334 
01335 
01336 
01337 // backwards compability for tf methods
01338 boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function<void(void)> callback)
01339 {
01340   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01341   return _transforms_changed_.connect(callback);
01342 }
01343 
01344 void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c)
01345 {
01346   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01347   c.disconnect();
01348 }
01349 
01350 
01351 bool BufferCore::_frameExists(const std::string& frame_id_str) const
01352 {
01353   boost::mutex::scoped_lock lock(frame_mutex_);
01354   return frameIDs_.count(frame_id_str);
01355 }
01356 
01357 bool BufferCore::_getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
01358 {
01359 
01360   boost::mutex::scoped_lock lock(frame_mutex_);
01361   CompactFrameID frame_number = lookupFrameNumber(frame_id);
01362   TimeCacheInterfacePtr frame = getFrame(frame_number);
01363 
01364   if (! frame)
01365     return false;
01366       
01367   CompactFrameID parent_id = frame->getParent(time, NULL);
01368   if (parent_id == 0)
01369     return false;
01370 
01371   parent = lookupFrameString(parent_id);
01372   return true;
01373 };
01374 
01375 void BufferCore::_getFrameStrings(std::vector<std::string> & vec) const
01376 {
01377   vec.clear();
01378 
01379   boost::mutex::scoped_lock lock(frame_mutex_);
01380 
01381   TransformStorage temp;
01382 
01383   //  for (std::vector< TimeCache*>::iterator  it = frames_.begin(); it != frames_.end(); ++it)
01384   for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++)
01385   {
01386     vec.push_back(frameIDs_reverse[counter]);
01387   }
01388   return;
01389 }
01390 
01391 
01392 
01393 
01394 void BufferCore::testTransformableRequests()
01395 {
01396   boost::mutex::scoped_lock lock(transformable_requests_mutex_);
01397   V_TransformableRequest::iterator it = transformable_requests_.begin();
01398 
01399   typedef boost::tuple<TransformableCallback&, TransformableRequestHandle, std::string,
01400                        std::string, ros::Time&, TransformableResult&> TransformableTuple;
01401   std::vector<TransformableTuple> transformables;
01402 
01403   for (; it != transformable_requests_.end();)
01404   {
01405     TransformableRequest& req = *it;
01406 
01407     // One or both of the frames may not have existed when the request was originally made.
01408     if (req.target_id == 0)
01409     {
01410       req.target_id = lookupFrameNumber(req.target_string);
01411     }
01412 
01413     if (req.source_id == 0)
01414     {
01415       req.source_id = lookupFrameNumber(req.source_string);
01416     }
01417 
01418     ros::Time latest_time;
01419     bool do_cb = false;
01420     TransformableResult result = TransformAvailable;
01421     // TODO: This is incorrect, but better than nothing.  Really we want the latest time for
01422     // any of the frames
01423     getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
01424     if (!latest_time.isZero() && req.time + cache_time_ < latest_time)
01425     {
01426       do_cb = true;
01427       result = TransformFailure;
01428     }
01429     else if (canTransformInternal(req.target_id, req.source_id, req.time, 0))
01430     {
01431       do_cb = true;
01432       result = TransformAvailable;
01433     }
01434 
01435     if (do_cb)
01436     {
01437       {
01438         boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_);
01439         M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle);
01440         if (it != transformable_callbacks_.end())
01441         {
01442           transformables.push_back(boost::make_tuple(boost::ref(it->second),
01443                                                      req.request_handle,
01444                                                      lookupFrameString(req.target_id),
01445                                                      lookupFrameString(req.source_id),
01446                                                      boost::ref(req.time),
01447                                                      boost::ref(result)));
01448         }
01449       }
01450 
01451       if (transformable_requests_.size() > 1)
01452       {
01453         transformable_requests_[it - transformable_requests_.begin()] = transformable_requests_.back();
01454       }
01455 
01456       transformable_requests_.erase(transformable_requests_.end() - 1);
01457     }
01458     else
01459     {
01460       ++it;
01461     }
01462   }
01463 
01464   // unlock before allowing possible user callbacks to avoid potential deadlock (#91)
01465   lock.unlock();
01466 
01467   BOOST_FOREACH (TransformableTuple tt, transformables)
01468   {
01469     tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>());
01470   }
01471 
01472   // Backwards compatability callback for tf
01473   _transforms_changed_();
01474 }
01475 
01476 
01477 std::string BufferCore::_allFramesAsDot(double current_time) const
01478 {
01479   std::stringstream mstream;
01480   mstream << "digraph G {" << std::endl;
01481   boost::mutex::scoped_lock lock(frame_mutex_);
01482 
01483   TransformStorage temp;
01484 
01485   if (frames_.size() == 1) {
01486     mstream <<"\"no tf data recieved\"";
01487   }
01488   mstream.precision(3);
01489   mstream.setf(std::ios::fixed,std::ios::floatfield);
01490 
01491   for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame
01492   {
01493     unsigned int frame_id_num;
01494     TimeCacheInterfacePtr counter_frame = getFrame(counter);
01495     if (!counter_frame) {
01496       continue;
01497     }
01498     if(!counter_frame->getData(ros::Time(), temp)) {
01499       continue;
01500     } else {
01501       frame_id_num = temp.frame_id_;
01502     }
01503     std::string authority = "no recorded authority";
01504     std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
01505     if (it != frame_authority_.end())
01506       authority = it->second;
01507 
01508     double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() -
01509                                                              counter_frame->getOldestTimestamp().toSec()), 0.0001);
01510 
01511     mstream << std::fixed; //fixed point notation
01512     mstream.precision(3); //3 decimal places
01513     mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
01514             << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
01515       //<< "Time: " << current_time.toSec() << "\\n"
01516             << "Broadcaster: " << authority << "\\n"
01517             << "Average rate: " << rate << " Hz\\n"
01518             << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" ";
01519     if (current_time > 0)
01520       mstream << "( "<<  current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)";
01521     mstream << "\\n"
01522       //    << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n"
01523       //    << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n"
01524       //    << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n"
01525             << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n"
01526             <<"\"];" <<std::endl;
01527   }
01528 
01529   for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
01530   {
01531     unsigned int frame_id_num;
01532     TimeCacheInterfacePtr counter_frame = getFrame(counter);
01533     if (!counter_frame) {
01534       if (current_time > 0) {
01535         mstream << "edge [style=invis];" <<std::endl;
01536         mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n"
01537                 << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "
01538                 << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
01539       }
01540       continue;
01541     }
01542     if (counter_frame->getData(ros::Time(), temp)) {
01543       frame_id_num = temp.frame_id_;
01544     } else {
01545         frame_id_num = 0;
01546     }
01547 
01548     if(frameIDs_reverse[frame_id_num]=="NO_PARENT")
01549     {
01550       mstream << "edge [style=invis];" <<std::endl;
01551       mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n";
01552       if (current_time > 0)
01553         mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n ";
01554       mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
01555     }
01556   }
01557   mstream << "}";
01558   return mstream.str();
01559 }
01560 
01561 std::string BufferCore::_allFramesAsDot() const
01562 {
01563   return _allFramesAsDot(0.0);
01564 }
01565 
01566 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
01567 {
01568   std::string error_string;
01569 
01570   output.clear(); //empty vector
01571 
01572   std::stringstream mstream;
01573   boost::mutex::scoped_lock lock(frame_mutex_);
01574 
01575   TransformAccum accum;
01576 
01577   // Get source frame/time using getFrame
01578   CompactFrameID source_id = lookupFrameNumber(source_frame);
01579   CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
01580   CompactFrameID target_id = lookupFrameNumber(target_frame);
01581 
01582   std::vector<CompactFrameID> source_frame_chain;
01583   int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain);
01584 
01585   if (retval != tf2_msgs::TF2Error::NO_ERROR)
01586   {
01587     switch (retval)
01588     {
01589     case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
01590       throw ConnectivityException(error_string);
01591     case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
01592       throw ExtrapolationException(error_string);
01593     case tf2_msgs::TF2Error::LOOKUP_ERROR:
01594       throw LookupException(error_string);
01595     default:
01596       logError("Unknown error code: %d", retval);
01597       assert(0);
01598     }
01599   }
01600   if (source_time != target_time)
01601   {
01602     std::vector<CompactFrameID> target_frame_chain;
01603     retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain);
01604 
01605     if (retval != tf2_msgs::TF2Error::NO_ERROR)
01606     {
01607       switch (retval)
01608       {
01609       case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
01610         throw ConnectivityException(error_string);
01611       case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
01612         throw ExtrapolationException(error_string);
01613       case tf2_msgs::TF2Error::LOOKUP_ERROR:
01614         throw LookupException(error_string);
01615       default:
01616         logError("Unknown error code: %d", retval);
01617         assert(0);
01618       }
01619     }
01620     int m = target_frame_chain.size()-1;
01621     int n = source_frame_chain.size()-1;
01622     for (; m >= 0 && n >= 0; --m, --n)
01623     {
01624       if (source_frame_chain[n] != target_frame_chain[m])
01625         break;
01626     }
01627     // Erase all duplicate items from frame_chain
01628     if (n > 0)
01629       source_frame_chain.erase(source_frame_chain.begin() + (n-1), source_frame_chain.end());
01630 
01631     if (m < target_frame_chain.size())
01632     {
01633       for (unsigned int i = 0; i <= m; ++i)
01634       {
01635         source_frame_chain.push_back(target_frame_chain[i]);
01636       }
01637     }
01638   }
01639 
01640   // Write each element of source_frame_chain as string
01641   for (unsigned int i = 0; i < source_frame_chain.size(); ++i)
01642   {
01643     output.push_back(lookupFrameString(source_frame_chain[i]));
01644  }
01645 }
01646 
01647 
01648 } // namespace tf2


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Oct 2 2017 02:24:34