Пример #1
0
  /** Converts transform (a geometry msg) to a TransformStorage object and adds it to the buffer. */
  protected boolean setTransform(TransformStamped transform) {
    // resolve the frame ID's
    String childFrameID = assertResolved(tfPrefix, transform.getChildFrameId());
    String frameID = assertResolved(tfPrefix, transform.getHeader().getFrameId());

    boolean errorExists = false;
    if (childFrameID == frameID) {
      node.getLog()
          .error(
              "TF_SELF_TRANSFORM: Ignoring transform with frame_id and child_frame_id  \""
                  + childFrameID
                  + "\" because they are the same");
      errorExists = true;
    }

    if (childFrameID == "/") { // empty frame id will be mapped to "/"
      node.getLog()
          .error("TF_NO_CHILD_FRAME_ID: Ignoring transform because child_frame_id not set ");
      errorExists = true;
    }

    if (frameID == "/") { // empty parent id will be mapped to "/"
      node.getLog()
          .error(
              "TF_NO_FRAME_ID: Ignoring transform with child_frame_id \""
                  + childFrameID
                  + "\" because frame_id not set");
      errorExists = true;
    }

    if (errorExists) return false;

    // lookup or insert child frame
    Frame frame = lookupOrInsertFrame(childFrameID);

    // convert tf message to JTransform datastructure
    transform.setChildFrameId(childFrameID);
    transform.getHeader().setFrameId(frameID);
    TransformStorage tf = transformStampedMsgToTF(transform);

    // try to insert tf in corresponding time cache. If result is FALSE, the tf contains old data.
    if (!frame.insertData(tf)) {
      node.getLog()
          .warn(
              "TF_OLD_DATA ignoring data from the past for frame \""
                  + childFrameID
                  + "\" at time "
                  + ((double) tf.getTimeStamp() / 1E9));
      return false;
    }

    return true;
  }
Пример #2
0
 /**
  * Returns the resolves version of the given frame ID, and asserts a debug message if the name was
  * not fully resolved.
  */
 private String assertResolved(String prefix, String frameID) {
   if (!frameID.startsWith("/"))
     node.getLog()
         .debug(
             "TF operating on not fully resolved frame id "
                 + frameID
                 + ", resolving using local prefix "
                 + prefix);
   return resolve(prefix, frameID);
 }
Пример #3
0
  /**
   * Returns the transform from the specified source frame to the target frame at a given time;
   * returns null if no transformation could be found.
   */
  public StampedTransform lookupTransform(String targetFrameID, String sourceFrameID, Time time) {
    // resolve the source and target IDs
    String resolvedTargetID = assertResolved(tfPrefix, targetFrameID);
    String resolvedSourceID = assertResolved(tfPrefix, sourceFrameID);

    // if source and target are the same, return the identity transform
    if (resolvedSourceID == resolvedTargetID) {
      StampedTransform out = StampedTransform.getIdentity();
      out.timeStamp = time;
      out.frameID = resolvedSourceID;
      out.childFrameID = resolvedTargetID;
      return out;
    }

    // lookup source and target frame
    Frame sourceFrame = frames.get(resolvedSourceID);
    Frame targetFrame = frames.get(resolvedTargetID);

    if (sourceFrame == null) {
      node.getLog()
          .error("Cannot transform: source frame \"" + resolvedSourceID + "\" does not exist.");
      return null;
    }

    if (targetFrame == null) {
      node.getLog()
          .error("Cannot transform: target frame \"" + resolvedTargetID + "\" does not exist.");
      return null;
    }

    // list that will contain transformations from source frame to some frame F
    LinkedList<TransformStorage> inverseTransforms = new LinkedList<TransformStorage>();
    // list that will contain transformations from frame F to target frame
    LinkedList<TransformStorage> forwardTransforms = new LinkedList<TransformStorage>();

    // fill the lists using lookupLists. If it returns FALSE, no transformation could be found.
    if (!lookupLists(
        targetFrame, sourceFrame, time.totalNsecs(), inverseTransforms, forwardTransforms)) {
      // TODO give warning
      node.getLog()
          .error(
              "Cannot transform: source + \""
                  + resolvedSourceID
                  + "\" and target \""
                  + resolvedTargetID
                  + "\" are not connected.");
      return null;
    }

    // create an identity transform with the correct time stamp
    StampedTransform out = StampedTransform.getIdentity();
    out.timeStamp = time;

    // multiply all transforms from source frame to frame F TODO: right?
    for (TransformStorage t : inverseTransforms) {
      out.mul(StorageToStampedTransform(t));
    }

    // multiply all transforms from frame F to target frame TODO: right?
    for (TransformStorage t : forwardTransforms) {
      out.mul(StorageToStampedTransform(t).invert(), out);
    }

    // return transform
    return out;
  }