ros python 构造 pose

#!/usr/bin/env python

import numpy as np
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from tf.transformations import quaternion_from_euler

#orientation x,y,z,w
#x=i[3]
#y=i[4]
#z=i[5]
#w=i[6]

g=np.array([i[3],i[4],i[5],i[6]])

#position x,y,z
#x=i[0]
#y=i[1]
#z=i[2]

print(Pose(Point(i[0],i[1],i[2]), Quaternion(*g)))