示例#1
0
  /** Converts the given TransformStamped message to the TransformStorage datastructure */
  protected TransformStorage transformStampedMsgToTF(TransformStamped msg) {
    geometry_msgs.Vector3 tMsg = msg.getTransform().getTranslation();
    geometry_msgs.Quaternion rMsg = msg.getTransform().getRotation();

    // add frames to map
    Frame childFrame = lookupOrInsertFrame(msg.getChildFrameId());
    Frame parentFrame = lookupOrInsertFrame(msg.getHeader().getFrameId());

    return new TransformStorage(
        new Vector3d(tMsg.getX(), tMsg.getY(), tMsg.getZ()),
        new Quat4d(rMsg.getX(), rMsg.getY(), rMsg.getZ(), rMsg.getW()),
        msg.getHeader().getStamp().totalNsecs(),
        parentFrame,
        childFrame);
  }
 public static void packRosQuaternionToQuat4d(Quaternion rosQuat, Quat4d quat) {
   quat.setX(rosQuat.getX());
   quat.setY(rosQuat.getY());
   quat.setZ(rosQuat.getZ());
   quat.setW(rosQuat.getW());
 }