/** * Gets the local position of the unit relative to its parent location. * * @return relative position * * @throws NotAvailableException is thrown if the unit config or parts of it are not available. */ default TranslationType.Translation getUnitPosition() throws NotAvailableException { try { if (!getConfig().hasPlacementConfig()) { throw new NotAvailableException("PlacementConfig"); } //todo release : rename PlacementConfig position into pose. if (!getConfig().getPlacementConfig().hasPosition()) { throw new NotAvailableException("Position"); } return getConfig().getPlacementConfig().getPosition().getTranslation(); } catch (final CouldNotPerformException ex) { throw new NotAvailableException("UnitPosition", ex); } }
@Override public void processData(String id, IdentifiableMessage<String, UnitConfig, UnitConfig.Builder> entry, ProtoBufMessageMap<String, UnitConfig, UnitConfig.Builder> entryMap, ProtoBufRegistry<String, UnitConfig, UnitConfig.Builder> registry) throws CouldNotPerformException, EntryModification { UnitConfig.Builder unitConfig = entry.getMessage().toBuilder(); if (unitConfig.getType() == UnitTemplateType.UnitTemplate.UnitType.LOCATION) { return; } if (unitConfig.hasPlacementConfig() && unitConfig.getPlacementConfig().hasPosition()) { final Pose pose = unitConfig.getPlacementConfig().getPosition(); if (pose.getTranslation().getX() == 0 && pose.getTranslation().getY() == 0 && pose.getTranslation().getZ() == 0) { unitConfig.getPlacementConfigBuilder().clearPosition(); throw new EntryModification(entry.setMessage(unitConfig), this); } } } }
public static Transform3D transform(final PoseType.Pose position) { RotationType.Rotation pRotation = position.getRotation(); TranslationType.Translation pTranslation = position.getTranslation(); Quat4d jRotation = new Quat4d(pRotation.getQx(), pRotation.getQy(), pRotation.getQz(), pRotation.getQw()); Vector3d jTranslation = new Vector3d(pTranslation.getX(), pTranslation.getY(), pTranslation.getZ()); return new Transform3D(jRotation, jTranslation, 1.0); } }