constructors_geometry.py

Go to the documentation of this file.

#!/usr/bin/env python
 
from geometry_msgs.msg import (Point, Pose, Quaternion, TransformStamped,
                               Vector3)
 
 
 
def ToPose(p, o):
    return Pose(position=ToPoint(p), orientation=ToQuaternion(o))
 
 
 
def NewPoint(x, y, z):
    return Point(x=x, y=y, z=z)
 
 
def ToPoint(p):
    return NewPoint(*p)
 
 
def FromPoint(p):
    return (p.x, p.y, p.z)
 
 
 
def NewQuaternion(x, y, z, w):
    return Quaternion(x=x, y=y, z=z, w=w)
 
 
def ToQuaternion(q):
    return NewQuaternion(*q)
 
 
def FromQuaternion(o):
    return (o.x, o.y, o.z, o.w)
 
 
 
def NewVector3(x, y, z):
    return Vector3(x=x, y=y, z=z)
 
 
def ToVector3(p):
    return NewVector3(*p)
 
 
def FromVector3(v):
    return (v.x, v.y, v.z)
 
 
 
def FromTransformStamped(msg):
    return (FromVector3(msg.transform.translation),
            FromQuaternion(msg.transform.rotation))
 
 
def ToTransformStamped(translation, rotation, stamp, child, parent):
    t = TransformStamped()
    t.header.stamp = stamp
    t.header.frame_id = parent
    t.child_frame_id = child
    t.transform.translation = ToVector3(translation)
    t.transform.rotation = ToQuaternion(rotation)
    return t
 
# eof