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 "tf/tf.h"
00033 #include <sys/time.h>
00034 #include "ros/assert.h"
00035 #include "ros/ros.h"
00036 #include "angles/angles.h"
00037
00038 using namespace tf;
00039
00040
00041
00042
00043 const double tf::Transformer::DEFAULT_CACHE_TIME;
00044
00045 std::string assert_resolved(const std::string& prefix, const std::string& frame_id)
00046 {
00047 if (frame_id.size() > 0)
00048 if (frame_id[0] != '/')
00049 ROS_DEBUG("TF operating on not fully resolved frame id %s, resolving using local prefix %s", frame_id.c_str(), prefix.c_str());
00050 return tf::resolve(prefix, frame_id);
00051 };
00052
00053 std::string tf::resolve(const std::string& prefix, const std::string& frame_name)
00054 {
00055
00056 if (frame_name.size() > 0)
00057 if (frame_name[0] == '/')
00058 {
00059 return frame_name;
00060 }
00061 if (prefix.size() > 0)
00062 {
00063 if (prefix[0] == '/')
00064 {
00065 std::string composite = prefix;
00066 composite.append("/");
00067 composite.append(frame_name);
00068 return composite;
00069 }
00070 else
00071 {
00072 std::string composite;
00073 composite = "/";
00074 composite.append(prefix);
00075 composite.append("/");
00076 composite.append(frame_name);
00077 return composite;
00078 }
00079
00080 }
00081 else
00082 {
00083 std::string composite;
00084 composite = "/";
00085 composite.append(frame_name);
00086 return composite;
00087 }
00088 };
00089
00090
00091
00092 Transformer::Transformer(bool interpolating,
00093 ros::Duration cache_time):
00094 cache_time(cache_time),
00095 interpolating (interpolating),
00096 using_dedicated_thread_(false),
00097 fall_back_to_wall_time_(false)
00098 {
00099 max_extrapolation_distance_.fromNSec(DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
00100 frameIDs_["NO_PARENT"] = 0;
00101 frames_.push_back(NULL);
00102 frameIDs_reverse.push_back("NO_PARENT");
00103
00104 return;
00105 }
00106
00107 Transformer::~Transformer()
00108 {
00109
00110 boost::mutex::scoped_lock(frame_mutex_);
00111 for (std::vector<TimeCache*>::iterator cache_it = frames_.begin(); cache_it != frames_.end(); ++cache_it)
00112 {
00113 delete (*cache_it);
00114 }
00115
00116 };
00117
00118
00119 void Transformer::clear()
00120 {
00121 boost::mutex::scoped_lock(frame_mutex_);
00122 if ( frames_.size() > 1 )
00123 {
00124 for (std::vector< TimeCache*>::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
00125 {
00126 (*cache_it)->clearList();
00127 }
00128 }
00129 }
00130
00131
00132 bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority)
00133 {
00134
00135 StampedTransform mapped_transform((btTransform)transform, transform.stamp_, transform.frame_id_, transform.child_frame_id_);
00136 mapped_transform.child_frame_id_ = assert_resolved(tf_prefix_, transform.child_frame_id_);
00137 mapped_transform.frame_id_ = assert_resolved(tf_prefix_, transform.frame_id_);
00138
00139
00140 bool error_exists = false;
00141 if (mapped_transform.child_frame_id_ == mapped_transform.frame_id_)
00142 {
00143 ROS_ERROR("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());
00144 error_exists = true;
00145 }
00146
00147 if (mapped_transform.child_frame_id_ == "/")
00148 {
00149 ROS_ERROR("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
00150 error_exists = true;
00151 }
00152
00153 if (mapped_transform.frame_id_ == "/")
00154 {
00155 ROS_ERROR("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());
00156 error_exists = true;
00157 }
00158
00159 if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
00160 std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w()))
00161 {
00162 ROS_ERROR("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)",
00163 mapped_transform.child_frame_id_.c_str(), authority.c_str(),
00164 mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
00165 mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
00166 );
00167 error_exists = true;
00168 }
00169
00170 if (error_exists)
00171 return false;
00172 unsigned int frame_number = lookupOrInsertFrameNumber(mapped_transform.child_frame_id_);
00173 if (getFrame(frame_number)->insertData(TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.frame_id_))))
00174 {
00175 frame_authority_[frame_number] = authority;
00176 }
00177 else
00178 {
00179 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());
00180 return false;
00181 }
00182
00183 {
00184 boost::mutex::scoped_lock lock(transforms_changed_mutex_);
00185 transforms_changed_();
00186 }
00187
00188 return true;
00189 };
00190
00191
00192 void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
00193 const ros::Time& time, StampedTransform& transform) const
00194 {
00195 std::string mapped_target_frame = assert_resolved(tf_prefix_, target_frame);
00196 std::string mapped_source_frame = assert_resolved(tf_prefix_, source_frame);
00197
00198
00199 if (mapped_source_frame == mapped_target_frame)
00200 {
00201 transform.setIdentity();
00202
00203 if (time == ros::Time())
00204 transform.stamp_ = now();
00205 else
00206 transform.stamp_ = time;
00207
00208 transform.child_frame_id_ = mapped_source_frame;
00209 transform.frame_id_ = mapped_target_frame;
00210 return;
00211 }
00212
00213
00214 int retval = NO_ERROR;
00215 ros::Time temp_time;
00216 std::string error_string;
00217
00218 if (time == ros::Time())
00219 retval = getLatestCommonTime(mapped_target_frame, mapped_source_frame, temp_time, &error_string);
00220 else
00221 temp_time = time;
00222
00223 TransformLists t_list;
00224
00225 if (retval == NO_ERROR)
00226 try
00227 {
00228 retval = lookupLists(lookupFrameNumber( mapped_target_frame), temp_time, lookupFrameNumber( mapped_source_frame), t_list, &error_string);
00229 }
00230 catch (tf::LookupException &ex)
00231 {
00232 error_string = ex.what();
00233 retval = LOOKUP_ERROR;
00234 }
00235 if (retval != NO_ERROR)
00236 {
00237 std::stringstream ss;
00238 ss << " When trying to transform between " << mapped_source_frame << " and " << mapped_target_frame <<".";
00239 if (retval == LOOKUP_ERROR)
00240 throw LookupException(error_string + ss.str());
00241 if (retval == CONNECTIVITY_ERROR)
00242 throw ConnectivityException(error_string + ss.str());
00243 }
00244
00245 if (test_extrapolation(temp_time, t_list, &error_string))
00246 {
00247 std::stringstream ss;
00248 if (time == ros::Time())
00249 {
00250 ss << "Could not find a common time " << mapped_source_frame << " and " << mapped_target_frame <<".";
00251 throw ConnectivityException(ss.str());
00252 }
00253 else
00254 {
00255 ss << " When trying to transform between " << mapped_source_frame << " and " << mapped_target_frame <<"."<< std::endl;
00256 throw ExtrapolationException(error_string + ss.str());
00257 }
00258 }
00259
00260
00261 transform.setData( computeTransformFromList(t_list));
00262 transform.stamp_ = temp_time;
00263 transform.frame_id_ = target_frame;
00264 transform.child_frame_id_ = source_frame;
00265
00266 };
00267
00268
00269 void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00270 const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
00271 {
00272 tf::StampedTransform temp1, temp2;
00273 lookupTransform(fixed_frame, source_frame, source_time, temp1);
00274 lookupTransform(target_frame, fixed_frame, target_time, temp2);
00275 transform.setData( temp2 * temp1);
00276 transform.stamp_ = temp2.stamp_;
00277 transform.frame_id_ = target_frame;
00278 transform.child_frame_id_ = source_frame;
00279
00280 };
00281
00282
00283 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
00284 const ros::Time& time, const ros::Duration& averaging_interval,
00285 geometry_msgs::Twist& twist) const
00286 {
00287 lookupTwist(tracking_frame, observation_frame, observation_frame, tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist);
00288 };
00289
00290
00291
00292 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00293 const tf::Point & reference_point, const std::string& reference_point_frame,
00294 const ros::Time& time, const ros::Duration& averaging_interval,
00295 geometry_msgs::Twist& twist) const
00296 {
00297 ros::Time latest_time, target_time;
00298 getLatestCommonTime(observation_frame, tracking_frame, latest_time, NULL);
00299
00300 if (ros::Time() == time)
00301 target_time = latest_time;
00302 else
00303 target_time = time;
00304
00305 ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time);
00306
00307 ros::Time start_time = std::max(ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval;
00308 ros::Duration corrected_averaging_interval = end_time - start_time;
00309 StampedTransform start, end;
00310 lookupTransform(observation_frame, tracking_frame, start_time, start);
00311 lookupTransform(observation_frame, tracking_frame, end_time, end);
00312
00313
00314 btMatrix3x3 temp = start.getBasis().inverse() * end.getBasis();
00315 btQuaternion quat_temp;
00316 temp.getRotation(quat_temp);
00317 btVector3 o = start.getBasis() * quat_temp.getAxis();
00318 btScalar ang = quat_temp.getAngle();
00319
00320 double delta_x = end.getOrigin().getX() - start.getOrigin().getX();
00321 double delta_y = end.getOrigin().getY() - start.getOrigin().getY();
00322 double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ();
00323
00324
00325 btVector3 twist_vel ((delta_x)/corrected_averaging_interval.toSec(),
00326 (delta_y)/corrected_averaging_interval.toSec(),
00327 (delta_z)/corrected_averaging_interval.toSec());
00328 btVector3 twist_rot = o * (ang / corrected_averaging_interval.toSec());
00329
00330
00331
00332
00333
00334
00335 tf::StampedTransform inverse;
00336 lookupTransform(reference_frame,tracking_frame, target_time, inverse);
00337 btVector3 out_rot = inverse.getBasis() * twist_rot;
00338 btVector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot);
00339
00340
00341
00342
00343 tf::Stamped<tf::Point> rp_orig(tf::Point(0,0,0), target_time, tracking_frame);
00344 transformPoint(reference_frame, rp_orig, rp_orig);
00345
00346 tf::Stamped<tf::Point> rp_desired(reference_point, target_time, reference_point_frame);
00347 transformPoint(reference_frame, rp_desired, rp_desired);
00348
00349 tf::Point delta = rp_desired - rp_orig;
00350
00351 out_vel = out_vel + out_rot * delta;
00352
00353
00354
00355
00356
00357
00358
00359
00360 twist.linear.x = out_vel.x();
00361 twist.linear.y = out_vel.y();
00362 twist.linear.z = out_vel.z();
00363 twist.angular.x = out_rot.x();
00364 twist.angular.y = out_rot.y();
00365 twist.angular.z = out_rot.z();
00366
00367 };
00368
00369 bool Transformer::waitForTransform(const std::string& target_frame, const std::string& source_frame,
00370 const ros::Time& time,
00371 const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
00372 std::string* error_msg) const
00373 {
00374 if (!using_dedicated_thread_)
00375 {
00376 std::string error_string = "Do not call waitForTransform unless you are using another thread for populating data. Without a dedicated thread it will always timeout. If you have a seperate thread servicing tf messages, call setUsingDedicatedThread(true)";
00377 ROS_ERROR("%s",error_string.c_str());
00378
00379 if (error_msg)
00380 *error_msg = error_string;
00381 return false;
00382 }
00383 ros::Time start_time = now();
00384 while (!canTransform(target_frame, source_frame, time, error_msg))
00385 {
00386 if (!ok())
00387 return false;
00388
00389 if ((now() - start_time) >= timeout)
00390 return false;
00391 usleep(polling_sleep_duration.sec * 1000000 + polling_sleep_duration.nsec / 1000);
00392 }
00393 return true;
00394 }
00395
00396
00397 bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame,
00398 const ros::Time& time,
00399 std::string* error_msg) const
00400 {
00401 std::string mapped_target_frame = assert_resolved(tf_prefix_, target_frame);
00402 std::string mapped_source_frame = assert_resolved(tf_prefix_, source_frame);
00403
00404 ros::Time local_time = time;
00405
00406
00407 if (mapped_source_frame == mapped_target_frame) return true;
00408
00409 if (local_time == ros::Time())
00410 if (NO_ERROR != getLatestCommonTime(mapped_source_frame, mapped_target_frame, local_time, error_msg))
00411 {
00412 return false;
00413 }
00414
00415
00416
00417 TransformLists t_list;
00419 int retval;
00420 try
00421 {
00422 retval = lookupLists(lookupFrameNumber( mapped_target_frame), local_time, lookupFrameNumber( mapped_source_frame), t_list, error_msg);
00423 }
00424 catch (tf::LookupException &ex)
00425 {
00426 return false;
00427 }
00428
00429
00430
00432 if (retval != NO_ERROR)
00433 {
00434 if (retval == LOOKUP_ERROR)
00435 {
00436 return false;
00437 }
00438 if (retval == CONNECTIVITY_ERROR)
00439 {
00440 return false;
00441 }
00442 }
00443
00444 if (test_extrapolation(local_time, t_list, error_msg))
00445 {
00446 return false;
00447 }
00448
00449 return true;
00450 };
00451
00452 bool Transformer::canTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00453 const ros::Time& source_time, const std::string& fixed_frame,
00454 std::string* error_msg) const
00455 {
00456 return canTransform(target_frame, fixed_frame, target_time) && canTransform(fixed_frame, source_frame, source_time, error_msg);
00457 };
00458
00459 bool Transformer::waitForTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00460 const ros::Time& source_time, const std::string& fixed_frame,
00461 const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
00462 std::string* error_msg) const
00463 {
00464 return waitForTransform(target_frame, fixed_frame, target_time, timeout, polling_sleep_duration, error_msg) && waitForTransform(fixed_frame, source_frame, source_time, timeout, polling_sleep_duration, error_msg);
00465 };
00466
00467
00468 bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
00469 {
00470 std::string mapped_frame_id = assert_resolved(tf_prefix_, frame_id);
00471 tf::TimeCache* cache;
00472 try
00473 {
00474 cache = getFrame(lookupFrameNumber(mapped_frame_id));
00475 }
00476 catch (tf::LookupException &ex)
00477 {
00478 ROS_ERROR("Transformer::getParent: %s",ex.what());
00479 return false;
00480 }
00481
00482 TransformStorage temp;
00483 if (! cache->getData(time, temp)) {
00484 ROS_DEBUG("Transformer::getParent: No data for parent of %s", mapped_frame_id.c_str());
00485 return false;
00486 }
00487 if (temp.frame_id_ == "NO_PARENT") {
00488 ROS_DEBUG("Transformer::getParent: No parent for %s", mapped_frame_id.c_str());
00489 return false;
00490 }
00491 parent= temp.frame_id_;
00492 return true;
00493
00494 };
00495
00496
00497 bool Transformer::frameExists(const std::string& frame_id_str) const
00498 {
00499 boost::mutex::scoped_lock(frame_mutex_);
00500 std::string frame_id_resolveped = assert_resolved(tf_prefix_, frame_id_str);
00501
00502 std::map<std::string, unsigned int>::const_iterator map_it = frameIDs_.find(frame_id_resolveped);
00503 if (map_it == frameIDs_.end())
00504 {
00505 return false;
00506 }
00507 else
00508 return true;
00509 }
00510
00511 void Transformer::setExtrapolationLimit(const ros::Duration& distance)
00512 {
00513 max_extrapolation_distance_ = distance;
00514 }
00515
00516 int Transformer::getLatestCommonTime(const std::string& source, const std::string& dest, ros::Time & time, std::string * error_string) const
00517 {
00518 std::string mapped_source = assert_resolved(tf_prefix_, source);
00519 std::string mapped_dest = assert_resolved(tf_prefix_, dest);
00520
00521 time = ros::Time(UINT_MAX, 999999999);
00522 int retval;
00523 TransformLists lists;
00524 try
00525 {
00526 retval = lookupLists(lookupFrameNumber(mapped_dest), ros::Time(), lookupFrameNumber(mapped_source), lists, error_string);
00527 }
00528 catch (tf::LookupException &ex)
00529 {
00530 time = ros::Time();
00531 if (error_string) *error_string = ex.what();
00532 return LOOKUP_ERROR;
00533 }
00534 if (retval == NO_ERROR)
00535 {
00536
00537 if (lists.inverseTransforms.size() == 0 && lists.forwardTransforms.size() == 0)
00538 {
00539 time = now();
00540 return retval;
00541 }
00542
00543 for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
00544 {
00545 if (time > lists.inverseTransforms[i].stamp_)
00546 time = lists.inverseTransforms[i].stamp_;
00547 }
00548 for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
00549 {
00550 if (time > lists.forwardTransforms[i].stamp_)
00551 time = lists.forwardTransforms[i].stamp_;
00552 }
00553
00554 }
00555 else
00556 time.fromSec(0);
00557
00558 return retval;
00559 };
00560
00561
00562
00563 int Transformer::lookupLists(unsigned int target_frame, ros::Time time, unsigned int source_frame, TransformLists& lists, std::string * error_string) const
00564 {
00565
00566
00567
00568
00569
00571
00572
00573 lists.forwardTransforms.clear();
00574 lists.inverseTransforms.clear();
00575
00576 if (target_frame == source_frame)
00577 return 0;
00578
00579 TransformStorage temp;
00580
00581 unsigned int frame = source_frame;
00582 unsigned int counter = 0;
00583 unsigned int last_inverse;
00584 if (getFrame(frame) == NULL)
00585 {
00586 if (error_string) *error_string = "Source frame '"+lookupFrameString(frame)+"' does not exist is tf tree.";
00587 return LOOKUP_ERROR;
00588 }
00589 while (true)
00590 {
00591
00592
00593 TimeCache* pointer = getFrame(frame);
00594 ROS_ASSERT(pointer);
00595
00596 if (! pointer->getData(time, temp))
00597 {
00598 last_inverse = frame;
00599
00600 break;
00601 }
00602
00603
00604 if (frame == 0)
00605 {
00606 last_inverse = frame;
00607 break;
00608 }
00609 lists.inverseTransforms.push_back(temp);
00610
00611 frame = temp.frame_id_num_;
00612
00613
00614
00615 if (counter++ > MAX_GRAPH_DEPTH)
00616 {
00617 if (error_string)
00618 {
00619 std::stringstream ss;
00620 ss<<"The tf tree is invalid because it contains a loop." << std::endl
00621 << allFramesAsString() << std::endl;
00622 *error_string =ss.str();
00623 }
00624 return LOOKUP_ERROR;
00625
00626 }
00627 }
00628
00629
00630
00631
00632
00633 frame = target_frame;
00634 counter = 0;
00635 unsigned int last_forward;
00636 if (getFrame(frame) == NULL)
00637 {
00638 if (error_string) *error_string = "Target frame '"+lookupFrameString(frame)+"' does not exist is tf tree.";
00639 return LOOKUP_ERROR;
00640 }
00641 while (true)
00642 {
00643
00644 TimeCache* pointer = getFrame(frame);
00645 ROS_ASSERT(pointer);
00646
00647
00648 if(! pointer->getData(time, temp))
00649 {
00650 last_forward = frame;
00651 break;
00652 }
00653
00654
00655 if (frame == 0)
00656 {
00657 last_forward = frame;
00658 break;
00659 }
00660
00661 lists.forwardTransforms.push_back(temp);
00662 frame = temp.frame_id_num_;
00663
00664
00665 if (counter++ > MAX_GRAPH_DEPTH){
00666 if (error_string)
00667 {
00668 std::stringstream ss;
00669 ss<<"The tf tree is invalid because it contains a loop." << std::endl
00670 << allFramesAsString() << std::endl;
00671 *error_string = ss.str();
00672 }
00673 return LOOKUP_ERROR;
00674 }
00675 }
00676
00677
00678
00679
00680
00681 std::string connectivity_error("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+
00682 lookupFrameString(source_frame)+"' because they are not part of the same tree."+
00683 "Tf has two or more unconnected trees.");
00684
00685 if (lists.inverseTransforms.size() == 0)
00686 {
00687 if (lists.forwardTransforms.size() == 0)
00688 {
00689 if (error_string) *error_string = connectivity_error;
00690 return CONNECTIVITY_ERROR;
00691 }
00692
00693 if (last_forward != source_frame)
00694 {
00695 if (error_string) *error_string = connectivity_error;
00696 return CONNECTIVITY_ERROR;
00697 }
00698 else return 0;
00699 }
00700
00701 if (lists.forwardTransforms.size() == 0)
00702 {
00703 if (lists.inverseTransforms.size() == 0)
00704 {
00705 if (error_string) *error_string = connectivity_error;
00706 return CONNECTIVITY_ERROR;
00707 }
00708
00709 try
00710 {
00711 if (lookupFrameNumber(lists.inverseTransforms.back().frame_id_) != target_frame)
00712 {
00713 if (error_string) *error_string = connectivity_error;
00714 return CONNECTIVITY_ERROR;
00715 }
00716 else return 0;
00717 }
00718 catch (tf::LookupException & ex)
00719 {
00720 if (error_string) *error_string = ex.what();
00721 return LOOKUP_ERROR;
00722 }
00723 }
00724
00725
00726
00727 if (last_forward != last_inverse)
00728 {
00729 if (error_string) *error_string = connectivity_error;
00730 return CONNECTIVITY_ERROR;
00731 }
00732
00733 try
00734 {
00735 if (lookupFrameNumber(lists.inverseTransforms.back().child_frame_id_) == 0 || lookupFrameNumber( lists.forwardTransforms.back().child_frame_id_) == 0)
00736 {
00737
00738 if (error_string) *error_string = connectivity_error;
00739 return CONNECTIVITY_ERROR;
00740 }
00741
00742
00743
00744
00745
00746
00747 while (lookupFrameNumber(lists.inverseTransforms.back().child_frame_id_) == lookupFrameNumber(lists.forwardTransforms.back().child_frame_id_))
00748 {
00749 lists.inverseTransforms.pop_back();
00750 lists.forwardTransforms.pop_back();
00751
00752
00753
00754
00755 if (lists.inverseTransforms.size() == 0 || lists.forwardTransforms.size() == 0)
00756 break;
00757 }
00758 }
00759 catch (tf::LookupException & ex)
00760 {
00761 if (error_string) *error_string = ex.what();
00762 return LOOKUP_ERROR;
00763 }
00764
00765
00766
00767 return 0;
00768
00769 }
00770
00771
00772 bool Transformer::test_extrapolation_one_value(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const
00773 {
00774 std::stringstream ss;
00775 ss << std::fixed;
00776 ss.precision(3);
00777
00778 if (tr.mode_ == ONE_VALUE)
00779 {
00780 if (tr.stamp_ - target_time > max_extrapolation_distance_ || target_time - tr.stamp_ > max_extrapolation_distance_)
00781 {
00782 if (error_string) {
00783 ss << "You requested a transform at time " << (target_time).toSec()
00784 << ",\n but the tf buffer only contains a single transform "
00785 << "at time " << tr.stamp_.toSec() << ".\n";
00786 if ( max_extrapolation_distance_ > ros::Duration(0))
00787 {
00788 ss << "The tf extrapollation distance is set to "
00789 << (max_extrapolation_distance_).toSec() <<" seconds.\n";
00790 }
00791 *error_string = ss.str();
00792 }
00793 return true;
00794 }
00795 }
00796 return false;
00797 }
00798
00799
00800 bool Transformer::test_extrapolation_past(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const
00801 {
00802 std::stringstream ss;
00803 ss << std::fixed;
00804 ss.precision(3);
00805
00806 if (tr.mode_ == EXTRAPOLATE_BACK && tr.stamp_ - target_time > max_extrapolation_distance_)
00807 {
00808 if (error_string) {
00809 ss << "You requested a transform that is " << (now() - target_time).toSec() << " seconds in the past, \n"
00810 << "but the tf buffer only has a history of " << (now() - tr.stamp_).toSec() << " seconds.\n";
00811 if ( max_extrapolation_distance_ > ros::Duration(0))
00812 {
00813 ss << "The tf extrapollation distance is set to "
00814 << (max_extrapolation_distance_).toSec() <<" seconds.\n";
00815 }
00816 *error_string = ss.str();
00817 }
00818 return true;
00819 }
00820 return false;
00821 }
00822
00823
00824 bool Transformer::test_extrapolation_future(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const
00825 {
00826 std::stringstream ss;
00827 ss << std::fixed;
00828 ss.precision(3);
00829
00830 if( tr.mode_ == EXTRAPOLATE_FORWARD && target_time - tr.stamp_ > max_extrapolation_distance_)
00831 {
00832 if (error_string){
00833 ss << "You requested a transform that is " << (now() - target_time).toSec()*1000 << " miliseconds in the past, \n"
00834 << "but the most recent transform in the tf buffer is " << (now() - tr.stamp_).toSec()*1000 << " miliseconds old.\n";
00835 if ( max_extrapolation_distance_ > ros::Duration(0))
00836 {
00837 ss << "The tf extrapollation distance is set to "
00838 << (max_extrapolation_distance_).toSec() <<" seconds.\n";
00839 }
00840 *error_string = ss.str();
00841 }
00842 return true;
00843 }
00844 return false;
00845 }
00846
00847
00848 bool Transformer::test_extrapolation(const ros::Time& target_time, const TransformLists& lists, std::string * error_string) const
00849 {
00850 std::stringstream ss;
00851 ss << std::fixed;
00852 ss.precision(3);
00853 for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
00854 {
00855 if (test_extrapolation_one_value(target_time, lists.inverseTransforms[i], error_string)) return true;
00856 if (test_extrapolation_past(target_time, lists.inverseTransforms[i], error_string)) return true;
00857 if (test_extrapolation_future(target_time, lists.inverseTransforms[i], error_string)) return true;
00858 }
00859
00860 for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
00861 {
00862 if (test_extrapolation_one_value(target_time, lists.forwardTransforms[i], error_string)) return true;
00863 if (test_extrapolation_past(target_time, lists.forwardTransforms[i], error_string)) return true;
00864 if (test_extrapolation_future(target_time, lists.forwardTransforms[i], error_string)) return true;
00865 }
00866
00867 return false;
00868 }
00869
00870
00871 btTransform Transformer::computeTransformFromList(const TransformLists & lists) const
00872 {
00873 btTransform retTrans;
00874 retTrans.setIdentity();
00876 for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
00877 {
00878 retTrans *= (lists.inverseTransforms[lists.inverseTransforms.size() -1 - i]);
00879 }
00880 for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
00881 {
00882 retTrans = (lists.forwardTransforms[lists.forwardTransforms.size() -1 - i]).inverse() * retTrans;
00883 }
00884
00885 return retTrans;
00886 }
00887
00888
00889 std::string Transformer::chainAsString(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string& fixed_frame) const
00890 {
00891 std::string error_string;
00892 std::stringstream mstream;
00893 TransformLists lists;
00895 try
00896 {
00897 lookupLists(lookupFrameNumber(target_frame), target_time, lookupFrameNumber(source_frame), lists, &error_string);
00898 }
00899 catch (tf::LookupException &ex)
00900 {
00901 mstream << ex.what();
00902 return mstream.str();
00903 }
00904 mstream << "Inverse Transforms:" <<std::endl;
00905 for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
00906 {
00907 mstream << lists.inverseTransforms[i].child_frame_id_<<", ";
00908 }
00909 mstream << std::endl;
00910
00911 mstream << "Forward Transforms: "<<std::endl ;
00912 for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
00913 {
00914 mstream << lists.forwardTransforms[i].child_frame_id_<<", ";
00915 }
00916 mstream << std::endl;
00917 return mstream.str();
00918 }
00919
00920 void Transformer::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
00921 {
00922 std::string error_string;
00923 TransformLists lists;
00925 try
00926 {
00927 lookupLists(lookupFrameNumber(target_frame), target_time, lookupFrameNumber(source_frame), lists, &error_string);
00928 }
00929 catch (tf::LookupException &ex)
00930 {
00931 return;
00932 }
00933
00934 output.clear();
00935 if (! lists.inverseTransforms.empty())
00936 {
00937 for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
00938 {
00939 output.push_back(lists.inverseTransforms[i].child_frame_id_);
00940 }
00941 output.push_back(lists.inverseTransforms.back().frame_id_);
00942
00943 for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
00944 {
00945 output.push_back(lists.forwardTransforms[i].child_frame_id_);
00946 }
00947 }
00948 else
00949 {
00950 output.push_back(source_frame);
00951
00952 for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
00953 {
00954 output.push_back(lists.forwardTransforms[i].child_frame_id_);
00955 }
00956
00957
00958 }
00959 }
00960
00961 std::string Transformer::allFramesAsString() const
00962 {
00963 std::stringstream mstream;
00964 boost::mutex::scoped_lock(frame_mutex_);
00965
00966 TransformStorage temp;
00967
00968
00969
00970
00971 for (unsigned int counter = 1; counter < frames_.size(); counter ++)
00972 {
00973 unsigned int frame_id_num;
00974 if( getFrame(counter)->getData(ros::Time(), temp))
00975 frame_id_num = temp.frame_id_num_;
00976 else
00977 {
00978 frame_id_num = 0;
00979 }
00980 mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <<std::endl;
00981 }
00982 return mstream.str();
00983 }
00984
00985 std::string Transformer::allFramesAsDot() const
00986 {
00987 std::stringstream mstream;
00988 mstream << "digraph G {" << std::endl;
00989 boost::mutex::scoped_lock(frame_mutex_);
00990
00991 TransformStorage temp;
00992
00993 ros::Time current_time = now();
00994
00995 if (frames_.size() ==1)
00996 mstream <<"\"no tf data recieved\"";
00997
00998 mstream.precision(3);
00999 mstream.setf(std::ios::fixed,std::ios::floatfield);
01000
01001
01002 for (unsigned int counter = 1; counter < frames_.size(); counter ++)
01003 {
01004 unsigned int frame_id_num;
01005 if( getFrame(counter)->getData(ros::Time(), temp))
01006 frame_id_num = temp.frame_id_num_;
01007 else
01008 {
01009 frame_id_num = 0;
01010 }
01011 if (frame_id_num != 0)
01012 {
01013 std::string authority = "no recorded authority";
01014 std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
01015 if (it != frame_authority_.end())
01016 authority = it->second;
01017
01018 double rate = getFrame(counter)->getListLength() / std::max((getFrame(counter)->getLatestTimestamp().toSec() -
01019 getFrame(counter)->getOldestTimestamp().toSec() ), 0.0001);
01020
01021 mstream << std::fixed;
01022 mstream.precision(3);
01023 mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
01024 << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
01025
01026 << "Broadcaster: " << authority << "\\n"
01027 << "Average rate: " << rate << " Hz\\n"
01028 << "Most recent transform: " << (current_time - getFrame(counter)->getLatestTimestamp()).toSec() << " sec old \\n"
01029
01030
01031
01032 << "Buffer length: " << (getFrame(counter)->getLatestTimestamp()-getFrame(counter)->getOldestTimestamp()).toSec() << " sec\\n"
01033 <<"\"];" <<std::endl;
01034 }
01035 }
01036
01037 for (unsigned int counter = 1; counter < frames_.size(); counter ++)
01038 {
01039 unsigned int frame_id_num;
01040 if( getFrame(counter)->getData(ros::Time(), temp))
01041 frame_id_num = temp.frame_id_num_;
01042 else
01043 {
01044 frame_id_num = 0;
01045 }
01046
01047 if(frameIDs_reverse[frame_id_num]=="NO_PARENT")
01048 {
01049 mstream << "edge [style=invis];" <<std::endl;
01050 mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n"
01051 << "\"Recorded at time: " << current_time.toSec() << "\"[ shape=plaintext ] ;\n "
01052 << "}" << "->" << "\"" << frameIDs_reverse[counter]<<"\";" <<std::endl;
01053 }
01054 }
01055 mstream << "}";
01056 return mstream.str();
01057 }
01058
01059 bool Transformer::ok() const { return true; }
01060
01061 void Transformer::getFrameStrings(std::vector<std::string> & vec) const
01062 {
01063 vec.clear();
01064
01065 boost::mutex::scoped_lock(frame_mutex_);
01066
01067 TransformStorage temp;
01068
01069
01070 for (unsigned int counter = 1; counter < frames_.size(); counter ++)
01071 {
01072 vec.push_back(frameIDs_reverse[counter]);
01073 }
01074 return;
01075 }
01076
01077 tf::TimeCache* Transformer::getFrame(unsigned int frame_id) const
01078 {
01079 if (frame_id == 0)
01080 return NULL;
01081 else
01082 return frames_[frame_id];
01083 };
01084
01085
01086 void Transformer::transformQuaternion(const std::string& target_frame, const Stamped<Quaternion>& stamped_in, Stamped<Quaternion>& stamped_out) const
01087 {
01088 tf::assertQuaternionValid(stamped_in);
01089
01090 StampedTransform transform;
01091 lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01092
01093 stamped_out.setData( transform * stamped_in);
01094 stamped_out.stamp_ = transform.stamp_;
01095 stamped_out.frame_id_ = target_frame;
01096 };
01097
01098
01099 void Transformer::transformVector(const std::string& target_frame,
01100 const Stamped<tf::Vector3>& stamped_in,
01101 Stamped<tf::Vector3>& stamped_out) const
01102 {
01103 StampedTransform transform;
01104 lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01105
01107 btVector3 end = stamped_in;
01108 btVector3 origin = btVector3(0,0,0);
01109 btVector3 output = (transform * end) - (transform * origin);
01110 stamped_out.setData( output);
01111
01112 stamped_out.stamp_ = transform.stamp_;
01113 stamped_out.frame_id_ = target_frame;
01114 };
01115
01116
01117 void Transformer::transformPoint(const std::string& target_frame, const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const
01118 {
01119 StampedTransform transform;
01120 lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01121
01122 stamped_out.setData(transform * stamped_in);
01123 stamped_out.stamp_ = transform.stamp_;
01124 stamped_out.frame_id_ = target_frame;
01125 };
01126
01127 void Transformer::transformPose(const std::string& target_frame, const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const
01128 {
01129 StampedTransform transform;
01130 lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
01131
01132 stamped_out.setData(transform * stamped_in);
01133 stamped_out.stamp_ = transform.stamp_;
01134 stamped_out.frame_id_ = target_frame;
01135 };
01136
01137
01138 void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
01139 const Stamped<Quaternion>& stamped_in,
01140 const std::string& fixed_frame,
01141 Stamped<Quaternion>& stamped_out) const
01142 {
01143 tf::assertQuaternionValid(stamped_in);
01144 StampedTransform transform;
01145 lookupTransform(target_frame, target_time,
01146 stamped_in.frame_id_,stamped_in.stamp_,
01147 fixed_frame, transform);
01148
01149 stamped_out.setData( transform * stamped_in);
01150 stamped_out.stamp_ = transform.stamp_;
01151 stamped_out.frame_id_ = target_frame;
01152 };
01153
01154
01155 void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time,
01156 const Stamped<Vector3>& stamped_in,
01157 const std::string& fixed_frame,
01158 Stamped<Vector3>& stamped_out) const
01159 {
01160 StampedTransform transform;
01161 lookupTransform(target_frame, target_time,
01162 stamped_in.frame_id_,stamped_in.stamp_,
01163 fixed_frame, transform);
01164
01166 btVector3 end = stamped_in;
01167 btVector3 origin = btVector3(0,0,0);
01168 btVector3 output = (transform * end) - (transform * origin);
01169 stamped_out.setData( output);
01170
01171 stamped_out.stamp_ = transform.stamp_;
01172 stamped_out.frame_id_ = target_frame;
01173 };
01174
01175
01176 void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time,
01177 const Stamped<Point>& stamped_in,
01178 const std::string& fixed_frame,
01179 Stamped<Point>& stamped_out) const
01180 {
01181 StampedTransform transform;
01182 lookupTransform(target_frame, target_time,
01183 stamped_in.frame_id_,stamped_in.stamp_,
01184 fixed_frame, transform);
01185
01186 stamped_out.setData(transform * stamped_in);
01187 stamped_out.stamp_ = transform.stamp_;
01188 stamped_out.frame_id_ = target_frame;
01189 };
01190
01191 void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
01192 const Stamped<Pose>& stamped_in,
01193 const std::string& fixed_frame,
01194 Stamped<Pose>& stamped_out) const
01195 {
01196 StampedTransform transform;
01197 lookupTransform(target_frame, target_time,
01198 stamped_in.frame_id_,stamped_in.stamp_,
01199 fixed_frame, transform);
01200
01201 stamped_out.setData(transform * stamped_in);
01202 stamped_out.stamp_ = transform.stamp_;
01203 stamped_out.frame_id_ = target_frame;
01204 };
01205
01206 boost::signals::connection Transformer::addTransformsChangedListener(boost::function<void(void)> callback)
01207 {
01208 boost::mutex::scoped_lock lock(transforms_changed_mutex_);
01209 return transforms_changed_.connect(callback);
01210 }
01211
01212 void Transformer::removeTransformsChangedListener(boost::signals::connection c)
01213 {
01214 boost::mutex::scoped_lock lock(transforms_changed_mutex_);
01215 c.disconnect();
01216 }