Add new member function StatusMonitor::overrideThreshold

Signed-off-by: yamacir-kit <[email protected]>

common/status_monitor/include/status_monitor/status_monitor.hpp

@@ -105,6 +105,38 @@ class StatusMonitor
     }
   }
 
+  template <typename T>
+  class ScopedExchanger
+  {
+    std::reference_wrapper<T> target;
+
+    T value;
+
+  public:
+    template <typename... Ts>
+    static auto locked_exchange(Ts &&... xs) -> decltype(auto)
+    {
+      auto lock = std::scoped_lock<std::mutex>(mutex);
+      return std::exchange(std::forward<decltype(xs)>(xs)...);
+    }
+
+    template <typename U>
+    explicit ScopedExchanger(T & x, U && y)
+    : target(x), value(locked_exchange(target.get(), std::forward<decltype(y)>(y)))
+    {
+    }
+
+    ~ScopedExchanger() { locked_exchange(target.get(), value); }
+  };
+
+  template <typename Thunk>
+  auto overrideThreshold(const std::chrono::seconds & t, Thunk thunk) -> decltype(auto)
+  {
+    auto exchanger = ScopedExchanger(threshold, t);
+
+    return thunk();
+  }
+
   auto write() const -> void;
 } static status_monitor;
 }  // namespace common

common/status_monitor/src/status_monitor.cpp

@@ -120,9 +120,11 @@ auto StatusMonitor::write() const -> void
     }
   }
 
+  json["good"] = good();
+
   json["name"] = name();
 
-  json["good"] = good();
+  json["threshold"] = threshold.count();
 
   json["unix_time"] = std::chrono::duration_cast<std::chrono::seconds>(
                         std::chrono::high_resolution_clock::now().time_since_epoch())

external/concealer/package.xml

@@ -27,14 +27,14 @@
   <!-- Common -->
   <depend>ament_index_cpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>libboost-dev</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>scenario_simulator_exception</depend>
   <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>traffic_simulator_msgs</depend>
-  <depend>libboost-dev</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>

openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp

@@ -23,6 +23,7 @@
 #include <openscenario_interpreter/syntax/scenario_definition.hpp>
 #include <openscenario_interpreter/syntax/scenario_object.hpp>
 #include <openscenario_interpreter/utility/overload.hpp>
+#include <status_monitor/status_monitor.hpp>
 
 #define DECLARE_PARAMETER(IDENTIFIER) \
   declare_parameter<decltype(IDENTIFIER)>(#IDENTIFIER, IDENTIFIER)
@@ -293,7 +294,24 @@ auto Interpreter::reset() -> void
     publisher_of_context->on_deactivate();
   }
 
-  SimulatorCore::deactivate();
+  declare_parameter<int>("initialize_duration", 30);
+
+  /*
+     Although we have not analyzed the details yet, the process of deactivating
+     the simulator core takes quite a long time (especially the
+     traffic_simulator::API::despawnEntities function is slow). During the
+     process, the interpreter becomes unresponsive, which often resulted in the
+     status monitor thread judging the interpreter as "not good". Therefore, we
+     will increase the threshold of the judgment only during the process of
+     deactivating the simulator core.
+
+     The threshold value here is set to the value of initialize_duration, but
+     there is no rationale for this; it should be larger than the original
+     threshold value of the status monitor and long enough for the simulator
+     core to be deactivated.
+  */
+  common::status_monitor.overrideThreshold(
+    std::chrono::seconds(get_parameter("initialize_duration").as_int()), SimulatorCore::deactivate);
 
   scenarios.pop_front();