From 1621cca59ba940fb7af5ea06eefe8b68978f724f Mon Sep 17 00:00:00 2001
From: Frank Rohde <frank.rohde@tu-dresden.de>
Date: Wed, 15 Dec 2021 14:48:10 +0100
Subject: [PATCH] fixed some bugs SRSConversion-behaviour can be tested

---
 build.gradle                                  |  2 --
 init_Industrierobotik.txt                     |  2 +-
 .../ipos/project/CustomLoggingFilter.java     |  6 ++---
 .../SRSConversion/Quaternion_math.java        |  2 +-
 .../SRSConversion/SRSConversion.java          | 10 +++++--
 .../Service/GenericSensorTransformer.java     |  4 ++-
 .../UseCaseController/Administration.java     | 27 ++++++++++++-------
 7 files changed, 33 insertions(+), 20 deletions(-)

diff --git a/build.gradle b/build.gradle
index ee7827f..e8895f4 100644
--- a/build.gradle
+++ b/build.gradle
@@ -43,8 +43,6 @@ bootRun{
 }
 
 dependencies {
-	implementation 'com.github.weeb-kun:list-utils:1.0.0'
-	implementation 'org.webjars.npm:list-item:1.1.1'
 	developmentOnly 'org.springframework.boot:spring-boot-devtools'
 	implementation 'org.eclipse.emf:org.eclipse.emf.ecore:2.+'
 	implementation 'org.eclipse.emf:org.eclipse.emf.common:2.+'
diff --git a/init_Industrierobotik.txt b/init_Industrierobotik.txt
index 8be63e5..42e2dba 100644
--- a/init_Industrierobotik.txt
+++ b/init_Industrierobotik.txt
@@ -2,4 +2,4 @@
 {"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}}, {"id": "CETI_ROBOLAB", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 1.0,"y": 1.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "CETI_OFFICE", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 2.0,"y": 2.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "TL_TESTHALL", "position": {"refSystemId": "ROOT", "point": {"x": 40.0,"y": 60.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "CETI_ROBOTARM_CELL", "position": {"refSystemId": "CETI_ROBOLAB", "point": {"x": 2.0,"y": 2.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}]}
 {"objectConfigs": [{"agentId": "Employee_1", "sensorId": "UWB_2", "agentType": "HUMAN", "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} }, {"id": "885", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 0,"y": 0,"z": 0.97}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "2934", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 4.24,"y": 1.33,"z": 1.0}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "3383", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 1.25,"y": 5.89,"z": 1.06}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "1107", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 4.24,"y": 5.83,"z": 1.05}, "accuracy": 0.01}, "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"], "monitoringTaskId": "RobolabMonitoringFrontend", "serializationType": "json"}]}
\ No newline at end of file
+{"monitoringRequests": [{"frameIds": ["cobot1_door_zone", "cobot1_window_zone"], "monitoringTaskId": "RobolabMonitoringCeti", "refSystemId": "CETI_ROBOTARM_CELL", "serializationType": "protobuf"}, {"frameIds": ["cobot1_door_zone", "cobot1_window_zone"], "monitoringTaskId": "RobolabMonitoringFrontend", "refSystemId": "CETI_ROBOTARM_CELL", "serializationType": "json"}]}
\ No newline at end of file
diff --git a/src/main/java/ipos/project/CustomLoggingFilter.java b/src/main/java/ipos/project/CustomLoggingFilter.java
index bbdd617..e464195 100644
--- a/src/main/java/ipos/project/CustomLoggingFilter.java
+++ b/src/main/java/ipos/project/CustomLoggingFilter.java
@@ -17,9 +17,9 @@ public class CustomLoggingFilter extends Filter<ILoggingEvent> {
         } else if (event.getMessage().contains("SDF-PUB:")) {
             return FilterReply.DENY;
         } else if (event.getMessage().contains("INDFRO:")){
-           return FilterReply.DENY;
+           return FilterReply.ACCEPT;
         } else if (event.getMessage().contains("INDFRO-DEBUG:")){
-            return FilterReply.DENY;
+            return FilterReply.ACCEPT;
         } else if (event.getMessage().contains("ODO:")){
             return FilterReply.DENY;
         } else if (event.getMessage().contains("Received a Message:")){ // MQTT-library meldet sich
@@ -29,7 +29,7 @@ public class CustomLoggingFilter extends Filter<ILoggingEvent> {
         }else if (event.getMessage().contains("SHELL:")){
             return FilterReply.ACCEPT;
         } else {
-            return FilterReply.ACCEPT;
+            return FilterReply.DENY;
         }
 
     }
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java b/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java
index 79b2ab5..6a977da 100644
--- a/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/Quaternion_math.java
@@ -159,7 +159,7 @@ public class Quaternion_math {
                     * invNorm);
         }
         // return an invalid result to flag the error
-        return null;
+        return new Quaternion_math(0,0,0,0);
     }
     public Quaternion_math normalizeThis() {
         return divThis(norm());
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java b/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
index f5c3dc6..429f0c0 100644
--- a/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
@@ -1,4 +1,5 @@
 package ipos.project.Functionality.SRSConversion;
+import org.apache.logging.log4j.LogManager;
 import org.eclipse.emf.common.util.EList;
 
 import ipos.project.DataModellntegration.iPos_Datamodel.*;
@@ -8,6 +9,9 @@ This class contains methods to convert the coordinate system of positions.
 All reference Systems should be defined in the ROOT system.
 */
 public class SRSConversion {
+
+    private static org.apache.logging.log4j.Logger LOG = LogManager.getLogger();
+
     /*
     This function converts the coordinate system of a position to a desired coordinate system.
     p1 is the coordinate of the position in the original coordinate system
@@ -35,8 +39,9 @@ public class SRSConversion {
         //System.out.println("Quaterion_2: " + quat_2.getX() + ", "+ quat_2.getY() + ", "+ quat_2.getZ() + ", " + quat_2.getW());
 
 
-        if (ref_id == pla_ref_id) {
+        if (pla_ref_id.equals(ref_id)) {
             //System.out.println("inversed Quaterion_1: " + quat_1.inverse().getX() + ", "+ quat_1.inverse().getY() + ", "+ quat_1.inverse().getZ() + ", " + quat_1.inverse().getW());
+            LOG.info("INDFRO-DEBUG: Transforming placing-SRS " + pla_ref_id + " to SRS " + ref.getOrigin().getPosition().getReferenceSystem().getId());
             quat_1.mulLeft(quat_2);
 
             //System.out.println("phase 1 transfrom: " + p1.getX() + ", "+ p1.getY() + ", "+ p1.getZ());
@@ -45,7 +50,8 @@ public class SRSConversion {
             pos.setReferenceSystem(ref.getOrigin().getPosition().getReferenceSystem());
 
         }
-        else if(ref_id == ref.getOrigin().getPosition().getReferenceSystem().getId()) {
+        else if(pla_ref_id.equals(ref.getOrigin().getPosition().getReferenceSystem().getId())) {
+            LOG.info("INDFRO-DEBUG: Transforming placing-SRS " + pla_ref_id + " to SRS " + ref_id);
             //System.out.println("inversed Quaterion_1: " + quat_1.inverse().getX() + ", "+ quat_1.inverse().getY() + ", "+ quat_1.inverse().getZ() + ", " + quat_1.inverse().getW());
             quat_1.mulLeft(quat_2.inverse());
 
diff --git a/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java b/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java
index 266b0e4..2a55243 100644
--- a/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java
+++ b/src/main/java/ipos/project/SensorValueIntegration/Service/GenericSensorTransformer.java
@@ -4,6 +4,7 @@ import com.google.protobuf.ProtocolStringList;
 import ipos.models.GenericSensor;
 import ipos.models.SimpleScene;
 import ipos.project.DataModellntegration.iPos_Datamodel.*;
+import ipos.project.Functionality.DataServices;
 import ipos.project.devkit.trans.IPos2protoTransformer;
 import org.apache.logging.log4j.LogManager;
 import org.eclipse.emf.ecore.EObject;
@@ -45,7 +46,8 @@ public class GenericSensorTransformer {
                                         protoSensPosEvent.getPosition().getPoint().getZ()
                                 ),
                                 protoSensPosEvent.getPosition().getAccuracy(),
-                                IPos2protoTransformer.createReferenceSystem("ROOT", "ROOT", null)
+                                DataServices.getReferenceSystemByIdOrNull(protoSensPosEvent.getPosition().getRefSystemId())
+
                         ),
                         IPos2protoTransformer.createOrientation(
                                 protoSensPosEvent.getOrientation().getX(),
diff --git a/src/main/java/ipos/project/UseCaseController/Administration.java b/src/main/java/ipos/project/UseCaseController/Administration.java
index 1ca7f8f..af95818 100644
--- a/src/main/java/ipos/project/UseCaseController/Administration.java
+++ b/src/main/java/ipos/project/UseCaseController/Administration.java
@@ -25,7 +25,7 @@ public class Administration {
 
     public static void handleZone(Zone zone){
         LOG.info("handling zone-object: " + zone.toString());
-        Administration.transformToRootCoordinates(zone);
+        Administration.transformToRootCoordinates(zone); // transformation is necessary, EventFilter should have to evaluate ROOT-Positions, therefore both, the PositionEvents and the Space-CentrePoint, should have RootPositions.
         DataServices.addZone(zone);
     }
 
@@ -45,12 +45,13 @@ public class Administration {
     public static void handlePoi(POI poi) {
         // muss checken, ob das RefSystem auf das verwiesen wird - tatsächlich existiert. Falls nicht: Speicherung ablehnen
         LOG.info("handling poi-object: " + poi.toString());
+        LOG.info("INDFRO-DEBUG: SRS of poi-object: " + poi.getPlacing().getPosition().getReferenceSystem().getId());
         Administration.transformToRootCoordinates(poi);
         DataServices.addPoi(poi);
     }
 
     public static void transformToRootCoordinates(PositionEvent posEvent) {
-        LOG.info("INDFRO-DEBUG: Transforming PositionEvent-object " + posEvent.toString() + "to ROOT-SRS");
+        LOG.info("INDFRO-DEBUG: Handling PositionEvent-object. SRS: " + posEvent.getPlacing().getPosition().getReferenceSystem().getId());
         Administration.transformToRootCoordinates(posEvent.getPlacing());
     }
 
@@ -60,7 +61,7 @@ public class Administration {
     }
 
     public static Space transformToRootCoordinates(Space space) {
-        LOG.info("INDFRO-DEBUG: Transforming space-object " + space.toString() + "to ROOT-SRS");
+        LOG.info("INDFRO-DEBUG: Handling space-object. SRS: " + space.getCentrePoint().getPosition().getReferenceSystem().getId());
         Administration.transformToRootCoordinates(space.getCentrePoint());
         return space;
     }
@@ -71,7 +72,6 @@ public class Administration {
     }
 
     public static void transformToRootCoordinates(POI poi) {
-        LOG.info("INDFRO-DEBUG: Transforming poi-object " + poi.toString() + "to ROOT-SRS");
         Administration.transformToRootCoordinates(poi.getPlacing());
     }
 
@@ -83,7 +83,7 @@ public class Administration {
      */
     public static void transformToRootCoordinates(Placing placingToBeTransformed) {
         if (isNotRootPosition(placingToBeTransformed.getPosition())) {
-            ReferenceSystem nextTransformationGoal_SRS = placingToBeTransformed.getPosition().getReferenceSystem().getOrigin().getPosition().getReferenceSystem();
+            ReferenceSystem nextTransformationGoal_SRS = placingToBeTransformed.getPosition().getReferenceSystem();
             LOG.info("INDFRO-DEBUG: (before) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed));
             SRSConversion.switchSRS(placingToBeTransformed, nextTransformationGoal_SRS); // assumption: not only position is changed but also ReferenceSystem-object is updated
             LOG.info("INDFRO-DEBUG: (after) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed));
@@ -116,7 +116,7 @@ public class Administration {
     }
 
     private static boolean isNotRootPosition(String srsId){
-        return srsId != PositionMonitoring.SRS_ID_ROOT;
+        return !srsId.equals(PositionMonitoring.SRS_ID_ROOT);
     }
 
     /**
@@ -139,19 +139,26 @@ public class Administration {
      * @param refSystemId
      */
     public static void transformToRequesterCoordinates(PositionEvent posEvent, String refSystemId) {
-        if(posEvent.getPlacing().getPosition().getReferenceSystem().getId() != PositionMonitoring.SRS_ID_ROOT){
-            LOG.warn("Warning: Function transformToRequesterCoordinates was called with a position that is not defined in reference to the ROOT-SRS. " +
+        if(! posEvent.getPlacing().getPosition().getReferenceSystem().getId().equals(PositionMonitoring.SRS_ID_ROOT)){
+            LOG.warn("INDFRO-DEBUG: Warning: Function transformToRequesterCoordinates was called with a position that is not defined in reference to the ROOT-SRS. " +
                     "All internal data processing should be based on ROOT-positions.");
             return;
         }
         if(isRootPosition(refSystemId)){
-            return; // nothing to be done, as posEvent is it is assumed that posEvent is already a ROOT-position
+            return; // nothing to be done if ROOT is the transformation target, as posEvent is assumed to be a ROOT-position
         }
         ReferenceSystem finalTarget = DataServices.getReferenceSystemByIdOrNull(refSystemId);
         Placing placingToBeTransformed = posEvent.getPlacing();
+        LOG.info("INDFRO-DEBUG: transformToRequesterCoordinates: Transforming posEvent (SRS: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() +" ) to requester coordinates (SRS: " + refSystemId + ")");
         List<ReferenceSystem> allTargetsFromRootToFinal = computeTargets(finalTarget);
+        LOG.info("INDFRO-DEBUG: Transformation targets (excluding " + PositionMonitoring.SRS_ID_ROOT + "): " + printTargets(allTargetsFromRootToFinal));
         transformPlacingSuccessively(placingToBeTransformed, allTargetsFromRootToFinal);
-        // Komponente SRSConversion zur Transformation der ROOT-Position posEvent in das SRS refSystemId
+    }
+
+    private static String printTargets(List<ReferenceSystem> allTargetsFromRootToFinal) {
+        return allTargetsFromRootToFinal.stream()
+                .map(srs -> srs.getId())
+                .collect(Collectors.joining(" -> "));
     }
 
     /**
-- 
GitLab