Commit 43e41dcc authored by Stefan Schuhbaeck's avatar Stefan Schuhbaeck
Browse files

implement TraCI commands for artery use

parent 46dcfdea
Pipeline #307279 passed with stages
in 127 minutes and 20 seconds
......@@ -456,12 +456,32 @@ public class TestClient extends org.vadere.manager.client.AbstractTestClient imp
}
@Override
public void simulationapi_getDepartedPedestrianId(String[] args) throws IOException {
}
@Override
public void simulationapi_getArrivedPedestrianIds(String[] args) throws IOException {
}
@Override
public void simulationapi_getPositionConversion(String[] args) throws IOException {
}
@Override
public void simulationapi_getTime(String[] args) throws IOException {
TraCIResponse res = simulationapi.getTime();
System.out.println(res.toString());
}
@Override
public void simulationapi_getSimSte(String[] args) throws IOException {
}
@Override
public void simulationapi_setSimConfig(String[] args) throws IOException {
System.out.println("not implemented");
......@@ -599,6 +619,11 @@ public class TestClient extends org.vadere.manager.client.AbstractTestClient imp
System.out.println(res.toString());
}
@Override
public void simulationapi_getNetworkBound(String[] args) throws IOException {
}
// vadere api
@Override
......
......@@ -30,9 +30,9 @@ public class CommandExecutor {
cmdMap.put(TraCICmd.SEND_FILE.id, ControlCommandHandler.instance::process_load_file);
cmdMap.put(TraCICmd.GET_PERSON_VALUE.id, PersonCommandHandler.instance::processGet);
cmdMap.put(TraCICmd.SET_PERSON_STATE.id, PersonCommandHandler.instance::processSet);
cmdMap.put(TraCICmd.SUB_PERSON_VARIABLE.id, PersonCommandHandler.instance::processValueSub);
cmdMap.put(TraCICmd.GET_VADERE_VALUE.id, VadereCommandHandler.instance::processGet);
cmdMap.put(TraCICmd.SET_VADERE_STATE.id, VadereCommandHandler.instance::processSet);
cmdMap.put(TraCICmd.SUB_PERSON_VARIABLE.id, PersonCommandHandler.instance::processValueSub);
cmdMap.put(TraCICmd.GET_SIMULATION_VALUE.id, SimulationCommandHandler.instance::processGet);
cmdMap.put(TraCICmd.SET_SIMULATION_STATE.id, SimulationCommandHandler.instance::processSet);
cmdMap.put(TraCICmd.SUB_SIMULATION_VALUE.id, SimulationCommandHandler.instance::processValueSub);
......@@ -67,6 +67,7 @@ public class CommandExecutor {
return TraCIPacket.createErr(cmd.getTraCICmd().id, e.getMessageForClient());
}
logger.tracef("return response for: %s", cmd.getTraCICmd().logShort());
return response;
}
}
......@@ -116,8 +116,11 @@ public abstract class CommandHandler<VAR extends Enum> {
}
public TraCICommand processValueSub(TraCICommand rawCmd, RemoteManager remoteManager,
TraCICmdHandler traCICmdHandler, TraCICmd getCommand, TraCICmd apiCmdResponse) {
public TraCICommand processValueSub(TraCICommand rawCmd,
RemoteManager remoteManager,
TraCICmdHandler traCICmdHandler,
TraCICmd getCommand,
TraCICmd apiCmdResponse) {
TraCIValueSubscriptionCommand cmd = (TraCIValueSubscriptionCommand) rawCmd;
List<TraCIGetCommand> getCommands = new ArrayList<>();
......
......@@ -84,7 +84,9 @@ public class ControlCommandHandler extends CommandHandler<ControlVar> {
logger.debugf("%s: execute %d subscriptions",
TraCICmd.SIM_STEP.name(),
remoteManager.getSubscriptions().size());
remoteManager.getSubscriptions().forEach(sub -> sub.executeSubscription(remoteManager));
remoteManager.getSubscriptions().forEach(sub -> {
sub.executeSubscription(remoteManager);
});
// remove subscriptions no longer valid
remoteManager.getSubscriptions().removeIf(Subscription::isMarkedForRemoval);
......
......@@ -210,8 +210,10 @@ public class PersonCommandHandler extends CommandHandler<PersonVar> {
remoteManager.accessState((manager, state) -> {
Pedestrian ped = state.getTopography().getPedestrianDynamicElements()
.getElement(Integer.parseInt(cmd.getElementIdentifier()));
if (checkIfPedestrianExists(ped, cmd))
cmd.setResponse(responseOK(PersonVar.SPEED.type, ped.getFreeFlowSpeed()));
if (checkIfPedestrianExists(ped, cmd)){
double speed = ped.getFootstepHistory().getAverageSpeedInMeterPerSecond();
cmd.setResponse(responseOK(PersonVar.SPEED.type, Double.isNaN(speed) ? 0.0 : speed));
}
});
return cmd;
}
......@@ -366,7 +368,15 @@ public class PersonCommandHandler extends CommandHandler<PersonVar> {
@PersonHandler(cmd = TraCICmd.GET_PERSON_VALUE, var = PersonVar.ANGLE, name = "getAngle")
public TraCICommand process_getAngle(TraCIGetCommand cmd, RemoteManager remoteManager) {
// return dummy value
cmd.setResponse(responseOK(PersonVar.ANGLE.type, 0.0));
remoteManager.accessState((manager, state) -> {
Pedestrian ped = state.getTopography().getPedestrianDynamicElements()
.getElement(Integer.parseInt(cmd.getElementIdentifier()));
if (checkIfPedestrianExists(ped, cmd)){
double angle = ped.getFootstepHistory().getNorthBoundHeadingAngleDeg();
cmd.setResponse(responseOK(PersonVar.ANGLE.type, angle));
}
});
return cmd;
}
......
package org.vadere.manager.traci.commandHandler;
import org.apache.commons.math3.util.Pair;
import org.vadere.annotation.traci.client.TraCIApi;
import org.vadere.manager.RemoteManager;
import org.vadere.manager.TraCICommandCreationException;
......@@ -13,12 +14,16 @@ import org.vadere.manager.traci.commands.TraCICommand;
import org.vadere.manager.traci.commands.TraCIGetCommand;
import org.vadere.manager.traci.commands.TraCISetCommand;
import org.vadere.manager.traci.commands.get.TraCIGetCacheHashCommand;
import org.vadere.manager.traci.commands.get.TraCIGetCompoundPayload;
import org.vadere.manager.traci.compound.CompoundObject;
import org.vadere.manager.traci.compound.object.PointConverter;
import org.vadere.manager.traci.compound.object.SimulationCfg;
import org.vadere.manager.traci.response.TraCIGetResponse;
import org.vadere.simulator.entrypoints.ScenarioFactory;
import org.vadere.simulator.projects.Scenario;
import org.vadere.simulator.utils.cache.ScenarioCache;
import org.vadere.state.scenario.Agent;
import org.vadere.state.scenario.ReferenceCoordinateSystem;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.logging.Logger;
......@@ -28,6 +33,10 @@ import java.lang.reflect.Method;
import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Collectors;
/**
* Handel GET/SET/SUB {@link org.vadere.manager.traci.commands.TraCICommand}s for the Simulation
......@@ -45,6 +54,9 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
public static SimulationCommandHandler instance;
private static Logger logger = Logger.getLogger(SimulationCommandHandler.class);
private Pair<Double, Set<Integer>> allPrevious; // time at witch the given ids were send over traci
private Pair<Double, List<String>> departedCache; // time at witch the given ids were send over traci
private Pair<Double, List<String>> arrivedCache; // time at witch the given ids were send over traci
static {
instance = new SimulationCommandHandler();
......@@ -53,6 +65,9 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
private SimulationCommandHandler() {
super();
init(SimulationHandler.class, SimulationHandlers.class);
allPrevious = Pair.create(-1.0, new HashSet<>()); // never called.
departedCache = Pair.create(-1.0, new ArrayList<>());
arrivedCache = Pair.create(-1.0, new ArrayList<>());
}
@Override
......@@ -81,7 +96,10 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
return responseERR("[" + var.toString() + "] " + err, TraCICmd.GET_SIMULATION_VALUE, TraCICmd.RESPONSE_GET_SIMULATION_VALUE);
}
public TraCICommand process_getNetworkBound(TraCIGetCommand cmd, RemoteManager remoteManager, SimulationVar traCIVar) {
@SimulationHandler(cmd = TraCICmd.GET_SIMULATION_VALUE, var = SimulationVar.NETWORK_BOUNDING_BOX_2D,
name = "getNetworkBound", ignoreElementId = true)
public TraCICommand process_getNetworkBound(TraCIGetCommand cmd, RemoteManager remoteManager) {
remoteManager.accessState((manager, state) -> {
Rectangle2D.Double rec = state.getTopography().getBounds();
......@@ -91,7 +109,7 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
ArrayList<VPoint> polyList = new ArrayList<>();
polyList.add(lowLeft);
polyList.add(highRight);
cmd.setResponse(responseOK(traCIVar.type, polyList));
cmd.setResponse(responseOK(SimulationVar.NETWORK_BOUNDING_BOX_2D.type, polyList));
});
return cmd;
......@@ -99,7 +117,7 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
@SimulationHandler(cmd = TraCICmd.GET_SIMULATION_VALUE, var = SimulationVar.CURR_SIM_TIME,
name = "getTime", ignoreElementId = true)
public TraCICommand process_getSimTime(TraCIGetCommand cmd, RemoteManager remoteManager, SimulationVar traCIVar) {
public TraCICommand process_getSimTime(TraCIGetCommand cmd, RemoteManager remoteManager) {
remoteManager.accessState((manager, state) -> {
// BigDecimal to ensure correct comparison in omentpp
......@@ -110,6 +128,20 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
return cmd;
}
@SimulationHandler(cmd = TraCICmd.GET_SIMULATION_VALUE, var = SimulationVar.VAR_DELTA_T,
name = "getSimSte", ignoreElementId = true)
public TraCICommand process_getSimStep(TraCIGetCommand cmd, RemoteManager remoteManager) {
remoteManager.accessState((manager, state) -> {
// BigDecimal to ensure correct comparison in omentpp
double time = state.getScenarioStore().getAttributesSimulation().getSimTimeStepLength();
cmd.setResponse(responseOK(SimulationVar.CURR_SIM_TIME.type, time));
});
return cmd;
}
@SimulationHandler(cmd = TraCICmd.SET_SIMULATION_STATE, var = SimulationVar.SIM_CONFIG,
name = "setSimConfig", ignoreElementId = true)
public TraCICommand process_setSimConfig(TraCISetCommand cmd, RemoteManager remoteManager) {
......@@ -150,30 +182,76 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
return rawCmd;
}
public TraCICommand process_getVehiclesStartTeleportIDs(TraCIGetCommand cmd, RemoteManager remoteManager, SimulationVar traCIVar) {
@SimulationHandler(cmd = TraCICmd.GET_SIMULATION_VALUE, var = SimulationVar.DEPARTED_PEDESTRIAN_IDS,
name = "getDepartedPedestrianId", dataTypeStr = "ArrayList<String>", ignoreElementId = true)
public TraCICommand process_getDepartedPedestrianIds(TraCIGetCommand cmd, RemoteManager remoteManager) {
cmd.setResponse(responseOK(traCIVar.type, new ArrayList<>()));
calcArrivedDeparted(remoteManager);
cmd.setResponse(responseOK(SimulationVar.DEPARTED_PEDESTRIAN_IDS.type, departedCache.getSecond()));
return cmd;
}
public TraCICommand process_getVehiclesEndTeleportIDs(TraCIGetCommand cmd, RemoteManager remoteManager, SimulationVar traCIVar) {
cmd.setResponse(responseOK(traCIVar.type, new ArrayList<>()));
@SimulationHandler(cmd = TraCICmd.GET_SIMULATION_VALUE, var = SimulationVar.ARRIVED_PEDESTRIAN_PEDESTRIAN_IDS,
name = "getArrivedPedestrianIds", dataTypeStr = "ArrayList<String>", ignoreElementId = true)
public TraCICommand process_getArrivedPedestrianIds(TraCIGetCommand cmd, RemoteManager remoteManager) {
calcArrivedDeparted(remoteManager);
cmd.setResponse(responseOK(SimulationVar.DEPARTED_PEDESTRIAN_IDS.type, arrivedCache.getSecond()));
return cmd;
}
public TraCICommand process_getVehiclesStartParkingIDs(TraCIGetCommand cmd, RemoteManager remoteManager, SimulationVar traCIVar) {
public void calcArrivedDeparted(RemoteManager remoteManager){
cmd.setResponse(responseOK(traCIVar.type, new ArrayList<>()));
return cmd;
remoteManager.accessState((manager, state) -> {
if (allPrevious.getFirst().equals(state.getSimTimeInSec())){
// do nothing everything up to date. (called multiple times in one timeStep.)
}else {
// ped that were there but are not now
Set<Integer> all_now = state.getTopography().getPedestrianDynamicElements().getElements()
.stream()
.map(Agent::getId)
.collect(Collectors.toSet());
Set<Integer> departed = new HashSet<>(all_now); // copy
departed.removeAll(allPrevious.getSecond()); // departed (newly created): Ids which were ARE present at time t but were not at time t-1
Set<Integer> arrived = new HashSet<>(allPrevious.getSecond()); // copy
arrived.removeAll(all_now); // arrived (removed in this timestep): Ids which are NOT present at time t but were at time t-1
allPrevious = Pair.create(state.getSimTimeInSec(), all_now);
departedCache = Pair.create(state.getSimTimeInSec(), departed.stream()
.map(i -> Integer.toString(i))
.collect(Collectors.toList()));
arrivedCache = Pair.create(state.getSimTimeInSec(), arrived.stream()
.map(i->Integer.toString(i))
.collect(Collectors.toList()));
}
});
}
public TraCICommand process_getVehiclesStopParkingIDs(TraCIGetCommand cmd, RemoteManager remoteManager, SimulationVar traCIVar) {
@SimulationHandler(cmd = TraCICmd.GET_SIMULATION_VALUE, var = SimulationVar.POSITION_CONVERSION,
name = "getPositionConversion", dataTypeStr = "ArrayList<String>", ignoreElementId = true)
public TraCICommand process_PostionConversion(TraCIGetCommand cmd, RemoteManager remoteManager) {
TraCIGetCompoundPayload pCmd = new TraCIGetCompoundPayload(cmd);
PointConverter pointConverter = new PointConverter(pCmd.getData());
remoteManager.accessState((manager, state) -> {
ReferenceCoordinateSystem coord =
state.getScenarioStore().getTopography()
.getAttributes().getReferenceCoordinateSystem();
coord.initialize();
if (!coord.supportsConversion()){
cmd.setResponse(responseERR("Conversion not supported. Is ReferenceCoordinateSystem correctly set in Vadere?",
TraCICmd.GET_SIMULATION_VALUE, TraCICmd.RESPONSE_GET_SIMULATION_VALUE));
} else {
Pair<TraCIDataType, VPoint> p = pointConverter.convert(coord);
cmd.setResponse(responseOK(p.getFirst(), p.getSecond()));
}
});
cmd.setResponse(responseOK(traCIVar.type, new ArrayList<>()));
return cmd;
}
public TraCICommand processValueSub(TraCICommand rawCmd, RemoteManager remoteManager) {
return processValueSub(rawCmd, remoteManager, this::processGet,
TraCICmd.GET_SIMULATION_VALUE, TraCICmd.RESPONSE_SUB_SIMULATION_VALUE);
......@@ -184,36 +262,21 @@ public class SimulationCommandHandler extends CommandHandler<SimulationVar> {
TraCIGetCommand cmd = (TraCIGetCommand) rawCmd;
SimulationVar var = SimulationVar.fromId(cmd.getVariableIdentifier());
switch (var) {
case NETWORK_BOUNDING_BOX_2D:
return process_getNetworkBound(cmd, remoteManager, var);
case CURR_SIM_TIME:
return process_getSimTime(cmd, remoteManager, var);
case VEHICLES_START_TELEPORT_IDS:
return process_getVehiclesStartTeleportIDs(cmd, remoteManager, var);
case VEHICLES_END_TELEPORT_IDS:
return process_getVehiclesEndTeleportIDs(cmd, remoteManager, var);
case VEHICLES_START_PARKING_IDS:
return process_getVehiclesStartParkingIDs(cmd, remoteManager, var);
case VEHICLES_STOP_PARKING_IDS:
return process_getVehiclesStopParkingIDs(cmd, remoteManager, var);
case CACHE_HASH:
return process_getCacheHash(cmd, remoteManager);
default:
return process_NotImplemented(cmd, remoteManager);
}
Method m = getHandler(cmd.getTraCICmd(), var);
return invokeHandler(m, this, cmd, remoteManager);
}
public TraCICommand processSet(TraCICommand rawCmd, RemoteManager remoteManager) {
TraCISetCommand cmd = (TraCISetCommand) rawCmd;
SimulationVar var = SimulationVar.fromId(cmd.getVariableId());
switch (var) {
case SIM_CONFIG:
return process_setSimConfig(cmd, remoteManager);
default:
return process_NotImplemented(cmd, remoteManager);
}
Method m = getHandler(cmd.getTraCICmd(), var);
return invokeHandler(m, this, cmd, remoteManager);
}
......
......@@ -2,14 +2,19 @@ package org.vadere.manager.traci.commandHandler.variables;
import org.vadere.manager.TraCIException;
import org.vadere.manager.traci.TraCIDataType;
import org.vadere.manager.traci.commands.TraCICommand;
public enum SimulationVar {
CURR_SIM_TIME(0x66, TraCIDataType.DOUBLE),
VAR_DELTA_T(0x7b, TraCIDataType.DOUBLE), //step length
NUM_LOADED_VEHICLES(0x71, TraCIDataType.INTEGER),
LOADED_VEHICLES_IDS(0x72, TraCIDataType.STRING_LIST),
NUM_DEPARTED_VEHICLES(0x73, TraCIDataType.INTEGER),
DEPARTED_VEHICLES_IDS(0x74, TraCIDataType.STRING_LIST),
NUM_DEPARTED_PEDESTRIAN(0x73, TraCIDataType.INTEGER), // alias NUM_DEPARTED_VEHICLES
DEPARTED_PEDESTRIAN_IDS(0x74, TraCIDataType.STRING_LIST), // alias DEPARTED_VEHICLES_IDS
NUM_ARRIVED_PEDESTRIAN(0x79, TraCIDataType.INTEGER), // alias VAR_ARRIVED_VEHICLES_NUMBER
ARRIVED_PEDESTRIAN_PEDESTRIAN_IDS(0x7a, TraCIDataType.STRING_LIST), // alias VAR_ARRIVED_VEHICLES_IDS
POSITION_CONVERSION(0x82, TraCIDataType.COMPOUND_OBJECT),
NUM_VEHICLES_START_TELEPORT(0x75, TraCIDataType.INTEGER),
VEHICLES_START_TELEPORT_IDS(0x76, TraCIDataType.STRING_LIST),
NUM_VEHICLES_END_TELEPORT(0x77, TraCIDataType.INTEGER),
......
package org.vadere.manager.traci.reader;
import org.vadere.manager.traci.TraCICmd;
import org.vadere.manager.traci.commandHandler.CommandExecutor;
import org.vadere.manager.traci.commands.TraCICommand;
import org.vadere.manager.traci.response.StatusResponse;
import org.vadere.manager.traci.response.TraCIResponse;
import org.vadere.util.logging.Logger;
import java.nio.ByteBuffer;
......@@ -16,6 +18,8 @@ import java.nio.ByteBuffer;
*/
public class TraCIPacketBuffer extends TraCIByteBuffer {
private static Logger logger = Logger.getLogger(TraCIPacketBuffer.class);
protected TraCIPacketBuffer(byte[] buf) {
super(buf);
}
......@@ -41,7 +45,7 @@ public class TraCIPacketBuffer extends TraCIByteBuffer {
return null;
int cmdLen = getCommandDataLen();
logger.tracef("read next command with length %d", cmdLen);
return TraCICommand.create(readByteBuffer(cmdLen));
}
......
......@@ -93,7 +93,7 @@ public class SimulationCommandHandlerTest extends CommandHandlerTest {
when(simState.getSimTimeInSec()).thenReturn(retVal);
}
};
TraCICommand ret = simCmdHandler.process_getSimTime(cmd, rm, var);
TraCICommand ret = simCmdHandler.process_getSimTime(cmd, rm);
checkGET_OK(ret);
testGetValue(ret, varID, varType, elementID, retVal);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment