transform_listener.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/transform_listener.h"
00033 
00034 #include <boost/numeric/ublas/matrix.hpp>
00035 #include <boost/numeric/ublas/io.hpp>
00036 
00037 
00038 using namespace tf;
00039 std::string tf::remap(const std::string& frame_id)
00040 {
00041   ros::NodeHandle n("~");
00042   return tf::resolve(getPrefixParam(n), frame_id);
00043 };
00044 
00045 
00046 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread):
00047   Transformer(true, max_cache_time), dedicated_listener_thread_(NULL)
00048 {
00049   if (spin_thread)
00050     initWithThread();
00051   else
00052     init();
00053 }
00054 
00055 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread):
00056   Transformer(true, max_cache_time), dedicated_listener_thread_(NULL), node_(nh)
00057 {
00058   if (spin_thread)
00059     initWithThread();
00060   else
00061     init();
00062 }
00063 
00064 TransformListener::~TransformListener()
00065 {
00066   using_dedicated_thread_ = false;
00067   if (dedicated_listener_thread_)
00068   {
00069     dedicated_listener_thread_->join();
00070     delete dedicated_listener_thread_;
00071   }
00072 }
00073 
00074 //Override Transformer::ok() for ticket:4882
00075 bool TransformListener::ok() const { return ros::ok(); }
00076 
00077 void TransformListener::init()
00078 {
00079   message_subscriber_tf_ = node_.subscribe<tf::tfMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1)); 
00080   
00081   
00082   if (! ros::service::exists("~tf_frames", false))  // Avoid doublely advertizing if multiple instances of this library
00083     {
00084       ros::NodeHandle nh("~");
00085       tf_frames_srv_ = nh.advertiseService("tf_frames", &TransformListener::getFrames, this);
00086     }
00087 
00088   ros::NodeHandle local_nh("~");
00089   
00090   tf_prefix_ = getPrefixParam(local_nh);
00091   last_update_ros_time_ = ros::Time::now();
00092 }
00093 
00094 void TransformListener::initWithThread()
00095 {
00096   using_dedicated_thread_ = true;
00097   ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf::tfMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_); 
00098     
00099     message_subscriber_tf_ = node_.subscribe(ops_tf);
00100   
00101   dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
00102 
00103   if (! ros::service::exists("~tf_frames", false))  // Avoid doublely advertizing if multiple instances of this library
00104     {
00105       ros::NodeHandle nh("~");
00106       tf_frames_srv_ = nh.advertiseService("tf_frames", &TransformListener::getFrames, this);
00107     }
00108     
00109   ros::NodeHandle local_nh("~");
00110   tf_prefix_ = getPrefixParam(local_nh);
00111   last_update_ros_time_ = ros::Time::now();
00112 }
00113 
00114 void TransformListener::transformQuaternion(const std::string& target_frame,
00115     const geometry_msgs::QuaternionStamped& msg_in,
00116     geometry_msgs::QuaternionStamped& msg_out) const
00117 {
00118   tf::assertQuaternionValid(msg_in.quaternion);
00119 
00120   Stamped<Quaternion> pin, pout;
00121   quaternionStampedMsgToTF(msg_in, pin);  
00122   transformQuaternion(target_frame, pin, pout);
00123   quaternionStampedTFToMsg(pout, msg_out);
00124 }
00125 
00126 void TransformListener::transformVector(const std::string& target_frame,
00127     const geometry_msgs::Vector3Stamped& msg_in,
00128     geometry_msgs::Vector3Stamped& msg_out) const
00129 {
00130   Stamped<Vector3> pin, pout;
00131   vector3StampedMsgToTF(msg_in, pin);
00132   transformVector(target_frame, pin, pout);
00133   vector3StampedTFToMsg(pout, msg_out);
00134 }
00135 
00136 void TransformListener::transformPoint(const std::string& target_frame,
00137     const geometry_msgs::PointStamped& msg_in,
00138     geometry_msgs::PointStamped& msg_out) const
00139 {
00140   Stamped<Point> pin, pout;
00141   pointStampedMsgToTF(msg_in, pin);
00142   transformPoint(target_frame, pin, pout);
00143   pointStampedTFToMsg(pout, msg_out);
00144 }
00145 
00146 void TransformListener::transformPose(const std::string& target_frame,
00147     const geometry_msgs::PoseStamped& msg_in,
00148     geometry_msgs::PoseStamped& msg_out) const
00149 {
00150   tf::assertQuaternionValid(msg_in.pose.orientation);
00151 
00152   Stamped<Pose> pin, pout;
00153   poseStampedMsgToTF(msg_in, pin);
00154   transformPose(target_frame, pin, pout);
00155   poseStampedTFToMsg(pout, msg_out);
00156 }
00157 /* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review
00158 void TransformListener::transformTwist(const std::string& target_frame,
00159     const geometry_msgs::TwistStamped& msg_in,
00160     geometry_msgs::TwistStamped& msg_out) const
00161 {
00162   tf::Vector3 twist_rot(msg_in.twist.angular.x,
00163                         msg_in.twist.angular.y,
00164                         msg_in.twist.angular.z);
00165   tf::Vector3 twist_vel(msg_in.twist.linear.x,
00166                         msg_in.twist.linear.y,
00167                         msg_in.twist.linear.z);
00168 
00169   tf::StampedTransform transform;
00170   lookupTransform(target_frame,msg_in.header.frame_id,  msg_in.header.stamp, transform);
00171 
00172 
00173   tf::Vector3 out_rot = transform.getBasis() * twist_rot;
00174   tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot);
00175 
00176   geometry_msgs::TwistStamped interframe_twist;
00177   lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, ros::Duration(0.1), interframe_twist); //\todo get rid of hard coded number
00178 
00179   msg_out.header.stamp = msg_in.header.stamp;
00180   msg_out.header.frame_id = target_frame;
00181   msg_out.twist.linear.x =  out_vel.x() + interframe_twist.twist.linear.x;
00182   msg_out.twist.linear.y =  out_vel.y() + interframe_twist.twist.linear.y;
00183   msg_out.twist.linear.z =  out_vel.z() + interframe_twist.twist.linear.z;
00184   msg_out.twist.angular.x =  out_rot.x() + interframe_twist.twist.angular.x;
00185   msg_out.twist.angular.y =  out_rot.y() + interframe_twist.twist.angular.y;
00186   msg_out.twist.angular.z =  out_rot.z() + interframe_twist.twist.angular.z;
00187 
00188   }*/
00189 
00190 void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00191     const geometry_msgs::QuaternionStamped& msg_in,
00192     const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const
00193 {
00194   tf::assertQuaternionValid(msg_in.quaternion);
00195   Stamped<Quaternion> pin, pout;
00196   quaternionStampedMsgToTF(msg_in, pin);
00197   transformQuaternion(target_frame, target_time, pin, fixed_frame, pout);
00198   quaternionStampedTFToMsg(pout, msg_out);
00199 }
00200 
00201 void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time,
00202     const geometry_msgs::Vector3Stamped& msg_in,
00203     const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const
00204 {
00205   Stamped<Vector3> pin, pout;
00206   vector3StampedMsgToTF(msg_in, pin);
00207   transformVector(target_frame, target_time, pin, fixed_frame, pout);
00208   vector3StampedTFToMsg(pout, msg_out);
00209 }
00210 
00211 void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time,
00212     const geometry_msgs::PointStamped& msg_in,
00213     const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const
00214 {
00215   Stamped<Point> pin, pout;
00216   pointStampedMsgToTF(msg_in, pin);
00217   transformPoint(target_frame, target_time, pin, fixed_frame, pout);
00218   pointStampedTFToMsg(pout, msg_out);
00219 }
00220 
00221 void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time,
00222     const geometry_msgs::PoseStamped& msg_in,
00223     const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const
00224 {
00225   tf::assertQuaternionValid(msg_in.pose.orientation);
00226 
00227   Stamped<Pose> pin, pout;
00228   poseStampedMsgToTF(msg_in, pin);
00229   transformPose(target_frame, target_time, pin, fixed_frame, pout);
00230   poseStampedTFToMsg(pout, msg_out);
00231 }
00232 
00233 void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const
00234 {
00235   StampedTransform transform;
00236   lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform);
00237 
00238   transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut);
00239 }
00240 void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time, 
00241     const sensor_msgs::PointCloud& cloudIn,
00242     const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const
00243 {
00244   StampedTransform transform;
00245   lookupTransform(target_frame, target_time,
00246       cloudIn.header.frame_id, cloudIn.header.stamp,
00247       fixed_frame,
00248       transform);
00249 
00250   transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut);
00251 
00252 
00253 }
00254 
00255 inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
00256 {
00257   // Use temporary variables in case &in == &out
00258   double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
00259   double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
00260   double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
00261   
00262   out.x = x; out.y = y; out.z = z;
00263 }
00264 
00265 
00266 void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
00267                                             const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, 
00268                                             sensor_msgs::PointCloud & cloudOut) const
00269 {
00270   tf::Vector3 origin = net_transform.getOrigin();
00271   tf::Matrix3x3 basis  = net_transform.getBasis();
00272 
00273   unsigned int length = cloudIn.points.size();
00274 
00275   // Copy relevant data from cloudIn, if needed
00276   if (&cloudIn != &cloudOut)
00277   {
00278     cloudOut.header = cloudIn.header;
00279     cloudOut.points.resize(length);
00280     cloudOut.channels.resize(cloudIn.channels.size());
00281     for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
00282       cloudOut.channels[i] = cloudIn.channels[i];
00283   }
00284 
00285   // Transform points
00286   cloudOut.header.stamp = target_time;
00287   cloudOut.header.frame_id = target_frame;
00288   for (unsigned int i = 0; i < length ; i++) {
00289     transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
00290   }
00291 }
00292 
00293 
00294 void TransformListener::subscription_callback(const tf::tfMessageConstPtr& msg)
00295 {
00296   ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_;
00297   float ros_dt = ros_diff.toSec();
00298 
00299   if (ros_dt < 0.0)
00300   {
00301     ROS_WARN("Saw a negative time change of %f seconds, clearing the tf buffer.", ros_dt);
00302     clear();
00303   }
00304 
00305   last_update_ros_time_ = ros::Time::now();
00306 
00307   const tf::tfMessage& msg_in = *msg;
00308   for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
00309   {
00310     StampedTransform trans;
00311     transformStampedMsgToTF(msg_in.transforms[i], trans);
00312     try
00313     {
00314       std::map<std::string, std::string>* msg_header_map = msg_in.__connection_header.get();
00315       std::string authority;
00316       std::map<std::string, std::string>::iterator it = msg_header_map->find("callerid");
00317       if (it == msg_header_map->end())
00318       {
00319         ROS_WARN("Message recieved without callerid");
00320         authority = "no callerid";
00321       }
00322       else 
00323       {
00324         authority = it->second;
00325       }
00326       setTransform(trans, authority);
00327     }
00328 
00329     catch (TransformException& ex)
00330     {
00332       std::string temp = ex.what();
00333       ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
00334     }
00335   }
00336 };
00337 
00338 
00339 
00340 
00341 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 22 2013 11:29:01