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