Merge branch 'master' of...

Merge branch 'master' of https://gitlab.eclipse.org/eclipse/simopenpass/scenario_api into modernize-cmake-build-with-cpm

MantleAPI/BUILD.bazel

@@ -13,7 +13,7 @@ cc_library(
     hdrs = glob(["include/**/*.h"]),
     includes = ["include"],
     visibility = ["//visibility:public"],
-    deps = ["@units"],
+    deps = ["@units_nhh"],
 )
 
 cc_library(

MantleAPI/include/MantleAPI/Common/i_geometry_helper.h

@@ -17,6 +17,8 @@
 
 #include <MantleAPI/Common/pose.h>
 
+#include <vector>
+
 namespace mantle_api
 {
 /// Interface that provides functionality to perform geometrical calculations
@@ -36,6 +38,26 @@ public:
       const Vec3<units::length::meter_t>& global_position,
       const Orientation3<units::angle::radian_t>& local_orientation,
       const Vec3<units::length::meter_t>& local_translation) const = 0;
+
+  /// @brief Transforms world polyline positions to local coordinate system.
+  /// @param polyline_points world polyline points to be transformed
+  /// @param local_origin  local coordinate system origin
+  /// @param local_orientation local system orientation
+  /// @return converted polyline points
+  virtual std::vector<Vec3<units::length::meter_t>> TransformPolylinePointsFromWorldToLocal(
+      const std::vector<Vec3<units::length::meter_t>>& polyline_points,
+      const Vec3<units::length::meter_t>& local_origin,
+      const Orientation3<units::angle::radian_t>& local_orientation) const = 0;
+
+  /// @brief Transforms world position to local coordinate system.
+  /// @param world_position world position to be transformed
+  /// @param local_origin  local coordinate system origin
+  /// @param local_orientation local system orientation
+  /// @return transformed point
+  virtual Vec3<units::length::meter_t> TransformPositionFromWorldToLocal(
+      const Vec3<units::length::meter_t>& world_position,
+      const Vec3<units::length::meter_t>& local_origin,
+      const Orientation3<units::angle::radian_t>& local_orientation) const = 0;
 };
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Common/i_identifiable.h

@@ -17,6 +17,7 @@
 
 #include <cstdint>
 #include <string>
+#include <limits>
 
 namespace mantle_api
 {

MantleAPI/include/MantleAPI/Execution/i_environment.h

@@ -22,9 +22,10 @@
 #include <MantleAPI/Map/i_coord_converter.h>
 #include <MantleAPI/Map/i_lane_location_query_service.h>
 #include <MantleAPI/Map/map_details.h>
-#include <MantleAPI/Traffic/i_entity_repository.h>
 #include <MantleAPI/Traffic/i_controller_repository.h>
+#include <MantleAPI/Traffic/i_entity_repository.h>
 
+#include <optional>
 #include <string>
 
 namespace mantle_api
@@ -81,12 +82,24 @@ public:
   virtual void SetRoadCondition(std::vector<FrictionPatch> friction_patches) = 0;
   virtual void SetTrafficSignalState(const std::string& traffic_signal_name, const std::string& traffic_signal_state) = 0;
 
-  // Execute a command that is specific for an environment implementation
+  /// @brief Execute a command that is specific for an environment implementation
   ///
   /// @param actors    the actors (if any) for which a command is executed
   /// @param type      type of the command
   /// @param command   custom payload
   virtual void ExecuteCustomCommand(const std::vector<std::string>& actors, const std::string& type, const std::string& command) = 0;
+
+  /// @brief Sets a named user defined value
+  ///
+  /// @param name  The name of the user defined value
+  /// @param value  The value
+  virtual void SetUserDefinedValue(const std::string& name, const std::string& value) = 0;
+
+  /// @brief Gets a named user defined value if it exists
+  ///
+  /// @param name  The name of the user defined value
+  /// @return The user defined value. No value if it doesn't exist.
+  virtual std::optional<std::string> GetUserDefinedValue(const std::string& name) = 0;
 };
 }  // namespace mantle_api
 

MantleAPI/include/MantleAPI/Map/i_coord_converter.h

@@ -29,9 +29,6 @@ public:
   virtual ~ICoordConverter() = default;
   /// Converts a track position to its corresponding inertial position.
   virtual Vec3<units::length::meter_t> Convert(Position position) const = 0;
-
-  /// Converts alane position to its corresponding inertial position.
-  virtual Position Convert(const Vec3<units::length::meter_t>& inert_pos) const = 0;
 };
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h

@@ -45,31 +45,103 @@ public:
   /// would add a rat-tail of more interfaces we need to define (ILaneLocation, ILane, ...?)
   // virtual const IIdentifiable& GetMapObjectById(UniqueId id) = 0;
 
+  /// @brief Tries to map a given position to a lane and then returns the orientation of the lane center line at the
+  /// given position
+  ///
+  /// @param position  Position that shall be mapped to a lane
+  /// @return Orientation of the lane at this position
+  /// @throw If the position cannot be mapped to a lane
   virtual Orientation3<units::angle::radian_t> GetLaneOrientation(
       const Vec3<units::length::meter_t>& position) const = 0;
+
+  /// @brief Shifts a position the given amount upwards along the lane normal. This function is used for shifting
+  /// entities upwards, such that their bounding boxes are touching with the bottom-plane the road surface. Thus,
+  /// entities do not "sink" into the road.
+  ///
+  /// @param position  Position on road surface
+  /// @param upwards_shift  Amount to shift position upwards along lane normal in [m]
+  /// @param allow_invalid_positions  If true and the position cannot be mapped to a lane, then an upwards vector in
+  ///                                 world z-direction is used instead of the lane normal for the shift
+  /// @return Upwards shifted position
+  /// @throw If the position cannot be mapped to a lane and allow_invalid_positions=false.
   virtual Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
                                                                      double upwards_shift,
                                                                      bool allow_invalid_positions = false) const = 0;
+
+  /// @brief Checks, if a given position can be mapped to a lane
+  ///
+  /// @param position  Position to be checked
+  /// @return true if the position can be mapped to a lane, false otherwise
   virtual bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const = 0;
+
+  /// @brief Returns a list of IDs representing all lanes enclosing the passed in position within their shape(s).
+  ///
+  /// @param position  Position to search for the Lane IDs
+  /// @return List of global lane IDs
   virtual std::vector<UniqueId> GetLaneIdsAtPosition(const Vec3<units::length::meter_t>& position) const = 0;
 
-  /// @brief Calculate the new pose which is at certain distance from the reference_pose_on_lane following a direction.
+  /// @brief Calculate the new pose which is at a certain longitudinal distance from the reference_pose_on_lane
+  ///        following a direction.
+  ///
+  /// @param reference_pose_on_lane  Starting position. Must be on a lane.
+  /// @param distance  Distance to go along the lane from the reference_pose_on_lane.
+  /// @param direction  Direction to go along the lane from the reference_pose_on_lane. The orientation of the reference
+  ///                   pose is used to determine the forward direction. "Forward" means along the (longitudinal) lane
+  ///                   direction that is closest to the orientation of the reference pose. The reference pose should
+  ///                   not be perpendicular to the lane.
+  /// @return Pose that is at the given distance from reference_pose_on_lane in the specified direction. The new
+  ///         orientation is parallel to the lane orientation at the target position. The lateral offset from the lane
+  ///         center line stays the same as at reference_pose_on_lane. If the reference_pose_on_lane cannot be mapped to
+  ///         a lane or the target position would be beyond the road network limits, no value is returned.
+  virtual std::optional<Pose> FindLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane,
+                                                         units::length::meter_t distance,
+                                                         Direction direction) const = 0;
+
+  /// @brief Calculate the longitudinal distance of two given positions on a lane.
+  ///
+  /// @param start_position  Starting position. Must be on a lane.
+  /// @param target_position Target position. Must be projectable to the lane center line of the start position or one of its' successors.
+  /// @return Longitudinal distance of the two positions along the lane center line. If there is more than one possible
+  ///         route through the road network to get from start to target the route strategy is currently up to the query
+  ///         service implementation.
+  ///         No value returned if the distance is not calculable.
+
+  virtual std::optional<units::length::meter_t> GetLongitudinalLaneDistanceBetweenPositions(
+      const mantle_api::Vec3<units::length::meter_t>& start_position,
+      const mantle_api::Vec3<units::length::meter_t>& target_position) const = 0;
+
+  /// @brief Calculate a new pose which is at a certain longitudinal distance in a relative lane from the
+  ///        reference_pose_on_lane.
   ///
-  ///@param reference_pose_on_lane  Starting position. Must be on a lane.
-  ///@param direction  Direction to go along the lane from the reference_pose_on_lane. The orientation of the reference pose is used to determine the forward direction. "Forward" means aligned with the positive x direction of the local coordinate system defined by the pose.
-  ///@return Pose that is at the given distance from reference_pose_on_lane in the specified direction. The new orientation is parallel to the lane orientation at the target position.
-  virtual std::optional<Pose> FindLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane, units::length::meter_t distance, Direction direction) const = 0;
+  /// @param reference_pose_on_lane  Starting position. Must be on a lane.
+  /// @param relative_target_lane  Shift of the target position in number of lanes relative to the lane where the
+  ///                              reference pose is located. Positive to the left, negative to the right.
+  /// @param distance  The longitudinal distance along the center line of the lane closest to the reference pose. The
+  ///                  orientation of the reference pose is used to determine the direction. A positive distance means
+  ///                  along the (longitudinal) lane direction that is closest to the orientation of the reference pose.
+  ///                  The reference pose should not be perpendicular to the lane.
+  /// @param lateral_offset  Lateral offset to the target lane of the new pose
+  /// @return Pose that is at the given distance from reference_pose_on_lane in a relative lane. The new orientation is
+  ///         parallel to the lane orientation at the target position. No value is returned, if the
+  ///         reference_pose_on_lane cannot be mapped to a lane or if the target position would be beyond the road
+  ///         network limits in longitudinal or lateral direction.
+  virtual std::optional<Pose> FindRelativeLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane,
+                                                                 int relative_target_lane,
+                                                                 units::length::meter_t distance,
+                                                                 units::length::meter_t lateral_offset) const = 0;
 
-  /// @brief Calculate a new pose which is at certain distance in a relative lane from the reference_pose_on_lane.
+  /// @brief Calculate the lane id of the relative target lane from a given position
   ///
-  ///@param reference_pose_on_lane  Starting position. Must be on a lane.
-  ///@param relative_target_lane  Shift of the target position in number of lanes relative to the lane where the reference pose is located. Positive to the left, negative to the right.
-  ///@param distance  The longitudinal distance along the centerline of the lane closest to the reference pose. The orientation of the reference pose is used to determine the direction. Positive distance means aligned with the positive x direction of the local coordinate system defined by the pose.
-  ///@param lateral_offset  Lateral offset to the target lane
-  ///@return Pose that is at the given distance from reference_pose_on_lane in a relative lane. The new orientation is parallel to the lane orientation at the target position.
-  virtual std::optional<Pose> FindRelativeLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane, int relative_target_lane, units::length::meter_t distance, units::length::meter_t lateral_offset) const = 0;
+  /// @param reference_pose_on_lane  Starting position. Must be on a lane.
+  /// @param relative_target_lane  Shift of the target position in number of lanes relative to the lane where the
+  ///                              reference pose is located. Positive to the left, negative to the right.
+  /// @return Lane id that is at the given lateral shift (relative_lane_target) from given position
+  ///         (reference_pose_on_lane). No value, if reference pose is not on a lane or if the lane doesn't have a
+  ///         suitable adjacent lane.
+  virtual std::optional<mantle_api::UniqueId> GetRelativeLaneId(const mantle_api::Pose& reference_pose_on_lane,
+                                                                int relative_lane_target) const = 0;
 };
 
 }  // namespace mantle_api
 
-#endif  // MANTLEAPI_MAP_ILANELOCATIONQUERYSERVICE_H
\ No newline at end of file
+#endif  // MANTLEAPI_MAP_ILANELOCATIONQUERYSERVICE_H

MantleAPI/include/MantleAPI/Traffic/control_strategy.h

 /*******************************************************************************
- * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2021-2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  *
  * This program and the accompanying materials are made
  * available under the terms of the Eclipse Public License 2.0
@@ -15,6 +15,7 @@
 #ifndef MANTLEAPI_TRAFFIC_CONTROLSTRATEGY_H
 #define MANTLEAPI_TRAFFIC_CONTROLSTRATEGY_H
 
+#include <MantleAPI/Common/i_identifiable.h>
 #include <MantleAPI/Common/spline.h>
 #include <MantleAPI/Common/trajectory.h>
 #include <MantleAPI/Common/vector.h>
@@ -44,7 +45,25 @@ enum class ControlStrategyType
   kFollowRoute,
   kAcquireLaneOffset,
   kFollowTrajectory,
-  kUpdateTrafficLightStates
+  kUpdateTrafficLightStates,
+  kPerformLaneChange
+};
+
+/// @brief Defines how a route should be calculated
+enum class RouteStrategy
+{
+  kUndefined = 0,
+  kFastest,
+  kLeastIntersections,
+  kShortest
+};
+
+/// @brief Groups a Waypoint with a RouteStrategy
+///        for the \ref FollowRouteControlStrategy
+struct RouteWaypoint
+{
+  mantle_api::Vec3<units::length::meter_t> waypoint{};
+  RouteStrategy route_strategy{RouteStrategy::kUndefined};
 };
 
 struct ControlStrategy
@@ -142,7 +161,7 @@ struct FollowRouteControlStrategy : public ControlStrategy
     type = ControlStrategyType::kFollowRoute;
   }
 
-  std::vector<mantle_api::Vec3<units::length::meter_t>> waypoints;
+  std::vector<RouteWaypoint> route_waypoints;
 };
 
 enum class Dimension
@@ -207,8 +226,8 @@ struct FollowTrajectoryControlStrategy : public ControlStrategy
 
   struct TrajectoryTimeReference
   {
-    ReferenceContext domainAbsoluteRelative;
-    double scale{0.0};
+    ReferenceContext domainAbsoluteRelative{ReferenceContext::kAbsolute};
+    double scale{1.0};
     units::time::second_t offset{0.0};
   };
 
@@ -222,6 +241,19 @@ struct FollowTrajectoryControlStrategy : public ControlStrategy
   std::optional<TrajectoryTimeReference> timeReference;
 };
 
+struct PerformLaneChangeControlStrategy : public ControlStrategy
+{
+  PerformLaneChangeControlStrategy()
+  {
+    movement_domain = MovementDomain::kLateral;
+    type = ControlStrategyType::kPerformLaneChange;
+  }
+
+  mantle_api::UniqueId target_lane_id{0};
+  units::length::meter_t target_lane_offset{0.0};
+  TransitionDynamics transition_dynamics{};
+};
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_TRAFFIC_CONTROLSTRATEGY_H

MantleAPI/include/MantleAPI/Traffic/entity_properties.h

 /*******************************************************************************
- * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2021-2022, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
  *
  * This program and the accompanying materials are made
  * available under the terms of the Eclipse Public License 2.0
@@ -47,6 +47,7 @@ struct EntityProperties
   EntityType type{EntityType::kOther};
   std::string model{};
   std::map<std::string, std::string> properties{};
+  units::mass::kilogram_t mass{};
 };
 
 inline bool operator==(const EntityProperties& lhs, const EntityProperties& rhs) noexcept
@@ -176,6 +177,10 @@ struct PedestrianProperties : public EntityProperties
 
 struct StaticObjectProperties : public EntityProperties
 {
+  //Amount to shift position along lane normal.
+  //It allows static objects like traffic signs to be placed at certain amount above the road.
+  //It is considered when the position of the entity is set.
+  units::length::meter_t vertical_offset{0.0};
 };
 
 }  // namespace mantle_api

MantleAPI/test/MantleAPI/Test/test_utils.h

@@ -34,28 +34,35 @@ namespace mantle_api
 class MockGeometryHelper : public mantle_api::IGeometryHelper
 {
 public:
-  virtual Vec3<units::length::meter_t> TranslateGlobalPositionLocally(
-      const Vec3<units::length::meter_t>& global_position,
-      const Orientation3<units::angle::radian_t>& local_orientation,
-      const Vec3<units::length::meter_t>& local_translation) const
+  MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, TranslateGlobalPositionLocally, (const Vec3<units::length::meter_t>& global_position, const Orientation3<units::angle::radian_t>& local_orientation, const Vec3<units::length::meter_t>& local_translation), (const override));
+
+  virtual std::vector<Vec3<units::length::meter_t>> TransformPolylinePointsFromWorldToLocal(
+      const std::vector<Vec3<units::length::meter_t>>& polyline_points,
+      const Vec3<units::length::meter_t>& position,
+      const Orientation3<units::angle::radian_t>& orientation) const
+  {
+    // do not transform but return original points
+    std::ignore = position;
+    std::ignore = orientation;
+    return polyline_points;
+  }
+
+  virtual Vec3<units::length::meter_t> TransformPositionFromWorldToLocal(
+      const Vec3<units::length::meter_t>& point_position,
+      const Vec3<units::length::meter_t>& position,
+      const Orientation3<units::angle::radian_t>& orientation) const
   {
-    // do not translate but return original position
-    std::ignore = local_orientation;
-    std::ignore = local_translation;
-    return global_position;
-  };
+    // do not transform but return original points
+    std::ignore = position;
+    std::ignore = orientation;
+    return point_position;
+  }
 };
 
 class MockConverter : public mantle_api::ICoordConverter
 {
 public:
   MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, Convert, (mantle_api::Position position), (const override));
-
-  mantle_api::Position Convert(const mantle_api::Vec3<units::length::meter_t>& vec) const override
-  {
-    std::ignore = vec;
-    return mantle_api::Position{};
-  }
 };
 
 class MockVehicle : public mantle_api::IVehicle
@@ -94,8 +101,14 @@ public:
   MOCK_METHOD(EntityVisibilityConfig, GetVisibility, (), (const, override));
 
   void SetProperties(std::unique_ptr<mantle_api::EntityProperties> properties) override { properties_ = std::move(properties); }
+
+  MOCK_METHOD(mantle_api::VehicleProperties*, GetPropertiesImpl, (), (const));
   mantle_api::VehicleProperties* GetProperties() const override
   {
+    if (auto* properties = GetPropertiesImpl())
+    {
+      return properties;
+    }
     return static_cast<mantle_api::VehicleProperties*>(properties_.get());
   }
 
@@ -112,15 +125,7 @@ class MockQueryService : public mantle_api::ILaneLocationQueryService
 public:
   MOCK_METHOD(Orientation3<units::angle::radian_t>, GetLaneOrientation, (const Vec3<units::length::meter_t>& position), (const override));
 
-  Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
-                                                             double upwards_shift,
-                                                             bool allowed_to_leave_lane = false) const override
-  {
-    std::ignore = position;
-    std::ignore = upwards_shift;
-    std::ignore = allowed_to_leave_lane;
-    return mantle_api::Vec3<units::length::meter_t>();
-  }
+  MOCK_METHOD(Vec3<units::length::meter_t>, GetUpwardsShiftedLanePosition, (const Vec3<units::length::meter_t>& position, double upwards_shift, bool allowed_to_leave_lane), (const override));
   bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const override
   {
     std::ignore = position;
@@ -133,14 +138,8 @@ public:
     return {};
   }
 
-  std::optional<Pose> FindLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane, units::length::meter_t distance, Direction direction) const override
-  {
-    std::ignore = reference_pose_on_lane;
-    std::ignore = distance;
-    std::ignore = direction;
-    Pose pose{};
-    return pose;
-  }
+  MOCK_METHOD(std::optional<Pose>, FindLanePoseAtDistanceFrom, (const Pose&, units::length::meter_t, Direction), (const, override));
+
   std::optional<Pose> FindRelativeLanePoseAtDistanceFrom(const Pose& reference_pose_on_lane, int relative_target_lane, units::length::meter_t distance, units::length::meter_t lateral_offset) const override
   {
     std::ignore = reference_pose_on_lane;
@@ -151,6 +150,18 @@ public:
     return pose;
   }
 
+  std::optional<UniqueId> GetRelativeLaneId(const mantle_api::Pose& reference_pose_on_lane, int relative_lane_target) const override
+  {
+    std::ignore = reference_pose_on_lane;
+    std::ignore = relative_lane_target;
+    return 0;
+  }
+
+  MOCK_METHOD(std::optional<units::length::meter_t>,
+              GetLongitudinalLaneDistanceBetweenPositions,
+              (const mantle_api::Vec3<units::length::meter_t>&, const mantle_api::Vec3<units::length::meter_t>&),
+              (const, override));
+
 private:
   MockVehicle test_vehicle_{};
 };
@@ -237,8 +248,14 @@ public:
   MOCK_METHOD(EntityVisibilityConfig, GetVisibility, (), (const, override));
 
   void SetProperties(std::unique_ptr<mantle_api::EntityProperties> properties) override { properties_ = std::move(properties); }
+  MOCK_METHOD(mantle_api::StaticObjectProperties*, GetPropertiesImpl, (), (const));
+
   mantle_api::StaticObjectProperties* GetProperties() const override
   {
+    if (auto* properties = GetPropertiesImpl())
+    {
+      return properties;
+    }
     return static_cast<mantle_api::StaticObjectProperties*>(properties_.get());
   }
 
@@ -295,9 +312,17 @@ public:
               (const std::string& name, const mantle_api::StaticObjectProperties& properties),
               ());
 
+  MOCK_METHOD(mantle_api::IEntity*,
+              GetImpl,
+              (const std::string& name),
+              ());
+
   std::optional<std::reference_wrapper<IEntity>> Get(const std::string& name) override
   {
-    std::ignore = name;
+    if (auto* entity = GetImpl(name))
+    {
+      return *entity;
+    }
     return test_vehicle_;
   }
 
@@ -405,6 +430,11 @@ public:
               (std::uint64_t entity_id, mantle_api::ControlStrategyType type),
               (const, override));
 
+  MOCK_METHOD(void,
+              SetWeather,
+              (mantle_api::Weather weather),
+              (override));
+
   const mantle_api::ILaneLocationQueryService& GetQueryService() const override { return query_service_; }
 
   const mantle_api::ICoordConverter* GetConverter() const override { return &converter_; }
@@ -415,8 +445,6 @@ public:
 
   MockControllerRepository& GetControllerRepository() override { return controller_repository_; }
 
-  void SetWeather(mantle_api::Weather weather) override { std::ignore = weather; }
-
   void SetRoadCondition(std::vector<mantle_api::FrictionPatch> friction_patches) override
   {
     std::ignore = friction_patches;
@@ -432,6 +460,16 @@ public:
               (const std::vector<std::string>& actors, const std::string& type, const std::string& command),
               (override));
 
+  MOCK_METHOD(void,
+              SetUserDefinedValue,
+              (const std::string& name, const std::string& value),
+              (override));
+
+  MOCK_METHOD(std::optional<std::string>,
+              GetUserDefinedValue,
+              (const std::string& name),
+              (override));
+
   void SetDateTime(mantle_api::Time date_time) override { std::ignore = date_time; }
 
   mantle_api::Time GetDateTime() override { return mantle_api::Time(); }