예제 #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);
  }
예제 #2
0
  /** Converts the given TransformStorage datastructure to a TransformStamped message */
  protected TransformStamped TFToTransformStampedMsg(TransformStorage tf) {
    Vector3d tTF = tf.getTranslation();
    Quat4d rTF = tf.getRotation();

    // convert quaternion and translation vector to corresponding messages
    geometry_msgs.Vector3 tMsg =
        node.getTopicMessageFactory().newFromType(geometry_msgs.Vector3._TYPE);
    geometry_msgs.Quaternion rMsg =
        node.getTopicMessageFactory().newFromType(geometry_msgs.Quaternion._TYPE);
    tMsg.setX(tTF.x);
    tMsg.setY(tTF.y);
    tMsg.setZ(tTF.z);
    rMsg.setX(rTF.x);
    rMsg.setY(rTF.y);
    rMsg.setZ(rTF.z);
    rMsg.setW(rTF.w);

    // create TransformStamped message
    TransformStamped msg = node.getTopicMessageFactory().newFromType(TransformStamped._TYPE);
    msg.getHeader().setFrameId(tf.getParentFrame().getFrameID());
    msg.getHeader().setStamp(new Time(tf.getTimeStamp()));
    msg.setChildFrameId(tf.getChildFrame().getFrameID());
    msg.getTransform().setTranslation(tMsg);
    msg.getTransform().setRotation(rMsg);

    return msg;
  }