Merge branch 'master' into fix/RJD-834_fix_follow_trajectory_action_a…

…utoware_cooperation

.github/workflows/BuildAndRun.yaml

@@ -39,10 +39,12 @@ jobs:
           fetch-depth: 0
           token: ${{ secrets.GITHUB_TOKEN }}
 
+      - run: git clone https://github.com/RobotecAI/scenario_simulator_v2_scenarios.git
+
       - name: Search packages in this repository
         id: list_packages
         run: |
-          echo package_list=$(colcon list --names-only | sed -e ':loop; N; $!b loop; s/\n/ /g') >> $GITHUB_OUTPUT
+          echo package_list=$(colcon list --names-only | tr '\n' ' ') >> $GITHUB_OUTPUT
 
       - name: Show target packages
         run: |
@@ -62,7 +64,7 @@ jobs:
       - name: Resolve rosdep
         run: |
           cd ~/ros2_ws
-          apt-get update
+          apt update
           apt install -y python3-pip
           rosdep update --include-eol-distros
           rosdep install -iy --from-paths src --rosdistro ${{ matrix.rosdistro }}
@@ -95,8 +97,15 @@ jobs:
       - name: Scenario test
         run: |
           source ~/ros2_ws/install/setup.bash
-          cd ~/ros2_ws
-          source install/local_setup.bash
+          source ~/ros2_ws/install/local_setup.bash
           ros2 launch scenario_test_runner scenario_test_runner.launch.py workflow:='$(find-pkg-share scenario_test_runner)/config/workflow_example.yaml' global_frame_rate:=20
           ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml
         shell: bash
+
+      - name: Basic test
+        run: |
+          source ~/ros2_ws/install/setup.bash
+          source ~/ros2_ws/install/local_setup.bash
+          ros2 launch scenario_test_runner scenario_test_runner.launch.py workflow:='$(find-pkg-share scenario_simulator_v2_scenarios)/workflow/basic.yaml'
+          ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml
+        shell: bash

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())

docs/ReleaseNotes.md

@@ -9,8 +9,10 @@ Major Changes :race_car: :red_car: :blue_car:
 
 Bug Fixes:bug:
 
-| Feature | Brief summary | Category | Pull request | Contributor |
-| ------- | ------------- | -------- | ------------ | ----------- |
+| Feature                        | Brief summary                                                  | Category            | Pull request                                                      | Contributor                                   |
+| ------------------------------ | -------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | --------------------------------------------- |
+| Revert #1096                   | Revert incomplete fix for de-spawned entity.                   | `traffic_simulator` | [#1159](https://github.com/tier4/scenario_simulator_v2/pull/1159) | [HansRobo](https://github.com/HansRobo)       |
+| Fix problems in route planning | Fixed a bug in a function related to Entity's route selection. | `traffic_simulator` | [#1126](https://github.com/tier4/scenario_simulator_v2/pull/1168) | [hakuturu583](https://github.com/hakuturu583) |
 
 Minor Tweaks :oncoming_police_car:
 

docs/developer_guide/OpenSCENARIOSupport.md

@@ -1,6 +1,6 @@
 # OpenSCENARIO Support
 
-The ROS 2 package `openscenario_interpreter` provides scenario-based simulation on [ASAM OpenSCENARIO 1.0](https://www.asam.net/standards/detail/openscenario/).
+The ROS 2 package `openscenario_interpreter` provides scenario-based simulation on [ASAM OpenSCENARIO 1.2](https://www.asam.net/standards/detail/openscenario-xml/).
 This section describes the differences between our OpenSCENARIO Interpreter and the OpenSCENARIO standard set by ASAM, and the OpenSCENARIO implementation by other companies and organizations.
 If you want to know about OpenSCENARIO, refer to the link below.
 

docs/user_guide/scenario_test_runner/ScenarioTestRunner.md

@@ -3,7 +3,7 @@
 You can test run (preview) the scenarios you created using the GUI scenario editor before running it with Autoware.
 
 The file you exported from the GUI scenario editor is in YAML based format called "[TIER IV Scenario Format Version 2.0](../../developer_guide/TIERIVScenarioFormatVersion2.md)". Then it is converted into XML based OpenSCENARIO format.
-The specification of this OpenSCENARIO format is found at [OpenSCENARIO](http://www.openscenario.org/) site.
+The specification of this OpenSCENARIO format is found at [OpenSCENARIO](https://www.asam.net/standards/detail/openscenario-xml/) site.
 
 ## Before Testing Scenarios
 

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());
       }
     }
   }

mkdocs.yml

@@ -64,7 +64,6 @@ nav:
       - Scenario Writing Tips: user_guide/ScenarioTips.md
       - Scenario Editor:
           - Overview: user_guide/scenario_editor/ScenarioEditorUserGuide.md
-          - Place an entity to relative position: user_guide/scenario_editor/RelativePosition.md
       - Scenario Test Runner:
           - Overview: user_guide/scenario_test_runner/ScenarioTestRunner.md
           - user_guide/scenario_test_runner/ScenarioFormatConversion.md

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();