transform_listener.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #include "tf/transform_listener.h"
33 
34 
35 using namespace tf;
36 std::string tf::remap(const std::string& frame_id)
37 {
38  ros::NodeHandle n("~");
39  return tf::resolve(getPrefixParam(n), frame_id);
40 };
41 
42 
43 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread):
44  Transformer(true, max_cache_time), tf2_listener_(Transformer::tf2_buffer_, node_, spin_thread)
45 {
46  //Everything is done inside tf2 init
47 }
48 
49 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread):
50  Transformer(true, max_cache_time), node_(nh), tf2_listener_(Transformer::tf2_buffer_, nh, spin_thread)
51 {
52  //Everything is done inside tf2 init
53 }
54 
56 {
57  //Everything is done inside tf2 init
58 }
59 
60 //Override Transformer::ok() for ticket:4882
61 bool TransformListener::ok() const { return ros::ok(); }
62 
63 
64 
65 void TransformListener::transformQuaternion(const std::string& target_frame,
66  const geometry_msgs::QuaternionStamped& msg_in,
67  geometry_msgs::QuaternionStamped& msg_out) const
68 {
69  tf::assertQuaternionValid(msg_in.quaternion);
70 
71  Stamped<Quaternion> pin, pout;
72  quaternionStampedMsgToTF(msg_in, pin);
73  transformQuaternion(target_frame, pin, pout);
74  quaternionStampedTFToMsg(pout, msg_out);
75 }
76 
77 void TransformListener::transformVector(const std::string& target_frame,
78  const geometry_msgs::Vector3Stamped& msg_in,
79  geometry_msgs::Vector3Stamped& msg_out) const
80 {
81  Stamped<Vector3> pin, pout;
82  vector3StampedMsgToTF(msg_in, pin);
83  transformVector(target_frame, pin, pout);
84  vector3StampedTFToMsg(pout, msg_out);
85 }
86 
87 void TransformListener::transformPoint(const std::string& target_frame,
88  const geometry_msgs::PointStamped& msg_in,
89  geometry_msgs::PointStamped& msg_out) const
90 {
91  Stamped<Point> pin, pout;
92  pointStampedMsgToTF(msg_in, pin);
93  transformPoint(target_frame, pin, pout);
94  pointStampedTFToMsg(pout, msg_out);
95 }
96 
97 void TransformListener::transformPose(const std::string& target_frame,
98  const geometry_msgs::PoseStamped& msg_in,
99  geometry_msgs::PoseStamped& msg_out) const
100 {
101  tf::assertQuaternionValid(msg_in.pose.orientation);
102 
103  Stamped<Pose> pin, pout;
104  poseStampedMsgToTF(msg_in, pin);
105  transformPose(target_frame, pin, pout);
106  poseStampedTFToMsg(pout, msg_out);
107 }
108 /* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review
109 void TransformListener::transformTwist(const std::string& target_frame,
110  const geometry_msgs::TwistStamped& msg_in,
111  geometry_msgs::TwistStamped& msg_out) const
112 {
113  tf::Vector3 twist_rot(msg_in.twist.angular.x,
114  msg_in.twist.angular.y,
115  msg_in.twist.angular.z);
116  tf::Vector3 twist_vel(msg_in.twist.linear.x,
117  msg_in.twist.linear.y,
118  msg_in.twist.linear.z);
119 
120  tf::StampedTransform transform;
121  lookupTransform(target_frame,msg_in.header.frame_id, msg_in.header.stamp, transform);
122 
123 
124  tf::Vector3 out_rot = transform.getBasis() * twist_rot;
125  tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot);
126 
127  geometry_msgs::TwistStamped interframe_twist;
128  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
129 
130  msg_out.header.stamp = msg_in.header.stamp;
131  msg_out.header.frame_id = target_frame;
132  msg_out.twist.linear.x = out_vel.x() + interframe_twist.twist.linear.x;
133  msg_out.twist.linear.y = out_vel.y() + interframe_twist.twist.linear.y;
134  msg_out.twist.linear.z = out_vel.z() + interframe_twist.twist.linear.z;
135  msg_out.twist.angular.x = out_rot.x() + interframe_twist.twist.angular.x;
136  msg_out.twist.angular.y = out_rot.y() + interframe_twist.twist.angular.y;
137  msg_out.twist.angular.z = out_rot.z() + interframe_twist.twist.angular.z;
138 
139  }*/
140 
141 void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
142  const geometry_msgs::QuaternionStamped& msg_in,
143  const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const
144 {
145  tf::assertQuaternionValid(msg_in.quaternion);
146  Stamped<Quaternion> pin, pout;
147  quaternionStampedMsgToTF(msg_in, pin);
148  transformQuaternion(target_frame, target_time, pin, fixed_frame, pout);
149  quaternionStampedTFToMsg(pout, msg_out);
150 }
151 
152 void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time,
153  const geometry_msgs::Vector3Stamped& msg_in,
154  const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const
155 {
156  Stamped<Vector3> pin, pout;
157  vector3StampedMsgToTF(msg_in, pin);
158  transformVector(target_frame, target_time, pin, fixed_frame, pout);
159  vector3StampedTFToMsg(pout, msg_out);
160 }
161 
162 void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time,
163  const geometry_msgs::PointStamped& msg_in,
164  const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const
165 {
166  Stamped<Point> pin, pout;
167  pointStampedMsgToTF(msg_in, pin);
168  transformPoint(target_frame, target_time, pin, fixed_frame, pout);
169  pointStampedTFToMsg(pout, msg_out);
170 }
171 
172 void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time,
173  const geometry_msgs::PoseStamped& msg_in,
174  const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const
175 {
176  tf::assertQuaternionValid(msg_in.pose.orientation);
177 
178  Stamped<Pose> pin, pout;
179  poseStampedMsgToTF(msg_in, pin);
180  transformPose(target_frame, target_time, pin, fixed_frame, pout);
181  poseStampedTFToMsg(pout, msg_out);
182 }
183 
184 void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const
185 {
186  StampedTransform transform;
187  lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform);
188 
189  transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut);
190 }
191 void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time,
192  const sensor_msgs::PointCloud& cloudIn,
193  const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const
194 {
195  StampedTransform transform;
196  lookupTransform(target_frame, target_time,
197  cloudIn.header.frame_id, cloudIn.header.stamp,
198  fixed_frame,
199  transform);
200 
201  transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut);
202 
203 
204 }
205 
206 inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
207 {
208  // Use temporary variables in case &in == &out
209  double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x();
210  double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y();
211  double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z();
212 
213  out.x = x; out.y = y; out.z = z;
214 }
215 
216 
217 void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
218  const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn,
219  sensor_msgs::PointCloud & cloudOut) const
220 {
221  tf::Vector3 origin = net_transform.getOrigin();
222  tf::Matrix3x3 basis = net_transform.getBasis();
223 
224  unsigned int length = cloudIn.points.size();
225 
226  // Copy relevant data from cloudIn, if needed
227  if (&cloudIn != &cloudOut)
228  {
229  cloudOut.header = cloudIn.header;
230  cloudOut.points.resize(length);
231  cloudOut.channels.resize(cloudIn.channels.size());
232  for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
233  cloudOut.channels[i] = cloudIn.channels[i];
234  }
235 
236  // Transform points
237  cloudOut.header.stamp = target_time;
238  cloudOut.header.frame_id = target_frame;
239  for (unsigned int i = 0; i < length ; i++) {
240  transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
241  }
242 }
void assertQuaternionValid(const tf::Quaternion &q)
Throw InvalidArgument if quaternion is malformed.
Definition: tf.h:393
std::string remap(const std::string &frame_id) __attribute__((deprecated))
resolve names
std::string getPrefixParam(ros::NodeHandle &nh)
Get the tf_prefix from the parameter server.
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:110
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
convert Stamped<Pose> to PoseStamped msg
static void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped &msg, Stamped< Quaternion > &bt)
convert QuaternionStamped msg to Stamped<Quaternion>
static void vector3StampedTFToMsg(const Stamped< Vector3 > &bt, geometry_msgs::Vector3Stamped &msg)
convert Stamped<Vector3> to Vector3Stamped msg
void transformPoint(const std::string &target_frame, const geometry_msgs::PointStamped &stamped_in, geometry_msgs::PointStamped &stamped_out) const
Transform a Stamped Point Message into the target frame This can throw all that lookupTransform can t...
Definition: exceptions.h:38
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:31
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
Definition: tf.cpp:159
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
static void pointStampedMsgToTF(const geometry_msgs::PointStamped &msg, Stamped< Point > &bt)
convert PointStamped msg to Stamped<Point>
tf2_ros::TransformListener tf2_listener_
replacing implementation with tf2_ros&#39;
void transformQuaternion(const std::string &target_frame, const geometry_msgs::QuaternionStamped &stamped_in, geometry_msgs::QuaternionStamped &stamped_out) const
Transform a Stamped Quaternion Message into the target frame This can throw all that lookupTransform ...
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
void transformPointCloud(const std::string &target_frame, const sensor_msgs::PointCloud &pcin, sensor_msgs::PointCloud &pcout) const
Transform a sensor_msgs::PointCloud natively This can throw all that lookupTransform can throw as wel...
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:267
TransformListener(ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)
Constructor for transform listener.
static void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped &msg, Stamped< Vector3 > &bt)
convert Vector3Stamped msg to Stamped<Vector3>
static void pointStampedTFToMsg(const Stamped< Point > &bt, geometry_msgs::PointStamped &msg)
convert Stamped<Point> to PointStamped msg
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
static void quaternionStampedTFToMsg(const Stamped< Quaternion > &bt, geometry_msgs::QuaternionStamped &msg)
convert Stamped<Quaternion> to QuaternionStamped msg
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
Get the transform between two frames by frame ID.
Definition: tf.cpp:238
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Transform a Stamped Pose Message into the target frame This can throw all that lookupTransform can th...
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
convert PoseStamped msg to Stamped<Pose>
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Return the length of a quaternion.
Definition: Quaternion.h:398
tf2_ros::Buffer tf2_buffer_
Definition: tf.h:387
void transformVector(const std::string &target_frame, const geometry_msgs::Vector3Stamped &stamped_in, geometry_msgs::Vector3Stamped &stamped_out) const
Transform a Stamped Vector Message into the target frame This can throw all that lookupTransform can ...
The Stamped Transform datatype used by tf.
void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out)
A Class which provides coordinate transforms between any two frames in a system.
Definition: tf.h:90
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Tue Jan 2 2018 03:17:40