Merge pull request tier4#1059 from tier4/feature/interpreter/entity_selection

Add `EntitySelection`

Author: yamacir-kit

openscenario/openscenario_interpreter/include/openscenario_interpreter/functional/equal_to.hpp

@@ -19,6 +19,7 @@
 #include <functional>
 #include <limits>
 #include <type_traits>
+#include <valarray>
 
 namespace openscenario_interpreter
 {
@@ -37,6 +38,26 @@ struct equal_to<T, typename std::enable_if<std::is_floating_point<T>::value>::ty
     return std::abs(lhs - rhs) < std::numeric_limits<typename std::decay<T>::type>::epsilon();
   }
 };
+
+template <typename T>
+struct equal_to<std::valarray<T>, typename std::enable_if<std::is_floating_point<T>::value>::type>
+{
+  constexpr auto operator()(
+    const std::valarray<T> & lhs, const std::valarray<T> & rhs) const noexcept
+  {
+    return std::abs(lhs - rhs) < std::numeric_limits<typename std::decay<T>::type>::epsilon();
+  }
+
+  constexpr auto operator()(const std::valarray<T> & lhs, const T & rhs) const noexcept
+  {
+    return std::abs(lhs - rhs) < std::numeric_limits<typename std::decay<T>::type>::epsilon();
+  }
+
+  constexpr auto operator()(const T & lhs, const std::valarray<T> & rhs) const noexcept
+  {
+    return std::abs(lhs - rhs) < std::numeric_limits<typename std::decay<T>::type>::epsilon();
+  }
+};
 }  // namespace syntax
 }  // namespace openscenario_interpreter
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp

@@ -22,7 +22,7 @@
 #include <memory>
 #include <openscenario_interpreter/name.hpp>
 #include <openscenario_interpreter/syntax/catalog_locations.hpp>
-#include <openscenario_interpreter/syntax/entity_ref.hpp>
+#include <openscenario_interpreter/syntax/entity.hpp>
 #include <openscenario_interpreter/utility/demangle.hpp>
 #include <unordered_map>
 #include <utility>
@@ -190,7 +190,7 @@ class Scope
 public:
   const std::string name;
 
-  std::list<EntityRef> actors;
+  std::list<Entity> actors;
 
   double seed;  // NOTE: `seed` is used only for sharing randomSeed in Stochastic now
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/acceleration_condition.hpp

@@ -21,6 +21,7 @@
 #include <openscenario_interpreter/syntax/rule.hpp>
 #include <openscenario_interpreter/syntax/triggering_entities.hpp>
 #include <pugixml.hpp>
+#include <valarray>
 
 namespace openscenario_interpreter
 {
@@ -42,7 +43,7 @@ struct AccelerationCondition : private SimulatorCore::ConditionEvaluation
 
   const TriggeringEntities triggering_entities;
 
-  std::vector<double> results;  // for description
+  std::vector<std::valarray<double>> results;  // for description
 
   explicit AccelerationCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/by_type.hpp

@@ -15,7 +15,9 @@
 #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__BY_TYPE_HPP_
 #define OPENSCENARIO_INTERPRETER__SYNTAX__BY_TYPE_HPP_
 
+#include <openscenario_interpreter/scope.hpp>
 #include <openscenario_interpreter/syntax/object_type.hpp>
+#include <pugixml.hpp>
 
 namespace openscenario_interpreter
 {
@@ -28,8 +30,9 @@ inline namespace syntax
  *  </xsd:complexType>
  *
  * -------------------------------------------------------------------------- */
-struct ByType
+struct ByType : public ObjectType
 {
+  explicit ByType(const pugi::xml_node &, Scope &);
 };
 }  // namespace syntax
 }  // namespace openscenario_interpreter

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp

@@ -26,6 +26,7 @@
 #include <openscenario_interpreter/syntax/rule.hpp>
 #include <openscenario_interpreter/syntax/triggering_entities.hpp>
 #include <pugixml.hpp>
+#include <valarray>
 
 namespace openscenario_interpreter
 {
@@ -105,7 +106,7 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua
 
   const TriggeringEntities triggering_entities;
 
-  std::vector<Double> results;  // for description
+  std::vector<std::valarray<double>> results;  // for description
 
   const bool consider_z;
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp

@@ -16,6 +16,7 @@
 #define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITIES_HPP_
 
 #include <openscenario_interpreter/scope.hpp>
+#include <openscenario_interpreter/syntax/entity.hpp>
 #include <openscenario_interpreter/syntax/entity_ref.hpp>
 #include <pugixml.hpp>
 
@@ -37,7 +38,7 @@ struct Entities : public std::unordered_map<std::string, Object>  // TODO to be
 {
   explicit Entities(const pugi::xml_node &, Scope &);
 
-  auto isAdded(const EntityRef &) const -> bool;
+  auto isAdded(const Entity &) const -> bool;
 
   auto ref(const EntityRef &) const -> Object;
 };

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity.hpp

@@ -0,0 +1,101 @@
+// 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__ENTITY_HPP_
+#define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_HPP_
+
+#include <algorithm>
+#include <cstddef>
+#include <functional>
+#include <iterator>
+#include <openscenario_interpreter/object.hpp>
+#include <openscenario_interpreter/syntax/entity_ref.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
+{
+struct Scope;
+
+inline namespace syntax
+{
+struct Entities;
+
+struct ScenarioObject;
+
+struct EntitySelection;
+
+struct Entity : public Object
+{
+  Entity() = default;
+
+  Entity(const ScenarioObject &);
+
+  Entity(const EntitySelection &);
+
+  Entity(const String &, const Scope &);
+
+  Entity(const pugi::xml_node &, const Scope &);
+
+  auto name() const -> String;
+
+  auto objects() const -> std::set<Entity>;
+
+  auto objectTypes() const -> std::set<ObjectType::value_type>;
+
+  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;
+    }
+  }
+
+  /**
+   * This function is for ScenarioObject only.
+   * @throws std::runtime_error if the entity is not a ScenarioObject.
+   * @note To iterate over all objects, use `apply` function instead.
+   * @note To get the name of the entity, use `name` function instead.
+   */
+  operator EntityRef() const;
+};
+
+auto operator==(const Entity &, const Entity &) -> bool;
+
+auto operator<<(std::ostream &, const Entity &) -> std::ostream &;
+}  // namespace syntax
+}  // namespace openscenario_interpreter
+
+template <>
+struct std::hash<openscenario_interpreter::Entity>
+{
+  auto operator()(const openscenario_interpreter::Entity & entity) const -> std::size_t
+  {
+    return std::hash<void *>{}(entity.get());
+  }
+};
+
+#endif  // OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_HPP_

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_action.hpp

@@ -18,6 +18,7 @@
 #include <openscenario_interpreter/scope.hpp>
 #include <openscenario_interpreter/syntax/add_entity_action.hpp>
 #include <openscenario_interpreter/syntax/delete_entity_action.hpp>
+#include <openscenario_interpreter/syntax/entity.hpp>
 #include <openscenario_interpreter/syntax/string.hpp>
 #include <pugixml.hpp>
 
@@ -38,7 +39,7 @@ inline namespace syntax
  * -------------------------------------------------------------------------- */
 struct EntityAction : public ComplexType
 {
-  const String entity_ref;
+  const Entity entity_ref;
 
   explicit EntityAction(const pugi::xml_node &, Scope &);
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_object.hpp

@@ -16,6 +16,7 @@
 #define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_OBJECT_HPP_
 
 #include <openscenario_interpreter/syntax/misc_object.hpp>
+#include <openscenario_interpreter/syntax/object_type.hpp>
 #include <openscenario_interpreter/syntax/pedestrian.hpp>
 #include <openscenario_interpreter/syntax/vehicle.hpp>
 
@@ -38,8 +39,18 @@ inline namespace syntax
 struct EntityObject : public Group
 {
   explicit EntityObject(const pugi::xml_node &, Scope &);
+
+  auto objectType() const -> ObjectType::value_type;
 };
 
+DEFINE_LAZY_VISITOR(
+  const EntityObject,
+  // CASE(CatalogReference),  //
+  CASE(Vehicle),     //
+  CASE(Pedestrian),  //
+  CASE(MiscObject),  //
+);
+
 DEFINE_LAZY_VISITOR(
   EntityObject,
   // CASE(CatalogReference),  //

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_ref.hpp

@@ -33,10 +33,7 @@ inline namespace syntax
 */
 struct EntityRef : public String
 {
-  template <typename... Ts>
-  EntityRef(Ts &&... xs) : String(std::forward<decltype(xs)>(xs)...)
-  {
-  }
+  EntityRef(const String & string) : String(string) {}
 
   template <typename Node, typename Scope>
   explicit EntityRef(const Node & node, Scope & scope)