public HeadToSubFrameTransformBuffer(int size) { this.size = size; transforms = new TimeStampedTransform3D[size]; for (int i = 0; i < size; i++) { transforms[i] = new TimeStampedTransform3D(); transforms[i].setTimeStamp(Long.MIN_VALUE); } }
public TimeStampedTransformBuffer(int size) { this.size = size; this.buffer = new TimeStampedTransform3D[size]; for(int i = 0; i < buffer.length; i++) { this.buffer[i] = new TimeStampedTransform3D(); this.buffer[i].setTimeStamp(Long.MAX_VALUE); } currentIndex = 0; }
public TimeStampedTransformBuffer(int size) { this.size = size; this.buffer = new TimeStampedTransform3D[size]; for(int i = 0; i < buffer.length; i++) { this.buffer[i] = new TimeStampedTransform3D(); this.buffer[i].setTimeStamp(Long.MAX_VALUE); } currentIndex = 0; }
public void put(RigidBodyTransform newestTransform, long timeStamp) { TimeStampedTransform3D recycledTimestampedTransformFromBuffer = buffer[currentIndex]; recycledTimestampedTransformFromBuffer.setTimeStamp(timeStamp); recycledTimestampedTransformFromBuffer.setTransform3D(newestTransform); newestTimestamp = timeStamp; currentIndex++; if (currentIndex >= size) { currentIndex = 0; filledBufferAtleastOnce = true; } if (oldestTimeStamp == Long.MAX_VALUE) { oldestTimeStamp = newestTimestamp; } else if(filledBufferAtleastOnce) { oldestTimeStamp = buffer[currentIndex].getTimeStamp(); } }
public void put(RigidBodyTransform newestTransform, long timeStamp) { TimeStampedTransform3D recycledTimestampedTransformFromBuffer = buffer[currentIndex]; recycledTimestampedTransformFromBuffer.setTimeStamp(timeStamp); recycledTimestampedTransformFromBuffer.setTransform3D(newestTransform); newestTimestamp = timeStamp; currentIndex++; if (currentIndex >= size) { currentIndex = 0; filledBufferAtleastOnce = true; } if (oldestTimeStamp == Long.MAX_VALUE) { oldestTimeStamp = newestTimestamp; } else if(filledBufferAtleastOnce) { oldestTimeStamp = buffer[currentIndex].getTimeStamp(); } }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
public synchronized void addRawTransformAndTimestamp(long timestamp, RigidBodyTransform transform3D) { transforms[currentIndex].setTimeStamp(timestamp); transforms[currentIndex].setTransform3D(transform3D); newestTimestamp = timestamp; currentIndex++; if (currentIndex >= size) { currentIndex = 0; } if (transforms[currentIndex].getTimeStamp() == Long.MIN_VALUE) { oldestTimestamp = newestTimestamp; } else { oldestTimestamp = transforms[currentIndex].getTimeStamp(); } }
public void interpolate(TimeStampedTransform3D timeStampedTransform1, TimeStampedTransform3D timeStampedTransform2, TimeStampedTransform3D resultToPack, long timeStamp) { long timeStamp1 = timeStampedTransform1.getTimeStamp(); long timeStamp2 = timeStampedTransform2.getTimeStamp(); MathTools.checkIntervalContains(timeStamp, timeStamp1, timeStamp2); RigidBodyTransform transform1 = timeStampedTransform1.getTransform3D(); RigidBodyTransform transform2 = timeStampedTransform2.getTransform3D(); resultToPack.setTimeStamp(timeStamp); double alpha = ((double) (timeStamp - timeStamp1)) / ((double) (timeStamp2 - timeStamp1)); RigidBodyTransform interpolatedTransform = resultToPack.getTransform3D(); computeInterpolation(transform1, transform2, interpolatedTransform, alpha); } }
public synchronized void addTimeStampedTransform(TimeStampedTransform3D timeStampedTransform3D) { transforms[currentIndex].setTransform3D(timeStampedTransform3D.getTransform3D()); transforms[currentIndex].setTimeStamp(timeStampedTransform3D.getTimeStamp()); newestTimestamp = timeStampedTransform3D.getTimeStamp(); currentIndex++; if (currentIndex >= size) { currentIndex = 0; } if (transforms[currentIndex].getTimeStamp() == Long.MIN_VALUE) { oldestTimestamp = newestTimestamp; } else { oldestTimestamp = transforms[currentIndex].getTimeStamp(); } }
public void interpolate(TimeStampedTransform3D timeStampedTransform1, TimeStampedTransform3D timeStampedTransform2, TimeStampedTransform3D resultToPack, long timeStamp) { long timeStamp1 = timeStampedTransform1.getTimeStamp(); long timeStamp2 = timeStampedTransform2.getTimeStamp(); MathTools.checkIfInRange(timeStamp, timeStamp1, timeStamp2); RigidBodyTransform transform1 = timeStampedTransform1.getTransform3D(); RigidBodyTransform transform2 = timeStampedTransform2.getTransform3D(); resultToPack.setTimeStamp(timeStamp); double alpha = ((double) (timeStamp - timeStamp1)) / ((double) (timeStamp2 - timeStamp1)); RigidBodyTransform interpolatedTransform = resultToPack.getTransform3D(); computeInterpolation(transform1, transform2, interpolatedTransform, alpha); } }
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = new StampedPosePacket(frameID, timeStampedTransform, overlap); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); packetCommunicator.send(posePacket); } };
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket(frameID, timeStampedTransform, overlap); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); stampedPosePublisher.publish(posePacket); } };
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = new StampedPosePacket(frameID, timeStampedTransform, RosLocalizationConstants.DEFAULT_OVERLAP); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); controllerCommunicationBridge.send(posePacket); } };
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket(frameID, timeStampedTransform, overlap); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); packetCommunicator.send(posePacket); } };
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = new StampedPosePacket(frameID, timeStampedTransform, overlap); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); rosModulePacketCommunicator.send(posePacket); } };
@Override protected void newPose(String frameID, TimeStampedTransform3D timeStampedTransform) { long timestamp = timeStampedTransform.getTimeStamp(); timestamp = ppsTimeOffsetProvider.adjustTimeStampToRobotClock(timestamp); timeStampedTransform.setTimeStamp(timestamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket(frameID, timeStampedTransform, RosLocalizationConstants.DEFAULT_OVERLAP); posePacket.setDestination(PacketDestination.CONTROLLER.ordinal()); if (DEBUG) System.out.println("Pose update received. \ntimestamp: " + timeStampedTransform.getTimeStamp()); controllerCommunicationBridge.send(posePacket); } };
transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp);
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3d(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quat4d(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3D(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quaternion(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }