From ee07d2721a3b07c27c185e7040544a619ff0bf3d Mon Sep 17 00:00:00 2001 From: Frank Rohde <frank.rohde@tu-dresden.de> Date: Wed, 15 Dec 2021 09:26:53 +0100 Subject: [PATCH] Added testdata and logging for srs-conversion functionality --- init_Industrierobotik.txt | 4 ++-- .../project/UseCaseController/Administration.java | 15 +++++++++++++-- .../UseCaseController/PositionMonitoring.java | 8 ++++---- testdata_raw_indfro.txt | 9 +++++---- 4 files changed, 24 insertions(+), 12 deletions(-) diff --git a/init_Industrierobotik.txt b/init_Industrierobotik.txt index d7de83d..8be63e5 100644 --- a/init_Industrierobotik.txt +++ b/init_Industrierobotik.txt @@ -1,5 +1,5 @@ -{"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": 6.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": 6.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": 4.0, "y": 4.0, "z":4.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}}]} +{"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": 6.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": 6.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": 4.0, "y": 4.0, "z":4.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}]}, {"id": "robolab_armarea", "space": [{"position": {"refSystemId": "CETI_ROBOTARM_CELL", "point": {"x": 0.0,"y": 0.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}}, {"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 diff --git a/src/main/java/ipos/project/UseCaseController/Administration.java b/src/main/java/ipos/project/UseCaseController/Administration.java index d82bdfc..ae0dd08 100644 --- a/src/main/java/ipos/project/UseCaseController/Administration.java +++ b/src/main/java/ipos/project/UseCaseController/Administration.java @@ -2,6 +2,7 @@ package ipos.project.UseCaseController; import ipos.project.DataModellntegration.iPos_Datamodel.*; import ipos.project.Functionality.DataServices; +import ipos.project.Functionality.SRSConversion.SRSConversion; import org.apache.logging.log4j.LogManager; import org.springframework.stereotype.Component; @@ -36,7 +37,7 @@ public class Administration { public static void handleRefSystem(ReferenceSystem refSystem) { // muss checken, ob das RefSystem auf das ggf. verwiesen wird - tatsächlich existiert. Falls nicht: Speicherung ablehnen LOG.info("handling refSystem-object: " + refSystem.toString()); - //Administration.transformToRootCoordinates(refSystem); + //this would delete the SRS-information of the refSystem-object and transform its origin into ROOT-SRS: Administration.transformToRootCoordinates(refSystem); // nur sicherstellen das alle Bezugssysteme wirklich existieren DataServices.addRefSystem(refSystem); } @@ -49,24 +50,28 @@ public class Administration { } public static void transformToRootCoordinates(PositionEvent posEvent) { + LOG.info("INDFRO-DEBUG: Transforming PositionEvent-object " + posEvent.toString() + "to ROOT-SRS"); Administration.transformToRootCoordinates(posEvent.getPlacing()); } public static void transformToRootCoordinates(Zone zone) { - zone.getSpace().stream() + zone.getSpace().stream() .map(Administration::transformToRootCoordinates); } public static Space transformToRootCoordinates(Space space) { + LOG.info("INDFRO-DEBUG: Transforming space-object " + space.toString() + "to ROOT-SRS"); Administration.transformToRootCoordinates(space.getCentrePoint()); return space; } public static void transformToRootCoordinates(ReferenceSystem refSystem) { + LOG.info("INDFRO-DEBUG: Transforming refSystem-object " + refSystem.toString() + "to ROOT-SRS"); Administration.transformToRootCoordinates(refSystem.getOrigin()); } public static void transformToRootCoordinates(POI poi) { + LOG.info("INDFRO-DEBUG: Transforming poi-object " + poi.toString() + "to ROOT-SRS"); Administration.transformToRootCoordinates(poi.getPlacing()); } @@ -80,12 +85,16 @@ public class Administration { if (isNotRootPosition(placing.getPosition())) { Position positionToBeTransformed = placing.getPosition(); ReferenceSystem nextTransformationGoal_SRS = positionToBeTransformed.getReferenceSystem().getOrigin().getPosition().getReferenceSystem(); + LOG.info("INDFRO-DEBUG: (before) ReferenceSystem: " + positionToBeTransformed.getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placing)); SRSConversion.switchSRS(positionToBeTransformed, nextTransformationGoal_SRS); // assumption: not only position is changed but also ReferenceSystem-object is updated + LOG.info("INDFRO-DEBUG: (after) ReferenceSystem: " + positionToBeTransformed.getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placing)); + LOG.info("INDFRO-DEBUG:"); transformToRootCoordinates(placing); // recursion stops when placing.getPosition() has been transformed in a ROOT-position } } public static void transformToRootCoordinates(Position position) { + LOG.info("INDFRO-DEBUG: Transforming Position-object " + position.toString() + "to ROOT-SRS"); Orientation dummyOrientation = createDummyOrientation(); Placing placing = DataServices.createPlacing(position, dummyOrientation); transformToRootCoordinates(placing); // placing.getPosition() points to the position-object that was passed to this function. The contents of this object will be changed by this call. @@ -166,7 +175,9 @@ public class Administration { private static void transformPlacingSuccessively(Placing placingToBeTransformed, List<ReferenceSystem> transformationTargets) { for (ReferenceSystem transTarget : transformationTargets){ + LOG.info("INDFRO-DEBUG: (before) ReferenceSystem: " + positionToBeTransformed.getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placing)); SRSConversion.switchSRS(placingToBeTransformed, transTarget); + LOG.info("INDFRO-DEBUG: (after) ReferenceSystem: " + positionToBeTransformed.getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placing)); } } } diff --git a/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java b/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java index 8591bc9..c88057f 100644 --- a/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java +++ b/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java @@ -69,7 +69,7 @@ public class PositionMonitoring { LOG.info("OP:"); LOG.info("OP:"); LOG.info("OP:----PositionMonitoring--------------------------"); - LOG.info("OP: position: " + logPosition(positionEvent)); + LOG.info("OP: position: " + logPosition(positionEvent.getPlacing())); } catch (ParseException e) { e.printStackTrace(); @@ -421,11 +421,11 @@ public class PositionMonitoring { String agentId = lObject.getAgent().getId(); 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); + LOG.info("INDFRO: Received: sensorType: " + sensorType + "; position: " + logPosition(posEvent.getPlacing()) + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId); } - private static String logPosition(PositionEvent posEvent) { - Point3D point = (Point3D) posEvent.getPlacing().getPosition().getPoint(); + public static String logPosition(Placing placing) { + Point3D point = (Point3D) placing.getPosition().getPoint(); return "(x: " + point.getX() + ", y: " + point.getY() + ", z: " + point.getZ() + ")"; } diff --git a/testdata_raw_indfro.txt b/testdata_raw_indfro.txt index 10c650b..f0997c5 100644 --- a/testdata_raw_indfro.txt +++ b/testdata_raw_indfro.txt @@ -1,6 +1,7 @@ {"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": "CETI_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": "CETI_ROBOTARM_CELL", "point": {"x": 1.0,"y": 0.5,"z": 2.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": "CETI_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": 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: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:24+00:00"}]} +{"nfcRawDataEvent" : [ {"timestamp" : "2021-10-14T19:33:25+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": 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 -- GitLab