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 
00035 using namespace tf;
00036 std::string tf::remap(const std::string& frame_id)
00037 {
00038   ros::NodeHandle n("~");
00039   return tf::resolve(getPrefixParam(n), frame_id);
00040 };
00041 
00042 
00043 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread):
00044   Transformer(true, max_cache_time), tf2_listener_(Transformer::tf2_buffer_, node_, spin_thread)
00045 {
00046   //Everything is done inside tf2 init
00047 }
00048 
00049 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread):
00050   Transformer(true, max_cache_time), node_(nh), tf2_listener_(Transformer::tf2_buffer_, nh, spin_thread)
00051 {
00052   //Everything is done inside tf2 init
00053 }
00054 
00055 TransformListener::~TransformListener()
00056 {
00057   //Everything is done inside tf2 init
00058 }
00059 
00060 //Override Transformer::ok() for ticket:4882
00061 bool TransformListener::ok() const { return ros::ok(); }
00062 
00063 
00064 
00065 void TransformListener::transformQuaternion(const std::string& target_frame,
00066     const geometry_msgs::QuaternionStamped& msg_in,
00067     geometry_msgs::QuaternionStamped& msg_out) const
00068 {
00069   tf::assertQuaternionValid(msg_in.quaternion);
00070 
00071   Stamped<Quaternion> pin, pout;
00072   quaternionStampedMsgToTF(msg_in, pin);  
00073   transformQuaternion(target_frame, pin, pout);
00074   quaternionStampedTFToMsg(pout, msg_out);
00075 }
00076 
00077 void TransformListener::transformVector(const std::string& target_frame,
00078     const geometry_msgs::Vector3Stamped& msg_in,
00079     geometry_msgs::Vector3Stamped& msg_out) const
00080 {
00081   Stamped<Vector3> pin, pout;
00082   vector3StampedMsgToTF(msg_in, pin);
00083   transformVector(target_frame, pin, pout);
00084   vector3StampedTFToMsg(pout, msg_out);
00085 }
00086 
00087 void TransformListener::transformPoint(const std::string& target_frame,
00088     const geometry_msgs::PointStamped& msg_in,
00089     geometry_msgs::PointStamped& msg_out) const
00090 {
00091   Stamped<Point> pin, pout;
00092   pointStampedMsgToTF(msg_in, pin);
00093   transformPoint(target_frame, pin, pout);
00094   pointStampedTFToMsg(pout, msg_out);
00095 }
00096 
00097 void TransformListener::transformPose(const std::string& target_frame,
00098     const geometry_msgs::PoseStamped& msg_in,
00099     geometry_msgs::PoseStamped& msg_out) const
00100 {
00101   tf::assertQuaternionValid(msg_in.pose.orientation);
00102 
00103   Stamped<Pose> pin, pout;
00104   poseStampedMsgToTF(msg_in, pin);
00105   transformPose(target_frame, pin, pout);
00106   poseStampedTFToMsg(pout, msg_out);
00107 }
00108 /* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review
00109 void TransformListener::transformTwist(const std::string& target_frame,
00110     const geometry_msgs::TwistStamped& msg_in,
00111     geometry_msgs::TwistStamped& msg_out) const
00112 {
00113   tf::Vector3 twist_rot(msg_in.twist.angular.x,
00114                         msg_in.twist.angular.y,
00115                         msg_in.twist.angular.z);
00116   tf::Vector3 twist_vel(msg_in.twist.linear.x,
00117                         msg_in.twist.linear.y,
00118                         msg_in.twist.linear.z);
00119 
00120   tf::StampedTransform transform;
00121   lookupTransform(target_frame,msg_in.header.frame_id,  msg_in.header.stamp, transform);
00122 
00123 
00124   tf::Vector3 out_rot = transform.getBasis() * twist_rot;
00125   tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot);
00126 
00127   geometry_msgs::TwistStamped interframe_twist;
00128   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
00129 
00130   msg_out.header.stamp = msg_in.header.stamp;
00131   msg_out.header.frame_id = target_frame;
00132   msg_out.twist.linear.x =  out_vel.x() + interframe_twist.twist.linear.x;
00133   msg_out.twist.linear.y =  out_vel.y() + interframe_twist.twist.linear.y;
00134   msg_out.twist.linear.z =  out_vel.z() + interframe_twist.twist.linear.z;
00135   msg_out.twist.angular.x =  out_rot.x() + interframe_twist.twist.angular.x;
00136   msg_out.twist.angular.y =  out_rot.y() + interframe_twist.twist.angular.y;
00137   msg_out.twist.angular.z =  out_rot.z() + interframe_twist.twist.angular.z;
00138 
00139   }*/
00140 
00141 void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00142     const geometry_msgs::QuaternionStamped& msg_in,
00143     const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const
00144 {
00145   tf::assertQuaternionValid(msg_in.quaternion);
00146   Stamped<Quaternion> pin, pout;
00147   quaternionStampedMsgToTF(msg_in, pin);
00148   transformQuaternion(target_frame, target_time, pin, fixed_frame, pout);
00149   quaternionStampedTFToMsg(pout, msg_out);
00150 }
00151 
00152 void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time,
00153     const geometry_msgs::Vector3Stamped& msg_in,
00154     const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const
00155 {
00156   Stamped<Vector3> pin, pout;
00157   vector3StampedMsgToTF(msg_in, pin);
00158   transformVector(target_frame, target_time, pin, fixed_frame, pout);
00159   vector3StampedTFToMsg(pout, msg_out);
00160 }
00161 
00162 void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time,
00163     const geometry_msgs::PointStamped& msg_in,
00164     const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const
00165 {
00166   Stamped<Point> pin, pout;
00167   pointStampedMsgToTF(msg_in, pin);
00168   transformPoint(target_frame, target_time, pin, fixed_frame, pout);
00169   pointStampedTFToMsg(pout, msg_out);
00170 }
00171 
00172 void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time,
00173     const geometry_msgs::PoseStamped& msg_in,
00174     const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const
00175 {
00176   tf::assertQuaternionValid(msg_in.pose.orientation);
00177 
00178   Stamped<Pose> pin, pout;
00179   poseStampedMsgToTF(msg_in, pin);
00180   transformPose(target_frame, target_time, pin, fixed_frame, pout);
00181   poseStampedTFToMsg(pout, msg_out);
00182 }
00183 
00184 void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const
00185 {
00186   StampedTransform transform;
00187   lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform);
00188 
00189   transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut);
00190 }
00191 void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time, 
00192     const sensor_msgs::PointCloud& cloudIn,
00193     const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const
00194 {
00195   StampedTransform transform;
00196   lookupTransform(target_frame, target_time,
00197       cloudIn.header.frame_id, cloudIn.header.stamp,
00198       fixed_frame,
00199       transform);
00200 
00201   transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut);
00202 
00203 
00204 }
00205 
00206 inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
00207 {
00208   // Use temporary variables in case &in == &out
00209   double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
00210   double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
00211   double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
00212   
00213   out.x = x; out.y = y; out.z = z;
00214 }
00215 
00216 
00217 void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
00218                                             const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, 
00219                                             sensor_msgs::PointCloud & cloudOut) const
00220 {
00221   tf::Vector3 origin = net_transform.getOrigin();
00222   tf::Matrix3x3 basis  = net_transform.getBasis();
00223 
00224   unsigned int length = cloudIn.points.size();
00225 
00226   // Copy relevant data from cloudIn, if needed
00227   if (&cloudIn != &cloudOut)
00228   {
00229     cloudOut.header = cloudIn.header;
00230     cloudOut.points.resize(length);
00231     cloudOut.channels.resize(cloudIn.channels.size());
00232     for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
00233       cloudOut.channels[i] = cloudIn.channels[i];
00234   }
00235 
00236   // Transform points
00237   cloudOut.header.stamp = target_time;
00238   cloudOut.header.frame_id = target_frame;
00239   for (unsigned int i = 0; i < length ; i++) {
00240     transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
00241   }
00242 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Wed Jan 3 2018 03:41:26