constructors_geometry.py
Go to the documentation of this file.
```00001 #!/usr/bin/env python
00002
00003 from geometry_msgs.msg import (Point, Pose, Quaternion, TransformStamped,
00004                                Vector3)
00005
00006
00007 ## Pose ##
00008 ##
00009 def ToPose(p, o):
00010     return Pose(position=ToPoint(p), orientation=ToQuaternion(o))
00011
00012
00013 ## Point ##
00014 ##
00015 def NewPoint(x, y, z):
00016     return Point(x=x, y=y, z=z)
00017
00018
00019 def ToPoint(p):
00020     return NewPoint(*p)
00021
00022
00023 def FromPoint(p):
00024     return (p.x, p.y, p.z)
00025
00026
00027 ## Orientation ##
00028 ##
00029 def NewQuaternion(x, y, z, w):
00030     return Quaternion(x=x, y=y, z=z, w=w)
00031
00032
00033 def ToQuaternion(q):
00034     return NewQuaternion(*q)
00035
00036
00037 def FromQuaternion(o):
00038     return (o.x, o.y, o.z, o.w)
00039
00040
00041 ## Vector3 ##
00042 ##
00043 def NewVector3(x, y, z):
00044     return Vector3(x=x, y=y, z=z)
00045
00046
00047 def ToVector3(p):
00048     return NewVector3(*p)
00049
00050
00051 def FromVector3(v):
00052     return (v.x, v.y, v.z)
00053
00054
00055 ## TransformStamped ##
00056 ##
00057 def FromTransformStamped(msg):
00058     return (FromVector3(msg.transform.translation),
00059             FromQuaternion(msg.transform.rotation))
00060
00061
00062 def ToTransformStamped(translation, rotation, stamp, child, parent):
00063     t = TransformStamped()