@Override public void set(ReachingManifoldMessage message, Map<Integer, RigidBodyBasics> rigidBodyHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { clear(); rigidBodyHashCode = message.getEndEffectorHashCode(); if (rigidBodyHashMap == null) rigidBody = null; else rigidBody = rigidBodyHashMap.get(rigidBodyHashCode); this.manifoldOriginPosition.set(message.getManifoldOriginPosition()); this.manifoldOriginOrientation.set(message.getManifoldOriginOrientation()); for (int i = 0; i < message.getManifoldConfigurationSpaceNames().size(); i++) { manifoldConfigurationSpaces.add(ConfigurationSpaceName.fromByte(message.getManifoldConfigurationSpaceNames().get(i))); manifoldLowerLimits.add(message.getManifoldLowerLimits().get(i)); manifoldUpperLimits.add(message.getManifoldUpperLimits().get(i)); } }
((IDLSequence.Byte) seq).add((byte) val.asInt());
public ParameterValue() { string_value_ = new java.lang.StringBuilder(255); bytes_value_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9"); }
for (int i = 0; i < dimension; i++) messageToPack.getValkyrieFingerMotorNames().add(valkyrieFingerMotorNames[i].toByte());
((IDLSequence.Byte) seq).add((byte) val.asInt());
child.add(((IDLSequence.Byte) seq).get(i));
child.add(((IDLSequence.Byte) seq).get(i));
int numberOfPoints = (int) Math.pow(resolutionForSingleSpace, reachingManifoldMessage.getManifoldConfigurationSpaceNames().size()); double[] configurationValues = new double[reachingManifoldMessage.getManifoldConfigurationSpaceNames().size()]; int[] configurationIndex = new int[reachingManifoldMessage.getManifoldConfigurationSpaceNames().size()]; for (int j = reachingManifoldMessage.getManifoldConfigurationSpaceNames().size(); j > 0; j--) for (int j = 0; j < reachingManifoldMessage.getManifoldConfigurationSpaceNames().size(); j++) .get(j));
private static Graphics3DObject createTrajectoryMessageVisualization(ReachingManifoldMessage reachingMessage, double radius, AppearanceDefinition appearance) int numberOfPoints = (int) Math.pow(configurationValueResolution, reachingMessage.getManifoldConfigurationSpaceNames().size()); int radialResolution = 16; double[] configurationValues = new double[reachingMessage.getManifoldConfigurationSpaceNames().size()]; int[] configurationIndex = new int[reachingMessage.getManifoldConfigurationSpaceNames().size()]; for (int j = reachingMessage.getManifoldConfigurationSpaceNames().size(); j > 0; j--) for (int j = 0; j < reachingMessage.getManifoldConfigurationSpaceNames().size(); j++) switch (ConfigurationSpaceName.fromByte(reachingMessage.getManifoldConfigurationSpaceNames().get(j)))
videoTimestamp = robotConfigurationData.getTimestamp(); latestUnmodifiedCameraImage = jpegDecompressor.decompressJPEGDataToBufferedImage(videoPacket.getData().toArray());
next.getFlag().add(flag); return next;
@Override public void receivedPacket(VideoPacket packet) { executorService.submit(() -> { Pair<List<Rectangle>, ValveDetector.HeatMap> rectanglesAndHeatMaps = valveDetector .detect(jpegDecompressor.decompressJPEGDataToBufferedImage(packet.getData().toArray())); rectanglesAndHeatMaps.getLeft().sort((r1, r2) -> -Integer.compare(r1.width * r1.height, r2.width * r2.height)); HeatMapPacket heatMapPacket = new HeatMapPacket(); heatMapPacket.setWidth(rectanglesAndHeatMaps.getRight().w); heatMapPacket.setHeight(rectanglesAndHeatMaps.getRight().h); heatMapPacket.getData().add(rectanglesAndHeatMaps.getRight().data); heatMapPacket.setName("Valve"); int[] packedBoxes = rectanglesAndHeatMaps.getLeft().stream() .flatMapToInt(rect -> IntStream.of(rect.x, rect.y, rect.width, rect.height)).toArray(); String[] names = new String[rectanglesAndHeatMaps.getLeft().size()]; for (int i = 0; i < names.length; i++) { names[i] = "Valve " + i; } BoundingBoxesPacket boundingBoxesPacket = MessageTools.createBoundingBoxesPacket(packedBoxes, names); ObjectDetectorResultPacket resultPacket = MessageTools.createObjectDetectorResultPacket(heatMapPacket, boundingBoxesPacket); resultPacket.setDestination(PacketDestination.BEHAVIOR_MODULE.ordinal()); detectionStatusClient.send(resultPacket); }); }
@Override public void doControl() { if (cameraData.isNewPacketAvailable()) { VideoPacket vidPack = cameraData.getLatestPacket(); //System.out.println(vidPack.videoSource.toString()); if (!testImage) { try { InputStream in = new ByteArrayInputStream(vidPack.getData().toArray()); ImageIO.write(ImageIO.read(in), "png", new java.io.File("testImage")); } catch (IOException e) { } testImage = true; } } else { //System.out.println("Nothing Works"); } }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof DynamicArrayPrimitives)) return false; DynamicArrayPrimitives otherMyClass = (DynamicArrayPrimitives) other; if (!this.bool_values_.equals(otherMyClass.bool_values_)) return false; if (!this.byte_values_.equals(otherMyClass.byte_values_)) return false; if (!this.char_values_.equals(otherMyClass.char_values_)) return false; if (!this.float32_values_.equals(otherMyClass.float32_values_)) return false; if (!this.float64_values_.equals(otherMyClass.float64_values_)) return false; if (!this.int8_values_.equals(otherMyClass.int8_values_)) return false; if (!this.uint8_values_.equals(otherMyClass.uint8_values_)) return false; if (!this.int16_values_.equals(otherMyClass.int16_values_)) return false; if (!this.uint16_values_.equals(otherMyClass.uint16_values_)) return false; if (!this.int32_values_.equals(otherMyClass.int32_values_)) return false; if (!this.uint32_values_.equals(otherMyClass.uint32_values_)) return false; if (!this.int64_values_.equals(otherMyClass.int64_values_)) return false; if (!this.uint64_values_.equals(otherMyClass.uint64_values_)) return false; if (!this.string_values_.equals(otherMyClass.string_values_)) return false; if(this.check_ != otherMyClass.check_) return false; return true; }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof BoundedArrayPrimitives)) return false; BoundedArrayPrimitives otherMyClass = (BoundedArrayPrimitives) other; if (!this.bool_values_.equals(otherMyClass.bool_values_)) return false; if (!this.byte_values_.equals(otherMyClass.byte_values_)) return false; if (!this.char_values_.equals(otherMyClass.char_values_)) return false; if (!this.float32_values_.equals(otherMyClass.float32_values_)) return false; if (!this.float64_values_.equals(otherMyClass.float64_values_)) return false; if (!this.int8_values_.equals(otherMyClass.int8_values_)) return false; if (!this.uint8_values_.equals(otherMyClass.uint8_values_)) return false; if (!this.int16_values_.equals(otherMyClass.int16_values_)) return false; if (!this.uint16_values_.equals(otherMyClass.uint16_values_)) return false; if (!this.int32_values_.equals(otherMyClass.int32_values_)) return false; if (!this.uint32_values_.equals(otherMyClass.uint32_values_)) return false; if (!this.int64_values_.equals(otherMyClass.int64_values_)) return false; if (!this.uint64_values_.equals(otherMyClass.uint64_values_)) return false; if (!this.string_values_.equals(otherMyClass.string_values_)) return false; if(this.check_ != otherMyClass.check_) return false; return true; }
private void detectAsync(VideoPacket videoPacket) { if (detectionRunning.get()) return; valveDetectorClient.send(videoPacket); CompletableFuture.runAsync(() -> { detectionRunning.set(true); try { while(results.peek() == null) { ThreadTools.sleep(5); } BufferedImage latestUnmodifiedCameraImage = jpegDecompressor.decompressJPEGDataToBufferedImage(videoPacket.getData().toArray()); detect(latestUnmodifiedCameraImage, videoPacket.getPosition(), videoPacket.getOrientation()); } finally { detectionRunning.set(false); } }); }
public static RigidBodyExplorationConfigurationMessage createRigidBodyExplorationConfigurationMessage(RigidBodyBasics rigidBody, ConfigurationSpaceName[] degreesOfFreedomToExplore, double[] explorationRangeUpperLimits, double[] explorationRangeLowerLimits) { RigidBodyExplorationConfigurationMessage message = new RigidBodyExplorationConfigurationMessage(); if (degreesOfFreedomToExplore.length != explorationRangeUpperLimits.length || degreesOfFreedomToExplore.length != explorationRangeLowerLimits.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore.length); message.setRigidBodyHashCode(rigidBody.hashCode()); byte[] degreesOfFreedomToExplore1 = ConfigurationSpaceName.toBytes(degreesOfFreedomToExplore); if (degreesOfFreedomToExplore1.length != explorationRangeUpperLimits.length || degreesOfFreedomToExplore1.length != explorationRangeLowerLimits.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore1.length + ", explorationRangeLowerLimits.length = "); message.getConfigurationSpaceNamesToExplore().reset(); message.getExplorationRangeUpperLimits().reset(); message.getExplorationRangeLowerLimits().reset(); message.getConfigurationSpaceNamesToExplore().add(degreesOfFreedomToExplore1); message.getExplorationRangeUpperLimits().add(explorationRangeUpperLimits); message.getExplorationRangeLowerLimits().add(explorationRangeLowerLimits); return message; }
public static RigidBodyExplorationConfigurationMessage createRigidBodyExplorationConfigurationMessage(RigidBodyBasics rigidBody, ConfigurationSpaceName[] degreesOfFreedomToExplore, double[] explorationRangeAmplitudes) { RigidBodyExplorationConfigurationMessage message = new RigidBodyExplorationConfigurationMessage(); if (degreesOfFreedomToExplore.length != explorationRangeAmplitudes.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore.length); message.setRigidBodyHashCode(rigidBody.hashCode()); byte[] degreesOfFreedomToExplore1 = ConfigurationSpaceName.toBytes(degreesOfFreedomToExplore); if (degreesOfFreedomToExplore1.length != explorationRangeAmplitudes.length) throw new RuntimeException("Inconsistent array lengths: unconstrainedDegreesOfFreedom.length = " + degreesOfFreedomToExplore1.length + ", explorationRangeLowerLimits.length = "); message.getConfigurationSpaceNamesToExplore().reset(); message.getExplorationRangeUpperLimits().reset(); message.getExplorationRangeLowerLimits().reset(); message.getConfigurationSpaceNamesToExplore().add(degreesOfFreedomToExplore1); for (int i = 0; i < degreesOfFreedomToExplore1.length; i++) { message.getExplorationRangeUpperLimits().add(explorationRangeAmplitudes[i]); message.getExplorationRangeLowerLimits().add(-explorationRangeAmplitudes[i]); } return message; }
public void set(BoundedArrayPrimitives other) { bool_values_.set(other.bool_values_); byte_values_.set(other.byte_values_); char_values_.set(other.char_values_); float32_values_.set(other.float32_values_); float64_values_.set(other.float64_values_); int8_values_.set(other.int8_values_); uint8_values_.set(other.uint8_values_); int16_values_.set(other.int16_values_); uint16_values_.set(other.uint16_values_); int32_values_.set(other.int32_values_); uint32_values_.set(other.uint32_values_); int64_values_.set(other.int64_values_); uint64_values_.set(other.uint64_values_); string_values_.set(other.string_values_); check_ = other.check_; }
public BoundedArrayPrimitives() { bool_values_ = new us.ihmc.idl.IDLSequence.Boolean (3, "type_7"); byte_values_ = new us.ihmc.idl.IDLSequence.Byte (3, "type_9"); char_values_ = new us.ihmc.idl.IDLSequence.Char (3, "type_8"); float32_values_ = new us.ihmc.idl.IDLSequence.Float (3, "type_5"); float64_values_ = new us.ihmc.idl.IDLSequence.Double (3, "type_6"); int8_values_ = new us.ihmc.idl.IDLSequence.Byte (3, "type_9"); uint8_values_ = new us.ihmc.idl.IDLSequence.Byte (3, "type_9"); int16_values_ = new us.ihmc.idl.IDLSequence.Short (3, "type_1"); uint16_values_ = new us.ihmc.idl.IDLSequence.Integer (3, "type_3"); int32_values_ = new us.ihmc.idl.IDLSequence.Integer (3, "type_2"); uint32_values_ = new us.ihmc.idl.IDLSequence.Long (3, "type_4"); int64_values_ = new us.ihmc.idl.IDLSequence.Long (3, "type_11"); uint64_values_ = new us.ihmc.idl.IDLSequence.Long (3, "type_12"); string_values_ = new us.ihmc.idl.IDLSequence.StringBuilderHolder (3, "type_d"); }