Merge pull request #1486 from tier4/fix/speed-condition/backward-comp…

…atibility

Fix/speed condition/backward compatibility

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

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)

openscenario/openscenario_interpreter/include/openscenario_interpreter/compatibility.hpp

@@ -0,0 +1,30 @@
+// Copyright 2015 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.
+
+#ifndef OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_
+#define OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_
+
+#include <iostream>
+
+namespace openscenario_interpreter
+{
+enum class Compatibility {
+  legacy,
+  standard,
+};
+
+auto operator>>(std::istream &, Compatibility &) -> std::istream &;
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__COMPATIBILITY_HPP_

openscenario/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;
 };

openscenario/openscenario_interpreter/src/compatibility.cpp

@@ -0,0 +1,38 @@
+// Copyright 2015 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 <openscenario_interpreter/compatibility.hpp>
+
+namespace openscenario_interpreter
+{
+auto operator>>(std::istream & input, Compatibility & compatiblity) -> std::istream &
+{
+  if (auto token = std::string(); input >> token) {
+    if (token == "legacy") {
+      compatiblity = Compatibility::legacy;
+      return input;
+    } else if (token == "standard") {
+      compatiblity = Compatibility::standard;
+      return input;
+    } else {
+      throw Error(
+        "Unknown compatiblity ", std::quoted(token),
+        " was specified. It must be \"legacy\" or \"standard\".");
+    }
+  } else {
+    compatiblity = Compatibility::legacy;
+    return input;
+  }
+}
+}  // namespace openscenario_interpreter

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

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

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},
         ]
@@ -173,12 +176,13 @@ def collect_prefixed_parameters():
         DeclareLaunchArgument("global_timeout",                      default_value=global_timeout                     ),
         DeclareLaunchArgument("launch_autoware",                     default_value=launch_autoware                    ),
         DeclareLaunchArgument("launch_rviz",                         default_value=launch_rviz                        ),
-        DeclareLaunchArgument("publish_empty_context",               default_value=publish_empty_context              ),
         DeclareLaunchArgument("output_directory",                    default_value=output_directory                   ),
+        DeclareLaunchArgument("publish_empty_context",               default_value=publish_empty_context              ),
         DeclareLaunchArgument("rviz_config",                         default_value=rviz_config                        ),
         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