Add new parameter speed_condition to switch compatibility

Signed-off-by: yamacir-kit <[email protected]>
tier4 · Dec 12, 2024 · 0785813 · 0785813
1 parent 82b72ce
commit 0785813
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 10 deletions.
diff --git a/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp b/common/scenario_simulator_exception/include/scenario_simulator_exception/concatenate.hpp
@@ -25,12 +25,8 @@ namespace common
 inline namespace scenario_simulator_exception
 {
 inline auto concatenate = [](auto &&... xs) {
-  auto write = [](auto && os, auto && x) {
-    os.get() << std::forward<decltype(x)>(x);
-    return std::forward<decltype(os)>(os);
-  };
   std::stringstream result;
-  fold_left(write, std::ref(result), std::forward<decltype(xs)>(xs)...);
+  (result << ... << std::forward<decltype(xs)>(xs));
   return result.str();
 };
 }  // namespace scenario_simulator_exception

diff --git a/openscenario/openscenario_interpreter/CMakeLists.txt b/openscenario/openscenario_interpreter/CMakeLists.txt
@@ -59,8 +59,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   ${${PROJECT_NAME}_POSIX_SOURCES}
   ${${PROJECT_NAME}_SYNTAX_SOURCES}
   ${${PROJECT_NAME}_UTILITY_SOURCES}
-  src/object.cpp
+  src/compatibility.cpp
   src/evaluate.cpp
+  src/object.cpp
   src/openscenario_interpreter.cpp
   src/record.cpp
   src/scope.cpp)

diff --git a/...ario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/...ario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp
@@ -15,6 +15,7 @@
 #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__SPEED_CONDITION_HPP_
 #define OPENSCENARIO_INTERPRETER__SYNTAX__SPEED_CONDITION_HPP_
 
+#include <openscenario_interpreter/compatibility.hpp>
 #include <openscenario_interpreter/scope.hpp>
 #include <openscenario_interpreter/simulator_core.hpp>
 #include <openscenario_interpreter/syntax/directional_dimension.hpp>
@@ -63,14 +64,17 @@ struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluatio
 
   std::vector<std::valarray<double>> results;  // for description
 
+  static inline auto compatibility = Compatibility::legacy;
+
   explicit SpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
   auto description() const -> String;
 
   static auto evaluate(const Entities *, const Entity &) -> Eigen::Vector3d;
 
   static auto evaluate(
-    const Entities *, const Entity &, const std::optional<DirectionalDimension> &) -> double;
+    const Entities *, const Entity &, const std::optional<DirectionalDimension> &,
+    const Compatibility = Compatibility::legacy) -> double;
 
   auto evaluate() -> Object;
 };

diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp
@@ -22,6 +22,7 @@
 #include <openscenario_interpreter/syntax/parameter_value_distribution.hpp>
 #include <openscenario_interpreter/syntax/scenario_definition.hpp>
 #include <openscenario_interpreter/syntax/scenario_object.hpp>
+#include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <openscenario_interpreter/utility/overload.hpp>
 #include <status_monitor/status_monitor.hpp>
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
@@ -49,6 +50,10 @@ Interpreter::Interpreter(const rclcpp::NodeOptions & options)
   DECLARE_PARAMETER(output_directory);
   DECLARE_PARAMETER(publish_empty_context);
   DECLARE_PARAMETER(record);
+
+  declare_parameter<std::string>("speed_condition", "legacy");
+  SpeedCondition::compatibility =
+    boost::lexical_cast<Compatibility>(get_parameter("speed_condition").as_string());
 }
 
 Interpreter::~Interpreter() {}

diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp
@@ -58,7 +58,8 @@ auto SpeedCondition::evaluate(const Entities * entities, const Entity & triggeri
 
 auto SpeedCondition::evaluate(
   const Entities * entities, const Entity & triggering_entity,
-  const std::optional<DirectionalDimension> & direction) -> double
+  const std::optional<DirectionalDimension> & direction, const Compatibility compatibility)
+  -> double
 {
   if (const Eigen::Vector3d v = evaluate(entities, triggering_entity); direction) {
     switch (*direction) {
@@ -71,7 +72,13 @@ auto SpeedCondition::evaluate(
         return v.z();
     }
   } else {
-    return v.norm();
+    switch (compatibility) {
+      default:
+      case Compatibility::legacy:
+        return v.x();
+      case Compatibility::standard:
+        return v.norm();
+    }
   }
 }
 
@@ -81,7 +88,7 @@ auto SpeedCondition::evaluate() -> Object
 
   return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) {
     results.push_back(triggering_entity.apply([&](const auto & triggering_entity) {
-      return evaluate(global().entities, triggering_entity, direction);
+      return evaluate(global().entities, triggering_entity, direction, compatibility);
     }));
     return not results.back().size() or std::invoke(rule, results.back(), value).min();
   }));

diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py
@@ -87,6 +87,7 @@ def launch_setup(context, *args, **kwargs):
     scenario                            = LaunchConfiguration("scenario",                               default=Path("/dev/null"))
     sensor_model                        = LaunchConfiguration("sensor_model",                           default="")
     sigterm_timeout                     = LaunchConfiguration("sigterm_timeout",                        default=8)
+    speed_condition                     = LaunchConfiguration("speed_condition",                        default="legacy")
     use_sim_time                        = LaunchConfiguration("use_sim_time",                           default=False)
     vehicle_model                       = LaunchConfiguration("vehicle_model",                          default="")
     # fmt: on
@@ -111,6 +112,7 @@ def launch_setup(context, *args, **kwargs):
     print(f"scenario                            := {scenario.perform(context)}")
     print(f"sensor_model                        := {sensor_model.perform(context)}")
     print(f"sigterm_timeout                     := {sigterm_timeout.perform(context)}")
+    print(f"speed_condition                     := {speed_condition.perform(context)}")
     print(f"use_sim_time                        := {use_sim_time.perform(context)}")
     print(f"vehicle_model                       := {vehicle_model.perform(context)}")
 
@@ -135,6 +137,7 @@ def make_parameters():
             {"rviz_config": rviz_config},
             {"sensor_model": sensor_model},
             {"sigterm_timeout": sigterm_timeout},
+            {"speed_condition": speed_condition},
             {"use_sim_time": use_sim_time},
             {"vehicle_model": vehicle_model},
         ]
@@ -179,6 +182,7 @@ def collect_prefixed_parameters():
         DeclareLaunchArgument("scenario",                            default_value=scenario                           ),
         DeclareLaunchArgument("sensor_model",                        default_value=sensor_model                       ),
         DeclareLaunchArgument("sigterm_timeout",                     default_value=sigterm_timeout                    ),
+        DeclareLaunchArgument("speed_condition",                     default_value=speed_condition                      ),
         DeclareLaunchArgument("use_sim_time",                        default_value=use_sim_time                       ),
         DeclareLaunchArgument("vehicle_model",                       default_value=vehicle_model                      ),
         # fmt: on