00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
00206
00207
00208
00209
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
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
00323 if (source_id == target_id)
00324 {
00325 f.finalize(Identity, time);
00326 return tf2_msgs::TF2Error::NO_ERROR;
00327 }
00328
00329
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
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
00356 top_parent = frame;
00357 break;
00358 }
00359
00360 CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string);
00361 if (parent == 0)
00362 {
00363
00364 top_parent = frame;
00365 extrapolation_might_have_occurred = true;
00366 break;
00367 }
00368
00369
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
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
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
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
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
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
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
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
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());
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
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
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
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
01003
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
01015 break;
01016 }
01017
01018 P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
01019
01020 if (latest.second == 0)
01021 {
01022
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
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
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())
01088 {
01089 common_parent = it->second;
01090 break;
01091 }
01092
01093 frame = latest.second;
01094
01095
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
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
01167 for (unsigned int counter = 1; counter < frames_.size(); counter ++)
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;
01194 mstream.precision(3);
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
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
01268 if (canTransformInternal(req.target_id, req.source_id, time, 0))
01269 {
01270 return 0;
01271 }
01272
01273
01274 if (req.target_id && req.source_id)
01275 {
01276 ros::Time latest_time;
01277
01278
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
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
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
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
01422
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
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
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 ++)
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;
01512 mstream.precision(3);
01513 mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
01514 << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
01515
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
01523
01524
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 ++)
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();
01571
01572 std::stringstream mstream;
01573 boost::mutex::scoped_lock lock(frame_mutex_);
01574
01575 TransformAccum accum;
01576
01577
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
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
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 }