((IDLSequence.Float) seq).add((float)val.asDouble());
private void writeMessage(LidarScanMessage lidarScanMessage) { if (!loggingEnabled.get() || logDataOutputStream == null) return; try { logDataOutputStream.writeLong(lidarScanMessage.getRobotTimestamp()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarPosition().getX32()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarPosition().getY32()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarPosition().getZ32()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarOrientation().getX32()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarOrientation().getY32()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarOrientation().getZ32()); logDataOutputStream.writeFloat(lidarScanMessage.getLidarOrientation().getS32()); logDataOutputStream.writeInt(lidarScanMessage.getScan().size()); for (float scanPoint : lidarScanMessage.getScan().toArray()) { logDataOutputStream.writeFloat(scanPoint); } } catch (IOException e) { if (DEBUG) e.printStackTrace(); closeDataOutputStream(); loggingEnabled.set(false); } }
child.add(((IDLSequence.Float) seq).get(i));
child.add(((IDLSequence.Float) seq).get(i));
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); } }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof RobotConfigurationData)) return false; RobotConfigurationData otherMyClass = (RobotConfigurationData) other; if(this.sequence_id_ != otherMyClass.sequence_id_) return false; if(this.timestamp_ != otherMyClass.timestamp_) return false; if(this.sensor_head_pps_timestamp_ != otherMyClass.sensor_head_pps_timestamp_) return false; if(this.joint_name_hash_ != otherMyClass.joint_name_hash_) return false; if (!this.joint_angles_.equals(otherMyClass.joint_angles_)) return false; if (!this.joint_velocities_.equals(otherMyClass.joint_velocities_)) return false; if (!this.joint_torques_.equals(otherMyClass.joint_torques_)) return false; if (!this.root_translation_.equals(otherMyClass.root_translation_)) return false; if (!this.root_orientation_.equals(otherMyClass.root_orientation_)) return false; if (!this.pelvis_linear_velocity_.equals(otherMyClass.pelvis_linear_velocity_)) return false; if (!this.pelvis_angular_velocity_.equals(otherMyClass.pelvis_angular_velocity_)) return false; if (!this.pelvis_linear_acceleration_.equals(otherMyClass.pelvis_linear_acceleration_)) return false; if (!this.force_sensor_data_.equals(otherMyClass.force_sensor_data_)) return false; if (!this.imu_sensor_data_.equals(otherMyClass.imu_sensor_data_)) return false; if(this.robot_motion_status_ != otherMyClass.robot_motion_status_) return false; if(this.last_received_packet_type_id_ != otherMyClass.last_received_packet_type_id_) return false; if(this.last_received_packet_unique_id_ != otherMyClass.last_received_packet_unique_id_) return false; if(this.last_received_packet_robot_timestamp_ != otherMyClass.last_received_packet_robot_timestamp_) return false; return true; }
@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 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; }
message.setRanges(lidarScan.getRanges().toArray());
@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; }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof BatteryState)) return false; BatteryState otherMyClass = (BatteryState) other; if (!this.header_.equals(otherMyClass.header_)) return false; if(this.voltage_ != otherMyClass.voltage_) return false; if(this.current_ != otherMyClass.current_) return false; if(this.charge_ != otherMyClass.charge_) return false; if(this.capacity_ != otherMyClass.capacity_) return false; if(this.design_capacity_ != otherMyClass.design_capacity_) return false; if(this.percentage_ != otherMyClass.percentage_) return false; if(this.power_supply_status_ != otherMyClass.power_supply_status_) return false; if(this.power_supply_health_ != otherMyClass.power_supply_health_) return false; if(this.power_supply_technology_ != otherMyClass.power_supply_technology_) return false; if(this.present_ != otherMyClass.present_) return false; if (!this.cell_voltage_.equals(otherMyClass.cell_voltage_)) return false; if (!us.ihmc.idl.IDLTools.equals(this.location_, otherMyClass.location_)) return false; if (!us.ihmc.idl.IDLTools.equals(this.serial_number_, otherMyClass.serial_number_)) return false; return true; }
public void set(BatteryState other) { std_msgs.msg.dds.HeaderPubSubType.staticCopy(other.header_, header_); voltage_ = other.voltage_; current_ = other.current_; charge_ = other.charge_; capacity_ = other.capacity_; design_capacity_ = other.design_capacity_; percentage_ = other.percentage_; power_supply_status_ = other.power_supply_status_; power_supply_health_ = other.power_supply_health_; power_supply_technology_ = other.power_supply_technology_; present_ = other.present_; cell_voltage_.set(other.cell_voltage_); location_.setLength(0); location_.append(other.location_); serial_number_.setLength(0); serial_number_.append(other.serial_number_); }
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"); }
public DynamicArrayPrimitives() { bool_values_ = new us.ihmc.idl.IDLSequence.Boolean (100, "type_7"); byte_values_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9"); char_values_ = new us.ihmc.idl.IDLSequence.Char (100, "type_8"); float32_values_ = new us.ihmc.idl.IDLSequence.Float (100, "type_5"); float64_values_ = new us.ihmc.idl.IDLSequence.Double (100, "type_6"); int8_values_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9"); uint8_values_ = new us.ihmc.idl.IDLSequence.Byte (100, "type_9"); int16_values_ = new us.ihmc.idl.IDLSequence.Short (100, "type_1"); uint16_values_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_3"); int32_values_ = new us.ihmc.idl.IDLSequence.Integer (100, "type_2"); uint32_values_ = new us.ihmc.idl.IDLSequence.Long (100, "type_4"); int64_values_ = new us.ihmc.idl.IDLSequence.Long (100, "type_11"); uint64_values_ = new us.ihmc.idl.IDLSequence.Long (100, "type_12"); string_values_ = new us.ihmc.idl.IDLSequence.StringBuilderHolder (100, "type_d"); }
@Override public boolean equals(Object other) { if(other == null) return false; if(other == this) return true; if(!(other instanceof LaserScan)) return false; LaserScan otherMyClass = (LaserScan) other; if (!this.header_.equals(otherMyClass.header_)) return false; if(this.angle_min_ != otherMyClass.angle_min_) return false; if(this.angle_max_ != otherMyClass.angle_max_) return false; if(this.angle_increment_ != otherMyClass.angle_increment_) return false; if(this.time_increment_ != otherMyClass.time_increment_) return false; if(this.scan_time_ != otherMyClass.scan_time_) return false; if(this.range_min_ != otherMyClass.range_min_) return false; if(this.range_max_ != otherMyClass.range_max_) return false; if (!this.ranges_.equals(otherMyClass.ranges_)) return false; if (!this.intensities_.equals(otherMyClass.intensities_)) return false; return true; }