From 1a42b0ef042e12fe4c3866f965e3f4244e1d19fa Mon Sep 17 00:00:00 2001
From: dev <paul.k.bourelly@leidos.com>
Date: Tue, 3 Sep 2024 11:39:48 -0400
Subject: [PATCH] Update CDASimAdapter to accomadate changes in SensorConfig
 file

---
 .../CDASimAdapter/src/CDASimConnection.cpp    | 16 ++++++++-
 .../test/TestCDASimConnection.cpp             |  2 +-
 .../CDASimAdapter/test/sensors_new.json       | 36 +++++++++++++++++++
 3 files changed, 52 insertions(+), 2 deletions(-)
 create mode 100755 src/v2i-hub/CDASimAdapter/test/sensors_new.json

diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp
index 4ab7cabd7..2bbd7ab18 100644
--- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp
+++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp
@@ -263,6 +263,20 @@ namespace CDASimAdapter{
         {
             PLOG(logERROR) << "Error parsing sensors from string: " << json_str << std::endl;
         }
-        return json_v;
+        Json::Value sensors_registration;
+        for ( int index = 0; index < json_v.size(); ++index ) {
+            Json::Value sensor;
+            sensor["sensorId"] = json_v[index]["sensorId"];
+            sensor["type"] = json_v[index]["type"];
+            if ( json_v[index]["ref"]["type"] != "CARTESIAN" ){
+                PLOG(logWARNING) << "Skipping sensor configuration for " + sensor["sensorId"].asString() + ". Invalid ref type! Only CARTESIAN ref type currently supported for CDASim!" ;
+                continue;
+            }
+            sensor["location"] = json_v[index]["ref"]["location"];
+            sensor["orientation"] =json_v[index]["ref"]["orientation"];
+            sensors_registration[index] = sensor;
+        }
+
+        return sensors_registration;
     }
 }
diff --git a/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp
index 710dc49d8..4ea09e29a 100644
--- a/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp
+++ b/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp
@@ -31,7 +31,7 @@ namespace CDASimAdapter {
             }
         public:
             std::shared_ptr<CDASimConnection> connection;
-            std::string sensors_file_path = "../../CDASimAdapter/test/sensors.json";
+            std::string sensors_file_path = "../../CDASimAdapter/test/sensors_new.json";
         
 
     };
diff --git a/src/v2i-hub/CDASimAdapter/test/sensors_new.json b/src/v2i-hub/CDASimAdapter/test/sensors_new.json
new file mode 100755
index 000000000..3f7cb1094
--- /dev/null
+++ b/src/v2i-hub/CDASimAdapter/test/sensors_new.json
@@ -0,0 +1,36 @@
+[
+  {
+    "sensorId": "SomeID",
+    "type": "SemanticLidar",
+    "ref": {
+      "type": "CARTESIAN",
+      "location": {
+        "x": 0.0,
+        "y": 0.0,
+        "z": 0.0
+      },
+      "orientation": {
+        "yaw": 0.0,
+        "pitch": 0.0,
+        "roll": 0.0
+        }
+    }
+  },
+  {
+    "sensorId": "SomeID2",
+    "type": "SemanticLidar",
+    "ref": {
+      "type": "CARTESIAN",
+      "location": {
+        "x": 1.0,
+        "y": 2.0,
+        "z": 0.0
+      },
+      "orientation": {
+        "yaw": 23.0,
+        "pitch": 0.0,
+        "roll": 0.0
+        }
+    }
+  }
+]
\ No newline at end of file