Merge remote-tracking branch 'origin/master' into feature/slope_vehic…

…le_model

# Conflicts:
#	simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
#	test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py

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

@@ -4,8 +4,9 @@
 
 Major Changes :race_car: :red_car: :blue_car:
 
-| Feature | Brief summary | Category | Pull request | Contributor |
-| ------- | ------------- | -------- | ------------ | ----------- |
+| Feature                 | Brief summary                                                        | Category            | Pull request                                                      | Contributor                                   |
+| ----------------------- | -------------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | --------------------------------------------- |
+| Extend matching length. | Enable consider tread when calculating matching length of EgoEntity. | `traffic_simulator` | [#1181](https://github.com/tier4/scenario_simulator_v2/pull/1181) | [hakuturu583](https://github.com/hakuturu583) |
 
 Bug Fixes:bug:
 

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

rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+
+project(real_time_factor_control_rviz_plugin)
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(Qt5 REQUIRED Core Widgets)
+
+ament_auto_find_build_dependencies()
+
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED src/real_time_factor_slider.cpp)
+
+target_compile_definitions(${PROJECT_NAME} PUBLIC QT_NO_KEYWORDS)
+
+target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
+
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package()

rviz_plugins/real_time_factor_control_rviz_plugin/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>real_time_factor_control_rviz_plugin</name>
+  <version>0.1.0</version>
+  <description>Slider controlling real time factor value.</description>
+  <maintainer email="[email protected]">Paweł Lech</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Paweł Lech</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>libqt5-core</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rviz_common</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugins/plugin_description.xml"/>
+  </export>
+</package>

rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,7 @@
+<library path="real_time_factor_control_rviz_plugin">
+  <class name="real_time_factor_control_rviz_plugin/RealTimeFactorSliderPanel"
+    type="real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel"
+    base_class_type="rviz_common::Panel">
+    <description> Slider controlling real time factor value. </description>
+  </class>
+</library>

rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp

@@ -0,0 +1,64 @@
+//
+// Copyright 2020 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+#include <pluginlib/class_list_macros.hpp>
+#include <qt5/QtWidgets/QHBoxLayout>
+#include <real_time_factor_slider.hpp>
+#include <rviz_common/display_context.hpp>
+
+namespace real_time_factor_control_rviz_plugin
+{
+RealTimeFactorSliderPanel::RealTimeFactorSliderPanel(QWidget * parent)
+: rviz_common::Panel(parent),
+  value_label_(new QLabel("x 1.00")),
+  slider_(new QSlider(Qt::Horizontal))
+{
+  value_label_->setAlignment(Qt::AlignCenter);
+
+  slider_->setMinimum(1);
+  slider_->setMaximum(200);
+  slider_->setTickInterval(1);
+  slider_->setValue(100);
+
+  auto layout = new QHBoxLayout(this);
+  layout->addWidget(value_label_);
+  layout->addWidget(slider_);
+
+  setLayout(layout);
+}
+
+auto RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int percentage) -> void
+{
+  std_msgs::msg::Float64 real_time_factor;
+  real_time_factor.data = percentage / 100.0;
+  real_time_factor_publisher->publish(real_time_factor);
+  value_label_->setText(QString("x ") + QString::number(real_time_factor.data, 'f', 2));
+}
+
+auto RealTimeFactorSliderPanel::onInitialize() -> void
+{
+  real_time_factor_publisher = getDisplayContext()
+                                 ->getRosNodeAbstraction()
+                                 .lock()
+                                 ->get_raw_node()
+                                 ->create_publisher<std_msgs::msg::Float64>("/real_time_factor", 1);
+
+  connect(slider_, SIGNAL(valueChanged(int)), SLOT(onChangedRealTimeFactorValue(int)));
+}
+}  // namespace real_time_factor_control_rviz_plugin
+
+PLUGINLIB_EXPORT_CLASS(
+  real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel, rviz_common::Panel)