def makePoses(self):
'''
产生位姿矩阵序列
'''
poses = []
points = self.makePoints()
for i in range(len(points)):
UX = Vector(points[i][0][0], points[i][0][1], points[i][0][2])
UY = Vector(points[i][1][0], points[i][1][1], points[i][1][2])
UZ = Vector(points[i][2][0], points[i][2][1], points[i][2][2])
Rot = Rotation(UX, UY, UZ)
pose = Pose()
pose.position.x = points[i][3][0]
pose.position.y = points[i][3][1]
pose.position.z = points[i][3][2]
q = Rot.GetQuaternion()
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
poses.append(pose)
return poses
def run(self):
rosRate = Rate(30)
broadcaster = StaticTransformBroadcaster()
while not is_shutdown():
rot = Rotation(self._rotMatrixArray[0], self._rotMatrixArray[1],
self._rotMatrixArray[2], self._rotMatrixArray[3],
self._rotMatrixArray[4], self._rotMatrixArray[5],
self._rotMatrixArray[6], self._rotMatrixArray[7],
self._rotMatrixArray[8])
quat = rot.GetQuaternion()
staticTransform = self._setTransform(self._robotBaseFrame,
self._HDFrame, quat)
broadcaster.sendTransform(staticTransform)
rosRate.sleep()
def _getOrientation(self, data):
q = Quaternion()
if data:
global hydro
if hydro:
if 'theta' in data:
# Assume Yaw Orientation
orientation = Rotation.RotZ(data.get('theta'))
elif any(k in ['roll', 'pitch', 'yaw']
for k in data.iterkeys()):
# Assume RPY Orientation
orientation = Rotation.RPY(data.get('roll', 0),
data.get('pitch', 0),
data.get('yaw', 0))
elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()):
orientation = Rotation.Quaternion(data.get('x', 0),
data.get('y', 0),
data.get('z', 0),
data.get('w', 0))
else:
orientation = Rotation()
orientation = orientation.GetQuaternion()
else:
if 'theta' in data:
# Assume Yaw Orientation
orientation = quaternion_from_euler(
0, 0, data.get('theta'))
elif any(k in ['roll', 'pitch', 'yaw']
for k in data.iterkeys()):
# Assume RPY Orientation
orientation = quaternion_from_euler(
data.get('roll', 0), data.get('pitch', 0),
data.get('yaw', 0))
elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()):
orientation = (data.get('x', 0), data.get('y', 0),
data.get('z', 0), data.get('w', 0))
else:
orientation = (0, 0, 0, 0)
q.x, q.y, q.z, q.w = orientation
return q
