From 8f8ca56a23366c5d641f05094fd61509c3c4f4e3 Mon Sep 17 00:00:00 2001
From: Frank Rohde <frank.rohde@tu-dresden.de>
Date: Wed, 12 Jan 2022 20:14:16 +0100
Subject: [PATCH] SRSConversion: Added logging of orientation
---
.../ipos/project/UseCaseController/Administration.java | 10 +++++-----
.../project/UseCaseController/PositionMonitoring.java | 7 ++++++-
2 files changed, 11 insertions(+), 6 deletions(-)
diff --git a/src/main/java/ipos/project/UseCaseController/Administration.java b/src/main/java/ipos/project/UseCaseController/Administration.java
index af95818..a574f3c 100644
--- a/src/main/java/ipos/project/UseCaseController/Administration.java
+++ b/src/main/java/ipos/project/UseCaseController/Administration.java
@@ -84,9 +84,9 @@ public class Administration {
public static void transformToRootCoordinates(Placing placingToBeTransformed) {
if (isNotRootPosition(placingToBeTransformed.getPosition())) {
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));
+ LOG.info("INDFRO-DEBUG: (before) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed) + "; Orientation: " + PositionMonitoring.logOrientation(placingToBeTransformed));
+ SRSConversion.switchSRS(placingToBeTransformed, nextTransformationGoal_SRS); // assumption: not only position is changed but also ReferenceSystem-object of the placingToBeTransformed is updated
+ LOG.info("INDFRO-DEBUG: (after) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed) + "; Orientation: " + PositionMonitoring.logOrientation(placingToBeTransformed));
LOG.info("INDFRO-DEBUG:");
transformToRootCoordinates(placingToBeTransformed); // recursion stops when placing.getPosition() has been transformed in a ROOT-position
}
@@ -181,9 +181,9 @@ public class Administration {
private static void transformPlacingSuccessively(Placing placingToBeTransformed, List<ReferenceSystem> transformationTargets) {
for (ReferenceSystem transTarget : transformationTargets){
- LOG.info("INDFRO-DEBUG: (before) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed));
+ LOG.info("INDFRO-DEBUG: (before) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed) + "; Orientation: " + PositionMonitoring.logOrientation(placingToBeTransformed));
SRSConversion.switchSRS(placingToBeTransformed, transTarget);
- LOG.info("INDFRO-DEBUG: (after) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed));
+ LOG.info("INDFRO-DEBUG: (after) ReferenceSystem: " + placingToBeTransformed.getPosition().getReferenceSystem().getId() + "; Position: " + PositionMonitoring.logPosition(placingToBeTransformed) + "; Orientation: " + PositionMonitoring.logOrientation(placingToBeTransformed));
}
}
}
diff --git a/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java b/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java
index c88057f..beef934 100644
--- a/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java
+++ b/src/main/java/ipos/project/UseCaseController/PositionMonitoring.java
@@ -421,7 +421,7 @@ 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.getPlacing()) + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId);
+ LOG.info("INDFRO: Received: sensorType: " + sensorType + "; position: " + logPosition(posEvent.getPlacing()) + "; orientation: " + logOrientation(posEvent.getPlacing()) + "; timestamp: " + timestamp + "; sensorId: " + sensorId + "; agentId: " + agentId);
}
public static String logPosition(Placing placing) {
@@ -429,6 +429,11 @@ public class PositionMonitoring {
return "(x: " + point.getX() + ", y: " + point.getY() + ", z: " + point.getZ() + ")";
}
+ public static String logOrientation(Placing placing) {
+ Quaternion orientation = (Quaternion) placing.getOrientation();
+ return "(x: " + orientation.getX() + ", y: " + orientation.getY() + ", z: " + orientation.getZ() + ", w: " + orientation.getW() + ")";
+ }
+
/**
* An event-filter may hot handle a position event if the filter should respect the results of
* sensor data fusion (e.g., wrt accuracy-criterium) and the result is negative for this event,
--
GitLab