Source Code for Module tf.listener

  1  # Copyright (c) 2008, Willow Garage, Inc. 
  2  # All rights reserved. 
  3  #  
  4  # Redistribution and use in source and binary forms, with or without 
  5  # modification, are permitted provided that the following conditions are met: 
  6  #  
  7  #     * Redistributions of source code must retain the above copyright 
  8  #       notice, this list of conditions and the following disclaimer. 
  9  #     * Redistributions in binary form must reproduce the above copyright 
 10  #       notice, this list of conditions and the following disclaimer in the 
 11  #       documentation and/or other materials provided with the distribution. 
 12  #     * Neither the name of the Willow Garage, Inc. nor the names of its 
 13  #       contributors may be used to endorse or promote products derived from 
 14  #       this software without specific prior written permission. 
 15  #  
 16  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 17  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 18  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 19  # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 20  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 21  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 22  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 23  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 24  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 25  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 26  # POSSIBILITY OF SUCH DAMAGE. 
 27   
 28  PKG = 'tf' 
 29  import roslib 
 30  roslib.load_manifest(PKG) 
 31   
 32  import rospy 
 33  import tf as TFX 
 34  from tf import transformations 
 35  import numpy 
 36   
 37  from tf.msg import tfMessage 
 38  import geometry_msgs.msg 
 39  import sensor_msgs.msg 
 40  from tf.srv import FrameGraph,FrameGraphResponse 
 41   
 42  import threading 
 43   
 44 -def xyz_to_mat44(pos): 
 45      return transformations.translation_matrix((pos.x, pos.y, pos.z)) 
 46   
 47 -def xyzw_to_mat44(ori): 
 48      return transformations.quaternion_matrix((ori.x, ori.y, ori.z, ori.w)) 
 49   
 50  ## Extends tf's Transformer, adding transform methods for ROS message 
 51  ## types PointStamped, QuaternionStamped and PoseStamped. 
 52 -class TransformerROS(TFX.Transformer): 
 53      """ 
 54      TransformerROS extends the base class :class:`tf.Transformer`, 
 55      adding methods for handling ROS messages.  
 56      """ 
 57   
 58      ## Looks up the transform for ROS message header hdr to frame 
 59      ## target_frame, and returns the transform as a Numpy 4x4 matrix. 
 60      # @param target_frame The target frame 
 61      # @param hdr          A ROS message header object 
 62   
 63 -    def asMatrix(self, target_frame, hdr): 
 64          """ 
 65          :param target_frame: the tf target frame, a string 
 66          :param hdr: a message header 
 67          :return: a :class:`numpy.matrix` 4x4 representation of the transform 
 68          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
 69           
 70          Uses :meth:`lookupTransform` to look up the transform for ROS message header hdr to frame 
 71          target_frame, and returns the transform as a :class:`numpy.matrix` 
 72          4x4. 
 73          """ 
 74          translation,rotation = self.lookupTransform(target_frame, hdr.frame_id, hdr.stamp) 
 75          return self.fromTranslationRotation(translation, rotation) 
 76   
 77      ## Returns a Numpy 4x4 matrix for a transform. 
 78      # @param translation  translation as (x,y,z) 
 79      # @param rotation     rotation as (x,y,z,w) 
 80   
 81 -    def fromTranslationRotation(self, translation, rotation): 
 82          """ 
 83          :param translation: translation expressed as a tuple (x,y,z) 
 84          :param rotation: rotation quaternion expressed as a tuple (x,y,z,w) 
 85          :return: a :class:`numpy.matrix` 4x4 representation of the transform 
 86          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
 87           
 88          Converts a transformation from :class:`tf.Transformer` into a representation as a 4x4 matrix. 
 89          """ 
 90   
 91          return numpy.dot(transformations.translation_matrix(translation), transformations.quaternion_matrix(rotation)) 
 92   
 93      ## Transforms a geometry_msgs PointStamped message to frame target_frame, returns the resulting PointStamped. 
 94      # @param target_frame The target frame 
 95      # @param ps           geometry_msgs.msg.PointStamped object 
 96   
 97 -    def transformPoint(self, target_frame, ps): 
 98          """ 
 99          :param target_frame: the tf target frame, a string 
100          :param ps: the geometry_msgs.msg.PointStamped message 
101          :return: new geometry_msgs.msg.PointStamped message, in frame target_frame 
102          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
103   
104          Transforms a geometry_msgs PointStamped message to frame target_frame, returns a new PointStamped message. 
105          """ 
106   
107          mat44 = self.asMatrix(target_frame, ps.header) 
108          xyz = tuple(numpy.dot(mat44, numpy.array([ps.point.x, ps.point.y, ps.point.z, 1.0])))[:3] 
109          r = geometry_msgs.msg.PointStamped() 
110          r.header.stamp = ps.header.stamp 
111          r.header.frame_id = target_frame 
112          r.point = geometry_msgs.msg.Point(*xyz) 
113          return r 
114   
115      ## Transforms a geometry_msgs Vector3Stamped message to frame target_frame, returns the resulting Vector3Stamped. 
116      # @param target_frame The target frame 
117      # @param ps           geometry_msgs.msg.Vector3Stamped object 
118   
119 -    def transformVector3(self, target_frame, v3s): 
120          """ 
121          :param target_frame: the tf target frame, a string 
122          :param v3s: the geometry_msgs.msg.Vector3Stamped message 
123          :return: new geometry_msgs.msg.Vector3Stamped message, in frame target_frame 
124          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
125   
126          Transforms a geometry_msgs Vector3Stamped message to frame target_frame, returns a new Vector3Stamped message. 
127          """ 
128   
129          mat44 = self.asMatrix(target_frame, v3s.header) 
130          mat44[0,3] = 0.0 
131          mat44[1,3] = 0.0 
132          mat44[2,3] = 0.0 
133          xyz = tuple(numpy.dot(mat44, numpy.array([v3s.vector.x, v3s.vector.y, v3s.vector.z, 1.0])))[:3] 
134          r = geometry_msgs.msg.Vector3Stamped() 
135          r.header.stamp = v3s.header.stamp 
136          r.header.frame_id = target_frame 
137          r.vector = geometry_msgs.msg.Vector3(*xyz) 
138          return r 
139   
140      ## Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns the resulting QuaternionStamped. 
141      # @param target_frame The target frame 
142      # @param ps           geometry_msgs.msg.QuaternionStamped object 
143   
144 -    def transformQuaternion(self, target_frame, ps): 
145          """ 
146          :param target_frame: the tf target frame, a string 
147          :param ps: the geometry_msgs.msg.QuaternionStamped message 
148          :return: new geometry_msgs.msg.QuaternionStamped message, in frame target_frame 
149          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
150   
151          Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns a new QuaternionStamped message. 
152          """ 
153   
154          # mat44 is frame-to-frame transform as a 4x4 
155          mat44 = self.asMatrix(target_frame, ps.header) 
156   
157          # pose44 is the given quat as a 4x4 
158          pose44 = xyzw_to_mat44(ps.quaternion) 
159   
160          # txpose is the new pose in target_frame as a 4x4 
161          txpose = numpy.dot(mat44, pose44) 
162   
163          # quat is orientation of txpose 
164          quat = tuple(transformations.quaternion_from_matrix(txpose)) 
165   
166          # assemble return value QuaternionStamped 
167          r = geometry_msgs.msg.QuaternionStamped() 
168          r.header.stamp = ps.header.stamp 
169          r.header.frame_id = target_frame 
170          r.quaternion = geometry_msgs.msg.Quaternion(*quat) 
171          return r 
172   
173      ## Transforms a geometry_msgs PoseStamped message to frame target_frame, returns the resulting PoseStamped. 
174      # @param target_frame The target frame 
175      # @param ps           geometry_msgs.msg.PoseStamped object 
176   
177 -    def transformPose(self, target_frame, ps): 
178          """ 
179          :param target_frame: the tf target frame, a string 
180          :param ps: the geometry_msgs.msg.PoseStamped message 
181          :return: new geometry_msgs.msg.PoseStamped message, in frame target_frame 
182          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
183   
184          Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message. 
185          """ 
186          # mat44 is frame-to-frame transform as a 4x4 
187          mat44 = self.asMatrix(target_frame, ps.header) 
188   
189          # pose44 is the given pose as a 4x4 
190          pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation)) 
191   
192          # txpose is the new pose in target_frame as a 4x4 
193          txpose = numpy.dot(mat44, pose44) 
194   
195          # xyz and quat are txpose's position and orientation 
196          xyz = tuple(transformations.translation_from_matrix(txpose))[:3] 
197          quat = tuple(transformations.quaternion_from_matrix(txpose)) 
198   
199          # assemble return value PoseStamped 
200          r = geometry_msgs.msg.PoseStamped() 
201          r.header.stamp = ps.header.stamp 
202          r.header.frame_id = target_frame 
203          r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat)) 
204          return r 
205   
206 -    def transformPointCloud(self, target_frame, point_cloud): 
207          """ 
208          :param target_frame: the tf target frame, a string 
209          :param ps: the sensor_msgs.msg.PointCloud message 
210          :return: new sensor_msgs.msg.PointCloud message, in frame target_frame 
211          :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise 
212   
213          Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message. 
214          """ 
215          r = sensor_msgs.msg.PointCloud() 
216          r.header.stamp = point_cloud.header.stamp 
217          r.header.frame_id = target_frame 
218          r.channels = point_cloud.channels 
219   
220          mat44 = self.asMatrix(target_frame, point_cloud.header) 
221          def xf(p): 
222              xyz = tuple(numpy.dot(mat44, numpy.array([p.x, p.y, p.z, 1.0])))[:3] 
223              return geometry_msgs.msg.Point(*xyz) 
224          r.points = [xf(p) for p in point_cloud.points] 
225          return r 
226   
227  ## Extends TransformerROS, subscribes to the /tf topic and 
228  ## updates the Transformer with the messages. 
229   
230 -class TransformListenerThread(threading.Thread): 
231 -    def __init__(self, tl): 
232          threading.Thread.__init__(self) 
233          self.tl = tl 
234       
235 -    def run(self): 
236          rospy.Subscriber("/tf",         tfMessage, self.transformlistener_callback) 
237          self.tl.frame_graph_server = rospy.Service('~tf_frames', FrameGraph, self.frame_graph_service) 
238          rospy.spin() 
239   
240 -    def transformlistener_callback(self, data): 
241          who = data._connection_header.get('callerid', "default_authority") 
242          for transform in data.transforms: 
243              self.tl.setTransform(transform, who) 
244   
245 -    def frame_graph_service(self, req): 
246          return FrameGraphResponse(self.tl.allFramesAsDot()) 
247   
248   
249 -class TransformListener(TransformerROS): 
250   
251      """ 
252      TransformListener is a subclass of :class:`tf.TransformerROS` that 
253      subscribes to the ``"/tf"`` message topic, and calls :meth:`tf.Transformer.setTransform` 
254      with each incoming transformation message. 
255   
256      In this way a TransformListener object automatically 
257      stays up to to date with all current transforms.  Typical usage might be:: 
258   
259          import tf 
260          from geometry_msgs.msg import PointStamped 
261   
262          class MyNode: 
263   
264              def __init__(self): 
265   
266                  self.tl = tf.TransformListener() 
267                  rospy.Subscriber("/sometopic", PointStamped, self.some_message_handler) 
268                  ... 
269               
270              def some_message_handler(self, point_stamped): 
271   
272                  # want to work on the point in the "world" frame 
273                  point_in_world = self.tl.transformPoint("world", point_stamped) 
274                  ... 
275           
276      """ 
277 -    def __init__(self, *args): 
278          TransformerROS.__init__(self, *args) 
279          thr = TransformListenerThread(self) 
280          thr.setDaemon(True) 
281          thr.start() 
282          self.setUsingDedicatedThread(True) 
283