Skip to content

Commit b823f65

Browse files
FunKuchenFunKuchen
authored
feat(mapping|sumo): improve handling of parking vehicles (#477)
* feat(mapping): InstantSpawningMode for simultaneous spawning * feat(mapping): Allow empty string for route in mapping, replaced switch with enhanced switch * fix(sumo): Set lastMovementInfo for just added vehicles * fix(perception): Add VehicleUpdates from multiple Ambassadors to perception index * feat(sumo): reduce update calls for standing vehicles
1 parent cd2e705 commit b823f65

File tree

8 files changed

+133
-43
lines changed

8 files changed

+133
-43
lines changed

‎fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/perception/CentralPerceptionComponent.java

Lines changed: 11 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,10 @@
3333
import org.slf4j.Logger;
3434
import org.slf4j.LoggerFactory;
3535

36+
import java.util.HashMap;
37+
import java.util.Map;
38+
39+
3640
/**
3741
* The {@link CentralPerceptionComponent} is responsible for keeping a spatial index of all vehicles,
3842
* which allows fast querying of nearby vehicles.
@@ -55,15 +59,15 @@ public class CentralPerceptionComponent {
5559
/**
5660
* The last {@link VehicleUpdates} interaction which is used to update the vehicleIndex.
5761
*/
58-
private VehicleUpdates latestVehicleUpdates;
62+
private final Map<String, VehicleUpdates> latestVehicleUpdates = new HashMap<>();
5963

6064
/**
6165
* The last {@link TrafficLightUpdates} interaction which is used to update the vehicleIndex.
6266
*/
6367
private TrafficLightUpdates latestTrafficLightUpdates;
6468

6569
/**
66-
* If set to true, the traffic light index will be updated when {@code updateSpatialIndices} is called.
70+
* If set to true, the vehicle light index will be updated when {@code updateSpatialIndices} is called.
6771
*/
6872
private boolean updateVehicleIndex = false;
6973

@@ -126,8 +130,10 @@ public void updateSpatialIndices() {
126130
if (updateVehicleIndex) {
127131
// do not update index until next VehicleUpdates interaction is received
128132
updateVehicleIndex = false;
129-
// using Iterables.concat allows iterating over both lists subsequently without creating a new list
130-
trafficObjectIndex.updateVehicles(Iterables.concat(latestVehicleUpdates.getAdded(), latestVehicleUpdates.getUpdated()));
133+
for (VehicleUpdates updates: latestVehicleUpdates.values()) {
134+
// using Iterables.concat allows iterating over both lists subsequently without creating a new list
135+
trafficObjectIndex.updateVehicles(Iterables.concat(updates.getAdded(), updates.getUpdated()));
136+
}
131137
}
132138
if (updateTrafficLightIndex) {
133139
// do not update index until next TrafficLightUpdates interaction is received
@@ -154,7 +160,7 @@ public void registerVehicleType(String vehicleId, VehicleType vehicleType) {
154160
* @param vehicleUpdates the interaction holding all vehicle updates
155161
*/
156162
public void updateVehicles(VehicleUpdates vehicleUpdates) {
157-
latestVehicleUpdates = vehicleUpdates;
163+
latestVehicleUpdates.put(vehicleUpdates.getSenderId(), vehicleUpdates);
158164
updateVehicleIndex = true;
159165
// we need to remove arrived vehicles in every simulation step, otherwise we could have dead vehicles in the index
160166
if (trafficObjectIndex.getNumberOfVehicles() > 0) {

‎fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/ambassador/VehicleFlowGenerator.java

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow.AbstractSpawningMode;
2020
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow.ConstantSpawningMode;
2121
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow.GrowAndShrinkSpawningMode;
22+
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow.InstantSpawningMode;
2223
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow.PoissonSpawningMode;
2324
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow.SpawningMode;
2425
import org.eclipse.mosaic.fed.mapping.ambassador.spawning.lane.HighwaySpecificLaneIndexSelector;
@@ -252,6 +253,7 @@ private SpawningMode createSpawningMode(
252253
targetFlow,
253254
maxTime
254255
);
256+
case INSTANT -> new InstantSpawningMode(startingTime);
255257
};
256258
}
257259

‎fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/ambassador/spawning/flow/InstantSpawningMode.java

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
/*
2+
* Copyright (c) 2025 Fraunhofer FOKUS and others. All rights reserved.
3+
*
4+
* See the NOTICE file(s) distributed with this work for additional
5+
* information regarding copyright ownership.
6+
*
7+
* This program and the accompanying materials are made available under the
8+
* terms of the Eclipse Public License 2.0 which is available at
9+
* http://www.eclipse.org/legal/epl-2.0
10+
*
11+
* SPDX-License-Identifier: EPL-2.0
12+
*
13+
* Contact: [email protected]
14+
*/
15+
16+
package org.eclipse.mosaic.fed.mapping.ambassador.spawning.flow;
17+
18+
public class InstantSpawningMode implements SpawningMode {
19+
private final long startingTime;
20+
21+
public InstantSpawningMode(long startingTime) {
22+
this.startingTime = startingTime;
23+
}
24+
25+
@Override
26+
public boolean isSpawningActive(long currentTime) {
27+
return currentTime >= startingTime;
28+
}
29+
30+
@Override
31+
public long getNextSpawningTime(long currentTime) {
32+
return Math.max(startingTime, currentTime);
33+
}
34+
}

‎fed/mosaic-mapping/src/main/java/org/eclipse/mosaic/fed/mapping/config/units/CVehicle.java

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,8 @@ public class CVehicle implements Comparable<CVehicle> {
4444
public enum SpawningMode {
4545
CONSTANT, POISSON,
4646
GROW, SHRINK, GROW_AND_SHRINK,
47-
GROW_EXPONENTIAL, SHRINK_EXPONENTIAL, GROW_AND_SHRINK_EXPONENTIAL
47+
GROW_EXPONENTIAL, SHRINK_EXPONENTIAL, GROW_AND_SHRINK_EXPONENTIAL,
48+
INSTANT
4849
}
4950

5051
/**

‎fed/mosaic-mapping/src/main/resources/CMappingAmbassadorScheme.json

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -339,7 +339,7 @@
339339
"type": "string",
340340
"enum": [
341341
"CONSTANT", "GROW", "POISSON", "SHRINK", "GROW_AND_SHRINK", "GROW_EXPONENTIAL", "SHRINK_EXPONENTIAL",
342-
"GROW_AND_SHRINK_EXPONENTIAL"
342+
"GROW_AND_SHRINK_EXPONENTIAL", "INSTANT"
343343
],
344344
"default": "CONSTANT"
345345
},

‎fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/AbstractSumoAmbassador.java

Lines changed: 27 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -303,7 +303,7 @@ public void connectToFederate(String host, InputStream in, InputStream err) thro
303303
// hold the thread for a second to allow sumo to print possible error message to the error stream
304304
Thread.sleep(1000);
305305
while (((line = sumoInputReader.readLine()) != null)) {
306-
if (line.length() > 0) {
306+
if (!line.isEmpty()) {
307307
if (log.isDebugEnabled()) {
308308
log.debug(line);
309309
}
@@ -333,7 +333,7 @@ public void connectToFederate(String host, InputStream in, InputStream err) thro
333333
log.error(myError);
334334
BufferedReader sumoErrorReader = new BufferedReader(new InputStreamReader(err, StandardCharsets.UTF_8));
335335
while (((line = sumoErrorReader.readLine()) != null)) {
336-
if (line.length() > 0) {
336+
if (!line.isEmpty()) {
337337
log.error(line);
338338
}
339339
}
@@ -551,10 +551,10 @@ private synchronized void receiveInteraction(VehicleUpdates vehicleUpdates) thro
551551
return;
552552
}
553553

554-
for (VehicleData updatedVehicle : vehicleUpdates.getUpdated()) {
555-
ExternalVehicleState externalVehicleState = externalVehicles.get(updatedVehicle.getName());
554+
for (VehicleData vehicle : Iterables.concat(vehicleUpdates.getAdded(), vehicleUpdates.getUpdated())) {
555+
ExternalVehicleState externalVehicleState = externalVehicles.get(vehicle.getName());
556556
if (externalVehicleState != null) {
557-
externalVehicleState.setLastMovementInfo(updatedVehicle);
557+
externalVehicleState.setLastMovementInfo(vehicle);
558558
}
559559
}
560560

@@ -657,7 +657,8 @@ private synchronized void receiveInteraction(SumoTraciRequest sumoTraciRequest)
657657
sumoTraciRequest.getCommand()
658658
);
659659

660-
SumoTraciResult sumoTraciResult = traci.writeByteArrayMessage(sumoTraciRequest.getRequestId(), sumoTraciRequest.getCommand());
660+
SumoTraciResult sumoTraciResult =
661+
traci.writeByteArrayMessage(sumoTraciRequest.getRequestId(), sumoTraciRequest.getCommand());
661662
rti.triggerInteraction(new SumoTraciResponse(sumoTraciRequest.getTime(), sumoTraciResult));
662663
} else {
663664
log.warn("SumoTraciRequests are not supported.");
@@ -714,13 +715,14 @@ private synchronized void receiveInteraction(VehicleLaneChange vehicleLaneChange
714715
}
715716

716717
int targetLaneId;
718+
int laneId;
717719

718720
switch (mode) {
719721
case BY_INDEX:
720722
targetLaneId = vehicleLaneChange.getTargetLaneIndex();
721723
break;
722724
case TO_LEFT:
723-
int laneId = vehicleLaneChange.getCurrentLaneId();
725+
laneId = vehicleLaneChange.getCurrentLaneId();
724726
targetLaneId = laneId + 1;
725727
break;
726728
case TO_RIGHT:
@@ -737,7 +739,8 @@ private synchronized void receiveInteraction(VehicleLaneChange vehicleLaneChange
737739
log.warn("VehicleLaneChange failed: unsupported lane change mode.");
738740
return;
739741
}
740-
bridge.getVehicleControl().changeLane(vehicleLaneChange.getVehicleId(), Math.max(0, targetLaneId), vehicleLaneChange.getDuration());
742+
bridge.getVehicleControl()
743+
.changeLane(vehicleLaneChange.getVehicleId(), Math.max(0, targetLaneId), vehicleLaneChange.getDuration());
741744

742745
if (sumoConfig.highlights.contains(CSumo.HIGHLIGHT_CHANGE_LANE)) {
743746
VehicleData vehicleData = bridge.getSimulationControl().getLastKnownVehicleData(vehicleLaneChange.getVehicleId());
@@ -1221,7 +1224,7 @@ public synchronized void processTimeAdvanceGrant(long time) throws InternalFeder
12211224
firstAdvanceTime = false;
12221225
}
12231226

1224-
setExternalVehiclesToLatestPositions();
1227+
setExternalVehiclesToLatestPositions(time);
12251228
TraciSimulationStepResult simulationStepResult = bridge.getSimulationControl().simulateUntil(time);
12261229

12271230
VehicleUpdates vehicleUpdates = simulationStepResult.getVehicleUpdates();
@@ -1246,22 +1249,25 @@ public synchronized void processTimeAdvanceGrant(long time) throws InternalFeder
12461249
}
12471250
}
12481251

1249-
private void setExternalVehiclesToLatestPositions() {
1252+
private void setExternalVehiclesToLatestPositions(long time) {
12501253
for (Map.Entry<String, ExternalVehicleState> external : externalVehicles.entrySet()) {
1251-
if (external.getValue().isAdded()) {
1252-
VehicleData latestVehicleData = external.getValue().getLastMovementInfo();
1254+
final String externalVehicle = external.getKey();
1255+
final ExternalVehicleState externalState = external.getValue();
1256+
if (externalState.isAdded() && externalState.isRequireUpdate(time)) {
1257+
VehicleData latestVehicleData = externalState.getLastMovementInfo();
12531258
if (latestVehicleData == null) {
1254-
log.warn("No position data available for external vehicle {}", external.getKey());
1255-
latestVehicleData = bridge.getSimulationControl().getLastKnownVehicleData(external.getKey());
1259+
log.warn("No position data available for external vehicle {}", externalVehicle);
1260+
latestVehicleData = bridge.getSimulationControl().getLastKnownVehicleData(externalVehicle);
12561261
}
12571262
if (latestVehicleData != null) {
12581263
try {
12591264
bridge.getVehicleControl().moveToXY(
1260-
external.getKey(),
1265+
externalVehicle,
12611266
latestVehicleData.getPosition().toCartesian(),
12621267
latestVehicleData.getHeading(),
12631268
sumoConfig.moveToXyMode
12641269
);
1270+
externalState.updatedInSumo(time);
12651271
} catch (InternalFederateException e) {
12661272
log.warn("Could not set position of vehicle " + external.getKey(), e);
12671273
}
@@ -1284,6 +1290,12 @@ private void removeExternalVehiclesFromUpdates(VehicleUpdates updates) {
12841290
updates.getRemovedNames().removeIf(vehicle -> externalVehicles.remove(vehicle) != null);
12851291
}
12861292

1293+
/**
1294+
* Changes parameters of externally added vehicles.
1295+
* So far only color change is supported.
1296+
* @param vehicleParametersChange Stores a list of vehicle parameters that should be changed.
1297+
* @throws InternalFederateException Throws an IllegalArgumentException if color could not be set correctly.
1298+
*/
12871299
public void changeExternalParameters(VehicleParametersChange vehicleParametersChange) throws InternalFederateException {
12881300
final String veh_id = vehicleParametersChange.getVehicleId();
12891301
for (final VehicleParameter param : vehicleParametersChange.getVehicleParameters()) {

‎fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/ExternalVehicleState.java

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
package org.eclipse.mosaic.fed.sumo.ambassador;
1717

1818
import org.eclipse.mosaic.lib.objects.vehicle.VehicleData;
19+
import org.eclipse.mosaic.rti.TIME;
1920

2021
/**
2122
* This class is used to hold and query data of externally simulated vehicles
@@ -24,13 +25,27 @@
2425
*/
2526
class ExternalVehicleState {
2627

28+
/**
29+
* Sets the longest time between two update commands. An external state
30+
* is updated in SUMO whenever it has changed. If the external state has
31+
* not changed, it is updated every TIME_DIFFERENCE_FOR_UPDATE seconds at most.
32+
*/
33+
public static final long TIME_DIFFERENCE_FOR_UPDATE = 4 * TIME.SECOND;
34+
2735
/**
2836
* Flag to indicate whether the {@link VehicleData} of the last update is added.
2937
* Every {@link ExternalVehicleState} is initialized as not added and will be
3038
* added during simulation.
3139
*/
3240
private boolean added = false;
3341

42+
/**
43+
* Stores the time at which this state was updated in SUMO.
44+
* This is used to prevent too frequent updates of vehicles whose state
45+
* did not change, e.g. for parking vehicles.
46+
*/
47+
private long timeOfLastUpdateInSumo;
48+
3449
private VehicleData lastMovementInfo = null;
3550

3651
/**
@@ -39,6 +54,7 @@ class ExternalVehicleState {
3954
* @param lastMovementInfo The last vehicle info.
4055
*/
4156
public void setLastMovementInfo(VehicleData lastMovementInfo) {
57+
this.timeOfLastUpdateInSumo = 0;
4258
this.lastMovementInfo = lastMovementInfo;
4359
}
4460

@@ -68,4 +84,20 @@ public VehicleData getLastMovementInfo() {
6884
public boolean isAdded() {
6985
return added;
7086
}
87+
88+
/**
89+
* Returns {@code true} if this external vehicle state requires an update in SUMO.
90+
*
91+
* @param time the current simulation time
92+
*/
93+
public boolean isRequireUpdate(long time) {
94+
return (time - timeOfLastUpdateInSumo) > TIME_DIFFERENCE_FOR_UPDATE;
95+
}
96+
97+
/**
98+
* Stores the time (ns) at which this external vehicle state was last updated in SUMO.
99+
*/
100+
public void updatedInSumo(long time) {
101+
this.timeOfLastUpdateInSumo = time;
102+
}
71103
}

‎fed/mosaic-sumo/src/main/java/org/eclipse/mosaic/fed/sumo/ambassador/SumoAmbassador.java

Lines changed: 24 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -278,9 +278,9 @@ private void addNotYetAddedVehicles(long time) throws InternalFederateException
278278
iterator.remove();
279279
continue;
280280
}
281-
// TODO: Find better solution. Currently, an arbitrary SUMO route for external vehicles is selected, since a registered
282-
// SUMO route is required when adding a vehicle to SUMO. Using an empty route id "" leads to an error.
283-
routeId = Iterables.getFirst(routes.keySet(), null);
281+
// We choose "" as the default routeId because we want it to be possible to spawn vehicles without a defined route.
282+
// This is especially useful for parking vehicles that are not supposed to move.
283+
routeId = Iterables.getFirst(routes.keySet(), "");
284284
laneId = "free";
285285
}
286286

@@ -289,13 +289,13 @@ private void addNotYetAddedVehicles(long time) throws InternalFederateException
289289
log.info("Adding new vehicle \"{}\" at simulation time {} ns (type={}, routeId={}, laneId={}, departPos={})",
290290
vehicleId, vehicleRegistration.getTime(), vehicleType, routeId, laneId, departPos);
291291

292-
if (!routes.containsKey(routeId)) {
292+
if (!routes.containsKey(routeId) && !routeId.isEmpty()) {
293293
throw new IllegalArgumentException(
294294
"Unknown route " + routeId + " for vehicle with departure time " + vehicleRegistration.getTime()
295295
);
296296
}
297297

298-
if (departIndex > 0) {
298+
if (departIndex > 0 && !routeId.isEmpty()) {
299299
routeId = cutAndAddRoute(routeId, departIndex);
300300
}
301301

@@ -391,38 +391,41 @@ private void applyChangesInVehicleTypeForVehicle(String vehicleId, VehicleType a
391391
}
392392

393393
private String extractDepartureSpeed(VehicleRegistration vehicleRegistration) {
394-
switch (vehicleRegistration.getDeparture().getDepartureSpeedMode()) {
395-
case PRECISE:
396-
return String.format(Locale.ENGLISH, "%.2f", vehicleRegistration.getDeparture().getDepartureSpeed());
397-
case RANDOM:
398-
return "random";
399-
case MAXIMUM:
400-
default:
401-
return "max";
402-
}
394+
return switch (vehicleRegistration.getDeparture().getDepartureSpeedMode()) {
395+
case PRECISE -> String.format(Locale.ENGLISH, "%.2f", vehicleRegistration.getDeparture().getDepartureSpeed());
396+
case RANDOM -> "random";
397+
case MAXIMUM -> "max";
398+
};
403399
}
404400

405401
private String extractDepartureLane(VehicleRegistration vehicleRegistration) {
406402
switch (vehicleRegistration.getDeparture().getLaneSelectionMode()) {
407-
case RANDOM:
403+
case RANDOM -> {
408404
return "random";
409-
case FREE:
405+
}
406+
case FREE -> {
410407
return "free";
411-
case ALLOWED:
408+
}
409+
case ALLOWED -> {
412410
return "allowed";
413-
case BEST:
411+
}
412+
case BEST -> {
414413
return "best";
415-
case FIRST:
414+
}
415+
case FIRST -> {
416416
return "first";
417-
case HIGHWAY:
417+
}
418+
case HIGHWAY -> {
418419
return isTruckOrTrailer(vehicleRegistration.getMapping().getVehicleType().getVehicleClass())
419420
? "first"
420421
: "best";
421-
default:
422+
}
423+
default -> {
422424
int extractedLaneId = vehicleRegistration.getDeparture().getDepartureLane();
423425
return extractedLaneId >= 0
424426
? Integer.toString(extractedLaneId)
425427
: "best";
428+
}
426429
}
427430
}
428431

0 commit comments

Comments
 (0)