tf.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00032 #include "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 // Must provide storage for non-integral static const class members.
00041 // Otherwise you get undefined symbol errors on OS X (why not on Linux?).
00042 // Thanks to Rob for pointing out the right way to do this.
00043 // In C++0x this must be initialized here #5401
00044 const double tf::Transformer::DEFAULT_CACHE_TIME = 10.0;
00045 
00046 
00047 enum WalkEnding
00048 {
00049   Identity,
00050   TargetParentOfSource,
00051   SourceParentOfTarget,
00052   FullPath,
00053 };
00054 
00055 struct CanTransformAccum
00056 {
00057   CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string)
00058   {
00059     return cache->getParent(time, error_string);
00060   }
00061 
00062   void accum(bool source)
00063   {
00064   }
00065 
00066   void finalize(WalkEnding end, ros::Time _time)
00067   {
00068   }
00069 
00070   TransformStorage st;
00071 };
00072 
00073 struct TransformAccum
00074 {
00075   TransformAccum()
00076   : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
00077   , source_to_top_vec(0.0, 0.0, 0.0)
00078   , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
00079   , target_to_top_vec(0.0, 0.0, 0.0)
00080   , result_quat(0.0, 0.0, 0.0, 1.0)
00081   , result_vec(0.0, 0.0, 0.0)
00082   {
00083   }
00084 
00085   CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string)
00086   {
00087     if (!cache->getData(time, st, error_string))
00088     {
00089       return 0;
00090     }
00091 
00092     return st.frame_id_;
00093   }
00094 
00095   void accum(bool source)
00096   {
00097     if (source)
00098     {
00099       source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
00100       source_to_top_quat = st.rotation_ * source_to_top_quat;
00101     }
00102     else
00103     {
00104       target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
00105       target_to_top_quat = st.rotation_ * target_to_top_quat;
00106     }
00107   }
00108 
00109   void finalize(WalkEnding end, ros::Time _time)
00110   {
00111     switch (end)
00112     {
00113     case Identity:
00114       break;
00115     case TargetParentOfSource:
00116       result_vec = source_to_top_vec;
00117       result_quat = source_to_top_quat;
00118       break;
00119     case SourceParentOfTarget:
00120       {
00121         tf::Quaternion inv_target_quat = target_to_top_quat.inverse();
00122         tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00123         result_vec = inv_target_vec;
00124         result_quat = inv_target_quat;
00125         break;
00126       }
00127     case FullPath:
00128       {
00129         tf::Quaternion inv_target_quat = target_to_top_quat.inverse();
00130         tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
00131 
00132         result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
00133         result_quat = inv_target_quat * source_to_top_quat;
00134       }
00135       break;
00136     };
00137 
00138     time = _time;
00139   }
00140 
00141   TransformStorage st;
00142   ros::Time time;
00143   tf::Quaternion source_to_top_quat;
00144   tf::Vector3 source_to_top_vec;
00145   tf::Quaternion target_to_top_quat;
00146   tf::Vector3 target_to_top_vec;
00147 
00148   tf::Quaternion result_quat;
00149   tf::Vector3 result_vec;
00150 };
00151 
00152 
00153 std::string assert_resolved(const std::string& prefix, const std::string& frame_id)
00154 {
00155   ROS_DEBUG("tf::assert_resolved just calls tf::resolve");
00156   return tf::resolve(prefix, frame_id);
00157 };
00158 
00159 std::string tf::resolve(const std::string& prefix, const std::string& frame_name)
00160 {
00161   //  printf ("resolveping prefix:%s with frame_name:%s\n", prefix.c_str(), frame_name.c_str());
00162   if (frame_name.size() > 0)
00163     if (frame_name[0] == '/')
00164     {
00165       return strip_leading_slash(frame_name);
00166     }
00167   if (prefix.size() > 0)
00168   {
00169     if (prefix[0] == '/')
00170     {
00171       std::string composite = strip_leading_slash(prefix);
00172       composite.append("/");
00173       composite.append(frame_name);
00174       return composite;
00175     }
00176     else
00177     {
00178       std::string composite;
00179       composite.append(prefix);
00180       composite.append("/");
00181       composite.append(frame_name);
00182       return composite;
00183     }
00184 
00185   }
00186   else
00187  {
00188     std::string composite;
00189     composite.append(frame_name);
00190     return composite;
00191   }
00192 };
00193 
00194 
00195 std::string tf::strip_leading_slash(const std::string& frame_name)
00196 {
00197   if (frame_name.size() > 0)
00198     if (frame_name[0] == '/')
00199     {
00200       std::string shorter = frame_name;
00201       shorter.erase(0,1);
00202       return shorter;
00203     }
00204   
00205   return frame_name;
00206 }
00207 
00208 
00209 Transformer::Transformer(bool interpolating,
00210                                 ros::Duration cache_time):
00211   fall_back_to_wall_time_(false),
00212   tf2_buffer_(cache_time)
00213 {
00214 
00215 }
00216 
00217 Transformer::~Transformer()
00218 {
00219 
00220 };
00221 
00222 
00223 void Transformer::clear()
00224 {
00225   tf2_buffer_.clear();
00226 }
00227 
00228 
00229 bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority)
00230 {
00231   geometry_msgs::TransformStamped msgtf;
00232   transformStampedTFToMsg(transform, msgtf);
00233   return tf2_buffer_.setTransform(msgtf, authority);
00234   
00235 };
00236 
00237 
00238 void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
00239                      const ros::Time& time, StampedTransform& transform) const
00240 {
00241   geometry_msgs::TransformStamped output = 
00242     tf2_buffer_.lookupTransform(strip_leading_slash(target_frame),
00243                                 strip_leading_slash(source_frame), time);
00244   transformStampedMsgToTF(output, transform);
00245   return;
00246 };
00247 
00248 
00249 void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00250                      const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
00251 {
00252   geometry_msgs::TransformStamped output = 
00253     tf2_buffer_.lookupTransform(strip_leading_slash(target_frame), target_time,
00254                                 strip_leading_slash(source_frame), source_time,
00255                                 strip_leading_slash(fixed_frame));
00256   transformStampedMsgToTF(output, transform);
00257 };
00258 
00259 
00260 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
00261                               const ros::Time& time, const ros::Duration& averaging_interval, 
00262                               geometry_msgs::Twist& twist) const
00263 {
00264   // ref point is origin of tracking_frame, ref_frame = obs_frame
00265   lookupTwist(tracking_frame, observation_frame, observation_frame, tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist);
00266 };
00267 
00268 
00269 
00270 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00271                  const tf::Point & reference_point, const std::string& reference_point_frame, 
00272                  const ros::Time& time, const ros::Duration& averaging_interval, 
00273                  geometry_msgs::Twist& twist) const
00274 {
00275   
00276   ros::Time latest_time, target_time;
00277   getLatestCommonTime(observation_frame, tracking_frame, latest_time, NULL); 
00278 
00279   if (ros::Time() == time)
00280     target_time = latest_time;
00281   else
00282     target_time = time;
00283 
00284   ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time);
00285   
00286   ros::Time start_time = std::max(ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval;  // don't collide with zero
00287   ros::Duration corrected_averaging_interval = end_time - start_time; //correct for the possiblity that start time was truncated above.
00288   StampedTransform start, end;
00289   lookupTransform(observation_frame, tracking_frame, start_time, start);
00290   lookupTransform(observation_frame, tracking_frame, end_time, end);
00291 
00292 
00293   tf::Matrix3x3 temp = start.getBasis().inverse() * end.getBasis();
00294   tf::Quaternion quat_temp;
00295   temp.getRotation(quat_temp);
00296   tf::Vector3 o = start.getBasis() * quat_temp.getAxis();
00297   tfScalar ang = quat_temp.getAngle();
00298   
00299   double delta_x = end.getOrigin().getX() - start.getOrigin().getX();
00300   double delta_y = end.getOrigin().getY() - start.getOrigin().getY();
00301   double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ();
00302 
00303 
00304   tf::Vector3 twist_vel ((delta_x)/corrected_averaging_interval.toSec(), 
00305                        (delta_y)/corrected_averaging_interval.toSec(),
00306                        (delta_z)/corrected_averaging_interval.toSec());
00307   tf::Vector3 twist_rot = o * (ang / corrected_averaging_interval.toSec());
00308 
00309 
00310   // This is a twist w/ reference frame in observation_frame  and reference point is in the tracking_frame at the origin (at start_time)
00311 
00312 
00313   //correct for the position of the reference frame
00314   tf::StampedTransform inverse;
00315   lookupTransform(reference_frame,tracking_frame,  target_time, inverse);
00316   tf::Vector3 out_rot = inverse.getBasis() * twist_rot;
00317   tf::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot);
00318 
00319 
00320   //Rereference the twist about a new reference point
00321   // Start by computing the original reference point in the reference frame:
00322   tf::Stamped<tf::Point> rp_orig(tf::Point(0,0,0), target_time, tracking_frame);
00323   transformPoint(reference_frame, rp_orig, rp_orig);
00324   // convert the requrested reference point into the right frame
00325   tf::Stamped<tf::Point> rp_desired(reference_point, target_time, reference_point_frame);
00326   transformPoint(reference_frame, rp_desired, rp_desired);
00327   // compute the delta
00328   tf::Point delta = rp_desired - rp_orig;
00329   // Correct for the change in reference point 
00330   out_vel = out_vel + out_rot * delta;
00331   // out_rot unchanged   
00332 
00333   /*
00334     printf("KDL: Rotation %f %f %f, Translation:%f %f %f\n", 
00335          out_rot.x(),out_rot.y(),out_rot.z(),
00336          out_vel.x(),out_vel.y(),out_vel.z());
00337   */   
00338 
00339   twist.linear.x =  out_vel.x();
00340   twist.linear.y =  out_vel.y();
00341   twist.linear.z =  out_vel.z();
00342   twist.angular.x =  out_rot.x();
00343   twist.angular.y =  out_rot.y();
00344   twist.angular.z =  out_rot.z();
00345 
00346 };
00347 
00348 bool Transformer::waitForTransform(const std::string& target_frame, const std::string& source_frame,
00349                                    const ros::Time& time,
00350                                    const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
00351                                    std::string* error_msg) const
00352 {
00353   return tf2_buffer_.canTransform(strip_leading_slash(target_frame),
00354                                   strip_leading_slash(source_frame), time, timeout, error_msg);
00355 }
00356 
00357 
00358 bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame,
00359                            const ros::Time& time, std::string* error_msg) const
00360 {
00361   return tf2_buffer_.canTransform(strip_leading_slash(target_frame),
00362                                   strip_leading_slash(source_frame), time, error_msg);
00363 }
00364 
00365 
00366 bool Transformer::canTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00367                                const ros::Time& source_time, const std::string& fixed_frame,
00368                                std::string* error_msg) const
00369 {
00370   return tf2_buffer_.canTransform(strip_leading_slash(target_frame), target_time,
00371                                   strip_leading_slash(source_frame), source_time,
00372                                   strip_leading_slash(fixed_frame), error_msg);
00373 };
00374 
00375 bool Transformer::waitForTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
00376                                    const ros::Time& source_time, const std::string& fixed_frame,
00377                                    const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
00378                                    std::string* error_msg) const
00379 {
00380   return tf2_buffer_.canTransform(strip_leading_slash(target_frame), target_time,
00381                                   strip_leading_slash(source_frame), source_time,
00382                                   strip_leading_slash(fixed_frame), timeout, error_msg);
00383 };
00384 
00385 
00386 bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
00387 {
00388   return tf2_buffer_._getParent(strip_leading_slash(frame_id), time, parent);
00389 };
00390 
00391 
00392 bool Transformer::frameExists(const std::string& frame_id_str) const
00393 {
00394   return tf2_buffer_._frameExists(strip_leading_slash(frame_id_str));
00395 }
00396 
00397 void Transformer::setExtrapolationLimit(const ros::Duration& distance)
00398 {
00399   ROS_WARN("Transformer::setExtrapolationLimit is deprecated and does not do anything");
00400 }
00401 
00402 
00403 struct TimeAndFrameIDFrameComparator
00404 {
00405   TimeAndFrameIDFrameComparator(CompactFrameID id)
00406   : id(id)
00407   {}
00408 
00409   bool operator()(const P_TimeAndFrameID& rhs) const
00410   {
00411     return rhs.second == id;
00412   }
00413 
00414   CompactFrameID id;
00415 };
00416 
00417 int Transformer::getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const
00418 {
00419   CompactFrameID target_id = tf2_buffer_._lookupFrameNumber(strip_leading_slash(target_frame));
00420   CompactFrameID source_id = tf2_buffer_._lookupFrameNumber(strip_leading_slash(source_frame));
00421 
00422   return tf2_buffer_._getLatestCommonTime(source_id, target_id, time, error_string);
00423 }
00424 
00425 
00426 //@todo - Fix this to work with new data structures
00427 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
00428 {
00429   tf2_buffer_._chainAsVector(target_frame, target_time, 
00430                              source_frame, source_time, 
00431                              fixed_frame, output);
00432 }
00433 
00434 std::string Transformer::allFramesAsString() const
00435 {
00436   return tf2_buffer_.allFramesAsString();
00437 }
00438 
00439 std::string Transformer::allFramesAsDot(double current_time) const
00440 {
00441   return tf2_buffer_._allFramesAsDot(current_time);
00442 }
00443 
00444 
00445 bool Transformer::ok() const { return true; }
00446 
00447 void Transformer::getFrameStrings(std::vector<std::string> & vec) const
00448 {
00449   tf2_buffer_._getFrameStrings(vec);
00450 }
00451 
00452 
00453 void Transformer::transformQuaternion(const std::string& target_frame, const Stamped<Quaternion>& stamped_in, Stamped<Quaternion>& stamped_out) const
00454 {
00455   tf::assertQuaternionValid(stamped_in);
00456 
00457   StampedTransform transform;
00458   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
00459 
00460   stamped_out.setData( transform * stamped_in);
00461   stamped_out.stamp_ = transform.stamp_;
00462   stamped_out.frame_id_ = target_frame;
00463 };
00464 
00465 
00466 void Transformer::transformVector(const std::string& target_frame,
00467                                   const Stamped<tf::Vector3>& stamped_in,
00468                                   Stamped<tf::Vector3>& stamped_out) const
00469 {
00470   StampedTransform transform;
00471   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
00472 
00474   tf::Vector3 end = stamped_in;
00475   tf::Vector3 origin = tf::Vector3(0,0,0);
00476   tf::Vector3 output = (transform * end) - (transform * origin);
00477   stamped_out.setData( output);
00478 
00479   stamped_out.stamp_ = transform.stamp_;
00480   stamped_out.frame_id_ = target_frame;
00481 };
00482 
00483 
00484 void Transformer::transformPoint(const std::string& target_frame, const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const
00485 {
00486   StampedTransform transform;
00487   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
00488 
00489   stamped_out.setData(transform * stamped_in);
00490   stamped_out.stamp_ = transform.stamp_;
00491   stamped_out.frame_id_ = target_frame;
00492 };
00493 
00494 void Transformer::transformPose(const std::string& target_frame, const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const
00495 {
00496   StampedTransform transform;
00497   lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
00498 
00499   stamped_out.setData(transform * stamped_in);
00500   stamped_out.stamp_ = transform.stamp_;
00501   stamped_out.frame_id_ = target_frame;
00502 };
00503 
00504 
00505 void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00506                                       const Stamped<Quaternion>& stamped_in,
00507                                       const std::string& fixed_frame,
00508                                       Stamped<Quaternion>& stamped_out) const
00509 {
00510   tf::assertQuaternionValid(stamped_in);
00511   StampedTransform transform;
00512   lookupTransform(target_frame, target_time,
00513                   stamped_in.frame_id_,stamped_in.stamp_,
00514                   fixed_frame, transform);
00515 
00516   stamped_out.setData( transform * stamped_in);
00517   stamped_out.stamp_ = transform.stamp_;
00518   stamped_out.frame_id_ = target_frame;
00519 };
00520 
00521 
00522 void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time,
00523                                   const Stamped<Vector3>& stamped_in,
00524                                   const std::string& fixed_frame,
00525                                   Stamped<Vector3>& stamped_out) const
00526 {
00527   StampedTransform transform;
00528   lookupTransform(target_frame, target_time,
00529                   stamped_in.frame_id_,stamped_in.stamp_,
00530                   fixed_frame, transform);
00531 
00533   tf::Vector3 end = stamped_in;
00534   tf::Vector3 origin = tf::Vector3(0,0,0);
00535   tf::Vector3 output = (transform * end) - (transform * origin);
00536   stamped_out.setData( output);
00537 
00538   stamped_out.stamp_ = transform.stamp_;
00539   stamped_out.frame_id_ = target_frame;
00540 };
00541 
00542 
00543 void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time,
00544                                  const Stamped<Point>& stamped_in,
00545                                  const std::string& fixed_frame,
00546                                  Stamped<Point>& stamped_out) const
00547 {
00548   StampedTransform transform;
00549   lookupTransform(target_frame, target_time,
00550                   stamped_in.frame_id_,stamped_in.stamp_,
00551                   fixed_frame, transform);
00552 
00553   stamped_out.setData(transform * stamped_in);
00554   stamped_out.stamp_ = transform.stamp_;
00555   stamped_out.frame_id_ = target_frame;
00556 };
00557 
00558 void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
00559                                 const Stamped<Pose>& stamped_in,
00560                                 const std::string& fixed_frame,
00561                                 Stamped<Pose>& stamped_out) const
00562 {
00563   StampedTransform transform;
00564   lookupTransform(target_frame, target_time,
00565                   stamped_in.frame_id_,stamped_in.stamp_,
00566                   fixed_frame, transform);
00567 
00568   stamped_out.setData(transform * stamped_in);
00569   stamped_out.stamp_ = transform.stamp_;
00570   stamped_out.frame_id_ = target_frame;
00571 };
00572 
00573 boost::signals2::connection Transformer::addTransformsChangedListener(boost::function<void(void)> callback)
00574 {
00575   return tf2_buffer_._addTransformsChangedListener(callback);
00576 }
00577 
00578 void Transformer::removeTransformsChangedListener(boost::signals2::connection c)
00579 {
00580   tf2_buffer_._removeTransformsChangedListener(c);
00581 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55