MOSAIC-CARMA-Streets: CDASimAdapter listens on port for incoming Simulated Object Dectections and publish the serialized object to Kafka

.devcontainer/docker-compose-vscode.yml

@@ -22,6 +22,7 @@ services:
       - TIME_SYNC_TOPIC=time_sync
       - TIME_SYNC_PORT=7575
       - SIM_V2X_PORT=5757
+      - SIM_INTERACTION_PORT=7576
       - V2X_PORT=8686
       - INFRASTRUCTURE_ID=1
     secrets:

configuration/amd64/docker-compose.yml

@@ -38,6 +38,17 @@ services:
       - db
     environment:
       - MYSQL_PASSWORD=/run/secrets/mysql_password
+      - SIMULATION_MODE=true
+      - SIMULATION_IP=127.0.0.1
+      - SIMULATION_REGISTRATION_PORT=6767
+      - LOCAL_IP=127.0.0.1
+      - TIME_SYNC_TOPIC=time_sync
+      - TIME_SYNC_PORT=7575
+      - SIM_V2X_PORT=5757
+      - SIM_INTERACTION_PORT=7576
+      - V2X_PORT=8686
+      - INFRASTRUCTURE_ID=1
+      - KAFKA_BROKER_ADDRESS=127.0.0.1:9092
     secrets:
       - mysql_password
     volumes:

src/tmx/Messages/include/MessageTypes.h

@@ -91,6 +91,7 @@ static CONSTEXPR const char *MSGSUBTYPE_INCOMING_STRING = "Incoming";
 static CONSTEXPR const char *MSGSUBTYPE_OUTGOING_STRING = "Outgoing";
 static CONSTEXPR const char *MSGSUBTYPE_SHUTDOWN_STRING = "Shutdown";
 static CONSTEXPR const char *MSGSUBTYPE_TIMESYNC_STRING = "TimeSync";
+static CONSTEXPR const char *MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING = "SensorDetectedObject";
 
 } /* End namespace messages */
 

src/tmx/Messages/include/simulation/SensorDetectedObject.h

@@ -0,0 +1,35 @@
+#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_
+#define INCLUDE_SIMULATED_SensorDetectedObject_H_
+
+#include <tmx/messages/message.hpp>
+#include <MessageTypes.h>
+
+namespace tmx
+{
+    namespace messages
+    {
+        namespace simulation
+        {
+            /**
+             * This SensorDetectedObject is used to communicate the sensor detected object information with various applications
+             * including internal infrastructure applications and external road user applications through simulated environment.
+             * It defines the message type and sub type and all data members.
+             */
+            class SensorDetectedObject : public tmx::message
+            {
+            public:
+                SensorDetectedObject(){};
+                SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {};
+                ~SensorDetectedObject(){};
+                // Message type for routing this message through TMX core
+                static constexpr const char *MessageType = MSGTYPE_APPLICATION_STRING;
+
+                // // Message sub type for routing this message through TMX core
+                static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING;
+            };
+
+        }
+    }
+
+}; // namespace tmx
+#endif

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

@@ -41,6 +41,11 @@ namespace tmx::utils::sim{
          * necessary in SIMULATION MODE for CDASim message forwarding.
          */
         constexpr inline static const char *SIM_V2X_PORT = "SIM_V2X_PORT";
+        /**
+         * @brief Name of environment variable for storing port for forwarding v2x messages to CDASim. Only
+         * necessary in SIMULATION MODE for CDASim message forwarding.
+         */
+        constexpr inline static const char *SIM_INTERACTION_PORT= "SIM_INTERACTION_PORT";
         /**
          * @brief Name of environment variable for storing port for receiving v2x messages from CDASim. Only
          * necessary in SIMULATION MODE for CDASim message forwarding.

src/v2i-hub/CARMAStreetsPlugin/manifest.json

@@ -116,6 +116,11 @@
             "key": "SpatConsumerGroupId",
             "default": "v2xhub_spat",
             "description": "Apache Kafka consumer group ID for spat consumer."
+        },
+        {
+            "key": "SimSensorDetectedObjTopic",
+            "default": "v2xhub_sim_sensor_detected_object",
+            "description": "Apache Kafka topic plugin will transmit simulated sensor detected object to."               
         }
 
     ]

src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp

@@ -25,6 +25,8 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) :
 	AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage);
 	AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage);
 	AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage);
+	AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage );
+
 	SubscribeToMessages();
 }
 
@@ -48,7 +50,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() {
 	GetConfigValue<string>("MobilityOperationTopic", _transmitMobilityOperationTopic);
 	GetConfigValue<string>("MobilityPathTopic", _transmitMobilityPathTopic);
  	GetConfigValue<string>("MapTopic", _transmitMAPTopic);
-	GetConfigValue<string>("SRMTopic", _transmitSRMTopic);
+	GetConfigValue<string>("SRMTopic", _transmitSRMTopic); 
+	GetConfigValue<string>("SimSensorDetectedObjTopic", _transmitSimSensorDetectedObjTopic); 
 	 // Populate strategies config
 	string config;
 	GetConfigValue<string>("MobilityOperationStrategies", config);
@@ -626,6 +629,13 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){
 	}
 
 }
+
+void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg)
+{
+	PLOG(logINFO) <<  "Produce sensor detected message in JSON format:  " << msg.to_string() <<std::endl;
+	produce_kafka_msg( msg.to_string(), _transmitSimSensorDetectedObjTopic);
+}
+
 bool CARMAStreetsPlugin::getEncodedtsm3( tsm3EncodedMessage *tsm3EncodedMsg,  Json::Value metadata, Json::Value payload_json )
 {
 	try

src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h

@@ -20,6 +20,7 @@
 #include <kafka/kafka_client.h>
 #include <kafka/kafka_consumer_worker.h>
 #include "JsonToJ2735SSMConverter.h"
+#include <simulation/SensorDetectedObject.h>
 #include "PluginClientClockAware.h"
 
 
@@ -48,6 +49,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware {
 	void HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg);
 	void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg);
 	void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg);
+	void HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg);
 	/**
 	 * @brief Overide PluginClientClockAware HandleTimeSyncMessage to producer TimeSyncMessage to kafka for CARMA Streets Time Synchronization.
 	 * @param msg TimeSyncMessage received by plugin when in simulation mode. Message provides current simulation time to all processes.
@@ -104,6 +106,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware {
 	std::string _transmitBSMTopic;
 	std::string _transmitMAPTopic;
 	std::string _transmitSRMTopic;
+	std::string _transmitSimSensorDetectedObjTopic;
 	std::string _kafkaBrokerIp;
 	std::string _kafkaBrokerPort;
 	std::shared_ptr<kafka_producer_worker> _kafka_producer_ptr;

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

@@ -0,0 +1,89 @@
+
+import socket
+import sys
+import json
+import time
+
+# Script for integration testing CDASimAdapter Time Sync functionality.
+# This python script sends periodic time sync messages to a configurable
+# host and port. To Test the Time Sync functionality of the CDASimAdapter
+# set port to the value of the TIME_SYNC_PORT environment variable.
+#
+# TODO Move this script into a more permanent location
+count_num = 0
+
+def generate_sim_external_object():
+    jsonResult = {
+        "metadata": {
+            "type": "Application",
+            "subtype": "SensorDetectedObject",
+            "timestamp": 123,
+            "isSimulated": True
+        },
+        "payload": {
+            "sensor": {
+            "id": "SomeID",
+            "type": "SematicLidar",
+            "location": {
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            },
+            "proj_string": "+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs"
+            },
+            "type": "Car",
+            "confidence": "0.7",
+            "objectId": "Object1",
+            "position": {
+            "x": 1.0,
+            "y": 2.5,
+            "z": 1.1
+            },
+            "positionCovariance" : [12,12,2, 34, 34, 55],
+            "velocity": {
+            "x": 1.0,
+            "y": 2.5,
+            "z": 1.1
+            },
+            "velocityCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"],
+            "angularVelocity":{
+            "x": 1.0,
+            "y": 2.5,
+            "z": 1.1
+            },
+            "angularVelocityCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"],
+            "size": {
+            "length": 0.1,
+            "width": 0.4,
+            "height": 1.5
+            }     
+        }
+    }
+    jsonResult = json.dumps(jsonResult)
+    return jsonResult
+port = 7576
+address = "127.0.0.1"
+host = (address, port)
+try:
+    sock = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM)
+except socket.error as err:
+    print('Socket error because of %s' %(err))
+
+
+while True :
+    try:
+        msg = generate_sim_external_object()
+        encoded_msg = str.encode(msg)
+        count_num += 1
+        sock.sendto(encoded_msg,host)
+        print( encoded_msg.decode(encoding= 'UTF-8'), 'was sent to ', host)
+        print(f'Message sent at ${time.time()}')
+        time.sleep(5)
+    except socket.gaierror:
+
+        print ('There an error resolving the host')
+        break
+
+sock.close()
+
+

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

@@ -60,6 +60,7 @@ namespace CDASimAdapter{
 
             if ( connection->is_connected() ) {
                 start_time_sync_thread_timer();
+                start_sensor_detected_object_detection_thread();
                 start_immediate_forward_thread();
                 start_message_receiver_thread();
             }else {
@@ -78,13 +79,19 @@ namespace CDASimAdapter{
 
     }
 
+    void CDASimAdapter::forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg) {
+        PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl;
+        this->BroadcastMessage<tmx::messages::simulation::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);        
+    }
+
     bool CDASimAdapter::connect() {
         try {
             std::string simulation_ip = sim::get_sim_config(sim::SIMULATION_IP);
             std::string local_ip =  sim::get_sim_config(sim::LOCAL_IP);
             PLOG(logINFO) << "Simulation and local IP successfully initialized!"<< std::endl;
             uint simulation_registration_port = std::stoul(sim::get_sim_config(sim::SIMULATION_REGISTRATION_PORT));
             uint time_sync_port = std::stoul(sim::get_sim_config(sim::TIME_SYNC_PORT));
+            uint simulated_interaction_port = std::stoul(sim::get_sim_config(sim::SIM_INTERACTION_PORT));
             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);
@@ -94,11 +101,11 @@ namespace CDASimAdapter{
                     " 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, simulated_interaction_port, v2x_port, location ));
             }
             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, simulated_interaction_port, v2x_port, location);
             }
         }       
         catch (const TmxException &e) {
@@ -160,6 +167,34 @@ namespace CDASimAdapter{
         }
     }
 
+    void CDASimAdapter::start_sensor_detected_object_detection_thread() {
+        PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl;
+        try 
+        {
+            if(!external_bject_detection_thread_timer)
+            {
+                external_bject_detection_thread_timer  = std::make_unique<tmx::utils::ThreadTimer>();
+            }            
+            external_bject_detection_thread_timer->AddPeriodicTick([this](){
+                PLOG(logDEBUG1) << "Listening for Sensor Detected Message from CDASim." << std::endl;
+                auto msg = connection->consume_sensor_detected_object_message();
+                if ( !msg.is_empty()) {             
+                    this->forward_simulated_detected_message(msg);
+                }
+                else 
+                {
+                    PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated sensor detected message!" << std::endl;
+                }
+            }//End lambda
+            , std::chrono::milliseconds(100));
+            external_bject_detection_thread_timer->Start();
+        }
+        catch ( const UdpServerRuntimeError &e ) 
+        {
+            PLOG(logERROR) << "Error occured :" << e.what() <<  std::endl;
+        }
+    }
+
     void CDASimAdapter::attempt_message_from_v2xhub() const {
         try {
             std::string msg = connection->consume_v2x_message_from_v2xhub();