geometry.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2010, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Revision $Id: geometry.py 13195 2011-02-13 20:09:44Z kwc $
00034 
00035 """
00036 Convenience classes for using code from the ROS geometry stack.
00037 """
00038 
00039 import rospy
00040 
00041 _error = None
00042 try:
00043     import geometry_msgs.msg
00044 except ImportError:
00045     _error = "cannot import geometry_msgs.msg, geometry integration will be disabled"
00046 
00047 if not _error:    
00048     try:
00049         import tf
00050         import tf.posemath
00051     except ImportError:
00052         _error = "cannot import tf, geometry integration will be disabled"
00053 
00054 # API review suggested this being in geometry, but no consensus on what the correct API is
00055 def header(stamp=None, frame_id=''):
00056     """
00057     Construct a std_msgs/Header instance.  The timestamp defaults to
00058     the current ROS time if no stamp is specified (or None).
00059     """
00060     if stamp is None:
00061         return rospy.Header(stamp=rospy.get_rostime(), frame_id=frame_id)
00062     else:
00063         return rospy.Header(stamp=stamp, frame_id=frame_id)
00064     
00065 #TODO: what's the best default frame_id?
00066 _DEFAULT_FRAME_ID = 'base_link'
00067 
00068 class Point(geometry_msgs.msg.Point):
00069     def __init__(self, x=0., y=0., z=0.):
00070         geometry_msgs.msg.Point.__init__(self, x, y, z)
00071 
00072 class Quaternion(geometry_msgs.msg.Quaternion):
00073     def __init__(self, x=0., y=0., z=0., w=1.):
00074         geometry_msgs.msg.Quaternion.__init__(self, x, y, z, w)
00075 
00076 class QuaternionStamped(geometry_msgs.msg.QuaternionStamped):
00077     def __init__(self, x=0., y=0., z=0., w=1., frame_id=_DEFAULT_FRAME_ID, stamp=None):
00078         """
00079         @param stamp: Header timestamp. defaults to rospy.Time(0)
00080         """
00081         geometry_msgs.msg.QuaternionStamped.__init__(self, None, geometry_msgs.msg.Quaternion(x, y, z, w))
00082         self.header.frame_id = frame_id
00083         if stamp is None:
00084             self.header.stamp = rospy.Time(0)
00085         else:
00086             self.header.stamp = stamp            
00087 
00088     def _transform(self, listener, target):
00089         """
00090         API for auto-transforming via ROSH tf bindings.
00091         """
00092         return listener.transformQuaternion(target, self)
00093         
00094 class PointStamped(geometry_msgs.msg.PointStamped):
00095     def __init__(self, x=0., y=0., z=0., frame_id=_DEFAULT_FRAME_ID, stamp=None):
00096         """
00097         @param stamp: Header timestamp. defaults to rospy.Time(0)
00098         """
00099         geometry_msgs.msg.PointStamped.__init__(self, None, geometry_msgs.msg.Point(x, y, z))
00100         self.header.frame_id = frame_id
00101         if stamp is None:
00102             self.header.stamp = rospy.Time(0)
00103         else:
00104             self.header.stamp = stamp            
00105             
00106     def _transform(self, listener, target):
00107         """
00108         API for auto-transforming via ROSH tf bindings.
00109         """
00110         return listener.transformPoint(target, self)
00111 
00112 class Vector3Stamped(geometry_msgs.msg.Vector3Stamped):
00113     def __init__(self, x=0., y=0., z=0., frame_id=_DEFAULT_FRAME_ID, stamp=None):
00114         """
00115         @param stamp: Header timestamp. defaults to rospy.Time(0)
00116         """
00117         geometry_msgs.msg.Vector3Stamped.__init__(self, None, geometry_msgs.msg.Vector3(x, y, z))
00118         self.header.frame_id = frame_id
00119         if stamp is None:
00120             self.header.stamp = rospy.Time(0)
00121         else:
00122             self.header.stamp = stamp            
00123 
00124     def _transform(self, listener, target):
00125         """
00126         API for auto-transforming via ROSH tf bindings.
00127         """
00128         return listener.transformVector3(target, self)
00129     
00130 class PoseStamped(geometry_msgs.msg.PoseStamped):
00131 
00132     def __init__(self, position=None, orientation=None, frame_id=_DEFAULT_FRAME_ID, stamp=None):
00133         """
00134         @param stamp: Header timestamp. defaults to rospy.Time(0)
00135         """
00136         geometry_msgs.msg.PoseStamped.__init__(self)
00137         self.header.frame_id = frame_id
00138         if stamp is None:
00139             self.header.stamp = rospy.Time(0)
00140         else:
00141             self.header.stamp = stamp            
00142 
00143         if position is not None:
00144             p = self.pose.position
00145             p.x = position.x
00146             p.y = position.y
00147             p.z = position.z
00148 
00149         if orientation is None:
00150             self.pose.orientation.w = 1
00151         else:
00152             o = self.pose.orientation
00153             o.x = orientation.x
00154             o.y = orientation.y
00155             o.z = orientation.z
00156             o.w = orientation.w            
00157             
00158     def _transform(self, listener, target):
00159         """
00160         API for auto-transforming via ROSH tf bindings.
00161         """
00162         return listener.transformPose(target, self)
00163     
00164     @staticmethod
00165     def from_2d(x=0., y=0., th=0., frame_id=_DEFAULT_FRAME_ID):
00166         """
00167         Create PoseStamped instance from x, y, theta
00168         """
00169         pose = tf.posemath.fromEuler(x, y, 0., 0., 0., th).toMsg()
00170         ps = PoseStamped(frame_id=frame_id)
00171         ps.pose = pose
00172         return ps
00173 
00174     def to_2d(self):
00175         """
00176         Convert pose to x, y, theta. This removes any z elements as well as roll and pitch.
00177         @return: (x, y, rz)
00178         """
00179         pm = tf.posemath.PoseMath(self)
00180         x, y, z, rx, ry, rz = pm.asEuler()
00181         return x, y, rz
00182 
00183     def to_euler():
00184         """
00185         Convert pose to x, y, z, rx, ry, rz. 
00186         @return: (x, y, z, rx, ry, rz)
00187         """
00188         pm = tf.posemath.PoseMath(self)
00189         return pm.asEuler()
00190 
00191 


rosh_geometry
Author(s): Ken Conley
autogenerated on Fri Aug 28 2015 12:40:19