Streets MOSAIC integration: CDASimAdapter register with Simulation ambassador and send sensors information

.devcontainer/docker-compose-vscode.yml

@@ -24,6 +24,7 @@ services:
       - SIM_V2X_PORT=5757
       - V2X_PORT=8686
       - INFRASTRUCTURE_ID=1
+      - SENSOR_JSON_FILE_PATH=/var/www/plugins/MAP/sensors.json
     secrets:
       - mysql_password
 

src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h

@@ -31,6 +31,11 @@ namespace tmx::utils::sim{
          * in SIMULATION MODE for connecting to CDASim.
          */
         constexpr inline static const char *SIMULATION_REGISTRATION_PORT = "SIMULATION_REGISTRATION_PORT";
+        /**
+         * @brief sensors file location
+        */
+        constexpr inline static const char *SENSOR_JSON_FILE_PATH = "SENSOR_JSON_FILE_PATH";
+
         /**
          * @brief Name of environment varaible for storing port for receiving time sync messages from CDASim. Only 
          * necessary in SIMULATION MODE for CDASim time sync.

src/v2i-hub/CDASimAdapter/scripts/udp_socket_server.py

@@ -0,0 +1,19 @@
+import socket
+'''
+Purpose: This script is to launch a UDP server and used to test the CDASimAdapter simulation registration.
+Usage: Run this script first, it shall open a socket listenning to port 6767.
+    Launch the v2xhub and enable the CDASimAdapter plugin. Upon the plugin startup,
+    it sends registration message to port 6767. This UDP server shall receive the registration message,
+    and print this message on the terminal.
+Run command: python3 udp_socket_server.py
+'''
+UDP_IP = "127.0.0.1"
+UDP_SOCKET_PORT_SIM_REGISTRATION = 6767
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sock.bind((UDP_IP, UDP_SOCKET_PORT_SIM_REGISTRATION))
+print("Server Listenning on port: %s" % UDP_SOCKET_PORT_SIM_REGISTRATION)
+
+while True:
+    data, addr = sock.recvfrom(1024)
+    print("recevied message: %s" % data)

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

@@ -88,17 +88,18 @@ namespace CDASimAdapter{
             uint v2x_port = std::stoul(sim::get_sim_config(sim::V2X_PORT));
             uint sim_v2x_port = std::stoul(sim::get_sim_config(sim::SIM_V2X_PORT));
             std::string infrastructure_id = sim::get_sim_config(sim::INFRASTRUCTURE_ID);
+            std::string sensor_json_file_path = sim::get_sim_config(sim::SENSOR_JSON_FILE_PATH);
 
             PLOG(logINFO) << "CDASim connecting " << simulation_ip << 
                     "\nUsing Registration Port : "  << std::to_string( simulation_registration_port) <<
                     " Time Sync Port: " << std::to_string( time_sync_port) << " and V2X Port: " << std::to_string(v2x_port) << std::endl;
             if ( connection ) {
                 connection.reset(new CDASimConnection( simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip,
-                                                time_sync_port, v2x_port, location ));
+                                                time_sync_port, v2x_port, location, sensor_json_file_path));
             }
             else {
                 connection = std::make_unique<CDASimConnection>(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip,
-                                                            time_sync_port, v2x_port, location);
+                                                            time_sync_port, v2x_port, location, sensor_json_file_path);
             }
         }       
         catch (const TmxException &e) {

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

@@ -6,10 +6,10 @@ using namespace tmx::utils;
 namespace CDASimAdapter{ 
     CDASimConnection::CDASimConnection(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port,
                                                         const std::string &local_ip,  const uint time_sync_port, const uint v2x_port, 
-                                                        const Point &location) : 
+                                                        const Point &location, const std::string &sensor_json_file_path) : 
                                                         _simulation_ip(simulation_ip), _infrastructure_id(infrastructure_id), _simulation_registration_port(simulation_registration_port),
                                                         _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _v2x_port(v2x_port),
-                                                        _location(location)  {
+                                                        _location(location) ,_sensor_json_file_path(sensor_json_file_path) {
         PLOG(logDEBUG) << "CARMA-Simulation connection initialized." << std::endl;                                                     
     } 
 
@@ -47,13 +47,22 @@ namespace CDASimAdapter{
         message["location"]["x"] = location.X;
         message["location"]["y"] = location.Y;
         message["location"]["z"] = location.Z;
+
+        //Read local sensor file and populate the sensors JSON
+        //Sample sensors.json: https://raw.githubusercontent.com/usdot-fhwa-OPS/V2X-Hub/develop/src/v2i-hub/CDASimAdapter/test/sensors.json
+        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;
         Json::StyledWriter writer;
         message_str = writer.write(message);
         return message_str;
     }
 
     bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, 
-                                const std::string &local_ip,  const uint time_sync_port, const uint v2x_port, 
+                                const std::string &local_ip,  const uint time_sync_port, const uint v2x_port,
                                 const Point &location) 
     {
         // Create JSON message with the content 
@@ -217,4 +226,36 @@ namespace CDASimAdapter{
         forward_message( msg , message_receiver_publisher );
     }
 
+    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;
+        }
+        std::string line;
+        std::stringstream ss;
+        while (std::getline(in_strm, line)) {
+            ss << line;
+        }
+        in_strm.close();
+
+       return string_to_json(ss.str());
+    }
+
+    Json::Value CDASimConnection::string_to_json(const std::string& json_str) const{
+        //Update JSON value with information from string
+        Json::Value json_v;
+        Json::CharReaderBuilder builder;
+        std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+        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;
+        }
+        return json_v;
+    }
 }

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

@@ -7,6 +7,7 @@
 #include <jsoncpp/json/json.h>
 #include <PluginLog.h>
 #include <gtest/gtest.h>
+#include <fstream>
 
 
 namespace CDASimAdapter {
@@ -33,7 +34,7 @@ namespace CDASimAdapter {
              */
             explicit CDASimConnection( const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, 
                                 const uint sim_v2x_port, const std::string &local_ip,  const uint time_sync_port, const uint v2x_port, 
-                                const tmx::utils::Point &location);
+                                const tmx::utils::Point &location, const std::string &sensor_json_file_path);
 
              /**
              * @brief Method to forward v2x message to CARMA Simulation
@@ -133,6 +134,19 @@ namespace CDASimAdapter {
             std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip,  const uint time_sync_port, 
                 const uint v2x_port, const tmx::utils::Point &location) const; 
 
+            /**
+             * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information.
+             * @param file_path A string of file location in the host machine.
+             * @return A reference to the location where the sensors inforation is updated and stored.
+            */
+            Json::Value read_json_file(const std::string& file_path) const;
+             /**
+             * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information.
+             * @param json_str A JSON string.
+             * @return A reference to JSON value.
+            */
+            Json::Value string_to_json(const std::string &json_str) const;
+
             std::string _simulation_ip;
             uint _simulation_registration_port;
             std::string _infrastructure_id;
@@ -142,6 +156,7 @@ namespace CDASimAdapter {
             uint _v2x_port;
             tmx::utils::Point _location;
             bool _connected = false;
+            std::string _sensor_json_file_path;
 
             std::shared_ptr<tmx::utils::UdpServer> carma_simulation_listener;
             std::shared_ptr<tmx::utils::UdpClient> carma_simulation_publisher;
@@ -151,6 +166,8 @@ namespace CDASimAdapter {
             std::shared_ptr<tmx::utils::UdpServer> time_sync_listener;
 
             FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json);
+            FRIEND_TEST(TestCARMASimulationConnection, read_json_file);
+            FRIEND_TEST(TestCARMASimulationConnection, string_to_json);
     };
 
 }

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

@@ -5,6 +5,7 @@
 #include <WGS84Point.h>
 #include <MockUdpClient.h>
 #include <MockUdpServer.h>
+#include <filesystem>
 
 
 using testing::_;
@@ -23,13 +24,14 @@ namespace CDASimAdapter {
             void SetUp() override {
                 // Initialize CARMA Simulation connection with (0,0,0) location.
                 Point location; 
-                connection = std::make_shared<CDASimConnection>("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location);
+                connection = std::make_shared<CDASimConnection>("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location, sensors_file_path);
             }
             void TearDown() override {
 
             }
         public:
             std::shared_ptr<CDASimConnection> connection;
+            std::string sensors_file_path = "../../CDASimAdapter/test/sensors.json";
 
 
     };
@@ -85,10 +87,13 @@ namespace CDASimAdapter {
         location.X = 1000;
         location.Y = 38.955; 
         location.Z = -77.149;
-
-        ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, location), 
-        "{\n   \"infrastructureId\" : \"4566\",\n   \"location\" : {\n      \"x\" : 1000.0,\n      \"y\" : 38.954999999999998,\n      \"z\" : -77.149000000000001\n   },\n   \"rxMessageIpAddress\" : \"127.0.0.1\",\n   \"rxMessagePort\" : 4568,\n   \"timeSyncPort\" : 4567\n}\n");
-
+        std::ifstream in_strm;
+        in_strm.open(sensors_file_path, std::ifstream::binary);
+        if(in_strm.is_open())
+        {
+            ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, location), 
+            "{\n   \"infrastructureId\" : \"4566\",\n   \"location\" : {\n      \"x\" : 1000.0,\n      \"y\" : 38.954999999999998,\n      \"z\" : -77.149000000000001\n   },\n   \"rxMessageIpAddress\" : \"127.0.0.1\",\n   \"rxMessagePort\" : 4568,\n   \"sensors\" : [\n      {\n         \"location\" : {\n            \"x\" : 0.0,\n            \"y\" : 0.0,\n            \"z\" : 0.0\n         },\n         \"orientation\" : {\n            \"pitch\" : 0.0,\n            \"roll\" : 0.0,\n            \"yaw\" : 0.0\n         },\n         \"sensorId\" : \"SomeID\",\n         \"type\" : \"SematicLidar\"\n      },\n      {\n         \"location\" : {\n            \"x\" : 1.0,\n            \"y\" : 2.0,\n            \"z\" : 0.0\n         },\n         \"orientation\" : {\n            \"pitch\" : 0.0,\n            \"roll\" : 0.0,\n            \"yaw\" : 23.0\n         },\n         \"sensorId\" : \"SomeID2\",\n         \"type\" : \"SematicLidar\"\n      }\n   ],\n   \"timeSyncPort\" : 4567\n}\n");
+        }
     }
 
     TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) {
@@ -101,4 +106,23 @@ namespace CDASimAdapter {
     TEST_F(TestCARMASimulationConnection, connect) {
         ASSERT_TRUE(connection->connect());
     }
+
+    TEST_F(TestCARMASimulationConnection, read_json_file)
+    {        
+        auto sensorJsonV = connection->read_json_file("Invalid_file_path" );
+        ASSERT_TRUE(sensorJsonV.empty());
+        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 );
+            ASSERT_FALSE(sensorJsonV.empty());
+        }        
+    }
+
+    TEST_F(TestCARMASimulationConnection, string_to_json)
+    {        
+        auto sensorJsonV = connection->string_to_json("Invalid Json");
+        ASSERT_TRUE(sensorJsonV.empty());
+    }
 }

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

@@ -0,0 +1,30 @@
+[
+  {
+    "sensorId": "SomeID",
+    "type": "SematicLidar",
+    "location": {
+      "x": 0.0,
+      "y": 0.0,
+      "z": 0.0
+     },
+     "orientation": {
+       "yaw": 0.0,
+       "pitch": 0.0,
+       "roll": 0.0
+      }
+  },
+  {
+    "sensorId": "SomeID2",
+    "type": "SematicLidar",
+    "location": {
+      "x": 1.0,
+      "y": 2.0,
+      "z": 0.0
+     },
+     "orientation": {
+       "yaw": 23.0,
+       "pitch": 0.0,
+       "roll": 0.0
+      }
+  }
+]