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