RSS: Cleanup

- Only calculate for new frames
- Return calculation result

LibCarla/source/carla/rss/RssCheck.cpp

@@ -296,6 +296,7 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const &timestamp,
                             ::ad::rss::situation::SituationSnapshot &output_situation_snapshot,
                             ::ad::rss::world::WorldModel &output_world_model,
                             EgoDynamicsOnRoute &output_rss_ego_dynamics_on_route) {
+  bool result = false;
   try {
     double const time_since_epoch_check_start_ms =
         std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
@@ -367,7 +368,7 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const &timestamp,
               << " after create world model " << std::endl;
 #endif
 
-    PerformCheck(_carla_rss_state);
+    result = PerformCheck(_carla_rss_state);
 
 #if DEBUG_TIMING
     t_end = std::chrono::high_resolution_clock::now();
@@ -403,12 +404,10 @@ bool RssCheck::CheckObjects(carla::client::Timestamp const &timestamp,
     std::cout << "-> EC " << std::chrono::duration<double, std::milli>(t_end - t_start).count() << " end check objects"
               << std::endl;
 #endif
-
-    return true;
   } catch (...) {
     _logger->error("Exception -> Check failed");
-    return false;
   }
+  return result;
 }
 
 ::ad::map::match::Object RssCheck::GetMatchObject(carla::SharedPtr<carla::client::Actor> const &actor,
@@ -924,21 +923,18 @@ void RssCheck::CreateWorldModel(carla::client::Timestamp const &timestamp, carla
   carla_rss_state.world_model = scene_creation.getWorldModel();
 }
 
-void RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const {
+bool RssCheck::PerformCheck(CarlaRssState &carla_rss_state) const {
   bool result = carla_rss_state.rss_check.calculateProperResponse(
       carla_rss_state.world_model, carla_rss_state.situation_snapshot, carla_rss_state.rss_state_snapshot,
       carla_rss_state.proper_response);
 
   if (!result) {
     _logger->warn("calculateProperResponse failed!");
-    carla_rss_state.proper_response.lateralResponseRight = ::ad::rss::state::LateralResponse::None;
-    carla_rss_state.proper_response.lateralResponseLeft = ::ad::rss::state::LateralResponse::None;
-    carla_rss_state.proper_response.longitudinalResponse = ::ad::rss::state::LongitudinalResponse::None;
   }
-
+  else if (!carla_rss_state.proper_response.isSafe) {
-  if (!carla_rss_state.proper_response.isSafe) {
     _logger->info("Unsafe route: {}", carla_rss_state.proper_response);
   }
+  return result;
 }
 
 void RssCheck::AnalyseCheckResults(CarlaRssState &carla_rss_state) const {

LibCarla/source/carla/rss/RssCheck.h

@@ -329,7 +329,7 @@ private:
                         carla::client::Vehicle const &carla_ego_vehicle, CarlaRssState &carla_rss_state) const;
 
   /// @brief Perform the actual RSS check
-  void PerformCheck(CarlaRssState &carla_rss_state) const;
+  bool PerformCheck(CarlaRssState &carla_rss_state) const;
 
   /// @brief Analyse the RSS check results
   void AnalyseCheckResults(CarlaRssState &carla_rss_state) const;

LibCarla/source/carla/rss/RssSensor.cpp

@@ -268,6 +268,15 @@ SharedPtr<sensor::SensorData> RssSensor::TickRssSensor(const Timestamp &timestam
     carla::rss::EgoDynamicsOnRoute ego_dynamics_on_route;
     if (_processing_lock.try_lock()) {
       spdlog::debug("RssSensor tick: T={}", timestamp.frame);
+
+      if ((timestamp.frame < _last_processed_frame) && ((_last_processed_frame - timestamp.frame) < 0xffffffffu))
+      {
+        _processing_lock.unlock();
+        spdlog::warn("RssSensor tick dropped: T={}", timestamp.frame);
+        return nullptr;
+      }
+      _last_processed_frame = timestamp.frame;
+
       carla::client::World world = GetWorld();
       SharedPtr<carla::client::ActorList> actors = world.GetActors();
 

LibCarla/source/carla/rss/RssSensor.h

@@ -136,6 +136,9 @@ private:
 
   /// reqired to store DropRoute() requests until next sensor tick
   bool _drop_route;
+
+  /// last processed frame
+  std::size_t _last_processed_frame;
 };
 
 }  // namespace client