Statusupdate, Demonstrator in Arbeit

init_Industrierobotik.txt

-{"frames": [{"id": "cobot1_door_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "cobot1_window_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 4.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "robolab_east", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]}, {"id": "robolab_west", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": -4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]} ]}
+{"frames": [{"id": "cobot1_door_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "cobot1_window_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 4.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":6.0}]}, {"id": "robolab_east", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8000000.0, "y": 100000000.0, "z":6000000.0}]}, {"id": "robolab_west", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": -4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]} ]}
 {"refSystems": [{"id": "ROOT"}, {"id": "CETI_ROOT", "position": {"refSystemId": "ROOT", "point": {"x": 3.05,"y": 2.08,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}]}
-{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "UWB_2", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}]}
+{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_2", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}, {"agentId": "turtlebot", "sensorId": "handy1", "agentType": "ROBOT", "sensorType": "IMU"}]}
 {"pois": [{"id": "UWB_BEACON_1", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 3.1,"y": 0.5,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "UWB_BEACON_2", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": -0.3,"y": 4.5,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "83221710", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.1,"y": 0.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "31762128", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.2,"y": 3.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }]}
 {"monitoringRequests": [{"frameIds": ["cobot1_door_zone", "cobot1_window_zone"], "monitoringTaskId": "RobolabMonitoringCeti", "serializationType": "protobuf"}, {"frameIds": ["cobot1_door_zone", "cobot1_window_zone", "robolab_east", "robolab_west"], "monitoringTaskId": "RobolabMonitoringFrontend", "serializationType": "json"}]}
\ No newline at end of file

init_Sensordatenfusion.txt

 {"frames": [{"id": "robolab", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 10.0, "y": 10.0, "z":10.0}]} ]}
 {"refSystems": [{"id": "ROOT"}]}
-{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "HUMAN", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}]}
+{"objectConfigs": [{"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}]}
 {"pois": [{"id": "83221710", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.1,"y": 0.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "31762128", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.2,"y": 3.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }]}
 {"monitoringRequests": [{"frameId": "robolab", "monitoringTaskId": "ipos/client/position", "serializationType": "json"}]}
\ No newline at end of file

src/main/java/ipos/project/CustomLoggingFilter.java

@@ -12,12 +12,18 @@ public class CustomLoggingFilter extends Filter<ILoggingEvent> {
             return FilterReply.DENY;
         } else if (event.getMessage().contains("SDF:")) {
             return FilterReply.DENY;
+        } else if (event.getMessage().contains("SDF-DEBUG:")) {
+            return FilterReply.DENY;
+        } else if (event.getMessage().contains("SDF-PUB:")) {
+            return FilterReply.DENY;
         } else if (event.getMessage().contains("INDFRO:")){
-           return FilterReply.ACCEPT;
+           return FilterReply.DENY;
         } else if (event.getMessage().contains("INDFRO-DEBUG:")){
             return FilterReply.DENY;
-        } else {
+        } else if (event.getMessage().contains("ODO:")){
             return FilterReply.DENY;
+        } else {
+            return FilterReply.ACCEPT;
         }
 
     }

src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/SimpleSceneIntegration.java

 package ipos.project.DataModellntegration.SimpleSceneIntegration;
 
+import com.fasterxml.jackson.core.JsonProcessingException;
 import com.google.protobuf.InvalidProtocolBufferException;
 import ipos.models.SimpleScene;
 import ipos.project.DataModellntegration.SimpleSceneIntegration.api.MqttRequestHandler;
 import ipos.project.DataModellntegration.SimpleSceneIntegration.service.SimpleSceneTransformer;
 import ipos.project.DataModellntegration.SimpleSceneIntegration.service.impl.ExternalPubServiceImpl;
-import ipos.project.DataModellntegration.iPos_Datamodel.DataStorageQueryResponse;
-import ipos.project.DataModellntegration.iPos_Datamodel.Point3D;
-import ipos.project.DataModellntegration.iPos_Datamodel.PositionEvent;
-import ipos.project.DataModellntegration.iPos_Datamodel.ZoneDescriptor;
+import ipos.project.DataModellntegration.iPos_Datamodel.*;
+import ipos.project.DataModellntegration.iPos_Datamodel.impl.PositionEventImpl;
 import ipos.project.UseCaseController.Administration;
 import ipos.project.UseCaseController.PositionMonitoring;
 import ipos.project.mapper.ProtoJsonMap;
@@ -17,6 +16,7 @@ import org.eclipse.emf.common.util.EList;
 import org.eclipse.paho.client.mqttv3.MqttMessage;
 import org.springframework.beans.factory.annotation.Autowired;
 import org.springframework.stereotype.Component;
+import com.fasterxml.jackson.databind.ObjectMapper;
 
 import java.io.BufferedReader;
 import java.io.File;
@@ -102,7 +102,8 @@ public class SimpleSceneIntegration {
 
             // verwenden: DataServices.create-Methoden verwenden. Für ZoneDeskriptoren neue create-Funktionen schreiben
             // PositionEvent posEvent_copy = duplicatePositionEvent(positionEvent);
-            SimpleScene.IposPositionEvent posEvent_proto = SimpleSceneTransformer.posEvent_internal2Proto(scale_position_frontend_app(positionEvent, 15), notificationType);
+            // scale_position_frontend_app(posEvent_copy, 15);
+            SimpleScene.IposPositionEvent posEvent_proto = SimpleSceneTransformer.posEvent_internal2Proto(positionEvent, notificationType);
             MqttMessage mqttMessage = mqttService.createMqttMsg(posEvent_proto, 0, false);
             logging_sdf(monitoringTaskId, serializationType, posEvent_proto);
             logging_indfro(positionEvent, monitoringTaskId, serializationType);
@@ -114,6 +115,18 @@ public class SimpleSceneIntegration {
         }
     }
 
+    private static PositionEvent duplicatePositionEvent(PositionEvent positionEvent) {
+        PositionEventImpl posEvent_copy = (PositionEventImpl) positionEvent;
+        ObjectMapper objectMapper = new ObjectMapper();
+        try {
+            posEvent_copy = objectMapper.readValue(objectMapper.writeValueAsString(positionEvent), PositionEventImpl.class);
+        } catch (JsonProcessingException e) {
+            LOG.warn("could not duplicate PositionEvent");
+            e.printStackTrace();
+        }
+        return posEvent_copy;
+    }
+
     private static void logging_indfro(PositionEvent positionEvent, String monitoringTaskId, String serializationType) {
         LOG.info("INDFRO: Publishing PositionEvent on topic " + monitoringTaskId + ": ZoneDescriptors:" + logZoneDescr(positionEvent.getZonedescriptors()) + "; serializationType: " + serializationType);
     }
@@ -137,8 +150,9 @@ public class SimpleSceneIntegration {
 
         // publishes JSON over MQTT
         if (PositionMonitoring.JSON_SERIALIZATION_TYPE.equals(serializationType)){
-            mqttService.publish(topic, jsonString, 0, false);
-            LOG.info("publishing JSON mqttMessage:" + jsonString + " on topic: " + topic);
+            String fakeTopic = "ipos/client/position";
+            mqttService.publish(fakeTopic, jsonString, 0, false);
+            LOG.info("publishing JSON mqttMessage:" + jsonString + " on topic: " + fakeTopic);
         }
     }
 
@@ -151,17 +165,28 @@ public class SimpleSceneIntegration {
     }
 
     private static void logging_sdf(String monitoringTaskId, String serializationType, SimpleScene.IposPositionEvent posEvent_proto) {
-        for (SimpleScene.IposObject obj : posEvent_proto.getObjectList()){
+        for (SimpleScene.IposObject obj : posEvent_proto.getObjectsList()){
             String agentId = obj.getId();
             String sensorId = obj.getSensorId();
             String sensorType = obj.getSensorType();
             String timestamp = obj.getLastPosUpdate();
             String topic = monitoringTaskId;
-            LOG.info("SDF: Publishing: sensorType: " + sensorType + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId + "; topic: " + monitoringTaskId + "; serializationType: " + serializationType);
+            float accuracy = obj.getPosition().getAccuracy();
+            String positionString = toPositionString(obj.getPosition());
+            LOG.info("SDF-PUB: Publishing: sensorType: " + sensorType + "; timestamp: " + timestamp + "; accuracy: " + accuracy + "position: " + positionString + "; sensorId: " + sensorId + "; agentId: " + agentId + "; topic: " + monitoringTaskId + "; serializationType: " + serializationType);
             LOG.info("SDF: ");
             LOG.info("SDF: ");
             LOG.info("SDF: ");
         }
     }
 
+    private static String toPositionString(SimpleScene.IposPosition position) {
+        String posString = "(";
+        posString += "x: " + position.getPoint().getX();
+        posString += "y: " + position.getPoint().getY();
+        posString += "z: " + position.getPoint().getZ();
+        posString += ")";
+        return posString;
+    }
+
 }

src/main/java/ipos/project/DataModellntegration/SimpleSceneIntegration/service/SimpleSceneTransformer.java

@@ -83,7 +83,7 @@ public class SimpleSceneTransformer {
         if (!PositionMonitoring.UNDEFINED_TYPE.equals(notificationType)) { // if undefined, protobuf-object will be created without explicit notificationtype-information
             protoIposPosEvent.setType(notificationType);
         }
-        protoIposPosEvent.addObject(protoIposObject);
+        protoIposPosEvent.addObjects(protoIposObject);
         return protoIposPosEvent;
     }
 

src/main/java/ipos/project/Functionality/DataServices.java

@@ -32,6 +32,10 @@ public class DataServices {
     }
 
     public static LocalizableObject getLObjectByIdOrNull(String lObjectId) {
+        if(wma == null){
+            LOG.info("wma is null. lObjectId: " + lObjectId);
+            return null;
+        }
         for (Agent agent : wma.getAllAgents()){
             for (LocalizableObject lObject : agent.getLObject()){
                 if (lObject.getId().equals(lObjectId)){

src/main/java/ipos/project/Functionality/Odometry.java

 package ipos.project.Functionality;
 
-import ipos.models.SimpleScene.IposPosition;
+import ipos.project.DataModellntegration.iPos_Datamodel.*;
+import org.apache.logging.log4j.LogManager;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
-import org.springframework.jms.annotation.JmsListener;
 import org.springframework.stereotype.Component;
 
+import java.sql.Timestamp;
+import java.text.ParseException;
+import java.text.SimpleDateFormat;
+import java.time.LocalDateTime;
+import java.time.ZoneId;
+import java.time.format.DateTimeFormatter;
+import java.util.Date;
+import java.util.MissingResourceException;
+
 @Component
 public class Odometry {
-    private final Logger LOG = LoggerFactory.getLogger(getClass());
+    private static org.apache.logging.log4j.Logger LOG = LogManager.getLogger();
+    private final String agentId = "";
+    private static double accuracy;
+    private static double timeOfLastCalibration;
+    private static double timeLastSensorEvent;
+    private static double [] a = new double[]{0, 0, 0};
+    private static double [] a_ori = new double[]{0, 0, 0};
+    private static double [] aOffset;
+    private static double [] v = new double[]{0, 0, 0};
+    private static double [] p = new double[]{0, 0, 0};
+    private static double [] delta = new double[]{0, 0, 0};
+    private static double [] vLast = new double[]{0, 0, 0};
+    private static double [] pLast = new double[]{0, 0, 0};
+    private static double NS2S = 1/1000.0;
+    private static boolean calibrated = false;
+    static SimpleDateFormat sdf2 = new SimpleDateFormat("yyyy-MM-dd hh:mm:ss.SSS");
+
+    //public Odometry(String agentId) {
+      //  this.agentId = agentId;
+    //}
+    private static Date convertToDateViaInstant(LocalDateTime dateToConvert) {
+        return java.util.Date.from(dateToConvert.atZone(ZoneId.systemDefault()).toInstant());
+    }
+
+    private static LocalDateTime convertToLocalDateTimeViaInstant(Date dateToConvert){
+        return dateToConvert.toInstant().atZone(ZoneId.systemDefault()).toLocalDateTime();
+    }
+
+    public static void calibrate (PositionEvent event) throws ParseException {
+        v = new double[] {0,0,0};
+        vLast = new double[] {0,0,0};
+        aOffset = new double[] {a_ori[0],a_ori[1],a_ori[2]};
+        accuracy = 0.03;
+        if (event.getPlacing().getPosition().getPoint() instanceof Point3D) {
+            pLast[0] = ((Point3D) event.getPlacing().getPosition().getPoint()).getX();
+            pLast[1] = ((Point3D) event.getPlacing().getPosition().getPoint()).getY();
+            pLast[2] = ((Point3D) event.getPlacing().getPosition().getPoint()).getZ();
+            p = pLast;
+        }
+        LOG.info("Odometry: timestamp" + event.getTimeStamp());
+        LocalDateTime eventTime = LocalDateTime.parse(event.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
+        Date date = convertToDateViaInstant(eventTime);
+
+        timeOfLastCalibration = date.getTime();
+        calibrated = true;
+    }
+
+    public static PositionEvent update(IMU imuRawdataEvent) throws ParseException {
+        IPos_DatamodelFactory datamodelFactory = IPos_DatamodelFactory.eINSTANCE;
+        Timestamp timestamp = new Timestamp(System.currentTimeMillis());
+        Date date = sdf2.parse(imuRawdataEvent.getTimeStamp());
+
+        if (calibrated) {
+            double dt = (date.getTime() - timeLastSensorEvent) * NS2S;
+            a_ori[0] = imuRawdataEvent.getAcceleration().getX();
+            a_ori[1] = imuRawdataEvent.getAcceleration().getY();
+            a_ori[2] = imuRawdataEvent.getAcceleration().getZ();
+            a[0] = imuRawdataEvent.getAcceleration().getX() - aOffset[0];
+            a[1] = imuRawdataEvent.getAcceleration().getY() - aOffset[1];
+            a[2] = imuRawdataEvent.getAcceleration().getZ() - aOffset[2];
+            for (int index = 0; index < 3; ++index) {
+                v[index] = a[index] * dt + vLast[index];
+                delta[index] = (v[index] + vLast[index]) / 2 * dt;
+                p[index] = pLast[index] + delta[index];
+                pLast[index] = p[index];
+                vLast[index] = v[index];
+            }
+            accuracy = (date.getTime() - timeOfLastCalibration + 500) * 0.0001;
+            timeLastSensorEvent = date.getTime();
+            Quaternion calOrientation = datamodelFactory.createQuaternion();
+            PositionEvent calPositionEvent = datamodelFactory.createPositionEvent();
+            Placing calPlacing = datamodelFactory.createPlacing();
+            LocalizableObject calLObject = datamodelFactory.createLocalizableObject();
+            Position calPosition = datamodelFactory.createPosition();
+            Point3D calPoint3D = datamodelFactory.createPoint3D();
+            Gaussian calGaussian = datamodelFactory.createGaussian();
+            calGaussian.setConfidenceInterval((float) accuracy);
+            calPoint3D.setX((float) p[0]);
+            calPoint3D.setY((float) p[1]);
+            calPoint3D.setZ((float) p[2]);
+            calOrientation.setX(0);
+            calOrientation.setY(0);
+            calOrientation.setZ(0);
+            calOrientation.setW(0);
+            calPosition.setPoint(calPoint3D);
+            calPosition.setAccuracy(calGaussian);
+            calPosition.setReferenceSystem(DataServices.getReferenceSystemByIdOrNull("ROOT"));
+            calPlacing.setPosition(calPosition);
+            calPlacing.setOrientation(calOrientation);
+            //calLObject.setId(imuRawdataEvent.getSensorId());
+            LocalDateTime timestamp_LDT=convertToLocalDateTimeViaInstant(date);
+
 
+            calPositionEvent.setLObjectId(imuRawdataEvent.getSensorId());
+            calPositionEvent.setTimeStamp(timestamp_LDT.toString() + "+00:00");
+            calPositionEvent.setPlacing(calPlacing);
+            return calPositionEvent;
+        }
+        else {
+            timeLastSensorEvent = date.getTime();
+            a_ori[0] = imuRawdataEvent.getAcceleration().getX();
+            a_ori[1] = imuRawdataEvent.getAcceleration().getY();
+            a_ori[2] = imuRawdataEvent.getAcceleration().getZ();
+
+            throw new MissingResourceException("Odometry not calibrated!", "Odometry", "calibration");
+        }
+    }
     //TODO
 //    @JmsListener(destination = "/positions123", containerFactory = "myFactory")
 //    public void receiveMessage(IposPosition pos) {

src/main/java/ipos/project/Functionality/SensorDataFusion.java

@@ -28,7 +28,7 @@ public class SensorDataFusion {
             Agent agent = DataServices.getAgentForLocalizableObject(positionEvent.getLObjectId());
             List<PositionEvent> eventsForAgent = getEventStorageForAgentOrCreate(agent.getId());
             updateRecentEventStorage(eventsForAgent, positionEvent);
-            return hasHighestAccuracyAmongStoredEvents(positionEvent, eventsForAgent);
+            return hasBestAccuracyAmongStoredEvents(positionEvent, eventsForAgent);
 
         } catch (IllegalArgumentException e) {
             LOG.warn("Sensor-data-fusion could not be applied to position-event as no LocalizableObject with " +
@@ -47,12 +47,13 @@ public class SensorDataFusion {
      * @param eventsForAgent
      * @return
      */
-    private static boolean hasHighestAccuracyAmongStoredEvents(PositionEvent positionEvent, List<PositionEvent> eventsForAgent) {
+    private static boolean hasBestAccuracyAmongStoredEvents(PositionEvent positionEvent, List<PositionEvent> eventsForAgent) {
         for (PositionEvent storedEvent : eventsForAgent){
             float storedAccuracy = ((Gaussian) storedEvent.getPlacing().getPosition().getAccuracy()).getConfidenceInterval();
             float eventAccuracy = ((Gaussian) positionEvent.getPlacing().getPosition().getAccuracy()).getConfidenceInterval();;
+            boolean eventsOriginatedFromDifferentSensors = !storedEvent.getLObjectId().equals(positionEvent.getLObjectId()); // important to make sure that the current IMU-position is not hidden behind older ones
             logging_sdf(positionEvent, storedEvent);
-            if (storedAccuracy < eventAccuracy){ // it is important not to use <= here, as at least the current position event itself is contained in the list
+            if (storedAccuracy < eventAccuracy & eventsOriginatedFromDifferentSensors){ // it is important not to use <= here, as at least the current position event itself is contained in the list
                 logging_sdf(positionEvent);
                 return false;
             }
@@ -63,7 +64,7 @@ public class SensorDataFusion {
     private static void logging_sdf(PositionEvent positionEvent, PositionEvent storedEvent) {
         LocalDateTime eventTime = LocalDateTime.parse(positionEvent.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
         LocalDateTime storedTime = LocalDateTime.parse(storedEvent.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
-        LOG.info("SDF: distance: " + Duration.between(storedTime, eventTime).toMillis());
+        LOG.info("SDF-DEBUG: distance: " + Duration.between(storedTime, eventTime).toMillis());
     }
 
     private static void logging_sdf(PositionEvent positionEvent) {
@@ -95,7 +96,7 @@ public class SensorDataFusion {
             }
             LocalDateTime eventTime = LocalDateTime.parse(currEvent.getTimeStamp(), DateTimeFormatter.ISO_OFFSET_DATE_TIME);
             long eventAge = Duration.between(eventTime, now).toMillis();
-            LOG.info("SDF: eventAge: " + eventAge + " ; timestamp: " + currEvent.getTimeStamp() + " ; now: " + now);
+            LOG.info("SDF-DEBUG: eventAge: " + eventAge + " ; timestamp: " + currEvent.getTimeStamp() + " ; now: " + now);
             if ( eventAge > PositionMonitoring.SDF_TIME_INTERVAL){
                 eventIt.remove();
                 removeSubsequentElements = true;

src/main/java/ipos/project/MainApp.java

@@ -21,6 +21,7 @@ public class MainApp {
     public static final String TESTRAWDATA_FILE = "./testdata_raw.txt";
     public static final String TESTDATA_INDFRO = "./testdata_raw_indfro.txt";
     public static final String TESTDATA_QUERY = "./testdata_raw_query.txt";
+    public static final String TESTDATA_EMPTY = "./testdata_raw_empty.txt";
 
 
     public static void main(String[] args) throws ParseException {
@@ -28,7 +29,7 @@ public class MainApp {
         // SimpleSceneIntegration.init();
         SimpleSceneIntegration.init(INDUSTRIEROBOTIK_FILE);
         // SimpleSceneIntegration.init(SENSORDATENFUSION_FILE);
-        GenericSensorValueProcessor.processTestData(TESTDATA_INDFRO);
-        SimpleSceneIntegration.handleMessageFile(TESTDATA_QUERY);
+        // GenericSensorValueProcessor.processTestData(TESTDATA_EMPTY);
+        // SimpleSceneIntegration.handleMessageFile(TESTDATA_EMPTY);
 	}
 }

src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java

@@ -6,6 +6,7 @@ import ipos.models.SimpleScene;
 import ipos.project.DataModellntegration.iPos_Datamodel.*;
 import org.eclipse.emf.ecore.EObject;
 
+import java.time.LocalDateTime;
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
@@ -98,7 +99,10 @@ public class GenericSensorTransformer {
 
    public static NFC nfc_GSensor2Internal(GenericSensor.NFCRawDataEvent proto_nfcRawdataEvent) {
         NFC internal_nfcRawdataEvent = modelFactory.createNFC();
-        internal_nfcRawdataEvent.setTimeStamp(proto_nfcRawdataEvent.getTimestamp());
+        // internal_nfcRawdataEvent.setTimeStamp(proto_nfcRawdataEvent.getTimestamp());
+
+        // fake nfc-timestamp (queube could not provide millisecond-accuracy as it relies on time server)
+       internal_nfcRawdataEvent.setTimeStamp(LocalDateTime.now() + "+00:00"); // suffix necessary to be parsable by DateTimeFormatter.ISO_OFFSET_DATE_TIME
        internal_nfcRawdataEvent.setTagId(proto_nfcRawdataEvent.getTagId());
         internal_nfcRawdataEvent.setScannerId(proto_nfcRawdataEvent.getScannerId());
         internal_nfcRawdataEvent.setType(proto_nfcRawdataEvent.getType());
@@ -115,6 +119,23 @@ public class GenericSensorTransformer {
         }
         return outputMap;
     }
+
+    public static IMU imu_GSensor2Internal(GenericSensor.IMURawDataEvent proto_imuRawDataEvent) {
+       IMU internal_imuRawDataEvent = modelFactory.createIMU();
+       internal_imuRawDataEvent.setTimeStamp(proto_imuRawDataEvent.getTimestamp());
+       internal_imuRawDataEvent.setSensorId(proto_imuRawDataEvent.getSensorId());
+       Acceleration internal_acceleration = modelFactory.createAcceleration();
+       internal_acceleration.setX(proto_imuRawDataEvent.getX());
+       internal_acceleration.setY(proto_imuRawDataEvent.getY());
+       internal_acceleration.setZ(proto_imuRawDataEvent.getZ());
+       internal_imuRawDataEvent.setAcceleration(internal_acceleration);
+       Quaternion orientation = createOrientation(proto_imuRawDataEvent.getOrientation().getX(),
+               proto_imuRawDataEvent.getOrientation().getY(),
+               proto_imuRawDataEvent.getOrientation().getZ(),
+               proto_imuRawDataEvent.getOrientation().getW());
+       internal_imuRawDataEvent.setAngularrate(orientation);
+       return internal_imuRawDataEvent;
+    }
 }
 
 

src/main/java/ipos/project/SensorValueIntegration/api/MqttPositionHandler.java

@@ -7,6 +7,7 @@ import ipos.models.SimpleScene;
 import ipos.models.SimpleScene.IposPosition;
 import ipos.models.GenericSensor.SensorEventWrapper;
 import ipos.project.DataModellntegration.SimpleSceneIntegration.service.impl.ExternalPubServiceImpl;
+import ipos.project.DataModellntegration.iPos_Datamodel.IMU;
 import ipos.project.DataModellntegration.iPos_Datamodel.NFC;
 import ipos.project.DataModellntegration.iPos_Datamodel.PositionEvent;
 import ipos.project.SensorValueIntegration.Service.GenericSensorTransformer;
@@ -54,12 +55,20 @@ public class MqttPositionHandler implements Handler {
         
         processPositionEvents(iposSensorEventWrapper.getSensorPositionEventList());
         processNFCRawdataEvents(iposSensorEventWrapper.getNfcRawDataEventList());
-
+        processIMURawdataEvents(iposSensorEventWrapper.getImuRawDataEventList());
         // this.jmsTemplate.convertAndSend("/request123", monReqInternal); // submit request to the internal broker
     }
 
+    private static void processIMURawdataEvents(List<GenericSensor.IMURawDataEvent> imuRawDataEventList) {
+        for (GenericSensor.IMURawDataEvent proto_imuRawDataEvent : imuRawDataEventList){
+            IMU internal_imuRawDataEvent = GenericSensorTransformer.imu_GSensor2Internal(proto_imuRawDataEvent);
+            PositionMonitoring.receiveMessage(internal_imuRawDataEvent);
+        }
+    }
+
     private static void processNFCRawdataEvents(List<GenericSensor.NFCRawDataEvent> nfcRawDataEventList) {
         for (GenericSensor.NFCRawDataEvent proto_nfcRawdataEvent : nfcRawDataEventList){
+            // transformer fakes nfc-timestamp...
             NFC internal_nfcRawDataEvent = GenericSensorTransformer.nfc_GSensor2Internal(proto_nfcRawdataEvent);
             PositionMonitoring.receiveMessage(internal_nfcRawDataEvent);
         }

src/main/java/ipos/project/UseCaseController/Administration.java

@@ -16,6 +16,7 @@ public class Administration {
     public Administration(){}
 
     public static void initialize(){
+        LOG.info("Application initialized");
         DataServices.clearWorldModel();
     }
 

src/main/java/ipos/project/UseCaseController/PositionMonitoring.java

@@ -3,6 +3,7 @@ package ipos.project.UseCaseController;
 import ipos.project.DataModellntegration.SimpleSceneIntegration.SimpleSceneIntegration;
 import ipos.project.DataModellntegration.iPos_Datamodel.*;
 import ipos.project.Functionality.DataServices;
+import ipos.project.Functionality.Odometry;
 import ipos.project.Functionality.PositionCalculation;
 import ipos.project.Functionality.SensorDataFusion;
 import ipos.project.Functionality.eventfilter.FilteringResult;
@@ -11,9 +12,9 @@ import ipos.project.Functionality.eventfilter.readConfig;
 import org.apache.logging.log4j.LogManager;
 import org.springframework.jms.annotation.JmsListener;
 import org.springframework.stereotype.Component;
-import reactor.util.function.Tuple4;
 
 import java.text.ParseException;
+import java.time.LocalDateTime;
 import java.util.*;
 import java.util.stream.Collectors;
 
@@ -47,10 +48,47 @@ public class PositionMonitoring {
         PositionEvent positionEvent = PositionCalculation.calcPositionFromNfcOrNull(nfcRawDataEvent);
         if (positionEvent != null) {
             updateLocalizableObject(positionEvent);
+            try {
+                Odometry.calibrate(positionEvent);
+            } catch (ParseException e) {
+                LOG.warn("Odometry-component could not be calibrated");
+                e.printStackTrace();
+            }
             receiveMessage(positionEvent);
         }
     }
 
+    static int imuCount = 0;
+
+    public static void receiveMessage(IMU imuRawdataEvent){
+
+        PositionEvent positionEvent = null; // if not calibrated: exception
+        try {
+            positionEvent = Odometry.update(imuRawdataEvent);
+            logOdo(positionEvent);
+        } catch (ParseException e) {
+            LOG.info("Odometry: ParseException: " + e.getMessage());
+        } catch (MissingResourceException e){
+            LOG.info("Odometry: " + e.getMessage());
+        }
+        if (positionEvent != null) {
+            updateLocalizableObject(positionEvent);
+            if ( imuCount < 5) {
+                imuCount += 1;
+            }else {
+                receiveMessage(positionEvent);
+                imuCount = 0;
+            }
+        }
+
+
+    }
+
+    private static void logOdo(PositionEvent positionEvent) {
+        float odoAccuracy = ((Gaussian) positionEvent.getPlacing().getPosition().getAccuracy()).getConfidenceInterval();
+        LOG.info("ODO: Odometry calculated position with accuracy: " + odoAccuracy);
+    }
+
     private static eventFilter getEventFilterByIdOrNull(String monitoringTaskId){
         for (eventFilter eFilter : eventFilters){
             if (eFilter.getMonitoringTaskId().equals(monitoringTaskId)){
@@ -62,7 +100,9 @@ public class PositionMonitoring {
 
     private static void updateLocalizableObject(PositionEvent positionEvent) {
         LocalizableObject lObject = DataServices.getLObjectByIdOrNull(positionEvent.getLObjectId());
-
+        if (lObject == null){
+            LOG.info("Warning: lObject not found. id contained in positionEvent: " + positionEvent.getLObjectId());
+        }
         // TODO: caution: all changes to the data model should be performed using DataServices.
         // Directly changing this object just works for the current simple case that we only have the object-representation of the world model
         // in general it has to be made sure that both, all links from this object and to this object keep intact
@@ -151,13 +191,11 @@ public class PositionMonitoring {
         logging_sdfindfro(posEvent);
 
         boolean posEventMeetsSdfAccuracyCondition = SensorDataFusion.isMostAccuratePositionAvailable(posEvent);
-        List<Tuple4<PositionEvent, String, String, String>> entryNotifications = new LinkedList<>();
         List<FilteringResult> filteringResults = new LinkedList<>();
-        Map<String, List<ZoneDescriptor>> eFilterZoneDescriptors = new HashMap<>();
         for (eventFilter eFilter : eventFilters){
            try {
                 if (eFilterMayHandlePosEvent(posEvent, posEventMeetsSdfAccuracyCondition, eFilter)){
-                    filteringResults.add(applyEfilterToPosEvent(posEvent, entryNotifications, eFilter));
+                    filteringResults.add(applyEfilterToPosEvent(posEvent, eFilter));
                 }
             } catch (ParseException e) {
                processFilteringResults(posEvent, filteringResults);
@@ -347,12 +385,13 @@ public class PositionMonitoring {
         LocalizableObject lObject = PositionMonitoring.getLObjectByIdOrNull(posEvent.getLObjectId());
         if (lObject == null){
             LOG.info("INDFRO: Warning, unknown sensorId was found. Can not log!");
+            return;
         }
         String sensorType = lObject.getSensorType();
         String timestamp = posEvent.getTimeStamp();
         String sensorId = posEvent.getLObjectId();
         String agentId = lObject.getAgent().getId();
-        LOG.info("SDF: Received: sensorType: " + sensorType + " timestamp: " + timestamp + " sensorId: " + sensorId + " agentId: " + agentId);
+        LOG.info("SDF: Received: sensorType: " + sensorType + " timestamp: " + timestamp + " now: " + LocalDateTime.now() + " sensorId: " + sensorId + " agentId: " + agentId);
         LOG.info("INDFRO:");
         LOG.info("INDFRO: Received: sensorType: " + sensorType + "; position: " + logPosition(posEvent) + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId);
     }
@@ -365,7 +404,9 @@ public class PositionMonitoring {
     /**
      * An event-filter may hot handle a position event if the filter should respect the results of
      * sensor data fusion (e.g., wrt accuracy-criterium) and the result is negative for this event,
-     * i.e., the event did not pass sensor data fusion and was rejected.
+     * i.e., the event did not pass sensor data fusion and was rejected. If sensor-data-fusion is not
+     * relevant, the eFilter should handle the event regardless of whether it was sorted out by sdf or
+     * not. If it is relevant, the eFilter should block the event if it was sorted out by sdf.
      * @param posEvent
      * @param posEventMeetsSdfAccuracyCondition
      * @param eFilter
@@ -376,7 +417,7 @@ public class PositionMonitoring {
         return !sdfIsRelevant || (sdfIsRelevant && posEventMeetsSdfAccuracyCondition);
     }
 
-    private static FilteringResult applyEfilterToPosEvent(PositionEvent posEvent, List<Tuple4<PositionEvent, String, String, String>> entryNotifications, eventFilter eFilter) throws ParseException {
+    private static FilteringResult applyEfilterToPosEvent(PositionEvent posEvent, eventFilter eFilter) throws ParseException {
         FilteringResult filteringResult = eFilter.process(posEvent);
         boolean posEventMeetsFilterConditions = !filteringResult.hasBlockedEvent();
         if(posEventMeetsFilterConditions) {

src/main/resources/application.yml

@@ -12,8 +12,10 @@ mqtt:
   setKeepAliveInterval: 10
   connectionTimeout: 20
   clientId: ipos.project.fw
-  # hostname: 192.168.0.111
-  hostname: broker.hivemq.com
+  # hostname: 192.168.0.111 # ceti-laptop robolab
+  # hostname: broker.hivemq.com # online
+  hostname: 192.168.0.143 # Frank-laptop robolab
+  # hostname: localhost
   port: 1883
 
 spring:

testdata_raw_indfro.txt

-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 4.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:21+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": -3.0,"y": 4.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:22+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:23+00:00"}]}
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 4.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:21+00:00"}]}
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": -3.0,"y": 4.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:22+00:00"}]}
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:23+00:00"}]}
 {"nfcRawDataEvent" : [ {"timestamp" : "2021-10-14T19:33:24+00:00", "tagId": "83221710", "type": "RFID", "scannerId": "98:CD:AC:26:2D:18"} ] }
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 1.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:34:25+00:00"}]}
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:34:25+00:00"}]}
\ No newline at end of file