Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * transformer.cpp - Fawkes tf transformer (based on ROS tf) 00003 * 00004 * Created: Thu Oct 20 10:56:25 2011 00005 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00006 ****************************************************************************/ 00007 00008 /* This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. A runtime exception applies to 00012 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00020 */ 00021 00022 /* This code is based on ROS tf with the following copyright and license: 00023 * 00024 * Copyright (c) 2008, Willow Garage, Inc. 00025 * All rights reserved. 00026 * 00027 * Redistribution and use in source and binary forms, with or without 00028 * modification, are permitted provided that the following conditions are met: 00029 * 00030 * * Redistributions of source code must retain the above copyright 00031 * notice, this list of conditions and the following disclaimer. 00032 * * Redistributions in binary form must reproduce the above copyright 00033 * notice, this list of conditions and the following disclaimer in the 00034 * documentation and/or other materials provided with the distribution. 00035 * * Neither the name of the Willow Garage, Inc. nor the names of its 00036 * contributors may be used to endorse or promote products derived from 00037 * this software without specific prior written permission. 00038 * 00039 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00040 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00041 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00042 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00043 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00044 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00045 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00046 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00047 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00048 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00049 * POSSIBILITY OF SUCH DAMAGE. 00050 */ 00051 00052 #include <tf/transformer.h> 00053 #include <tf/time_cache.h> 00054 #include <tf/exceptions.h> 00055 00056 #include <core/threading/mutex_locker.h> 00057 #include <iostream> 00058 #include <sstream> 00059 #include <algorithm> 00060 00061 namespace fawkes { 00062 namespace tf { 00063 #if 0 /* just to make Emacs auto-indent happy */ 00064 } 00065 } 00066 #endif 00067 00068 /** @class Transformer <tf/transformer.h> 00069 * Coordinate transforms between any two frames in a system. 00070 * 00071 * This class provides a simple interface to allow recording and 00072 * lookup of relationships between arbitrary frames of the system. 00073 * 00074 * TF assumes that there is a tree of coordinate frame transforms 00075 * which define the relationship between all coordinate frames. For 00076 * example your typical robot would have a transform from global to 00077 * real world. And then from base to hand, and from base to head. 00078 * But Base to Hand really is composed of base to shoulder to elbow to 00079 * wrist to hand. TF is designed to take care of all the intermediate 00080 * steps for you. 00081 * 00082 * Internal Representation TF will store frames with the parameters 00083 * necessary for generating the transform into that frame from it's 00084 * parent and a reference to the parent frame. Frames are designated 00085 * using an std::string 0 is a frame without a parent (the top of a 00086 * tree) The positions of frames over time must be pushed in. 00087 * 00088 * All function calls which pass frame ids can potentially throw the 00089 * exception fawkes::tf::LookupException 00090 * 00091 * 00092 * @fn bool Transformer::is_enabled() const 00093 * Check if transformer is enabled. 00094 * @return true if enabled, false otherwise 00095 * 00096 * @var static const unsigned int Transformer::MAX_GRAPH_DEPTH 00097 * Maximum number of times to recurse before assuming the tree 00098 * has a loop 00099 * 00100 * @param static const int64_t Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE 00101 * The default amount of time to extrapolate. 00102 */ 00103 00104 /** The default amount of time to extrapolate. */ 00105 const float Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0.f; 00106 00107 /// Flag to advise accumulator finalization. 00108 enum WalkEnding 00109 { 00110 Identity, ///< identity 00111 TargetParentOfSource, ///< Target is parent of source 00112 SourceParentOfTarget, ///< Source is parent of target 00113 FullPath, ///< Full path between source and target. 00114 }; 00115 00116 /// Internal error values 00117 enum ErrorValues { 00118 NO_ERROR = 0, ///< No error occured. 00119 LOOKUP_ERROR, ///< Frame ID lookup error 00120 CONNECTIVITY_ERROR, ///< No connection between frames found 00121 EXTRAPOLATION_ERROR ///< Extrapolation required out of limits. 00122 }; 00123 00124 /** Accumulator to test for transformability. 00125 * Operations are basically noops. 00126 */ 00127 struct CanTransformAccum 00128 { 00129 /** Gather frame number. 00130 * @param cache cache 00131 * @param time time 00132 * @param error_string string containing error message in case of failure 00133 * @return parent frame number 00134 */ 00135 CompactFrameID 00136 gather(TimeCache* cache, fawkes::Time time, std::string* error_string) 00137 { 00138 return cache->get_parent(time, error_string); 00139 } 00140 00141 /** Accumulate. 00142 * @param source true if looking from source 00143 */ 00144 void accum(bool source) 00145 { 00146 } 00147 00148 /** Finalize accumulation. 00149 * @param end flag how the walk ended 00150 * @param _time time 00151 */ 00152 void finalize(WalkEnding end, fawkes::Time _time) 00153 { 00154 } 00155 00156 /// Transform storage to use. 00157 TransformStorage st; 00158 }; 00159 00160 /** Accumulator to accumulate transforms between two frames. 00161 */ 00162 struct TransformAccum 00163 { 00164 /** Constructor. */ 00165 TransformAccum() 00166 : source_to_top_quat(0.0, 0.0, 0.0, 1.0) 00167 , source_to_top_vec(0.0, 0.0, 0.0) 00168 , target_to_top_quat(0.0, 0.0, 0.0, 1.0) 00169 , target_to_top_vec(0.0, 0.0, 0.0) 00170 , result_quat(0.0, 0.0, 0.0, 1.0) 00171 , result_vec(0.0, 0.0, 0.0) 00172 { 00173 } 00174 00175 /** Gather frame number. 00176 * @param cache cache 00177 * @param time time 00178 * @param error_string string containing error message in case of failure 00179 * @return parent frame number 00180 */ 00181 CompactFrameID 00182 gather(TimeCache* cache, fawkes::Time time, std::string* error_string) 00183 { 00184 if (!cache->get_data(time, st, error_string)) 00185 { 00186 return 0; 00187 } 00188 00189 return st.frame_id_; 00190 } 00191 00192 /** Accumulate. 00193 * @param source true if looking from source 00194 */ 00195 void accum(bool source) 00196 { 00197 if (source) 00198 { 00199 source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_; 00200 source_to_top_quat = st.rotation_ * source_to_top_quat; 00201 } 00202 else 00203 { 00204 target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; 00205 target_to_top_quat = st.rotation_ * target_to_top_quat; 00206 } 00207 } 00208 00209 /** Finalize accumulation. 00210 * @param end flag how the walk ended 00211 * @param _time time 00212 */ 00213 void finalize(WalkEnding end, fawkes::Time _time) 00214 { 00215 switch (end) 00216 { 00217 case Identity: 00218 break; 00219 case TargetParentOfSource: 00220 result_vec = source_to_top_vec; 00221 result_quat = source_to_top_quat; 00222 break; 00223 case SourceParentOfTarget: 00224 { 00225 btQuaternion inv_target_quat = target_to_top_quat.inverse(); 00226 btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); 00227 result_vec = inv_target_vec; 00228 result_quat = inv_target_quat; 00229 break; 00230 } 00231 case FullPath: 00232 { 00233 btQuaternion inv_target_quat = target_to_top_quat.inverse(); 00234 btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); 00235 00236 result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec; 00237 result_quat = inv_target_quat * source_to_top_quat; 00238 } 00239 break; 00240 }; 00241 00242 time = _time; 00243 } 00244 00245 /// Transform storage. 00246 TransformStorage st; 00247 /// time stamp 00248 fawkes::Time time; 00249 /// Source to top accumulated quaternion 00250 btQuaternion source_to_top_quat; 00251 /// Source to top accumulated vector 00252 btVector3 source_to_top_vec; 00253 /// Target to top accumulated quaternion 00254 btQuaternion target_to_top_quat; 00255 /// Target to top accumulated vector 00256 btVector3 target_to_top_vec; 00257 00258 /// After finalize contains result quaternion. 00259 btQuaternion result_quat; 00260 /// After finalize contains result vector. 00261 btVector3 result_vec; 00262 }; 00263 00264 /** Constructor. 00265 * @param cache_time time in seconds to cache incoming transforms 00266 */ 00267 Transformer::Transformer(float cache_time) 00268 : cache_time_(cache_time), 00269 fall_back_to_wall_time_(false) 00270 { 00271 max_extrapolation_distance_ = DEFAULT_MAX_EXTRAPOLATION_DISTANCE; 00272 frameIDs_["NO_PARENT"] = 0; 00273 //unused but needed for iteration over all elements 00274 frames_.push_back(NULL); 00275 frameIDs_reverse.push_back("NO_PARENT"); 00276 00277 frame_mutex_ = new Mutex(); 00278 } 00279 00280 00281 /** Destructor. */ 00282 Transformer::~Transformer() 00283 { 00284 // deallocate all frames 00285 MutexLocker lock(frame_mutex_); 00286 std::vector<TimeCache*>::iterator cache_it; 00287 for (cache_it = frames_.begin(); cache_it != frames_.end(); ++cache_it) 00288 { 00289 delete (*cache_it); 00290 } 00291 00292 }; 00293 00294 00295 /** Set transformer enabled or disabled. 00296 * @param enabled true to enable, false to disable 00297 */ 00298 void 00299 Transformer::set_enabled(bool enabled) 00300 { 00301 enabled_ = enabled; 00302 } 00303 00304 /** Clear cached transforms. */ 00305 void 00306 Transformer::clear() 00307 { 00308 MutexLocker lock(frame_mutex_); 00309 if ( frames_.size() > 1 ) 00310 { 00311 std::vector< TimeCache*>::iterator cache_it; 00312 for (cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it) 00313 { 00314 (*cache_it)->clear_list(); 00315 } 00316 } 00317 } 00318 00319 00320 /** Check if frame exists. 00321 * @param frame_id_str frame ID 00322 * @result true if frame exists, false otherwise 00323 */ 00324 bool 00325 Transformer::frame_exists(const std::string& frame_id_str) const 00326 { 00327 MutexLocker lock(frame_mutex_); 00328 std::string frame_id_resolveped = /*assert_resolved(tf_prefix_,*/ frame_id_str; 00329 00330 return frameIDs_.count(frame_id_resolveped); 00331 } 00332 00333 void 00334 Transformer::create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const 00335 { 00336 if (!out) return; 00337 00338 *out = std::string("Could not find a connection between '"+lookup_frame_string(target_frame)+"' and '"+ 00339 lookup_frame_string(source_frame)+"' because they are not part of the same tree."+ 00340 "Tf has two or more unconnected trees."); 00341 } 00342 00343 00344 /** Walk from frame to top-parent of both. 00345 * @param f accumulator 00346 * @param time timestamp 00347 * @param target_id frame number of target 00348 * @param source_id frame number of source 00349 * @param error_string accumulated error string 00350 * @return error flag from ErrorValues 00351 */ 00352 template<typename F> 00353 int 00354 Transformer::walk_to_top_parent(F& f, fawkes::Time time, 00355 CompactFrameID target_id, CompactFrameID source_id, 00356 std::string* error_string) const 00357 { 00358 // Short circuit if zero length transform to allow lookups on non existant links 00359 if (source_id == target_id) 00360 { 00361 f.finalize(Identity, time); 00362 return NO_ERROR; 00363 } 00364 00365 //If getting the latest get the latest common time 00366 if (time == fawkes::Time(0,0)) 00367 { 00368 int retval = get_latest_common_time(target_id, source_id, time, error_string); 00369 if (retval != NO_ERROR) 00370 { 00371 return retval; 00372 } 00373 } 00374 00375 // Walk the tree to its root from the source frame, accumulating the transform 00376 CompactFrameID frame = source_id; 00377 CompactFrameID top_parent = frame; 00378 uint32_t depth = 0; 00379 while (frame != 0) 00380 { 00381 TimeCache* cache = get_frame(frame); 00382 00383 if (!cache) 00384 { 00385 // There will be no cache for the very root of the tree 00386 top_parent = frame; 00387 break; 00388 } 00389 00390 CompactFrameID parent = f.gather(cache, time, 0); 00391 if (parent == 0) 00392 { 00393 // Just break out here... there may still be a path from source -> target 00394 top_parent = frame; 00395 break; 00396 } 00397 00398 // Early out... target frame is a direct parent of the source frame 00399 if (frame == target_id) 00400 { 00401 f.finalize(TargetParentOfSource, time); 00402 return NO_ERROR; 00403 } 00404 00405 f.accum(true); 00406 00407 top_parent = frame; 00408 frame = parent; 00409 00410 ++depth; 00411 if (depth > MAX_GRAPH_DEPTH) 00412 { 00413 if (error_string) 00414 { 00415 std::stringstream ss; 00416 //ss << "The tf tree is invalid because it contains a loop." << std::endl 00417 // << allFramesAsString() << std::endl; 00418 *error_string = ss.str(); 00419 } 00420 return LOOKUP_ERROR; 00421 } 00422 } 00423 00424 // Now walk to the top parent from the target frame, accumulating its transform 00425 frame = target_id; 00426 depth = 0; 00427 while (frame != top_parent) 00428 { 00429 TimeCache* cache = get_frame(frame); 00430 00431 if (!cache) 00432 { 00433 break; 00434 } 00435 00436 CompactFrameID parent = f.gather(cache, time, error_string); 00437 if (parent == 0) 00438 { 00439 if (error_string) 00440 { 00441 std::stringstream ss; 00442 ss << "Looking up transform from frame [" << lookup_frame_string(source_id) << 00443 "] to frame [" << lookup_frame_string(target_id) << "] failed"; 00444 *error_string = ss.str(); 00445 } 00446 00447 return EXTRAPOLATION_ERROR; 00448 } 00449 00450 // Early out... source frame is a direct parent of the target frame 00451 if (frame == source_id) 00452 { 00453 f.finalize(SourceParentOfTarget, time); 00454 return NO_ERROR; 00455 } 00456 00457 f.accum(false); 00458 00459 frame = parent; 00460 00461 ++depth; 00462 if (depth > MAX_GRAPH_DEPTH) 00463 { 00464 if (error_string) 00465 { 00466 std::stringstream ss; 00467 //ss << "The tf tree is invalid because it contains a loop." << std::endl 00468 // << allFramesAsString() << std::endl; 00469 *error_string = "The tf tree is invalid because it contains a loop."; 00470 } 00471 return LOOKUP_ERROR; 00472 } 00473 } 00474 00475 if (frame != top_parent) 00476 { 00477 create_connectivity_error_string(source_id, target_id, error_string); 00478 return CONNECTIVITY_ERROR; 00479 } 00480 00481 f.finalize(FullPath, time); 00482 00483 return NO_ERROR; 00484 } 00485 00486 /** Time and frame ID comparator class. */ 00487 struct TimeAndFrameIDFrameComparator 00488 { 00489 /** Constructor. 00490 * @param id frame number to look for 00491 */ 00492 TimeAndFrameIDFrameComparator(CompactFrameID id) 00493 : id(id) 00494 {} 00495 00496 /** Application operator. 00497 * @param rhs right-hand side to compare to 00498 * @return true if the frame numbers are equal, false otherwise 00499 */ 00500 bool operator()(const P_TimeAndFrameID& rhs) const 00501 { 00502 return rhs.second == id; 00503 } 00504 00505 /// Frame number to look for. 00506 CompactFrameID id; 00507 }; 00508 00509 00510 /** Get latest common time of two frames. 00511 * @param target_id target frame number 00512 * @param source_id source frame number 00513 * @param time upon return contains latest common timestamp 00514 * @param error_string upon error contains accumulated error message. 00515 * @return value from ErrorValues 00516 */ 00517 int 00518 Transformer::get_latest_common_time(CompactFrameID target_id, CompactFrameID source_id, 00519 fawkes::Time & time, std::string *error_string) const 00520 { 00521 if (source_id == target_id) 00522 { 00523 //Set time to latest timestamp of frameid in case of target and source frame id are the same 00524 time.stamp(); 00525 return NO_ERROR; 00526 } 00527 00528 std::vector<P_TimeAndFrameID> lct_cache; 00529 00530 // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time 00531 // in the target is a direct parent 00532 CompactFrameID frame = source_id; 00533 P_TimeAndFrameID temp; 00534 uint32_t depth = 0; 00535 fawkes::Time common_time = fawkes::TIME_MAX; 00536 while (frame != 0) 00537 { 00538 TimeCache* cache = get_frame(frame); 00539 00540 if (!cache) { 00541 // There will be no cache for the very root of the tree 00542 break; 00543 } 00544 00545 P_TimeAndFrameID latest = cache->get_latest_time_and_parent(); 00546 00547 if (latest.second == 0) { 00548 // Just break out here... there may still be a path from source -> target 00549 break; 00550 } 00551 00552 if (!latest.first.is_zero()) { 00553 common_time = std::min(latest.first, common_time); 00554 } 00555 00556 lct_cache.push_back(latest); 00557 00558 frame = latest.second; 00559 00560 // Early out... target frame is a direct parent of the source frame 00561 if (frame == target_id) { 00562 time = common_time; 00563 if (time == fawkes::TIME_MAX) { 00564 time = fawkes::Time(); 00565 } 00566 return NO_ERROR; 00567 } 00568 00569 ++depth; 00570 if (depth > MAX_GRAPH_DEPTH) { 00571 if (error_string) { 00572 /* 00573 std::stringstream ss; 00574 ss<<"The tf tree is invalid because it contains a loop." << std::endl 00575 << allFramesAsString() << std::endl; 00576 *error_string = ss.str(); 00577 */ 00578 } 00579 return LOOKUP_ERROR; 00580 } 00581 } 00582 00583 // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent 00584 frame = target_id; 00585 depth = 0; 00586 common_time = fawkes::TIME_MAX; 00587 CompactFrameID common_parent = 0; 00588 while (true) 00589 { 00590 TimeCache* cache = get_frame(frame); 00591 00592 if (!cache) { 00593 break; 00594 } 00595 00596 P_TimeAndFrameID latest = cache->get_latest_time_and_parent(); 00597 00598 if (latest.second == 0) { 00599 break; 00600 } 00601 00602 if (!latest.first.is_zero()) { 00603 common_time = std::min(latest.first, common_time); 00604 } 00605 00606 std::vector<P_TimeAndFrameID>::iterator it = 00607 std::find_if(lct_cache.begin(), lct_cache.end(), 00608 TimeAndFrameIDFrameComparator(latest.second)); 00609 00610 if (it != lct_cache.end()) { // found a common parent 00611 common_parent = it->second; 00612 break; 00613 } 00614 00615 frame = latest.second; 00616 00617 // Early out... source frame is a direct parent of the target frame 00618 if (frame == source_id) { 00619 time = common_time; 00620 if (time == fawkes::TIME_MAX) { 00621 time = fawkes::Time(); 00622 } 00623 return NO_ERROR; 00624 } 00625 00626 ++depth; 00627 if (depth > MAX_GRAPH_DEPTH) { 00628 if (error_string) { 00629 std::stringstream ss; 00630 //ss<<"The tf tree is invalid because it contains a loop." << std::endl 00631 // << allFramesAsString() << std::endl; 00632 *error_string = ss.str(); 00633 } 00634 return LOOKUP_ERROR; 00635 } 00636 } 00637 00638 if (common_parent == 0) { 00639 create_connectivity_error_string(source_id, target_id, error_string); 00640 return CONNECTIVITY_ERROR; 00641 } 00642 00643 // Loop through the source -> root list until we hit the common parent 00644 { 00645 std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin(); 00646 std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end(); 00647 for (; it != end; ++it) { 00648 if (!it->first.is_zero()) { 00649 common_time = std::min(common_time, it->first); 00650 } 00651 00652 if (it->second == common_parent) { 00653 break; 00654 } 00655 } 00656 } 00657 00658 if (common_time == fawkes::TIME_MAX) { 00659 common_time = fawkes::Time(); 00660 } 00661 00662 time = common_time; 00663 return NO_ERROR; 00664 } 00665 00666 /** Get latest common time of two frames. 00667 * @param source_frame source frame ID 00668 * @param target_frame frame target frame ID 00669 * @param time upon return contains latest common timestamp 00670 * @param error_string upon error contains accumulated error message. 00671 * @return value from ErrorValues 00672 */ 00673 int 00674 Transformer::get_latest_common_time(const std::string &source_frame, 00675 const std::string &target_frame, 00676 fawkes::Time& time, 00677 std::string* error_string) const 00678 { 00679 std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame; 00680 std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame; 00681 00682 if (!frame_exists(mapped_tgt) || !frame_exists(mapped_src)) { 00683 time = fawkes::Time(); 00684 return LOOKUP_ERROR; 00685 } 00686 00687 CompactFrameID source_id = lookup_frame_number(mapped_src); 00688 CompactFrameID target_id = lookup_frame_number(mapped_tgt); 00689 return get_latest_common_time(source_id, target_id, time, error_string); 00690 } 00691 00692 /** Set a transform. 00693 * @param transform transform to set 00694 * @param authority authority from which the transform was received. 00695 * @return true on success, false otherwise 00696 */ 00697 bool 00698 Transformer::set_transform(const StampedTransform &transform, const std::string &authority) 00699 { 00700 StampedTransform mapped_transform((btTransform)transform, transform.stamp, 00701 transform.frame_id, transform.child_frame_id); 00702 mapped_transform.child_frame_id = /*assert_resolved(tf_prefix_, */ transform.child_frame_id; 00703 mapped_transform.frame_id = /*assert_resolved(tf_prefix_,*/ transform.frame_id; 00704 00705 00706 bool error_exists = false; 00707 if (mapped_transform.child_frame_id == mapped_transform.frame_id) 00708 { 00709 printf("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), mapped_transform.child_frame_id.c_str()); 00710 error_exists = true; 00711 } 00712 00713 if (mapped_transform.child_frame_id == "/")//empty frame id will be mapped to "/" 00714 { 00715 printf("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str()); 00716 error_exists = true; 00717 } 00718 00719 if (mapped_transform.frame_id == "/")//empty parent id will be mapped to "/" 00720 { 00721 printf("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", mapped_transform.child_frame_id.c_str(), authority.c_str()); 00722 error_exists = true; 00723 } 00724 00725 if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())|| 00726 std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w())) 00727 { 00728 printf("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)", 00729 mapped_transform.child_frame_id.c_str(), authority.c_str(), 00730 mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(), 00731 mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w() 00732 ); 00733 error_exists = true; 00734 } 00735 00736 if (error_exists) 00737 return false; 00738 00739 { 00740 MutexLocker lock(frame_mutex_); 00741 CompactFrameID frame_number = lookup_or_insert_frame_number(mapped_transform.child_frame_id); 00742 TimeCache* frame = get_frame(frame_number); 00743 if (frame == NULL) { 00744 frames_[frame_number] = new TimeCache(cache_time_); 00745 frame = frames_[frame_number]; 00746 } 00747 00748 if (frame->insert_data(TransformStorage(mapped_transform, lookup_or_insert_frame_number(mapped_transform.frame_id), frame_number))) 00749 { 00750 frame_authority_[frame_number] = authority; 00751 } else { 00752 //ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", mapped_transform.child_frame_id.c_str(), mapped_transform.stamp.toSec(), authority.c_str()); 00753 return false; 00754 } 00755 } 00756 00757 return true; 00758 }; 00759 00760 00761 /** An accessor to get a frame. 00762 * This is an internal function which will get the pointer to the 00763 * frame associated with the frame id 00764 * @param frame_number The frameID of the desired reference frame 00765 * @return time cache for given frame number 00766 * @exception LookupException thrown if lookup fails 00767 */ 00768 TimeCache * 00769 Transformer::get_frame(unsigned int frame_number) const 00770 { 00771 if (frame_number == 0) /// @todo check larger values too 00772 return NULL; 00773 else 00774 return frames_[frame_number]; 00775 } 00776 00777 00778 /** An accessor to get access to the frame cache. 00779 * @param frame_id frame ID 00780 * @return time cache for given frame ID 00781 * @exception LookupException thrown if lookup fails 00782 */ 00783 const TimeCache * 00784 Transformer::get_frame_cache(const std::string &frame_id) const 00785 { 00786 CompactFrameID frame_number = lookup_frame_number(frame_id); 00787 if (frame_number == 0) { 00788 throw LookupException("Failed to lookup frame %s", frame_id.c_str()); 00789 } else { 00790 return frames_[frame_number]; 00791 } 00792 } 00793 00794 00795 /** String to number for frame lookup. 00796 * @param frameid_str frame ID 00797 * @return frame number 00798 * @exception LookupException if frame ID is unknown 00799 */ 00800 CompactFrameID 00801 Transformer::lookup_frame_number(const std::string &frameid_str) const 00802 { 00803 unsigned int retval = 0; 00804 //MutexLocker lock(frame_mutex_); 00805 M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str); 00806 if (map_it == frameIDs_.end()) 00807 { 00808 throw LookupException("Frame id %s does not exist! Frames (%zu): %s", 00809 frameid_str.c_str(), frameIDs_.size(), "" /*allFramesAsString()*/); 00810 } 00811 else 00812 retval = map_it->second; 00813 return retval; 00814 }; 00815 00816 00817 /** String to number for frame lookup with dynamic allocation of new 00818 * frames. 00819 * @param frameid_str frame ID 00820 * @return frame number, if none existed for the given frame ID a new 00821 * one is created 00822 */ 00823 CompactFrameID 00824 Transformer::lookup_or_insert_frame_number(const std::string &frameid_str) 00825 { 00826 unsigned int retval = 0; 00827 //MutexLocker lock(frame_mutex_); 00828 M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str); 00829 if (map_it == frameIDs_.end()) 00830 { 00831 retval = frames_.size(); 00832 frameIDs_[frameid_str] = retval; 00833 frames_.push_back( new TimeCache(cache_time_)); 00834 frameIDs_reverse.push_back(frameid_str); 00835 } 00836 else 00837 retval = frameIDs_[frameid_str]; 00838 return retval; 00839 }; 00840 00841 /** Get frame ID given its number. 00842 * @param frame_num frame number 00843 * @return frame ID string 00844 * @exception LookupException thrown if number invalid 00845 */ 00846 std::string 00847 Transformer::lookup_frame_string(unsigned int frame_num) const 00848 { 00849 if (frame_num >= frameIDs_reverse.size()) { 00850 throw LookupException("Reverse lookup of frame id %u failed!", frame_num); 00851 } 00852 else return frameIDs_reverse[frame_num]; 00853 } 00854 00855 00856 /** Lookup transform. 00857 * @param target_frame target frame ID 00858 * @param source_frame source frame ID 00859 * @param time time for which to get the transform, set to (0,0) to get latest 00860 * common time frame 00861 * @param transform upon return contains the transform 00862 * @exception ConnectivityException thrown if no connection between 00863 * the source and target frame could be found in the tree. 00864 * @exception ExtrapolationException returning a value would have 00865 * required extrapolation beyond current limits. 00866 * @exception LookupException at least one of the two given frames is 00867 * unknown 00868 */ 00869 void Transformer::lookup_transform(const std::string& target_frame, 00870 const std::string& source_frame, 00871 const fawkes::Time& time, 00872 StampedTransform& transform) const 00873 { 00874 if (! enabled_) { 00875 throw DisabledException("Transformer has been disabled"); 00876 } 00877 00878 std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame; 00879 std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame; 00880 00881 if (mapped_tgt == mapped_src) { 00882 transform.setIdentity(); 00883 transform.child_frame_id = mapped_src; 00884 transform.frame_id = mapped_tgt; 00885 transform.stamp = fawkes::Time(); 00886 return; 00887 } 00888 00889 MutexLocker lock(frame_mutex_); 00890 00891 CompactFrameID target_id = lookup_frame_number(mapped_tgt); 00892 CompactFrameID source_id = lookup_frame_number(mapped_src); 00893 00894 std::string error_string; 00895 TransformAccum accum; 00896 int rv = walk_to_top_parent(accum, time, target_id, source_id, &error_string); 00897 if (rv != NO_ERROR) 00898 { 00899 switch (rv) 00900 { 00901 case CONNECTIVITY_ERROR: 00902 throw ConnectivityException("Connectivity: %s", error_string.c_str()); 00903 case EXTRAPOLATION_ERROR: 00904 throw ExtrapolationException("Extrapolation: %s", error_string.c_str()); 00905 case LOOKUP_ERROR: 00906 throw LookupException("Lookup: %s", error_string.c_str()); 00907 default: 00908 throw Exception("lookup_transform: unknown error code: %d", rv); 00909 break; 00910 } 00911 } 00912 00913 transform.setOrigin(accum.result_vec); 00914 transform.setRotation(accum.result_quat); 00915 transform.child_frame_id = mapped_src; 00916 transform.frame_id = mapped_tgt; 00917 transform.stamp = accum.time; 00918 } 00919 00920 00921 /** Lookup transform at latest common time. 00922 * @param target_frame target frame ID 00923 * @param source_frame source frame ID 00924 * @param transform upon return contains the transform 00925 * @exception ConnectivityException thrown if no connection between 00926 * the source and target frame could be found in the tree. 00927 * @exception ExtrapolationException returning a value would have 00928 * required extrapolation beyond current limits. 00929 * @exception LookupException at least one of the two given frames is 00930 * unknown 00931 */ 00932 void Transformer::lookup_transform(const std::string& target_frame, 00933 const std::string& source_frame, 00934 StampedTransform& transform) const 00935 { 00936 lookup_transform(target_frame, source_frame, fawkes::Time(0,0), transform); 00937 } 00938 00939 00940 /** Lookup transform assuming a fixed frame. 00941 * This will lookup a transformation from source to target, assuming 00942 * that there is a fixed frame, by first finding the transform of the 00943 * source frame in the fixed frame, and then a transformation from the 00944 * fixed frame to the target frame. 00945 * @param target_frame target frame ID 00946 * @param target_time time for the target frame 00947 * @param source_frame source frame ID 00948 * @param source_time time in the source frame 00949 * @param fixed_frame ID of fixed frame 00950 * @param transform upon return contains the transform 00951 * @exception ConnectivityException thrown if no connection between 00952 * the source and target frame could be found in the tree. 00953 * @exception ExtrapolationException returning a value would have 00954 * required extrapolation beyond current limits. 00955 * @exception LookupException at least one of the two given frames is 00956 * unknown 00957 */ 00958 void 00959 Transformer::lookup_transform(const std::string& target_frame, 00960 const fawkes::Time& target_time, 00961 const std::string& source_frame, 00962 const fawkes::Time& source_time, 00963 const std::string& fixed_frame, 00964 StampedTransform& transform) const 00965 { 00966 StampedTransform temp1, temp2; 00967 lookup_transform(fixed_frame, source_frame, source_time, temp1); 00968 lookup_transform(target_frame, fixed_frame, target_time, temp2); 00969 transform.set_data(temp2 * temp1); 00970 transform.stamp = temp2.stamp; 00971 transform.frame_id = target_frame; 00972 transform.child_frame_id = source_frame; 00973 } 00974 00975 /** Test if a transform is possible. 00976 * @param target_frame The frame into which to transform 00977 * @param source_frame The frame from which to transform 00978 * @param time The time at which to transform 00979 * @return true if the transformation can be calculated, false otherwise 00980 */ 00981 bool 00982 Transformer::can_transform(const std::string& target_frame, 00983 const std::string& source_frame, 00984 const fawkes::Time& time) const 00985 { 00986 std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame; 00987 std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame; 00988 00989 if (mapped_tgt == mapped_src) return true; 00990 00991 if (!frame_exists(mapped_tgt) || !frame_exists(mapped_src)) return false; 00992 00993 MutexLocker lock(frame_mutex_); 00994 CompactFrameID target_id = lookup_frame_number(mapped_tgt); 00995 CompactFrameID source_id = lookup_frame_number(mapped_src); 00996 00997 return can_transform_no_lock(target_id, source_id, time); 00998 } 00999 01000 01001 /** Test if a transform is possible. 01002 * @param target_frame The frame into which to transform 01003 * @param target_time The time into which to transform 01004 * @param source_frame The frame from which to transform 01005 * @param source_time The time from which to transform 01006 * @param fixed_frame The frame in which to treat the transform as 01007 * constant in time 01008 * @return true if the transformation can be calculated, false otherwise 01009 */ 01010 bool 01011 Transformer::can_transform(const std::string& target_frame, 01012 const fawkes::Time& target_time, 01013 const std::string& source_frame, 01014 const fawkes::Time& source_time, 01015 const std::string& fixed_frame) const 01016 { 01017 return 01018 can_transform(target_frame, fixed_frame, target_time) && 01019 can_transform(fixed_frame, source_frame, source_time); 01020 } 01021 01022 01023 /** Test if a transform is possible. 01024 * Internal check that does not lock the frame mutex. 01025 * @param target_id The frame number into which to transform 01026 * @param source_id The frame number from which to transform 01027 * @param time The time at which to transform 01028 * @return true if the transformation can be calculated, false otherwise 01029 */ 01030 bool 01031 Transformer::can_transform_no_lock(CompactFrameID target_id, 01032 CompactFrameID source_id, 01033 const fawkes::Time& time) const 01034 { 01035 if (target_id == 0 || source_id == 0) return false; 01036 01037 CanTransformAccum accum; 01038 if (walk_to_top_parent(accum, time, target_id, source_id, NULL) == NO_ERROR) 01039 return true; 01040 else 01041 return false; 01042 } 01043 01044 01045 /** Transform a stamped Quaternion into the target frame. 01046 * This transforms the quaternion relative to the frame set in the 01047 * stamped quaternion into the target frame. 01048 * @param target_frame frame into which to transform 01049 * @param stamped_in stamped quaternion, defines source frame and time 01050 * @param stamped_out stamped output quaternion in target_frame 01051 * @exception ConnectivityException thrown if no connection between 01052 * the source and target frame could be found in the tree. 01053 * @exception ExtrapolationException returning a value would have 01054 * required extrapolation beyond current limits. 01055 * @exception LookupException at least one of the two given frames is 01056 * unknown 01057 * @exception InvalidArgument thrown if the Quaternion is invalid, most 01058 * likely an uninitialized Quaternion (0,0,0,0). 01059 */ 01060 void 01061 Transformer::transform_quaternion(const std::string& target_frame, 01062 const Stamped<Quaternion>& stamped_in, 01063 Stamped<Quaternion>& stamped_out) const 01064 { 01065 assert_quaternion_valid(stamped_in); 01066 01067 StampedTransform transform; 01068 lookup_transform(target_frame, stamped_in.frame_id, 01069 stamped_in.stamp, transform); 01070 01071 stamped_out.set_data(transform * stamped_in); 01072 stamped_out.stamp = transform.stamp; 01073 stamped_out.frame_id = target_frame; 01074 } 01075 01076 /** Transform a stamped vector into the target frame. 01077 * This transforms the vector given relative to the frame set in the 01078 * stamped vector into the target frame. 01079 * @param target_frame frame into which to transform 01080 * @param stamped_in stamped vector, defines source frame and time 01081 * @param stamped_out stamped output vector in target_frame 01082 * @exception ConnectivityException thrown if no connection between 01083 * the source and target frame could be found in the tree. 01084 * @exception ExtrapolationException returning a value would have 01085 * required extrapolation beyond current limits. 01086 * @exception LookupException at least one of the two given frames is 01087 * unknown 01088 */ 01089 void 01090 Transformer::transform_vector(const std::string& target_frame, 01091 const Stamped<Vector3>& stamped_in, 01092 Stamped<Vector3>& stamped_out) const 01093 { 01094 StampedTransform transform; 01095 lookup_transform(target_frame, stamped_in.frame_id, 01096 stamped_in.stamp, transform); 01097 01098 // may not be most efficient 01099 Vector3 end = stamped_in; 01100 Vector3 origin = Vector3(0,0,0); 01101 Vector3 output = (transform * end) - (transform * origin); 01102 stamped_out.set_data(output); 01103 01104 stamped_out.stamp = transform.stamp; 01105 stamped_out.frame_id = target_frame; 01106 } 01107 01108 /** Transform a stamped point into the target frame. 01109 * This transforms the point given relative to the frame set in the 01110 * stamped point into the target frame. 01111 * @param target_frame frame into which to transform 01112 * @param stamped_in stamped point, defines source frame and time 01113 * @param stamped_out stamped output point in target_frame 01114 * @exception ConnectivityException thrown if no connection between 01115 * the source and target frame could be found in the tree. 01116 * @exception ExtrapolationException returning a value would have 01117 * required extrapolation beyond current limits. 01118 * @exception LookupException at least one of the two given frames is 01119 * unknown 01120 */ 01121 void 01122 Transformer::transform_point(const std::string& target_frame, 01123 const Stamped<Point>& stamped_in, 01124 Stamped<Point>& stamped_out) const 01125 { 01126 StampedTransform transform; 01127 lookup_transform(target_frame, stamped_in.frame_id, 01128 stamped_in.stamp, transform); 01129 01130 stamped_out.set_data(transform * stamped_in); 01131 stamped_out.stamp = transform.stamp; 01132 stamped_out.frame_id = target_frame; 01133 01134 } 01135 01136 /** Transform a stamped pose into the target frame. 01137 * This transforms the pose given relative to the frame set in the 01138 * stamped vector into the target frame. 01139 * @param target_frame frame into which to transform 01140 * @param stamped_in stamped pose, defines source frame and time 01141 * @param stamped_out stamped output pose in target_frame 01142 * @exception ConnectivityException thrown if no connection between 01143 * the source and target frame could be found in the tree. 01144 * @exception ExtrapolationException returning a value would have 01145 * required extrapolation beyond current limits. 01146 * @exception LookupException at least one of the two given frames is 01147 * unknown 01148 */ 01149 void 01150 Transformer::transform_pose(const std::string& target_frame, 01151 const Stamped<Pose>& stamped_in, 01152 Stamped<Pose>& stamped_out) const 01153 { 01154 StampedTransform transform; 01155 lookup_transform(target_frame, stamped_in.frame_id, 01156 stamped_in.stamp, transform); 01157 01158 stamped_out.set_data(transform * stamped_in); 01159 stamped_out.stamp = transform.stamp; 01160 stamped_out.frame_id = target_frame; 01161 } 01162 01163 01164 /** Transform a stamped Quaternion into the target frame assuming a fixed frame. 01165 * This transforms the quaternion relative to the frame set in the 01166 * stamped quaternion into the target frame. 01167 * This will transform the quaternion from source to target, assuming 01168 * that there is a fixed frame, by first finding the transform of the 01169 * source frame in the fixed frame, and then a transformation from the 01170 * fixed frame into the target frame. 01171 * @param target_frame frame into which to transform 01172 * @param target_time desired time in the target frame 01173 * @param stamped_in stamped quaternion, defines source frame and time 01174 * @param fixed_frame ID of fixed frame 01175 * @param stamped_out stamped output quaternion in target_frame 01176 * @exception ConnectivityException thrown if no connection between 01177 * the source and target frame could be found in the tree. 01178 * @exception ExtrapolationException returning a value would have 01179 * required extrapolation beyond current limits. 01180 * @exception LookupException at least one of the two given frames is 01181 * unknown 01182 * @exception InvalidArgument thrown if the Quaternion is invalid, most 01183 * likely an uninitialized Quaternion (0,0,0,0). 01184 */ 01185 void 01186 Transformer::transform_quaternion(const std::string& target_frame, 01187 const fawkes::Time& target_time, 01188 const Stamped<Quaternion>& stamped_in, 01189 const std::string& fixed_frame, 01190 Stamped<Quaternion>& stamped_out) const 01191 { 01192 assert_quaternion_valid(stamped_in); 01193 StampedTransform transform; 01194 lookup_transform(target_frame, target_time, 01195 stamped_in.frame_id, stamped_in.stamp, 01196 fixed_frame, transform); 01197 01198 stamped_out.set_data(transform * stamped_in); 01199 stamped_out.stamp = transform.stamp; 01200 stamped_out.frame_id = target_frame; 01201 } 01202 01203 /** Transform a stamped vector into the target frame assuming a fixed frame. 01204 * This transforms the vector relative to the frame set in the 01205 * stamped quaternion into the target frame. 01206 * This will transform the vector from source to target, assuming 01207 * that there is a fixed frame, by first finding the transform of the 01208 * source frame in the fixed frame, and then a transformation from the 01209 * fixed frame into the target frame. 01210 * @param target_frame frame into which to transform 01211 * @param target_time desired time in the target frame 01212 * @param stamped_in stamped vector, defines source frame and time 01213 * @param fixed_frame ID of fixed frame 01214 * @param stamped_out stamped output vector in target_frame 01215 * @exception ConnectivityException thrown if no connection between 01216 * the source and target frame could be found in the tree. 01217 * @exception ExtrapolationException returning a value would have 01218 * required extrapolation beyond current limits. 01219 * @exception LookupException at least one of the two given frames is 01220 * unknown 01221 */ 01222 void 01223 Transformer::transform_vector(const std::string& target_frame, 01224 const fawkes::Time& target_time, 01225 const Stamped<Vector3>& stamped_in, 01226 const std::string& fixed_frame, 01227 Stamped<Vector3>& stamped_out) const 01228 { 01229 StampedTransform transform; 01230 lookup_transform(target_frame, target_time, 01231 stamped_in.frame_id,stamped_in.stamp, 01232 fixed_frame, transform); 01233 01234 // may not be most efficient 01235 Vector3 end = stamped_in; 01236 Vector3 origin(0,0,0); 01237 Vector3 output = (transform * end) - (transform * origin); 01238 stamped_out.set_data( output); 01239 01240 stamped_out.stamp = transform.stamp; 01241 stamped_out.frame_id = target_frame; 01242 01243 } 01244 01245 /** Transform a stamped point into the target frame assuming a fixed frame. 01246 * This transforms the point relative to the frame set in the 01247 * stamped quaternion into the target frame. 01248 * This will transform the point from source to target, assuming 01249 * that there is a fixed frame, by first finding the transform of the 01250 * source frame in the fixed frame, and then a transformation from the 01251 * fixed frame into the target frame. 01252 * @param target_frame frame into which to transform 01253 * @param target_time desired time in the target frame 01254 * @param stamped_in stamped point, defines source frame and time 01255 * @param fixed_frame ID of fixed frame 01256 * @param stamped_out stamped output point in target_frame 01257 * @exception ConnectivityException thrown if no connection between 01258 * the source and target frame could be found in the tree. 01259 * @exception ExtrapolationException returning a value would have 01260 * required extrapolation beyond current limits. 01261 * @exception LookupException at least one of the two given frames is 01262 * unknown 01263 */ 01264 void 01265 Transformer::transform_point(const std::string& target_frame, 01266 const fawkes::Time& target_time, 01267 const Stamped<Point>& stamped_in, 01268 const std::string& fixed_frame, 01269 Stamped<Point>& stamped_out) const 01270 { 01271 StampedTransform transform; 01272 lookup_transform(target_frame, target_time, 01273 stamped_in.frame_id, stamped_in.stamp, 01274 fixed_frame, transform); 01275 01276 stamped_out.set_data(transform * stamped_in); 01277 stamped_out.stamp = transform.stamp; 01278 stamped_out.frame_id = target_frame; 01279 } 01280 01281 /** Transform a stamped pose into the target frame assuming a fixed frame. 01282 * This transforms the pose relative to the frame set in the 01283 * stamped quaternion into the target frame. 01284 * This will transform the pose from source to target, assuming 01285 * that there is a fixed frame, by first finding the transform of the 01286 * source frame in the fixed frame, and then a transformation from the 01287 * fixed frame into the target frame. 01288 * @param target_frame frame into which to transform 01289 * @param target_time desired time in the target frame 01290 * @param stamped_in stamped pose, defines source frame and time 01291 * @param fixed_frame ID of fixed frame 01292 * @param stamped_out stamped output pose in target_frame 01293 * @exception ConnectivityException thrown if no connection between 01294 * the source and target frame could be found in the tree. 01295 * @exception ExtrapolationException returning a value would have 01296 * required extrapolation beyond current limits. 01297 * @exception LookupException at least one of the two given frames is 01298 * unknown 01299 */ 01300 void 01301 Transformer::transform_pose(const std::string& target_frame, 01302 const fawkes::Time& target_time, 01303 const Stamped<Pose>& stamped_in, 01304 const std::string& fixed_frame, 01305 Stamped<Pose>& stamped_out) const 01306 { 01307 StampedTransform transform; 01308 lookup_transform(target_frame, target_time, 01309 stamped_in.frame_id, stamped_in.stamp, 01310 fixed_frame, transform); 01311 01312 stamped_out.set_data(transform * stamped_in); 01313 stamped_out.stamp = transform.stamp; 01314 stamped_out.frame_id = target_frame; 01315 } 01316 01317 } // end namespace tf 01318 } // end namespace fawkes