diff --git a/common/status_monitor/include/status_monitor/status_monitor.hpp b/common/status_monitor/include/status_monitor/status_monitor.hpp index 3b8d083d542..e11373d82df 100644 --- a/common/status_monitor/include/status_monitor/status_monitor.hpp +++ b/common/status_monitor/include/status_monitor/status_monitor.hpp @@ -105,6 +105,38 @@ class StatusMonitor } } + template + class ScopedExchanger + { + std::reference_wrapper target; + + T value; + + public: + template + static auto locked_exchange(Ts &&... xs) -> decltype(auto) + { + auto lock = std::scoped_lock(mutex); + return std::exchange(std::forward(xs)...); + } + + template + explicit ScopedExchanger(T & x, U && y) + : target(x), value(locked_exchange(target.get(), std::forward(y))) + { + } + + ~ScopedExchanger() { locked_exchange(target.get(), value); } + }; + + template + 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 diff --git a/common/status_monitor/src/status_monitor.cpp b/common/status_monitor/src/status_monitor.cpp index da5879ce2f5..b00c8395bad 100644 --- a/common/status_monitor/src/status_monitor.cpp +++ b/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::high_resolution_clock::now().time_since_epoch()) diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 66da3364dd1..c64be4d513d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -27,6 +27,7 @@ ament_index_cpp geometry_msgs + libboost-dev nav_msgs rclcpp scenario_simulator_exception @@ -34,7 +35,6 @@ tf2_geometry_msgs tf2_ros traffic_simulator_msgs - libboost-dev ament_cmake_clang_format ament_cmake_copyright diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index 919150c5655..bf5d563d88e 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #define DECLARE_PARAMETER(IDENTIFIER) \ declare_parameter(#IDENTIFIER, IDENTIFIER) @@ -293,7 +294,24 @@ auto Interpreter::reset() -> void publisher_of_context->on_deactivate(); } - SimulatorCore::deactivate(); + declare_parameter("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();