Updated detection to use multidimensional arrays.

Update CARLA Ambassador to trigger interactions for detections
Updated Detector registration to include infrastructure ID

co-simulation/fed/mosaic-carla/src/main/java/org/eclipse/mosaic/fed/carla/ambassador/CarlaAmbassador.java

@@ -25,7 +25,9 @@
 import org.eclipse.mosaic.fed.sumo.traci.writer.ListTraciWriter;
 import org.eclipse.mosaic.fed.sumo.traci.writer.StringTraciWriter;
 import org.eclipse.mosaic.interactions.application.*;
+import org.eclipse.mosaic.interactions.detector.DetectedObjectInteraction;
 import org.eclipse.mosaic.interactions.detector.DetectorRegistration;
+import org.eclipse.mosaic.lib.objects.detector.DetectedObject;
 import org.eclipse.mosaic.lib.objects.detector.Detector;
 import org.eclipse.mosaic.lib.util.ProcessLoggingThread;
 import org.eclipse.mosaic.lib.util.objects.ObjectInstantiation;
@@ -115,7 +117,7 @@ public class CarlaAmbassador extends AbstractFederateAmbassador {
      */
     private final PriorityBlockingQueue<CarlaV2xMessageReception> carlaV2xInteractionQueue = new PriorityBlockingQueue<>();
 
-    private List<Detector> registeredDetectors = new ArrayList<>();
+    private List<DetectorRegistration> registeredDetectors = new ArrayList<>();
 
     /**
      * Creates a new {@link CarlaAmbassador} object.
@@ -358,16 +360,25 @@ public synchronized void processTimeAdvanceGrant(long time) throws InternalFeder
                 nextTimeStep += carlaConfig.updateInterval * TIME.MILLI_SECOND;
                 isSimulationStep = false;
             }
+            // TODO: What is this. Why are we request a time advance based on this counter and 
+            // what is it counting. It is labeled as counting the times we attempt to connect to 
+            // CARLA but it seems to increment every time processTimeAdvanceGrant is called
             rti.requestAdvanceTime(nextTimeStep + this.executedTimes, 0, (byte) 2);
             this.executedTimes++;
-
-            //call CarlaXmlRpcClient to ask for data whenever time advances
-            for (Detector detector: registeredDetectors ) {
-                carlaXmlRpcClient.get_detected_objects(detector.getSensorId());
+            List<DetectedObjectInteraction> detectedObjectInteractions = new ArrayList();
+            // Get all detections from all currently registered detectors.
+            for (DetectorRegistration registration: registeredDetectors ) {
+                DetectedObject[] detections = carlaXmlRpcClient.getDetectedObjects( registration.getInfrastructureId() , registration.getDetector().getSensorId());
+                for (DetectedObject detected: detections) {
+                    detectedObjectInteractions.add(new DetectedObjectInteraction(time, detected));
+                }
+            }
+            // trigger all detection interactions
+            for (DetectedObjectInteraction detectionInteraction: detectedObjectInteractions) {
+                this.rti.triggerInteraction(detectionInteraction);
             }
         } catch (IllegalValueException e) {
             log.error("Error during advanceTime(" + time + ")", e);
-            throw new InternalFederateException(e);
         }
         catch (XmlRpcException e) {
             log.error("Failed to connect to CARLA Adapter : ", e);
@@ -513,8 +524,8 @@ else if (interaction.getTypeId().equals(DetectorRegistration.TYPE_ID)) {
 
     private void receiveInteraction(DetectorRegistration interaction) {
         try {
-            carlaXmlRpcClient.create_sensor(interaction);
-            registeredDetectors.add(interaction.getDetector());
+            carlaXmlRpcClient.createSensor(interaction);
+            registeredDetectors.add(interaction);
         }
         catch(XmlRpcException e) {
             log.error("Error occurred attempting to create sensor : {}", interaction.getDetector(), e);

co-simulation/fed/mosaic-carla/src/main/java/org/eclipse/mosaic/fed/carla/carlaconnect/CarlaXmlRpcClient.java

@@ -16,18 +16,22 @@
 package org.eclipse.mosaic.fed.carla.carlaconnect;
 
 import java.net.URL;
+import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
 import org.apache.xmlrpc.XmlRpcException;
 import org.apache.xmlrpc.client.XmlRpcClient;
 import org.apache.xmlrpc.client.XmlRpcClientConfigImpl;
 import org.apache.xmlrpc.client.XmlRpcClientException;
+import org.eclipse.mosaic.interactions.detector.DetectedObjectInteraction;
 import org.eclipse.mosaic.interactions.detector.DetectorRegistration;
 import org.eclipse.mosaic.lib.objects.detector.DetectedObject;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
+import com.google.gson.Gson;
+
 /**
  * This is a class uses xmlrpc to connect with CARLA CDASim adapter service
  */
@@ -71,12 +75,12 @@ public void initialize(URL xmlRpcServerUrl)
     }
 
 
-    public void create_sensor(DetectorRegistration registration) throws XmlRpcException{
+    public void createSensor(DetectorRegistration registration) throws XmlRpcException{
         List<Double> location = Arrays.asList(registration.getDetector().getLocation().getX(), registration.getDetector().getLocation().getY(), registration.getDetector().getLocation().getZ());
         List<Double> orientation = Arrays.asList(registration.getDetector().getOrientation().getPitch(), registration.getDetector().getOrientation().getRoll(), registration.getDetector().getOrientation().getYaw());
 
         if (isConnected) {
-            Object[] params = new Object[]{1/** TODO Infrastructure ID */, registration.getDetector().getSensorId(), location, orientation};
+            Object[] params = new Object[]{registration.getInfrastructureId(), registration.getDetector().getSensorId(), location, orientation};
             Object result = client.execute(CREATE_SENSOR, params);
             log.info((String)result);
         }
@@ -85,14 +89,20 @@ public void create_sensor(DetectorRegistration registration) throws XmlRpcExcept
         }
     }
 
-    public void get_detected_objects(String sensorId) throws XmlRpcException{
+    public DetectedObject[] getDetectedObjects(String infrastructureId ,String sensorId) throws XmlRpcException{
         if (isConnected) {
-            Object[] params = new Object[]{1/** TODO Infrastructure ID */, sensorId};
+            Object[] params = new Object[]{infrastructureId, sensorId};
             Object result = client.execute(GET_DETECTED_OBJECTS, params);
-            log.info((String)result);
+            log.info("Detections from infrastructure {} sensor {} : {}", infrastructureId, sensorId, result);
+            String jsonResult = (String)result;
+            Gson gson = new Gson();
+            DetectedObject[] parsedMessage = gson.fromJson(jsonResult,
+                    DetectedObject[].class);
+            return parsedMessage;
         }
         else {
-            log.warn("XMLRpcClient is not connected to CARLA Adapter!");
+            throw new XmlRpcException("XMLRpcClient is not connected to CARLA Adapter!");
+
         }
     }
 

co-simulation/fed/mosaic-infrastructure/src/main/java/org/eclipse/mosaic/fed/infrastructure/ambassador/InfrastructureMessageAmbassador.java

@@ -311,7 +311,7 @@ public synchronized void processTimeAdvanceGrant(long time) throws InternalFeder
                     log.debug("Sending SensorRegistration interactions for sensor : {}", reg.getSensors());
                     for (Detector sensor : reg.getSensors()) {
                         // Trigger Sensor registrations for all listed sensors.
-                        this.rti.triggerInteraction(new DetectorRegistration(time,sensor));
+                        this.rti.triggerInteraction(new DetectorRegistration(time,sensor,reg.getInfrastructureId()));
                     }
                 } 
                 else {

co-simulation/lib/mosaic-interactions/src/main/java/org/eclipse/mosaic/interactions/detector/DetectorRegistration.java

@@ -22,14 +22,17 @@ public class DetectorRegistration extends Interaction {
     public static final String TYPE_ID = createTypeIdentifier(DetectorRegistration.class);
 
     private Detector detector;
+
+    private String infrastructureId;
     /**
      * Constructor
      * @param time for interaction.
      * @param sensor to register.
      */
-    public DetectorRegistration(long time, Detector sensor) {
+    public DetectorRegistration(long time, Detector sensor, String infrastructureId) {
         super(time);
         this.detector = sensor;
+        this.infrastructureId = infrastructureId;
     }
     /**
      * Getter for sensor/detector information.
@@ -45,15 +48,20 @@ public Detector getDetector() {
     public void setDetector(Detector sensor) {
         this.detector = sensor;
     }
-
+    public String getInfrastructureId() {
+        return infrastructureId;
+    }
+    public void setInfrastructureId(String infrastructureId) {
+        this.infrastructureId = infrastructureId;
+    }
     @Override
     public int hashCode() {
         final int prime = 31;
         int result = super.hashCode();
         result = prime * result + ((detector == null) ? 0 : detector.hashCode());
+        result = prime * result + ((infrastructureId == null) ? 0 : infrastructureId.hashCode());
         return result;
     }
-
     @Override
     public boolean equals(Object obj) {
         if (this == obj)
@@ -68,8 +76,15 @@ public boolean equals(Object obj) {
                 return false;
         } else if (!detector.equals(other.detector))
             return false;
+        if (infrastructureId == null) {
+            if (other.infrastructureId != null)
+                return false;
+        } else if (!infrastructureId.equals(other.infrastructureId))
+            return false;
         return true;
     }
 
 
+
+
 }

co-simulation/lib/mosaic-interactions/src/test/java/org/eclipse/mosaic/interactions/detector/DetectorRegistrationTest.java

@@ -29,7 +29,7 @@ public class DetectorRegistrationTest {
     DetectorRegistration detectorRegistration;
     @Before
     public void setUp() throws Exception {
-        detectorRegistration = new DetectorRegistration(0, null);
+        detectorRegistration = new DetectorRegistration(0, null, "");
     }
 
     @Test
@@ -39,7 +39,7 @@ public void testGetterSetterConstructor() {
         detectorRegistration.setDetector(detector);
         assertEquals(detector, detectorRegistration.getDetector());
 
-        DetectorRegistration detectorRegistration1 = new DetectorRegistration(0, detector);
+        DetectorRegistration detectorRegistration1 = new DetectorRegistration(0, detector, "rsu_1");
 
         assertNotEquals(detectorRegistration, detectorRegistration1);
         DetectorRegistration detectorRegistration2 = detectorRegistration1;

co-simulation/lib/mosaic-objects/src/main/java/org/eclipse/mosaic/lib/objects/detector/DetectedObject.java

@@ -37,15 +37,15 @@ public final class DetectedObject implements Serializable {
 
     private CartesianPoint position;
 
-    private Double[] positionCovariance = new Double[9];
+    private Double[][] positionCovariance = new Double[3][3];
 
     private Vector3d velocity;
 
-    private Double[] velocityCovariance = new Double[9];
+    private Double[][] velocityCovariance = new Double[3][3];
 
     private Vector3d angularVelocity;
 
-    private Double[] angularVelocityCovariance = new Double[9];
+    private Double[][] angularVelocityCovariance = new Double[3][3];
 
     private Size size;
     /**
@@ -96,56 +96,15 @@ public String getProjString() {
         return projString;
     }
 
-    /**
-     * Getter for 3x3 covariance associated with position represented as
-     * a 9 element vector for JSON serialization/deserialization.
-     * @return
-     */
-    public Double[] getPositionCovariance() {
-        return positionCovariance;
-    }
-
-    /**
-     * Setter for 3x3 covariance associated with position represented as
-     * a 9 element vector for JSON serialization/deserialization.
-     * @param positionCovariance
-     */
-    public void setPositionCovariance(Double[] positionCovariance) {
+    public void setPositionCovariance(Double[][] positionCovariance) {
         this.positionCovariance = positionCovariance;
     }
 
-    /**
-     * Getter for 3x3 covariance associated with velocity represented as
-     * a 9 element vector for JSON serialization/deserialization.
-     * @return
-     */
-    public Double[] getVelocityCovariance() {
-        return velocityCovariance;
-    }
-
-    /**
-     * Setter for 3x3 covariance associated with velocity represented as
-     * a 9 element vector for JSON serialization/deserialization.
-     * @param positionCovariance
-     */
-    public void setVelocityCovariance(Double[] velocityCovariance) {
+    public void setVelocityCovariance(Double[][] velocityCovariance) {
         this.velocityCovariance = velocityCovariance;
     }
-    /**
-     * Getter for 3x3 covariance associated with angular velocity represented 
-     * as a 9 element vector for JSON serialization/deserialization.
-     * @return
-     */
-    public Double[] getAngularVelocityCovariance() {
-        return angularVelocityCovariance;
-    }
 
-    /**
-     * Setter for 3x3 covariance associated with angular velocity represented 
-     * as a 9 element vector for JSON serialization/deserialization.
-     * @param positionCovariance
-     */
-    public void setAngularVelocityCovariance(Double[] angularVelocityCovariance) {
+    public void setAngularVelocityCovariance(Double[][] angularVelocityCovariance) {
         this.angularVelocityCovariance = angularVelocityCovariance;
     }
 
@@ -181,6 +140,18 @@ public CartesianPoint getPosition() {
         return position;
     }
 
+    public Double[][] getPositionCovariance() {
+        return positionCovariance;
+    }
+
+    public Double[][] getVelocityCovariance() {
+        return velocityCovariance;
+    }
+
+    public Double[][] getAngularVelocityCovariance() {
+        return angularVelocityCovariance;
+    }
+
     /**
      * Getter for object velocity.
      * @return
@@ -289,11 +260,11 @@ public int hashCode() {
         result = prime * result + ((projString == null) ? 0 : projString.hashCode());
         result = prime * result + ((objectId == null) ? 0 : objectId.hashCode());
         result = prime * result + ((position == null) ? 0 : position.hashCode());
-        result = prime * result + Arrays.hashCode(positionCovariance);
+        result = prime * result + Arrays.deepHashCode(positionCovariance);
         result = prime * result + ((velocity == null) ? 0 : velocity.hashCode());
-        result = prime * result + Arrays.hashCode(velocityCovariance);
+        result = prime * result + Arrays.deepHashCode(velocityCovariance);
         result = prime * result + ((angularVelocity == null) ? 0 : angularVelocity.hashCode());
-        result = prime * result + Arrays.hashCode(angularVelocityCovariance);
+        result = prime * result + Arrays.deepHashCode(angularVelocityCovariance);
         result = prime * result + ((size == null) ? 0 : size.hashCode());
         return result;
     }
@@ -331,21 +302,21 @@ public boolean equals(Object obj) {
                 return false;
         } else if (!position.equals(other.position))
             return false;
-        if (!Arrays.equals(positionCovariance, other.positionCovariance))
+        if (!Arrays.deepEquals(positionCovariance, other.positionCovariance))
             return false;
         if (velocity == null) {
             if (other.velocity != null)
                 return false;
         } else if (!velocity.equals(other.velocity))
             return false;
-        if (!Arrays.equals(velocityCovariance, other.velocityCovariance))
+        if (!Arrays.deepEquals(velocityCovariance, other.velocityCovariance))
             return false;
         if (angularVelocity == null) {
             if (other.angularVelocity != null)
                 return false;
         } else if (!angularVelocity.equals(other.angularVelocity))
             return false;
-        if (!Arrays.equals(angularVelocityCovariance, other.angularVelocityCovariance))
+        if (!Arrays.deepEquals(angularVelocityCovariance, other.angularVelocityCovariance))
             return false;
         if (size == null) {
             if (other.size != null)
@@ -355,4 +326,6 @@ public boolean equals(Object obj) {
         return true;
     }
 
+
+
 }