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


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 27 2015 13:09:58