コード例 #1
0
ファイル: TFListener.java プロジェクト: andreihaidu/knowrob
  /** 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
ファイル: TFListener.java プロジェクト: andreihaidu/knowrob
  /** 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);
  }
コード例 #3
0
 public static void packVector3fToGeometry_msgsVector3(Vector3f vector, Vector3 rosVectorToPack) {
   rosVectorToPack.setX(vector.getX());
   rosVectorToPack.setY(vector.getY());
   rosVectorToPack.setZ(vector.getZ());
 }
コード例 #4
0
 public static void packRosVector3ToVector3d(Vector3 rosVector, Vector3d vectorToPack) {
   vectorToPack.setX(rosVector.getX());
   vectorToPack.setY(rosVector.getY());
   vectorToPack.setZ(rosVector.getZ());
 }