示例#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;
  }
示例#2
0
 /**
  * Converts the TransformStorage datastructure (represented by quaternion and vector) to the
  * StampedTransform datastructure (represented by a 4x4 matrix)
  */
 protected StampedTransform StorageToStampedTransform(TransformStorage ts) {
   return new StampedTransform(
       ts.getTranslation(),
       ts.getRotation(),
       new Time(ts.getTimeStamp()),
       ts.getParentFrame().getFrameID(),
       ts.getChildFrame().getFrameID());
 }