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