constructors_geometry.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from geometry_msgs.msg import (Point, Pose, Quaternion, TransformStamped,
4  Vector3)
5 
6 
7 ## Pose ##
8 
9 def ToPose(p, o):
10  return Pose(position=ToPoint(p), orientation=ToQuaternion(o))
11 
12 
13 ## Point ##
14 
15 def NewPoint(x, y, z):
16  return Point(x=x, y=y, z=z)
17 
18 
19 def ToPoint(p):
20  return NewPoint(*p)
21 
22 
23 def FromPoint(p):
24  return (p.x, p.y, p.z)
25 
26 
27 ## Orientation ##
28 
29 def NewQuaternion(x, y, z, w):
30  return Quaternion(x=x, y=y, z=z, w=w)
31 
32 
33 def ToQuaternion(q):
34  return NewQuaternion(*q)
35 
36 
38  return (o.x, o.y, o.z, o.w)
39 
40 
41 ## Vector3 ##
42 
43 def NewVector3(x, y, z):
44  return Vector3(x=x, y=y, z=z)
45 
46 
47 def ToVector3(p):
48  return NewVector3(*p)
49 
50 
51 def FromVector3(v):
52  return (v.x, v.y, v.z)
53 
54 
55 ## TransformStamped ##
56 
58  return (FromVector3(msg.transform.translation),
59  FromQuaternion(msg.transform.rotation))
60 
61 
62 def ToTransformStamped(translation, rotation, stamp, child, parent):
63  t = TransformStamped()
64  t.header.stamp = stamp
65  t.header.frame_id = parent
66  t.child_frame_id = child
67  t.transform.translation = ToVector3(translation)
68  t.transform.rotation = ToQuaternion(rotation)
69  return t
70 
71 # eof
def NewQuaternion(x, y, z, w)
Orientation ##.
def ToTransformStamped(translation, rotation, stamp, child, parent)
def FromTransformStamped(msg)
TransformStamped ##.


frame_editor
Author(s): ipa-lth , ipa-frn
autogenerated on Wed Oct 21 2020 03:36:00