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