public static SimulatedLidarScanPacket nextSimulatedLidarScanPacket(Random random) { SimulatedLidarScanPacket next = new SimulatedLidarScanPacket(); int size = Math.abs(random.nextInt(1000000)); for (int i = 0; i < size; i++) { next.getRanges().add(random.nextFloat()); } next.setSensorId(random.nextInt()); next.getLidarScanParameters().set(nextLidarScanParametersMessage(random)); 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); }); }
public static PointCloudWorldPacket nextPointCloudWorldPacket(Random random) { PointCloudWorldPacket next = new PointCloudWorldPacket(); next.setTimestamp(random.nextLong()); next.getGroundQuadTreeSupport().add(RandomNumbers.nextFloatArray(random, random.nextInt(), 100.0f)); next.getDecayingWorldScan().add(RandomNumbers.nextFloatArray(random, random.nextInt(), 100.0f)); next.setDefaultGroundHeight(random.nextFloat()); return next; }
((IDLSequence.Float) seq).add((float)val.asDouble());
((IDLSequence.Float) seq).add((float)val.asDouble());
lidarScanMessage.getScan().add(logDataInputStream.readFloat());
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 0.5) @Test(timeout = 30000) public void testAddingStuff() { RobotConfigurationDataBuffer buffer = new RobotConfigurationDataBuffer(); FullHumanoidRobotModel setterFullRobotModel = getFullRobotModel(); FullHumanoidRobotModel getterFullRobotModel = getFullRobotModel(); OneDoFJointBasics[] setterJoints = FullRobotModelUtils.getAllJointsExcludingHands(setterFullRobotModel); OneDoFJointBasics[] getterJoints = FullRobotModelUtils.getAllJointsExcludingHands(getterFullRobotModel); ForceSensorDefinition[] forceSensorDefinitions = setterFullRobotModel.getForceSensorDefinitions(); IMUDefinition[] imuDefinitions = setterFullRobotModel.getIMUDefinitions(); assertTrue(buffer.updateFullRobotModel(false, 0, getterFullRobotModel, null) == -1); for (int i = 0; i < RobotConfigurationDataBuffer.BUFFER_SIZE * 2; i++) { RobotConfigurationData test = RobotConfigurationDataFactory.create(setterJoints, forceSensorDefinitions, imuDefinitions); test.setTimestamp(i * 10); test.getJointAngles().add(i * 10); buffer.receivedPacket(test); } for (int i = 0; i < RobotConfigurationDataBuffer.BUFFER_SIZE; i++) { assertEquals(buffer.updateFullRobotModel(true, i * 10 + 5, getterFullRobotModel, null), -1); } for (int i = RobotConfigurationDataBuffer.BUFFER_SIZE; i < 2.0 * RobotConfigurationDataBuffer.BUFFER_SIZE - 1; i++) { long timestamp = buffer.updateFullRobotModel(true, i * 10 + 5, getterFullRobotModel, null); assertEquals(getterJoints[0].getQ(), i * 10.0, 1e-7); assertEquals(i * 10, timestamp); } }
public static RobotConfigurationData nextRobotConfigurationData(Random random) { RobotConfigurationData next = new RobotConfigurationData(); int size = random.nextInt(10000); next.setTimestamp(random.nextLong()); next.setSensorHeadPpsTimestamp(random.nextLong()); next.setJointNameHash(random.nextInt(10000)); next.getJointAngles().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getJointVelocities().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getJointTorques().add(RandomNumbers.nextFloatArray(random, size, 1.0f)); next.getRootTranslation().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getPelvisLinearVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getPelvisAngularVelocity().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getRootOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random)); next.getPelvisLinearAcceleration().set(EuclidCoreRandomTools.nextVector3D32(random)); size = Math.abs(random.nextInt(1000)); for (int i = 0; i < next.getForceSensorData().size(); i++) next.getForceSensorData().add().set(nextSpatialVectorMessage(random)); for (IMUPacket imuPacket : nextIMUPackets(random)) next.getImuSensorData().add().set(imuPacket); next.setRobotMotionStatus(RandomNumbers.nextEnum(random, RobotMotionStatus.class).toByte()); next.setLastReceivedPacketTypeId(random.nextInt(1000)); next.setLastReceivedPacketUniqueId(random.nextLong()); next.setLastReceivedPacketRobotTimestamp(random.nextLong()); return next; }
jointAngles.add((float) (i * j));
public void set(Float other) { resetQuick(); for(int i = 0; i < other.size(); i++) { add(other.get(i)); } }
public static void setGroundQuadTreeSupport(Point3DReadOnly[] pointCloud, PointCloudWorldPacket pointCloudWorldPacket) { pointCloudWorldPacket.getGroundQuadTreeSupport().reset(); for (int i = 0; i < pointCloud.length; i++) { Point3DReadOnly point = pointCloud[i]; pointCloudWorldPacket.getGroundQuadTreeSupport().add((float) point.getX()); pointCloudWorldPacket.getGroundQuadTreeSupport().add((float) point.getY()); pointCloudWorldPacket.getGroundQuadTreeSupport().add((float) point.getZ()); } }
public static KinematicsToolboxOutputStatus nextKinematicsToolboxOutputStatus(Random random) { KinematicsToolboxOutputStatus next = new KinematicsToolboxOutputStatus(); next.setJointNameHash(random.nextInt()); next.getDesiredJointAngles().add(RandomNumbers.nextFloatArray(random, random.nextInt(100), 1.0f)); next.getDesiredRootTranslation().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getDesiredRootOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random)); next.setSolutionQuality(random.nextDouble()); return next; }
public static HeatMapPacket nextHeatMapPacket(Random random) { HeatMapPacket next = new HeatMapPacket(); next.setHeight(RandomNumbers.nextInt(random, -100, 100)); next.setWidth(RandomNumbers.nextInt(random, -100, 100)); next.getData().add(RandomNumbers.nextFloatArray(random, next.getHeight() * next.getWidth(), 1.0f)); next.setName(Integer.toHexString(random.nextInt())); return next; }
@Override public void readElement(int i, CDR cdr) { add(cdr.read_type_5()); }
public static void packLocalizationPointMap(Point3DReadOnly[] pointCloud, LocalizationPointMapPacket localizationPointMapPacket) { localizationPointMapPacket.getLocalizationPointMap().reset(); for (int i = 0; i < pointCloud.length; i++) { Point3DReadOnly point = pointCloud[i]; localizationPointMapPacket.getLocalizationPointMap().add((float) point.getX()); localizationPointMapPacket.getLocalizationPointMap().add((float) point.getY()); localizationPointMapPacket.getLocalizationPointMap().add((float) point.getZ()); } }
public static LegCompliancePacket createLegCompliancePacket(float[] maxVelocityDeltas, RobotSide robotSide) { LegCompliancePacket message = new LegCompliancePacket(); message.getMaxVelocityDeltas().add(maxVelocityDeltas); message.setRobotSide(robotSide.toByte()); return message; }
public static void setDecayingWorldScan(Point3DReadOnly[] pointCloud, PointCloudWorldPacket pointCloudWorldPacket) { pointCloudWorldPacket.getDecayingWorldScan().reset(); for (int i = 0; i < pointCloud.length; i++) { Point3DReadOnly point = pointCloud[i]; pointCloudWorldPacket.getDecayingWorldScan().add((float) point.getX()); pointCloudWorldPacket.getDecayingWorldScan().add((float) point.getY()); pointCloudWorldPacket.getDecayingWorldScan().add((float) point.getZ()); } }
public static LocalizationPointMapPacket nextLocalizationPointMapPacket(Random random) { LocalizationPointMapPacket next = new LocalizationPointMapPacket(); next.setTimestamp(random.nextLong()); next.getLocalizationPointMap().add(RandomNumbers.nextFloatArray(random, random.nextInt(10000), 1.0f)); return next; }
public static LegCompliancePacket nextLegCompliancePacket(Random random) { LegCompliancePacket next = new LegCompliancePacket(); next.setRobotSide(RandomNumbers.nextEnum(random, RobotSide.class).toByte()); next.getMaxVelocityDeltas().add(RandomNumbers.nextFloatArray(random, random.nextInt(1000), 1.0f)); return next; }