Merge pull request #1423 from tier4/feature/by_object_type

Support `ByType` element in `CollisionCondition`

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/by_object_type.hpp

@@ -0,0 +1,69 @@
+// 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__SYNTAX__BY_OBJECT_TYPE_HPP_
+#define OPENSCENARIO_INTERPRETER__SYNTAX__BY_OBJECT_TYPE_HPP_
+
+#include <algorithm>
+#include <iterator>
+#include <openscenario_interpreter/object.hpp>
+#include <openscenario_interpreter/scope.hpp>
+#include <openscenario_interpreter/syntax/object_type.hpp>
+#include <openscenario_interpreter/syntax/string.hpp>
+#include <pugixml.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+#include <set>
+#include <type_traits>
+#include <valarray>
+#include <vector>
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+/*
+   OpenSCENARIO XML 1.3 ByObjectType
+
+   Defines an object type to select entities.
+
+   <xsd:complexType name="ByObjectType">
+     <xsd:attribute name="type" type="ObjectType" use="required"/>
+   </xsd:complexType>
+*/
+struct ByObjectType : public Scope
+{
+  ObjectType type;
+
+  explicit ByObjectType(const pugi::xml_node &, Scope &);
+
+  auto objects() const -> std::set<Entity>;
+
+  template <typename Function>
+  auto apply(const Function & function) const
+  {
+    using Result = std::invoke_result_t<Function, Entity>;
+    auto objects = this->objects();
+    if constexpr (std::is_same_v<Result, void>) {
+      std::for_each(std::begin(objects), std::end(objects), function);
+    } else {
+      auto results = std::valarray<Result>(objects.size());
+      std::transform(std::begin(objects), std::end(objects), std::begin(results), function);
+      return results;
+    }
+  }
+};
+}  // namespace syntax
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__SYNTAX__BY_OBJECT_TYPE_HPP_

openscenario/openscenario_interpreter/src/syntax/by_object_type.cpp

@@ -0,0 +1,44 @@
+// 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/syntax/by_object_type.hpp>
+#include <openscenario_interpreter/syntax/entities.hpp>
+#include <openscenario_interpreter/syntax/entity.hpp>
+#include <openscenario_interpreter/syntax/scenario_object.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace openscenario_interpreter
+{
+inline namespace syntax
+{
+ByObjectType::ByObjectType(const pugi::xml_node & node, Scope & scope)
+: Scope(scope), type(readAttribute<ObjectType>("type", node, scope))
+{
+  if (type.value == ObjectType::external) {
+    THROW_SEMANTIC_ERROR("ObjectType::external does not support yet");
+  }
+}
+
+auto ByObjectType::objects() const -> std::set<Entity>
+{
+  std::set<Entity> result;
+  for (const auto & [name, object] : *global().entities) {
+    if (object.is<ScenarioObject>() and object.as<ScenarioObject>().objectType() == type) {
+      result.emplace(name, *this);
+    }
+  }
+  return result;
+}
+}  // namespace syntax
+}  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/src/syntax/collision_condition.cpp

@@ -14,6 +14,7 @@
 
 #include <openscenario_interpreter/reader/element.hpp>
 #include <openscenario_interpreter/simulator_core.hpp>
+#include <openscenario_interpreter/syntax/by_object_type.hpp>
 #include <openscenario_interpreter/syntax/collision_condition.hpp>
 #include <openscenario_interpreter/syntax/entities.hpp>
 #include <openscenario_interpreter/syntax/entity.hpp>
@@ -30,7 +31,7 @@ CollisionCondition::CollisionCondition(
   another_given_entity(
     choice(node,
       std::make_pair("EntityRef", [&](auto && node) { return make<Entity>(node, scope); }),
-      std::make_pair("ByType",    [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }))),
+      std::make_pair("ByType",    [&](auto && node) { return make<ByObjectType>(node, scope); }))),
   triggering_entities(triggering_entities)
 // clang-format on
 {
@@ -40,11 +41,14 @@ auto CollisionCondition::description() const -> std::string
 {
   std::stringstream description;
 
-  description << triggering_entities.description() << " colliding with another given entity "
-              << another_given_entity << "?";
-
-  // TODO (yamacir-kit): If another_given_entity.is<ByType>(), description
-  // will be "Is any of [A, B, C] colliding with another T typed entities?"
+  if (another_given_entity.is<Entity>()) {
+    description << "Is any of " << triggering_entities.description()
+                << " colliding with another given entity " << another_given_entity.as<Entity>()
+                << "?";
+  } else {
+    description << "Is any of " << triggering_entities.description() << " colliding with another "
+                << another_given_entity.as<ByObjectType>().type << " typed entities?";
+  }
 
   return description.str();
 }
@@ -60,8 +64,18 @@ auto CollisionCondition::evaluate() const -> Object
       });
       return not evaluation.size() or evaluation.min();
     }));
+  } else if (another_given_entity.is<ByObjectType>()) {
+    return asBoolean(triggering_entities.apply([&](auto && triggering_entity) {
+      auto evaluation = triggering_entity.apply([&](const auto & object) {
+        auto evaluation =
+          another_given_entity.as<ByObjectType>().apply([&](const auto & another_entity) {
+            return evaluateCollisionCondition(object, another_entity);
+          });
+        return evaluation.size() and evaluation.max();
+      });
+      return not evaluation.size() or evaluation.min();
+    }));
   } else {
-    // TODO ByType
     return false_v;
   }
 }

test_runner/scenario_test_runner/config/workflow.txt

@@ -23,3 +23,4 @@ $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/LaneChangeAction
 $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/SpeedAction_SpeedCondition_AccelerationCondition.yaml
 $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/TeleportAction_CollisionCondition_StandStillCondition.yaml
 $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/TimeHeadwayCondition.yaml
+$(find-pkg-share scenario_test_runner)/scenario/collision_condition_by_type.yaml