Пример #1
0
    @Override
    public void run() {
      try {
        Float32 proximity = ultasonicPublisher.newMessage();
        proximity.setData(brick.SENSOR.readUltrasonic(NXT.Sensor.P4));
        ultasonicPublisher.publish(proximity);

        Float32 voltage = voltagePublisher.newMessage();
        voltage.setData((float) brick.SYSTEM.getVBatt());
        voltagePublisher.publish(voltage);

        Odometry odometry = odometryPublisher.newMessage();
        setupHeader(odometry.getHeader(), seq);
        odometry.setChildFrameId("center");

        TransformStamped tfs = tfPublisher.newMessage();
        setupHeader(tfs.getHeader(), seq);
        tfs.setChildFrameId("center");

        long tachoL = brick.MOTOR.getAbsoluteTacho(NXT.Motor.A);
        long tachoR = brick.MOTOR.getAbsoluteTacho(NXT.Motor.B);
        odoComputer.computeOdometry(tachoL, tachoR, odometry, tfs);
        seq++;
        odometryPublisher.publish(odometry);
        tfPublisher.publish(tfs);
      } catch (IOException e) {
        connectedNode.getLog().error("Sensors error", e);
      }
    }
Пример #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;
  }
Пример #3
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;
  }
Пример #4
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);
  }
 /**
  * Updates the tree with the provided {@link geometry_msgs.TransformStamped} message.
  *
  * <p>Note that the tree is updated lazily. Modifications to the provided {@link
  * geometry_msgs.TransformStamped} message may cause unpredictable results.
  *
  * @param transformStamped the {@link geometry_msgs.TransformStamped} message to update with
  */
 public void update(geometry_msgs.TransformStamped transformStamped) {
   Preconditions.checkNotNull(transformStamped);
   GraphName source = GraphName.of(transformStamped.getChildFrameId());
   LazyFrameTransform lazyFrameTransform = new LazyFrameTransform(transformStamped);
   add(source, lazyFrameTransform);
 }