diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java index c431b037a..4d65b9942 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/ApplicationAmbassador.java @@ -36,6 +36,7 @@ import org.eclipse.mosaic.interactions.electricity.VehicleBatteryUpdates; import org.eclipse.mosaic.interactions.electricity.VehicleChargingDenial; import org.eclipse.mosaic.interactions.environment.EnvironmentSensorUpdates; +import org.eclipse.mosaic.interactions.environment.LidarUpdates; import org.eclipse.mosaic.interactions.mapping.ChargingStationRegistration; import org.eclipse.mosaic.interactions.mapping.RsuRegistration; import org.eclipse.mosaic.interactions.mapping.ServerRegistration; @@ -61,6 +62,7 @@ import org.eclipse.mosaic.lib.objects.vehicle.BatteryData; import org.eclipse.mosaic.lib.objects.vehicle.VehicleData; import org.eclipse.mosaic.lib.objects.vehicle.VehicleDeparture; +import org.eclipse.mosaic.lib.objects.vehicle.sensor.LidarData; import org.eclipse.mosaic.lib.util.FileUtils; import org.eclipse.mosaic.lib.util.objects.ObjectInstantiation; import org.eclipse.mosaic.lib.util.scheduling.DefaultEventScheduler; @@ -298,6 +300,8 @@ protected void processInteraction(final Interaction interaction) throws Internal this.process((TrafficLightUpdates) interaction); } else if (interaction.getTypeId().startsWith(VehicleUpdates.TYPE_ID)) { this.process((VehicleUpdates) interaction); + } else if (interaction.getTypeId().startsWith(LidarUpdates.TYPE_ID)) { + this.process((LidarUpdates) interaction); } else if (interaction.getTypeId().startsWith(VehicleBatteryUpdates.TYPE_ID)) { this.process((VehicleBatteryUpdates) interaction); } else if (interaction.getTypeId().startsWith(VehicleRoutesInitialization.TYPE_ID)) { @@ -692,6 +696,19 @@ private void process(final VehicleUpdates vehicleUpdates) { addEvent(triggerGarbageCollection); } + private void process(final LidarUpdates lidarUpdates) { + for (LidarData data :lidarUpdates.getUpdated()) { + final AbstractSimulationUnit simulationUnit = UnitSimulator.UnitSimulator.getUnitFromId(data.getName()); + final Event event = new Event( + data.getTime(), + simulationUnit, + data, + EventNicenessPriorityRegister.LIDAR_UPDATED + ); + addEvent(event); + } + } + private void addVehicleIfNotYetAdded(long time, String unitName) { final VehicleRegistration vehicleRegistration = vehicleRegistrations.remove(unitName); if (vehicleRegistration != null) { diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/VehicleUnit.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/VehicleUnit.java index 71ce0fa5d..ab0875192 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/VehicleUnit.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/VehicleUnit.java @@ -24,6 +24,7 @@ import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.NopPerceptionModule; import org.eclipse.mosaic.fed.application.ambassador.simulation.perception.SimplePerceptionConfiguration; import org.eclipse.mosaic.fed.application.app.api.CommunicationApplication; +import org.eclipse.mosaic.fed.application.app.api.LidarApplication; import org.eclipse.mosaic.fed.application.app.api.VehicleApplication; import org.eclipse.mosaic.fed.application.app.api.os.VehicleOperatingSystem; import org.eclipse.mosaic.fed.application.app.api.perception.PerceptionModule; @@ -45,6 +46,7 @@ import org.eclipse.mosaic.lib.objects.vehicle.VehicleData; import org.eclipse.mosaic.lib.objects.vehicle.VehicleRoute; import org.eclipse.mosaic.lib.objects.vehicle.VehicleType; +import org.eclipse.mosaic.lib.objects.vehicle.sensor.LidarData; import org.eclipse.mosaic.lib.routing.database.DatabaseRouting; import org.eclipse.mosaic.lib.util.scheduling.Event; @@ -123,6 +125,12 @@ private void updateVehicleInfo(final VehicleData currentVehicleData) { } } + private void updateLidarInfo(final LidarData currentLidarData) { + for (LidarApplication application : getApplicationsIterator(LidarApplication.class)) { + application.onLidarUpdated(currentLidarData); + } + } + @Override public void processEvent(@Nonnull final Event event) throws Exception { // never remove the preProcessEvent call! @@ -155,6 +163,10 @@ protected boolean handleEventResource(Object resource, long eventType) { if (resource instanceof BatteryData) { throw new RuntimeException(ErrorRegister.VEHICLE_NotElectric.toString()); } + if (resource instanceof LidarData) { + updateLidarInfo((LidarData) resource); + return true; + } return false; } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/util/EventNicenessPriorityRegister.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/util/EventNicenessPriorityRegister.java index fccefab17..e0d8e8f8c 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/util/EventNicenessPriorityRegister.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/util/EventNicenessPriorityRegister.java @@ -24,6 +24,7 @@ public class EventNicenessPriorityRegister { public final static long VEHICLE_ADDED = -99_999_900; public final static long VEHICLE_UPDATED = -99_999_800; public final static long VEHICLE_REMOVED = -99_999_700; + public final static long LIDAR_UPDATED = -99_999_650; // update traffic detectors public final static long UPDATE_TRAFFIC_DETECTORS = -99_999_600; diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/LidarApplication.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/LidarApplication.java new file mode 100644 index 000000000..c42978de0 --- /dev/null +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/app/api/LidarApplication.java @@ -0,0 +1,24 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.fed.application.app.api; + +import org.eclipse.mosaic.lib.objects.vehicle.sensor.LidarData; + +import javax.annotation.Nonnull; + +public interface LidarApplication extends Application { + void onLidarUpdated(@Nonnull LidarData updatedLidarData); +} diff --git a/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/AbstractSumoAmbassador.java b/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/AbstractSumoAmbassador.java index 4e2436454..e46299f6d 100644 --- a/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/AbstractSumoAmbassador.java +++ b/fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/AbstractSumoAmbassador.java @@ -1045,6 +1045,7 @@ private void receiveInteraction(VehicleSensorActivation vehicleSensorActivation) */ private void receiveInteraction(VehicleParametersChange vehicleParametersChange) throws InternalFederateException { if (externalVehicles.containsKey(vehicleParametersChange.getVehicleId())) { + changeExternalParameters(vehicleParametersChange); return; } @@ -1286,6 +1287,18 @@ private void removeExternalVehiclesFromUpdates(VehicleUpdates updates) { updates.getRemovedNames().removeIf(vehicle -> externalVehicles.remove(vehicle) != null); } + public void changeExternalParameters(VehicleParametersChange vehicleParametersChange) throws InternalFederateException { + final String veh_id = vehicleParametersChange.getVehicleId(); + for (final VehicleParameter param : vehicleParametersChange.getVehicleParameters()) { + if (param.getParameterType() == VehicleParameter.VehicleParameterType.COLOR) { + final Color color = param.getValue(); + bridge.getVehicleControl().setColor(veh_id, color.getRed(), color.getGreen(), color.getBlue(), color.getAlpha()); + } else { + log.warn("Parameter type {} is not supported for external vehicles", param.getParameterType().name()); + } + } + } + /** * This handles the case that sumo handles routing and creates new routes while doing so. * diff --git a/lib/mosaic-interactions/src/main/java/org/eclipse/mosaic/interactions/environment/LidarUpdates.java b/lib/mosaic-interactions/src/main/java/org/eclipse/mosaic/interactions/environment/LidarUpdates.java new file mode 100644 index 000000000..d71923160 --- /dev/null +++ b/lib/mosaic-interactions/src/main/java/org/eclipse/mosaic/interactions/environment/LidarUpdates.java @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.interactions.environment; + +import static org.apache.commons.lang3.builder.ToStringStyle.SHORT_PREFIX_STYLE; + +import org.eclipse.mosaic.lib.objects.vehicle.sensor.LidarData; +import org.eclipse.mosaic.rti.api.Interaction; + +import org.apache.commons.lang3.builder.EqualsBuilder; +import org.apache.commons.lang3.builder.HashCodeBuilder; +import org.apache.commons.lang3.builder.ToStringBuilder; + +import java.util.List; +import java.util.stream.Collectors; + +public class LidarUpdates extends Interaction { + private static final long serialVersionUID = 1L; + + /** + * String identifying the type of this interaction. + */ + public final static String TYPE_ID = createTypeIdentifier(LidarUpdates.class); + + /** + * Time at which the next sensor update will be sent. + */ + private long nextUpdate; + + + /** + * List of {@link LidarData} containing LiDAR data from the simulator. + */ + private final List updated; + + + + public LidarUpdates(long time, List updated) { + super(time); + this.updated = updated; + } + + public List getUpdated() { + return this.updated; + } + + public void setNextUpdate(long nextUpdate) { + this.nextUpdate = nextUpdate; + } + + @Override + public int hashCode() { + return new HashCodeBuilder(5, 17) + .append(nextUpdate) + .append(updated) + .toHashCode(); + } + + @Override + public boolean equals(Object obj) { + if (obj == null) { + return false; + } + if (obj == this) { + return true; + } + if (obj.getClass() != getClass()) { + return false; + } + + LidarUpdates other = (LidarUpdates) obj; + return new EqualsBuilder() + .append(this.nextUpdate, other.nextUpdate) + .append(this.updated, other.updated) + .isEquals(); + } + + @Override + public String toString() { + return new ToStringBuilder(this, SHORT_PREFIX_STYLE) + .appendSuper(super.toString()) + .append("updated", updated.stream().map(LidarData::getName).collect(Collectors.joining(","))) + .toString(); + } + +} + diff --git a/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/sensor/LidarData.java b/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/sensor/LidarData.java new file mode 100644 index 000000000..eab37cc27 --- /dev/null +++ b/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/vehicle/sensor/LidarData.java @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2024 Fraunhofer FOKUS and others. All rights reserved. + * + * See the NOTICE file(s) distributed with this work for additional + * information regarding copyright ownership. + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0 + * + * SPDX-License-Identifier: EPL-2.0 + * + * Contact: mosaic@fokus.fraunhofer.de + */ + +package org.eclipse.mosaic.lib.objects.vehicle.sensor; + +import org.eclipse.mosaic.lib.spatial.PointCloud; +import org.eclipse.mosaic.lib.util.gson.PolymorphismTypeAdapterFactory; + +import com.google.gson.annotations.JsonAdapter; + +import java.io.Serializable; + +public class LidarData implements Serializable { + @JsonAdapter(PolymorphismTypeAdapterFactory.class) + private final PointCloud lidarData; + + private final long time; + private final String name; + /** + * Creates a new {@link LidarData}. + * + * @param time time of the last update + * @param name name of the unit + * @param lidarData Object that contains the LidarFrame data + */ + public LidarData(long time, String name, PointCloud lidarData) { + this.lidarData = lidarData; + this.time = time; + this.name = name; + } + + /** + * Returns lidar data produced by the traffic or vehicle simulator. + * Should be of type LidarFrame. + */ + public Object getLidarData() { + return this.lidarData; + } + + public long getTime() { + return this.time; + } + + public String getName() { + return this.name; + } + + + /** + * A builder for creating {@link LidarData} objects + */ + public static class Builder { + private final long time; + private final String name; + private PointCloud lidarData; + + /** + * Init the builder with the current simulation time [ns] and name of the vehicle. + */ + public Builder(long time, String name) { + this.time = time; + this.name = name; + } + + /** + * Set position related values for the vehicle info. + */ + public LidarData.Builder lidarData(PointCloud lidarData) { + this.lidarData = lidarData; + return this; + } + + /** + * Returns the final {@link LidarData} based on the properties given before. + */ + public LidarData create() { + return new LidarData( + time, name, lidarData); + } + } +}