initial implementation of CompoundObject for TraCI communication

VadereAnnotation/src/org/vadere/annotation/traci/client/TraCIApi.java

@@ -15,9 +15,11 @@ public @interface TraCIApi {
 			"org.vadere.manager.TraCISocket",
 			"org.vadere.manager.traci.commands.TraCIGetCommand",
 			"org.vadere.manager.traci.commands.TraCISetCommand",
+			"org.vadere.manager.traci.compoundobjects.CompoundObject",
 			"org.vadere.manager.traci.respons.TraCIGetResponse",
 			"org.vadere.manager.traci.writer.TraCIPacket",
 			"org.vadere.manager.traci.respons.TraCIResponse",
+
 			"java.io.IOException",
 			"java.util.ArrayList",
 			"org.vadere.util.geometry.shapes.VPoint"

VadereManager/src/org/vadere/manager/client/TestClient.java

@@ -5,6 +5,8 @@ import org.vadere.manager.traci.commands.control.TraCICloseCommand;
 import org.vadere.manager.traci.commands.control.TraCIGetVersionCommand;
 import org.vadere.manager.traci.commands.control.TraCISendFileCommand;
 import org.vadere.manager.traci.commands.control.TraCISimStepCommand;
+import org.vadere.manager.traci.compoundobjects.CompoundObject;
+import org.vadere.manager.traci.compoundobjects.CompoundObjectBuilder;
 import org.vadere.manager.traci.reader.TraCIPacketBuffer;
 import org.vadere.manager.traci.respons.TraCIGetResponse;
 import org.vadere.manager.traci.respons.TraCIResponse;
@@ -306,19 +308,18 @@ public class TestClient extends org.vadere.manager.client.AbstractTestClient imp
 	@Override
 	public void personapi_createNew(String[] args) throws IOException {
 		if(args.length < 5){
-			System.out.println("command needs argument element id, x-coordinate, y-coordinate");
+			System.out.println("command needs argument element id, x-coordinate, y-coordinate, list of targets");
 			return;
 		}
 
 		String elementIdentifier = args[1];
 		String x = args[2];
 		String y = args[3];
-//		ArrayList<String> targets = new ArrayList<>();
-//		for (int i = 4; i < args.length; i++){
-//			targets.add(args[i]);
-//		}
+		String[] targets = Arrays.copyOfRange(args,4,args.length);
 
-		TraCIResponse res =  personapi.createNew(elementIdentifier, new VPoint(Double.parseDouble(x), Double.parseDouble(y)));
+
+		CompoundObject compoundObj = CompoundObjectBuilder.createPerson(x, y, targets);
+		TraCIResponse res =  personapi.createNew(elementIdentifier, compoundObj);
 		System.out.println(res.toString());
 	}
 

VadereManager/src/org/vadere/manager/traci/commandHandler/PersonCommandHandler.java

@@ -292,7 +292,7 @@ public class PersonCommandHandler extends CommandHandler<PersonVar>{
 		return cmd;
 	}
 
-	@PersonHandler(cmd = TraCICmd.SET_PERSON_STATE, var = PersonVar.ADD, name = "createNew", dataTypeStr = "VPoint")
+	@PersonHandler(cmd = TraCICmd.SET_PERSON_STATE, var = PersonVar.ADD, name = "createNew", dataTypeStr = "CompoundObject")
 	public TraCICommand process_addPerson(TraCISetCommand cmd, RemoteManager remoteManager) {
 		VPoint tmp = (VPoint) cmd.getVariableValue();
 		Integer id =  Integer.parseInt(cmd.getElementId());

VadereManager/src/org/vadere/manager/traci/commandHandler/variables/PersonVar.java

@@ -28,7 +28,7 @@ public enum PersonVar {
 	NEXT_EDGE(0xc1, TraCIDataType.STRING), // get
 	REMAINING_STAGES(0xc2, TraCIDataType.INTEGER), // get
 	VEHICLE(0xc3, TraCIDataType.STRING), // get
-	ADD(0x80, TraCIDataType.POS_2D), // set todo: change back to TraCIDataType.COMPOUND_OBJECT
+	ADD(0x80, TraCIDataType.COMPOUND_OBJECT), // set
 	APPEND_STAGE(0xc4, TraCIDataType.COMPOUND_OBJECT), // set
 	REMOVE_STAGE(0xc5, TraCIDataType.INTEGER), // set
 	REROUTE(0x90, TraCIDataType.COMPOUND_OBJECT), // set

VadereManager/src/org/vadere/manager/traci/compoundobjects/CompoundObject.java

+package org.vadere.manager.traci.compoundobjects;
+
+import org.apache.commons.lang3.tuple.Pair;
+import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.TraCIDataType;
+
+import java.util.Iterator;
+
+public class CompoundObject {
+
+	private TraCIDataType[] type;
+	private Object[] data;
+	private int cur;
+
+	public CompoundObject(int noElements) {
+		this.type = new TraCIDataType[noElements];
+		this.data = new Object[noElements];
+		this.cur = 0;
+	}
+
+	public String types(){
+		StringBuilder sb = new StringBuilder();
+		sb.append("[");
+		for (TraCIDataType i : this.type) {
+			sb.append(i.name()).append(", ");
+		}
+		sb.delete(sb.length()-2, sb.length());
+		sb.append("]");
+		return sb.toString();
+	}
+
+	public int size(){
+		return data.length;
+	}
+
+	public CompoundObject add(int type, Object data){
+		return  add(TraCIDataType.fromId(type), data);
+	}
+
+	public CompoundObject add(TraCIDataType type, Object data){
+		if (cur > this.data.length)
+			throw new TraCIException("CompoundObject already full. Received " + types());
+
+		this.type[cur] = type;
+		this.data[cur] = data;
+
+		cur++;
+		return this;
+	}
+
+	public Object getData(int index, TraCIDataType type){
+		if (index > this.data.length)
+			throw new TraCIException("Cannot access data with index %d", index);
+		if (!this.type[index].equals(type))
+			throw new TraCIException("Type mismatch of CompoundObject element %s != %s  at index %d", this.type[index].name(), type.name(), index);
+		return this.data[index];
+	}
+
+	public Object getData(int index){
+		if (index > this.data.length)
+			throw new TraCIException("Cannot access data with index %d", index);
+		return this.data[index];
+	}
+
+	public Iterator<Pair<TraCIDataType, Object>> itemIterator(){
+		return new Iter(this);
+	}
+
+
+
+	private class Iter implements Iterator<Pair<TraCIDataType, Object>>{
+
+		private final CompoundObject compoundObject;
+		private int curr;
+
+		Iter(CompoundObject compoundObject){
+			this.compoundObject = compoundObject;
+			this.curr = 0;
+		}
+
+		@Override
+		public boolean hasNext() {
+			return curr < compoundObject.size();
+		}
+
+		@Override
+		public Pair<TraCIDataType, Object> next() {
+			Pair<TraCIDataType, Object> p = Pair.of(compoundObject.type[curr], compoundObject.data[curr]);
+			curr++;
+			return p;
+		}
+	}
+
+}

VadereManager/src/org/vadere/manager/traci/compoundobjects/CompoundObjectBuilder.java

+package org.vadere.manager.traci.compoundobjects;
+
+import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.util.geometry.shapes.VPoint;
+
+import java.util.LinkedList;
+
+public class CompoundObjectBuilder {
+
+	private LinkedList<TraCIDataType> types;
+
+	public CompoundObjectBuilder(){
+		this.types = new LinkedList<>();
+	}
+
+	public CompoundObjectBuilder rest(){
+		types.clear();
+		return this;
+	}
+
+	public CompoundObjectBuilder add(TraCIDataType type){
+		types.add(type);
+		return this;
+	}
+
+	public CompoundObject build(Object... data){
+		CompoundObject obj = new CompoundObject(data.length);
+		if (types.size() == data.length){
+			int idx = 0;
+			for (TraCIDataType type : types) {
+				obj.add(type, data[idx]);
+				idx++;
+			}
+
+		} else {
+			throw  new TraCIException("CompoundObjectBuilder error. Number of Types does not match received number of data items");
+		}
+
+		return obj;
+	}
+
+
+	static public CompoundObjectBuilder builder(){
+		return new CompoundObjectBuilder();
+	}
+
+	static public CompoundObject createPerson(String x, String y, String... targets){
+
+		VPoint p = new VPoint(Double.parseDouble(x), Double.parseDouble(y));
+
+		return CompoundObjectBuilder.builder()
+				.rest()
+				.add(TraCIDataType.POS_2D)
+				.add(TraCIDataType.STRING_LIST)
+				.build(p, targets);
+	}
+
+
+	static public CompoundObject createIdPosData(String id, String x, String y){
+
+		VPoint p = new VPoint(Double.parseDouble(x), Double.parseDouble(y));
+
+		return CompoundObjectBuilder.builder()
+				.rest()
+				.add(TraCIDataType.STRING)
+				.add(TraCIDataType.POS_2D)
+				.build(id, p);
+	}
+
+
+}

VadereManager/src/org/vadere/manager/traci/compoundobjects/GenericCompoundObject.java

+package org.vadere.manager.traci.compoundobjects;
+
+import org.vadere.manager.TraCIException;
+
+public abstract class GenericCompoundObject {
+
+
+	GenericCompoundObject(CompoundObject o, int size){
+		assertElementCount(o, size);
+		init(o);
+	}
+
+	protected void assertElementCount(final CompoundObject o, int size){
+		if (o.size() == size)
+			throw new TraCIException("Cannot create %s from CompoundObject containing %s", getClass().getName(), o.types());
+	}
+
+	abstract protected void init(CompoundObject o);
+}

VadereManager/src/org/vadere/manager/traci/compoundobjects/IdPosData.java

+package org.vadere.manager.traci.compoundobjects;
+
+import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class IdPosData extends GenericCompoundObject {
+
+	String id;
+	VPoint pos;
+
+	IdPosData(CompoundObject o) {
+		super(o, 2);
+	}
+
+	@Override
+	protected void init(CompoundObject o) {
+		id = (String)o.getData(0, TraCIDataType.STRING);
+		pos = (VPoint)o.getData(1, TraCIDataType.POS_2D);
+	}
+
+	public String getId() {
+		return id;
+	}
+
+	public VPoint getPos() {
+		return pos;
+	}
+}

VadereManager/src/org/vadere/manager/traci/compoundobjects/PersonCreateData.java

+package org.vadere.manager.traci.compoundobjects;
+
+import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.util.geometry.shapes.VPoint;
+
+import java.util.ArrayList;
+
+public class PersonCreateData extends GenericCompoundObject{
+
+	private String id;
+	private VPoint pos;
+	private ArrayList<String> targets;
+
+	PersonCreateData(CompoundObject o){
+		super(o, 3);
+	}
+
+	@Override
+	protected void init(CompoundObject o) {
+		id = (String) o.getData(0, TraCIDataType.STRING);
+		pos = (VPoint)o.getData(1, TraCIDataType.POS_2D);
+		targets = (ArrayList<String>)o.getData(2, TraCIDataType.STRING_LIST);
+	}
+
+	public String getId() {
+		return id;
+	}
+
+	public VPoint getPos() {
+		return pos;
+	}
+
+	public ArrayList<String> getTargets() {
+		return targets;
+	}
+}

VadereManager/src/org/vadere/manager/traci/reader/TraCIByteBuffer.java

 package org.vadere.manager.traci.reader;
 
 import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.compoundobjects.CompoundObject;
 import org.vadere.manager.traci.TraCIDataType;
 import org.vadere.manager.traci.sumo.LightPhase;
 import org.vadere.manager.traci.sumo.RoadMapPosition;
@@ -208,6 +209,23 @@ public class TraCIByteBuffer implements TraCIReader {
 		return new Color(r, g, b, a);
 	}
 
+	@Override
+	public CompoundObject readCompoundObject(){
+		ensureBytes(4);
+		int noElements = readInt();
+
+		CompoundObject compoundObject = new CompoundObject(noElements);
+
+		for(int i = 0; i<= noElements; i++){
+			TraCIDataType type = TraCIDataType.fromId(readUnsignedByte());
+			if (type.equals(TraCIDataType.COMPOUND_OBJECT))
+				throw new TraCIException("Recursive CompoundObject are not allowed.");
+			compoundObject.add(type, readTypeValue(type));
+		}
+
+		return compoundObject;
+	}
+
 	@Override
 	public Object readTypeValue(TraCIDataType type) {
 
@@ -241,7 +259,7 @@ public class TraCIByteBuffer implements TraCIReader {
 			case COLOR:
 				return readColor();
 			case COMPOUND_OBJECT:
-				return null; // todo: simple fix. For now we ignore Compound Objects.
+				return readCompoundObject();
 			default:
 				throw new TraCIException("Unknown Datatype: " + type.toString());
 		}

VadereManager/src/org/vadere/manager/traci/reader/TraCIReader.java

 package org.vadere.manager.traci.reader;
 
+import org.vadere.manager.traci.compoundobjects.CompoundObject;
 import org.vadere.manager.traci.TraCIDataType;
 import org.vadere.manager.traci.sumo.RoadMapPosition;
 import org.vadere.manager.traci.sumo.TrafficLightPhase;
@@ -60,6 +61,8 @@ public interface TraCIReader {
 
 	Color readColor();
 
+	CompoundObject readCompoundObject();
+
 	boolean hasRemaining();
 
 	void ensureBytes(int num);

VadereManager/src/org/vadere/manager/traci/writer/ByteArrayOutputStreamTraCIWriter.java

 package org.vadere.manager.traci.writer;
 
+import org.apache.commons.lang3.tuple.Pair;
 import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.compoundobjects.CompoundObject;
 import org.vadere.manager.traci.TraCIDataType;
 import org.vadere.manager.traci.sumo.RoadMapPosition;
 import org.vadere.manager.traci.sumo.TrafficLightPhase;
@@ -14,6 +16,7 @@ import java.nio.ByteBuffer;
 import java.nio.charset.Charset;
 import java.nio.charset.StandardCharsets;
 import java.util.Arrays;
+import java.util.Iterator;
 import java.util.List;
 
 public class ByteArrayOutputStreamTraCIWriter implements TraCIWriter {
@@ -90,6 +93,8 @@ public class ByteArrayOutputStreamTraCIWriter implements TraCIWriter {
 			case COLOR:
 				writeColor((Color) data);
 				break;
+			case COMPOUND_OBJECT:
+				writeCompoundObject((CompoundObject) data);
 			default:
 				logger.errorf("cannot write %s", dataType.toString());
 
@@ -272,6 +277,20 @@ public class ByteArrayOutputStreamTraCIWriter implements TraCIWriter {
 		return this;
 	}
 
+	@Override
+	public TraCIWriter writeCompoundObject(CompoundObject compoundObject) {
+		writeUnsignedByteWithId(TraCIDataType.COMPOUND_OBJECT.id);
+		writeInt(compoundObject.size());
+		Iterator<Pair<TraCIDataType, Object>> iter = compoundObject.itemIterator();
+		while (iter.hasNext()){
+			Pair<TraCIDataType, Object> p = iter.next();
+			if (p.getLeft().equals(TraCIDataType.COMPOUND_OBJECT))
+					throw new TraCIException("Recursive CompoundObject are not allowed.");
+			writeObjectWithId(p.getLeft(), p.getRight());
+		}
+		return this;
+	}
+
 	@Override
 	public int stringByteCount(String str) {
 		return str.getBytes(StandardCharsets.US_ASCII).length;

VadereManager/src/org/vadere/manager/traci/writer/TraCIWriter.java

 package org.vadere.manager.traci.writer;
 
+import org.vadere.manager.traci.compoundobjects.CompoundObject;
 import org.vadere.manager.traci.TraCIDataType;
 import org.vadere.manager.traci.sumo.RoadMapPosition;
 import org.vadere.manager.traci.sumo.TrafficLightPhase;
@@ -84,6 +85,8 @@ public interface TraCIWriter {
 
 	TraCIWriter writeColor(Color color);
 
+	TraCIWriter writeCompoundObject(CompoundObject compoundObject);
+
 	TraCIWriter writeCommandLength(int cmdLen);
 
 	int stringByteCount(String str);