Beispiel #1
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;
  }
 private static void packQuat4dToGeometry_msgsQuaternion(Quat4d quat, Quaternion orientation) {
   orientation.setW(quat.getW());
   orientation.setX(quat.getX());
   orientation.setY(quat.getY());
   orientation.setZ(quat.getZ());
 }