Hotfix - Update BSM processing frequency for BSMs received from non-a…

…pproaching active Emergency Response Vehicles (#2103)
usdot-fhwa-stol · May 9, 2023 · c7c860e · c7c860e
2 parents 2b06977 + 6b2d54f
commit c7c860e
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 13 deletions.
diff --git a/...nclude/approaching_emergency_vehicle_plugin/approaching_emergency_vehicle_plugin_node.hpp b/...nclude/approaching_emergency_vehicle_plugin/approaching_emergency_vehicle_plugin_node.hpp
@@ -443,6 +443,9 @@ namespace approaching_emergency_vehicle_plugin
     // Boolean flag to indicate whether if each ERV and CMV are going on same direction
     std::unordered_map<std::string, bool> is_same_direction_;
 
+    // Unordered map to store the latest time a BSM was processed for a given active ERV
+    std::unordered_map<std::string, rclcpp::Time> latest_erv_update_times_;
+
     // Pointer for map projector
     boost::optional<std::string> map_projector_;
 

diff --git a/approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp b/approaching_emergency_vehicle_plugin/src/approaching_emergency_vehicle_plugin_node.cpp
@@ -152,7 +152,7 @@ namespace approaching_emergency_vehicle_plugin
     add_on_set_parameters_callback(std::bind(&ApproachingEmergencyVehiclePlugin::parameter_update_callback, this, std_ph::_1));
 
     // Setup subscribers
-    incoming_bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("incoming_bsm", 10,
+    incoming_bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("incoming_bsm", 1,
                                             std::bind(&ApproachingEmergencyVehiclePlugin::incomingBsmCallback, this, std_ph::_1));
 
     georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 10,
@@ -507,6 +507,9 @@ namespace approaching_emergency_vehicle_plugin
       return boost::optional<ErvInformation>();
     }
 
+    // Update the latest processing time of this ERV
+    latest_erv_update_times_[erv_information.vehicle_id] = this->now();
+
     // Generate ERV's route based on its current position and its destination points
     lanelet::Optional<lanelet::routing::Route> erv_future_route = generateErvRoute(erv_information.current_latitude, erv_information.current_longitude, erv_destination_points);
 
@@ -810,21 +813,23 @@ namespace approaching_emergency_vehicle_plugin
       return;
     }
 
-    // If there is already an ERV approaching the ego vehicle, only process this BSM futher if enough time has passed since the previously processed BSM
-    if(has_tracked_erv_){
+    // Get the vehicle ID associated with the received BSM
+    std::stringstream ss;
+    for(size_t i = 0; i < msg->core_data.id.size(); ++i){
+      ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned)msg->core_data.id.at(i);
+    }
+    std::string erv_vehicle_id = ss.str();
+    RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Received a BSM from " << erv_vehicle_id);
 
-      // Get the vehicle ID associated with the received BSM
-      std::stringstream ss;
-      for(size_t i = 0; i < msg->core_data.id.size(); ++i){
-        ss << std::setfill('0') << std::setw(2) << std::hex << (unsigned)msg->core_data.id.at(i);
-      }
-      std::string erv_vehicle_id = ss.str();
+    if(has_tracked_erv_){
+      // If there is already an ERV approaching the ego vehicle, only process this BSM further if it is from that ERV and enough time has passed since the previously processed BSM
 
       if(erv_vehicle_id == tracked_erv_.vehicle_id){
-        double seconds_since_prev_bsm = (this->now() - tracked_erv_.latest_update_time).seconds();
+        double seconds_since_prev_processed_bsm = (this->now() - tracked_erv_.latest_update_time).seconds();
 
-        if(seconds_since_prev_bsm < (1.0 / config_.bsm_processing_frequency)){
+        if(seconds_since_prev_processed_bsm < (1.0 / config_.bsm_processing_frequency)){
           // Do not process ERV's BSM further since not enough time has passed since its previously processed BSM
+          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ignoring BSM from tracked ERV " << erv_vehicle_id << " since a BSM from it was processed " << seconds_since_prev_processed_bsm << " seconds ago");
           return;
         }
       }
@@ -833,6 +838,19 @@ namespace approaching_emergency_vehicle_plugin
         return;
       }
     }
+    else{
+
+      // If BSM is from a detected active ERV, only process it further if enough time has passed since the previously processed BSM from this ERV
+      if (latest_erv_update_times_.find(erv_vehicle_id) != latest_erv_update_times_.end()){
+        double seconds_since_prev_processed_bsm = (this->now() - latest_erv_update_times_[erv_vehicle_id]).seconds();
+
+        if(seconds_since_prev_processed_bsm < (1.0 / config_.bsm_processing_frequency)){
+          // Do not process ERV's BSM further since not enough time has passed since its previously processed BSM
+          RCLCPP_DEBUG_STREAM(rclcpp::get_logger(logger_name), "Ignoring BSM from non tracked ERV " << erv_vehicle_id << " since a BSM from it was processed " << seconds_since_prev_processed_bsm << " seconds ago");
+          return;
+        }
+      }
+    }
 
     // Get ErvInformation object with information from the BSM if it is from an active ERV that is approaching the ego vehicle
     boost::optional<ErvInformation> erv_information = getErvInformationFromBsm(std::move(msg));

diff --git a/approaching_emergency_vehicle_plugin/test/test_approaching_erv.cpp b/approaching_emergency_vehicle_plugin/test/test_approaching_erv.cpp
@@ -294,10 +294,14 @@ namespace approaching_emergency_vehicle_plugin{
         regional_ext.route_destination_points.push_back(position2);
         erv_bsm.regional.push_back(regional_ext);
 
+        // Verify that there are no elements in latest_erv_update_times_ since no active ERV BSM has been processed yet
+        ASSERT_EQ(worker_node->latest_erv_update_times_.size(), 0);
+
         // Verify that ego vehicle does not determine that ERV is approaching since ego vehicle is not engaged (BSM is not processed)
         std::unique_ptr<carma_v2x_msgs::msg::BSM> erv_bsm_ptr = std::make_unique<carma_v2x_msgs::msg::BSM>(erv_bsm);
         worker_node->incomingBsmCallback(std::move(erv_bsm_ptr)); 
         ASSERT_FALSE(worker_node->has_tracked_erv_);
+        ASSERT_EQ(worker_node->latest_erv_update_times_.size(), 0); // ERV BSM was received, but guidance was not engaged, so BSM was not processed
 
         // Set is_guidance_engaged_ flag to true to enable incomingBsmCallback to fully process the incoming BSM
         worker_node->is_guidance_engaged_ = true;
@@ -306,6 +310,7 @@ namespace approaching_emergency_vehicle_plugin{
         std::unique_ptr<carma_v2x_msgs::msg::BSM> erv_bsm_ptr2 = std::make_unique<carma_v2x_msgs::msg::BSM>(erv_bsm);
         worker_node->incomingBsmCallback(std::move(erv_bsm_ptr2)); 
         ASSERT_FALSE(worker_node->has_tracked_erv_);
+        ASSERT_EQ(worker_node->latest_erv_update_times_.size(), 0); // ERV BSM was received, but ERV was slower than ego vehicle, so BSM was not processed
 
         // Increase ERV's speed to twice the ego vehicle speed so that the BSM is processed and the ERV is tracked by this plugin
         erv_bsm.core_data.speed = 20.0;
@@ -314,6 +319,7 @@ namespace approaching_emergency_vehicle_plugin{
         ASSERT_TRUE(worker_node->has_tracked_erv_);
         ASSERT_EQ(worker_node->tracked_erv_.lane_index, 0);
         ASSERT_NEAR(worker_node->tracked_erv_.current_speed, 20.0, 0.001);
+        ASSERT_EQ(worker_node->latest_erv_update_times_.size(), 1);
 
         // Set BSM processing frequency to 0.5 Hz 
         worker_node->config_.bsm_processing_frequency = 0.5; // (Hz) Only process ERV BSMs that are at least 2 seconds apart
@@ -352,6 +358,9 @@ namespace approaching_emergency_vehicle_plugin{
             executor.spin_once();
         }
 
+        double seconds_since_prev_processed_bsm = (worker_node->now() - worker_node->latest_erv_update_times_[worker_node->tracked_erv_.vehicle_id]).seconds();
+        ASSERT_NEAR(seconds_since_prev_processed_bsm, 3.0, 0.15);
+
         // Verify that the plugin no longer has an approaching ERV
         ASSERT_FALSE(worker_node->has_tracked_erv_);
 
@@ -384,6 +393,9 @@ namespace approaching_emergency_vehicle_plugin{
             executor.spin_once();
         }
 
+        seconds_since_prev_processed_bsm = (worker_node->now() - worker_node->latest_erv_update_times_[worker_node->tracked_erv_.vehicle_id]).seconds();
+        ASSERT_NEAR(seconds_since_prev_processed_bsm, 5.0, 0.15);
+
         std::unique_ptr<carma_v2x_msgs::msg::BSM> erv_bsm_ptr6 = std::make_unique<carma_v2x_msgs::msg::BSM>(erv_bsm_opposing);
         worker_node->incomingBsmCallback(std::move(erv_bsm_ptr6)); 
         EXPECT_EQ(worker_node->is_same_direction_.size(), 1);
@@ -995,8 +1007,8 @@ namespace approaching_emergency_vehicle_plugin{
             executor.spin_once();
         }
 
-        // Verify that node has broadcasted 2 (of 3) warning messages
-        ASSERT_EQ(worker_node->num_warnings_broadcasted_, 2);
+        // Verify that node has broadcasted 1 (of 3) warning messages
+        ASSERT_EQ(worker_node->num_warnings_broadcasted_, 1);
 
         // Set the internal data members of the plugin to match the incoming EmergencyVehicleAck message contents
         worker_node->tracked_erv_.vehicle_id = "ERV";