Merge branch 'master' into feature/ego_slope

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>

external/concealer/src/field_operator_application.cpp

@@ -16,6 +16,7 @@
 #include <cstdlib>
 #include <exception>
 #include <scenario_simulator_exception/exception.hpp>
+#include <system_error>
 
 namespace concealer
 {
@@ -70,71 +71,76 @@ auto FieldOperatorApplication::checkAutowareProcess() -> void
 
 auto FieldOperatorApplication::shutdownAutoware() -> void
 {
-  AUTOWARE_INFO_STREAM("Shutting down Autoware: (1/3) Stop publishing/subscribing.");
-  {
-    stopRequest();
-  }
-
-  if (process_id != 0 && not is_autoware_exited) {
-    is_autoware_exited = true;
-
-    AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Send SIGINT to Autoware launch process.");
-    {
-      sendSIGINT();
-    }
-
-    AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Terminating Autoware.");
-    {
-      sigset_t mask{};
-      {
-        sigset_t orig_mask{};
-
-        sigemptyset(&mask);
-        sigaddset(&mask, SIGCHLD);
-
-        if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0) {
-          AUTOWARE_SYSTEM_ERROR("sigprocmask");
-          std::exit(EXIT_FAILURE);
-        }
-      }
-
-      timespec timeout{};
-      {
-        timeout.tv_sec = 5;
-        timeout.tv_nsec = 0;
+  if (stopRequest(); process_id != 0 && not std::exchange(is_autoware_exited, true)) {
+    const auto sigset = [this]() {
+      if (auto signal_set = sigset_t();
+          sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) {
+        RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what());
+        std::exit(EXIT_FAILURE);
+      } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) {
+        RCLCPP_ERROR_STREAM(get_logger(), std::system_error(error, std::system_category()).what());
+        std::exit(EXIT_FAILURE);
+      } else {
+        return signal_set;
       }
-
-      while (sigtimedwait(&mask, NULL, &timeout) < 0) {
-        switch (errno) {
-          case EINTR:  // Interrupted by a signal other than SIGCHLD.
-            break;
-
-          case EAGAIN:
-            AUTOWARE_ERROR_STREAM(
-              "Shutting down Autoware: (2/3) Autoware launch process does not respond. Kill it.");
-            kill(process_id, SIGKILL);
-            break;
-
-          default:
-            AUTOWARE_SYSTEM_ERROR("sigtimedwait");
-            std::exit(EXIT_FAILURE);
-        }
+    }();
+
+    const auto timeout = [this]() {
+      auto sigterm_timeout = [this](auto value) {
+        auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation");
+        node.declare_parameter<int>("sigterm_timeout", value);
+        node.get_parameter<int>("sigterm_timeout", value);
+        return value;
+      };
+      auto timeout = timespec();
+      timeout.tv_sec = sigterm_timeout(5);
+      timeout.tv_nsec = 0;
+      return timeout;
+    }();
+
+    if (sendSIGINT(); sigtimedwait(&sigset, nullptr, &timeout) < 0) {
+      switch (errno) {
+        case EINTR:
+          /*
+             The wait was interrupted by an unblocked, caught signal. It shall
+             be documented in system documentation whether this error causes
+             these functions to fail.
+          */
+          RCLCPP_ERROR_STREAM(
+            get_logger(),
+            "The wait for Autoware launch process termination was interrupted by an unblocked, "
+            "caught signal.");
+          break;
+
+        case EAGAIN:
+          /*
+             No signal specified by set was generated within the specified
+             timeout period.
+          */
+          RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it.");
+          killpg(process_id, SIGKILL);
+          break;
+
+        default:
+        case EINVAL:
+          /*
+             The timeout argument specified a tv_nsec value less than zero or
+             greater than or equal to 1000 million.
+          */
+          RCLCPP_ERROR_STREAM(
+            get_logger(),
+            "The parameter sigterm_timeout specified a value less than zero or greater than or "
+            "equal to 1000 million.");
+          break;
       }
     }
 
-    AUTOWARE_INFO_STREAM("Shutting down Autoware: (3/3) Waiting for Autoware to be exited.");
-    {
-      int status = 0;
-
-      const int waitpid_options = 0;
-
-      if (waitpid(process_id, &status, waitpid_options) < 0) {
-        if (errno == ECHILD) {
-          AUTOWARE_WARN_STREAM("Try to wait for the autoware process but it was already exited.");
-        } else {
-          AUTOWARE_SYSTEM_ERROR("waitpid");
-          std::exit(EXIT_FAILURE);
-        }
+    if (int status = 0; waitpid(process_id, &status, 0) < 0) {
+      if (errno == ECHILD) {
+        RCLCPP_ERROR_STREAM(
+          get_logger(), "Try to wait for the autoware process but it was already exited.");
+      } else {
+        RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what());
       }
     }
   }

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,26 @@ auto Interpreter::reset() -> void
     publisher_of_context->on_deactivate();
   }
 
-  SimulatorCore::deactivate();
+  if (not has_parameter("initialize_duration")) {
+    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();
 

test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py

@@ -115,11 +115,12 @@ def make_parameters():
             {"record": record},
             {"rviz_config": rviz_config},
             {"sensor_model": sensor_model},
+            {"sigterm_timeout": sigterm_timeout},
             {"vehicle_model": vehicle_model},
         ]
         parameters += make_vehicle_parameters()
         return parameters
-    
+
     def make_vehicle_parameters():
         parameters = []
 
@@ -177,7 +178,7 @@ def description():
             namespace="simulation",
             output="screen",
             on_exit=ShutdownOnce(),
-            parameters=[{"port": port}, {"use_sim_time": True}, {"consider_pose_by_road_slope": consider_pose_by_road_slope}]+make_vehicle_parameters(),
+            parameters=make_parameters() + [{"use_sim_time": True}, {"consider_pose_by_road_slope": consider_pose_by_road_slope}],
             condition=IfCondition(launch_simple_sensor_simulator),
         ),
         # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear.