Update CDASimAdapter to accomadate changes in SensorConfig file (#640)

<!-- Thanks for the contribution, this is awesome. -->

# PR Details
## Description
### Background (Related changes)
This PR was created to accommodate changes in the Sensor Configuration
file introduced by this PR
(usdot-fhwa-stol/carma-streets#427). To
summarize, the sensor configuration file format was changed to allow for
reference location to be specified in WGS84 coordinates or cartesian
coordinates. For Simulation cartesian coordinates work well since many
simulation environments do not have real-world equivalents and cartesian
coordinates are easy to pull from different map editors and viewers like
[JSOM](https://josm.openstreetmap.de/). For real world deployments
however WGS84 coordinates are far easier to use since using cartesian
coordinates introduces a dependency on an lanlet2 map for the area. It
is far easier to use built in or separate GPS devices to get a reference
location for a mounted sensor.
#### Old Format of Sensor Configuration JSON:
```json
[
  {
    "sensorId": "SomeID",
    "type": "SemanticLidar",
    "location": {
      "x": 1.0, /* in meters */
      "y": 2.0, /* in meters */
      "z": -3.2 /* in meters */
     },
     "orientation": {
       "yaw": 0.0,
       "pitch": 0.0,
       "roll": 0.0
      }
  }
]
```  
#### New Format of Sensor Configuration JSON:
```json
[
  {
    "sensorId": "sensor_1",
    "type": "SemanticLidar",
    "ref": {
      "type": "CARTESIAN",
      "location": {
        "x": 1.0, /* in meters */
        "y": 2.0, /* in meters */
        "z": -3.2 /* in meters */
      },
       "orientation": { 
        "yaw": 0.0,
        "pitch": 0.0,
        "roll": 0.0
        }
    }
  } 
]
```

### Changes

This change in Sensor Configuration file means that we need to change
how we read and populate the registration message that goes out from the
CDASim Adapter Plugin to CDASim. The content of this registration
remains unchanged, we just need to read this information from the new
`ref` element in the Sensor Configuration JSON and populate the existing
registration. Lastly, we added logic for the registration to ignore any
sensor with a WGS84 reference location since we currently only have
support
```json
{
  "infrastructureId": "id_123",
  "location": {
    "x": 0,
    "y": 0,
    "z": 0
  },
  "rxMessageIpAddress": "127.0.0.1",
  "rxMessagePort": 8686,
  "sensors": {
    "sensorId": "sensor1",
    "type": "SemanticLidar",
    "location": {
      "x": 0.0,
      "y": 0.0,
      "z": 0.0
     },
     "orientation": {
       "yaw": 0.0,
       "pitch": 0.0,
       "roll": 0.0
      }
  },
  "simulatedInteractionPort": 7576,
  "timeSyncPort": 7575
}

```

<!--- Describe your changes in detail -->

## Related Issue
[FCP-37
](https://usdot-carma.atlassian.net/browse/FCP-37)<!--- This project
only accepts pull requests related to open issues -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please link to the issue here: -->

## Motivation and Context
See description
<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?
Local Integration Testing and unit testing
<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [x] Defect fix (non-breaking change that fixes an issue)
- [ ] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [x] My change requires a change to the documentation.
- [x] I have updated the documentation accordingly.
- [x] I have read the **CONTRIBUTING** document.
[V2XHUB Contributing
Guide](https://github.com/usdot-fhwa-OPS/V2X-Hub/blob/develop/Contributing.md)
- [x] I have added tests to cover my changes.
- [x] All new and existing tests passed.

src/v2i-hub/CDASimAdapter/README.md

@@ -0,0 +1,82 @@
+# CDASim Adapter Plugin Plugin Documentation
+
+## Introduction
+
+This Plugin is an adapter for integration with the **CDASim Co-simulation** enviroment. 
+
+## Related Plugins
+
+A list of plugins related to the CDASim Adapter Plugin.
+
+### Immediate Forward Plugin
+
+For RSU Immediate Message Forwarding (IMF) functionality forward J2735 messages to CDASim.
+
+### Message Receiver Plugin
+
+The Message Receiver Plugin for receiving incoming J2735 message from CDASim.
+
+## Configuration/Deployment
+
+### System Configuration
+
+This plugin uses several system configuration parameters set as enviroment variables described below.
+
+**SIMULATION_MODE** – Environment variable for enabling simulation components for V2X-Hub. If set to "true" or "TRUE" simulation components will be enable. Otherwise, simulation components will not be enabled.
+
+**SIMULATION_IP** – Environment variable for storing IP address of CDASim application.
+
+**SIMULATION_REGISTRATION_PORT** – Environment variable for storing port on CDASim that handles registration attempts.
+
+**TIME_SYNC_PORT** – Environment varaible for storing port for receiving time sync messages from CDASim.
+
+**V2X_PORT** – Environment variable for storing port for receiving v2x messages from CDASim
+
+**SIM_V2X_PORT** – Environment variable for storing port for sending v2x messages to CDASim
+
+**V2XHUB_IP** – Environment variable for storing IP address of V2X Hub.
+
+**INFRASTRUCTURE_ID** – Environment variable for storing infrastructure id of V2X Hub.
+
+**SENSOR_JSON_FILE_PATH** – Environment variable for storing path to sensor configuration file. This is an optional simulation environment variable that allows for setting up simulated sensor for a V2X-Hub instance. Example file can be found in the **CDASimAdapterPlugin** tests [here](../src/v2i-hub/CDASimAdapter/test/sensors.json).
+
+### Plugin Configuration
+
+In addition to the system configuration, the CDASim Adapter also has plugin configuration parameters that can be set via the admin portal.
+
+**LogLevel**:The log level for this plugin"
+
+**X**:Cartesian X coordinate in simulated map (in meters).
+
+**Y**:Cartesian Y coordinate in simulated map (in meters).
+
+**Z**:Cartesian Z coordinate in simulated map (in meters).
+
+**MaxConnectionAttempts**:Number of connection attempts CDASimAdapter plugin will execute before failing. Valid values are any integers greater than or equal to 1. Any value less than 1 will result in unlimited connection attemtps.
+
+**ConnectionSleepTime**:Number of seconds to wait after a failed CDASim connection attempt, before retrying to establish connection. Valid values are equal or greater than 1. Any value less than 1 will be treated as 1.
+
+## Design
+
+### Communication
+
+![Alt text](docs/communication_diagram.png)
+The integration with the **CDASim Co-simulation** enviorment consists of 4 major parts:
+
+**Registration Adapter**: The CDASimAdapter will establish a unique connection with CDASim and provide configuration information about harware like RSUs or Sensors associated with V2X-Hub.
+
+**Time Adapter**: The CDASimAdapter will consume and broadcast simulation time from the CDASim enviromment to ensure all plugins are using simulation time in place of machine time.
+
+**Message Adapter**: The CDASimAdapter will forward V2X messages to CDASim for simulated radio broadcast. It will also listen for incoming simulated V2X message recptions for the RSUs it registered with CDASim.
+
+**Simulation Adapter**: This final part captures any simulated events V2X-Hub needs to listen for. An example is Sensor Detections. When registering a sensor with CDASim, the sensor is created and detections from that sensor need to be forwarded to V2X-Hub. The simulation adapter will listen for these events from CDASim and broadcast them on the TMX message bus for other plugin.
+
+### Messages
+
+**Time Sync Message**: A V2X-Hub message that all PluginClientClockAware plugins subscribe to when running V2X-Hub in the simulation environment. This updates the base class clock object to allow for seamless integration of plugins into the simulation environment. 
+
+**Sensor Detected Object**: V2X-Hub's generic message for detection data. This message is generated when the CDASim adapter receives a detection interaction for one of the sensors it registers.
+
+## Functionality Testing
+
+Included in this directory is a `scripts` directory that includes scripts to mock a CDASim Server listening for simulation registration events (`udp_socket_server.py`), time steps sent from CDASim (`send_timestep_udp.py`), and simulated detection events sent from CDASim (`send_sim_detected_object_udp.py`). These can be used to test functiontionality associated with the **CDASimAdapter Plugin**

src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp

@@ -54,12 +54,14 @@ namespace CDASimAdapter{
         //Sample sensors.json: https://raw.githubusercontent.com/usdot-fhwa-OPS/V2X-Hub/develop/src/v2i-hub/CDASimAdapter/test/sensors.json
         // Sensor configuration is an optional part of registration message.
         if ( !_sensor_json_file_path.empty() ) {
-            auto sensors_json_v = read_json_file(_sensor_json_file_path);
-            if(sensors_json_v.empty())
-            {
-                PLOG(logWARNING) << "Sensors JSON is empty!" << std::endl;
-            }     
-            message["sensors"] = sensors_json_v;
+            try {
+                auto sensors_json_v = read_sensor_configuration_file(_sensor_json_file_path);
+                message["sensors"] = sensors_json_v;
+            }
+            catch(const std::invalid_argument &e) {
+                PLOG(tmx::utils::logWARNING) << "Error occured reading sensor configuration file: " << e.what() 
+                    << std::endl << "Not sending any Sensor Registration information with CDASim Infrastructure Registration!";
+            }
         }
         else {
             PLOG(logWARNING) << "No sensors where configured for this V2X-Hub instance.";
@@ -234,14 +236,12 @@ namespace CDASimAdapter{
     }
 
     Json::Value CDASimConnection::read_json_file(const std::string& file_path) const{
-        Json::Value sensors_json_v;
         //Read file from disk
         std::ifstream in_strm;
         in_strm.open(file_path, std::ifstream::binary);
         if(!in_strm.is_open())
         {
-            PLOG(logERROR) << "File cannot be opened. File path: " << file_path <<std::endl;
-            return sensors_json_v;
+            throw std::invalid_argument("File cannot be opened. File path: " + file_path);
         }
         std::string line;
         std::stringstream ss;
@@ -261,8 +261,63 @@ namespace CDASimAdapter{
         JSONCPP_STRING err;
         if(!reader->parse(json_str.c_str(), json_str.c_str() + json_str.length(), &json_v, &err))
         {
-            PLOG(logERROR) << "Error parsing sensors from string: " << json_str << std::endl;
+            throw std::invalid_argument("Error parsing JSON from string: " + json_str);
         }
         return json_v;
     }
+
+    Json::Value CDASimConnection::read_sensor_configuration_file(const std::string &file_path) const {
+        // Sensor Configuration File JSON format
+        /*  {
+            "sensorId": "sensor_1",
+            "type": "SemanticLidar",
+            "ref": {
+                "type": "CARTESIAN",
+                "location": {
+                    "x": 1.0, 
+                    "y": 2.0, 
+                    "z": -3.2 
+                },
+                "orientation": { 
+                    "yaw": 0.0,
+                    "pitch": 0.0,
+                    "roll": 0.0
+                    }
+            }
+        } 
+        ] */
+        auto sensor_configuration = read_json_file(file_path);
+        Json::Value sensors_registration;
+        for ( int index = 0; index < sensor_configuration.size(); ++index ) {
+            Json::Value sensor;
+            if ( sensor_configuration[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["sensorId"] = sensor_configuration[index]["sensorId"];
+            sensor["type"] = sensor_configuration[index]["type"];
+            sensor["location"] = sensor_configuration[index]["ref"]["location"];
+            sensor["orientation"] =sensor_configuration[index]["ref"]["orientation"];
+            sensors_registration.append(sensor);
+        }
+        /** Sensor Registration JSON Format
+        [
+            {
+            "sensorId": "SomeID",
+            "type": "SemanticLidar",
+            "location": {
+                "x": 1.0, 
+                "y": 2.0, 
+                "z": -3.2 
+                },
+                "orientation": {
+                "yaw": 0.0,
+                "pitch": 0.0,
+                "roll": 0.0
+                }
+            }
+        ]
+        */
+        return sensors_registration;
+    }
 }

src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp

@@ -135,17 +135,26 @@ namespace CDASimAdapter {
                                 const tmx::utils::Point &location) const; 
 
             /**
-             * @brief Read Json file specified by the file path from disk, and convert the json into Json::Value object.
-             * @param file_path A string of file path in the host machine.
+             * @brief Helper method to read JSON file and return resulting Json::Value. 
+             * @param file_path A string of file path in the host machine to the JSON file.
              * @return A Json::Value object.
+             * @throw std::invalid_argument exception if no file is found or file is not readable for provided path.
             */
             Json::Value read_json_file(const std::string& file_path) const;
-             /**
-             * @brief Convert the Json string into Json::Value object.
-             * @param json_str A JSON string.
-             * @return A Json::Value object.
-            */
+            /**
+             * @brief Helper method to convert string JSON content to JSON::Value.
+             * @param json_str A JSON string content of Sensor Configuration JSON file
+             * @return A Json::Value object parsed from string.
+             * @throw std::invalid_argument exception if provided string is not valid JSON
+             */
             Json::Value string_to_json(const std::string &json_str) const;
+            /**
+             * @brief Method to read Sensor Configuration JSON file and return Sensor Registration information
+             * required for Infrastructure Registration to CDASim.
+             * @param file_path Path to Sensor Configuration file.
+             * @return Json::Value correspoding to Sensor Registration information for Infrastructure Registration to CDASim.
+             */
+            Json::Value read_sensor_configuration_file(const std::string &file_path) const;
 
             std::string _simulation_ip;
             uint _simulation_registration_port;
@@ -171,6 +180,8 @@ namespace CDASimAdapter {
             FRIEND_TEST(TestCDASimConnection, get_handshake_json_no_sensor_config);
             FRIEND_TEST(TestCDASimConnection, read_json_file);
             FRIEND_TEST(TestCDASimConnection, string_to_json);
+            FRIEND_TEST(TestCDASimConnection, read_sensor_configuration_file);
+
 
 
     };

src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp

@@ -101,20 +101,26 @@ namespace CDASimAdapter {
 
     TEST_F(TestCDASimConnection, read_json_file)
     {        
-        auto sensorJsonV = connection->read_json_file("Invalid_file_path" );
-        ASSERT_TRUE(sensorJsonV.empty());
+        EXPECT_THROW(connection->read_json_file("Invalid_file_path" ), std::invalid_argument);
         std::ifstream in_strm;
         in_strm.open(sensors_file_path, std::ifstream::binary);
         if(in_strm.is_open())
         {
-            sensorJsonV = connection->read_json_file(sensors_file_path );
+            auto sensorJsonV = connection->read_json_file(sensors_file_path );
             ASSERT_FALSE(sensorJsonV.empty());
         }        
     }
 
     TEST_F(TestCDASimConnection, string_to_json)
     {        
-        auto sensorJsonV = connection->string_to_json("Invalid Json");
-        ASSERT_TRUE(sensorJsonV.empty());
+        EXPECT_THROW(connection->string_to_json("Invalid Json"), std::invalid_argument);
+    }
+
+    TEST_F(TestCDASimConnection, read_sensor_configuration_file) {
+        auto sensor_registration = connection->read_sensor_configuration_file("../../CDASimAdapter/test/sensors_including_invalid_entries.json");
+        EXPECT_EQ(2, sensor_registration.size());
+        EXPECT_EQ("SomeID", sensor_registration[0]["sensorId"].asString());
+        EXPECT_EQ("SomeID2", sensor_registration[1]["sensorId"].asString());
+
     }
 }

src/v2i-hub/CDASimAdapter/test/sensors.json

@@ -2,29 +2,35 @@
   {
     "sensorId": "SomeID",
     "type": "SemanticLidar",
-    "location": {
-      "x": 0.0,
-      "y": 0.0,
-      "z": 0.0
-     },
-     "orientation": {
-       "yaw": 0.0,
-       "pitch": 0.0,
-       "roll": 0.0
-      }
+    "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",
-    "location": {
-      "x": 1.0,
-      "y": 2.0,
-      "z": 0.0
-     },
-     "orientation": {
-       "yaw": 23.0,
-       "pitch": 0.0,
-       "roll": 0.0
-      }
+    "ref": {
+      "type": "CARTESIAN",
+      "location": {
+        "x": 1.0,
+        "y": 2.0,
+        "z": 0.0
+      },
+      "orientation": {
+        "yaw": 23.0,
+        "pitch": 0.0,
+        "roll": 0.0
+        }
+    }
   }
 ]

src/v2i-hub/CDASimAdapter/test/sensors_including_invalid_entries.json

@@ -0,0 +1,48 @@
+[
+  {
+    "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": "sensor_1",
+    "type": "SemanticLidar",
+    "ref": {
+      "type": "WGS84",
+      "location": {
+        "latitude": 38.9549716548523, 
+        "longitude": -77.14935313519123, 
+        "altitude": 5.5 
+      }
+    }
+  }, 
+  {
+    "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
+        }
+    }
+  }
+]