- rename 'SimulationTime' typedef to 'Time' and rename simulation_time.h to time_utils.h

- add new SimulationTime struct containing the current_sim_time and the last_delta_time
- Add 'SimulationTime GetSimulationTime()' to IEnvironment for fixing SimulationTimeCondition
- Add 'HasControlStrategyGoalBeenReached' to IEnvironment to check with the controller, if an action can be set to 'completed' state
- Add ControlStrategyType to avoid dynamic_cast
- followed C++ guideline for enum values with leading k...
- cleaned up deprecated controller configs

Signed-off-by: Arun Das <arun.das@bmw.de>

MantleAPI/include/MantleAPI/Common/simulation_time.h

@@ -15,30 +15,16 @@
 #ifndef MANTLEAPI_COMMON_SIMULATION_TIME_H
 #define MANTLEAPI_COMMON_SIMULATION_TIME_H
 
-#include <chrono>
+#include "time_utils.h"
 
 namespace mantle_api
 {
-using SimulationTime = std::chrono::duration<std::int64_t, std::milli>;
 
-/// @brief Converts input in [s] to @ref SimulationTime.
-/// @tparam T Input type, eg. `double`.
-/// @param duration Input value
-/// @return Duration representing the given input in units of @ref SimulationTime.
-template <typename T>
-inline SimulationTime SecondsToSimulationTime(T duration)
+struct SimulationTime
 {
-    return std::chrono::duration_cast<SimulationTime>(std::chrono::duration<T>{duration});
-}
+    mantle_api::Time current_sim_time_ms{0};
+    mantle_api::Time last_delta_time_ms{0};
+};
 
-/// @brief Converts input @ref SimulationTime to [s].
-/// @param time Simulation time
-/// @return Duration ins seconds representing the passed in @ref SimulationTime.
-inline double SimulationTimeToSeconds(const SimulationTime& time)
-{
-    return static_cast<double>(time.count()) / 1000.0;
-}
-  
 }  // namespace mantle_api
-
 #endif  // MANTLEAPI_COMMON_SIMULATION_TIME_H

MantleAPI/include/MantleAPI/Common/spline.h

@@ -16,7 +16,7 @@
 #define MANTLEAPI_COMMON_SPLINE_H
 
 #include <MantleAPI/Common/floating_point_helper.h>
-#include <MantleAPI/Common/simulation_time.h>
+#include <MantleAPI/Common/time_utils.h>
 
 #include <array>
 
@@ -24,15 +24,15 @@ namespace mantle_api
 {
 struct SplineSection
 {
-    mantle_api::SimulationTime start_time{0};
-    mantle_api::SimulationTime end_time{0};
+    mantle_api::Time start_time{0};
+    mantle_api::Time end_time{0};
     /// @brief Represents the polynomial.
     ///
     /// The array stores in format \f$[a_3, a_2, a_1, a_0]\f$ for a polynomial in form
     /// \f[
     ///   P(x) = \sum_{i=0}^{3} a_{i} x^{i} = a_3 x^3 + a_2 x^2 + a_1 x + a_0
     /// \f]
-    std::array<double, 4> polynomial;
+    std::array<double, 4> polynomial{0, 0, 0, 0};
 };
 
 /// @brief Equality comparison for SplineSection.

MantleAPI/include/MantleAPI/Common/time_utils.h

+/*******************************************************************************
+ * Copyright (c) 2021, 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
+ * which is available at https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/** @file  time_utils.h */
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_COMMON_TIME_UTILS_H
+#define MANTLEAPI_COMMON_TIME_UTILS_H
+
+#include <chrono>
+
+namespace mantle_api
+{
+using Time = std::chrono::duration<std::int64_t, std::milli>;
+
+/// @brief Converts input in [s] to @ref Time.
+/// @tparam T Input type, eg. `double`.
+/// @param duration Input value
+/// @return Duration representing the given input in units of @ref Time.
+template <typename T>
+inline Time SecondsToTime(T duration)
+{
+    return std::chrono::duration_cast<Time>(std::chrono::duration<T>{duration});
+}
+
+/// @brief Converts input @ref Time to [s].
+/// @param time Time
+/// @return Duration in seconds representing the passed in @ref Time.
+inline double TimeToSeconds(const Time& time)
+{
+    return static_cast<double>(time.count()) / 1000.0;
+}
+
+}  // namespace mantle_api
+
+#endif  // MANTLEAPI_COMMON_TIME_UTILS_H

MantleAPI/include/MantleAPI/Common/vector.h

@@ -31,7 +31,10 @@ struct Vec3
     T x{};
     T y{};
     T z{};
+
+    inline T Length() const { return sqrt((x * x) + (y * y) + (z * z)); }
 };
+
 using Vec3d = Vec3<double>;
 
 inline bool operator==(const Vec3d& lhs, const Vec3d& rhs) noexcept

MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h

@@ -15,16 +15,17 @@
 #ifndef MANTLEAPI_ENVIRONMENTALCONDITIONS_DATETIME_H
 #define MANTLEAPI_ENVIRONMENTALCONDITIONS_DATETIME_H
 
-#include <MantleAPI/Common/simulation_time.h>
+#include <MantleAPI/Common/time_utils.h>
 
 #include <chrono>
 
 namespace mantle_api
 {
 
-struct DateTime
+// TODO: Delete this struct and use Time directly in Get/SetDateTime once the move to the MantleAPI is complete
+struct [[deprecated]] DateTime
 {
-    SimulationTime date_time_ms{0};
+    Time date_time_ms{0};
 };
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Execution/i_environment.h

@@ -15,6 +15,7 @@
 #ifndef MANTLEAPI_EXECUTION_IENVIRONMENT_H
 #define MANTLEAPI_EXECUTION_IENVIRONMENT_H
 
+#include <MantleAPI/Common/simulation_time.h>
 #include <MantleAPI/EnvironmentalConditions/date_time.h>
 #include <MantleAPI/EnvironmentalConditions/road_condition.h>
 #include <MantleAPI/EnvironmentalConditions/weather.h>
@@ -32,9 +33,6 @@ class IEnvironment
   public:
     virtual ~IEnvironment() = default;
 
-    virtual void Init() = 0;
-    virtual void Step() = 0;
-
     /// Load a map file and parse it into the memory.
     ///
     /// @param file_path raw map file path from the scenario file. Resolving to a valid file path is done in the
@@ -64,6 +62,13 @@ class IEnvironment
         std::uint64_t controller_id,
         std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
 
+    /// Checks, if a control strategy of a certain type in a specific controller is fulfilled
+    ///
+    /// @param controller_id    The controller to check
+    /// @param type             The control strategy type
+    virtual bool HasControlStrategyGoalBeenReached(std::uint64_t controller_id,
+                                                   mantle_api::ControlStrategyType type) const = 0;
+
     virtual const ILaneLocationQueryService& GetQueryService() const = 0;
     virtual const ICoordConverter* GetConverter() const = 0;
 
@@ -74,6 +79,9 @@ class IEnvironment
     virtual void SetDateTime(DateTime date_time) = 0;
     virtual DateTime GetDateTime() = 0;
 
+    /// @brief Time since start of simulation and delta time to previous step
+    virtual SimulationTime GetSimulationTime() = 0;
+
     virtual void SetWeather(Weather weather) = 0;
     virtual void SetRoadCondition(std::vector<FrictionPatch> friction_patches) = 0;
 };

MantleAPI/include/MantleAPI/Execution/scenario_info.h

@@ -15,7 +15,7 @@
 #ifndef MANTLEAPI_EXECUTION_SCENARIOINFO_H
 #define MANTLEAPI_EXECUTION_SCENARIOINFO_H
 
-#include <MantleAPI/Common/simulation_time.h>
+#include <MantleAPI/Common/time_utils.h>
 
 #include <map>
 #include <string>
@@ -25,7 +25,7 @@ namespace mantle_api
 
 struct ScenarioInfo
 {
-    SimulationTime scenario_timeout_duration;
+    Time scenario_timeout_duration;
     std::string description;
     std::map<std::string, std::string> additional_information;
 };

MantleAPI/include/MantleAPI/Traffic/control_strategy.h

@@ -25,10 +25,22 @@ namespace mantle_api
 
 enum class MovementDomain
 {
-    undefined = 0,
-    lateral,
-    longitudinal,
-    both
+    kUndefined = 0,
+    kLateral,
+    kLongitudinal,
+    kBoth
+};
+
+enum class ControlStrategyType
+{
+    kUndefined = 0,
+    kKeepVelocity,
+    kKeepLaneOffset,
+    kFollowHeadingSpline,
+    kFollowLateralOffsetSpline,
+    kFollowVelocitySpline,
+    kFollowRoute,
+    kAcquireLaneOffset
 };
 
 struct ControlStrategy
@@ -37,12 +49,13 @@ struct ControlStrategy
 
     // TODO: extend by bool use_dynamic_constraints when needed (false assumed at the moment)
 
-    MovementDomain movement_domain{MovementDomain::undefined};
+    MovementDomain movement_domain{MovementDomain::kUndefined};
+    ControlStrategyType type{ControlStrategyType::kUndefined};
 };
 
 inline bool operator==(const ControlStrategy& lhs, const ControlStrategy& rhs) noexcept
 {
-    return lhs.movement_domain == rhs.movement_domain;
+    return lhs.movement_domain == rhs.movement_domain && lhs.type == rhs.type;
 }
 
 inline bool operator!=(const ControlStrategy& lhs, const ControlStrategy& rhs) noexcept
@@ -52,74 +65,119 @@ inline bool operator!=(const ControlStrategy& lhs, const ControlStrategy& rhs) n
 
 struct KeepVelocityControlStrategy : public ControlStrategy
 {
-    KeepVelocityControlStrategy() { movement_domain = MovementDomain::longitudinal; }
-    // Doesn`t need configuration attributes. Controller keeps current velocity on adding entity or update
+    KeepVelocityControlStrategy()
+    {
+        movement_domain = MovementDomain::kLongitudinal;
+        type = ControlStrategyType::kKeepVelocity;
+    }
+    // Doesn't need configuration attributes. Controller keeps current velocity on adding entity or update
 };
 
 struct KeepLaneOffsetControlStrategy : public ControlStrategy
 {
-    KeepLaneOffsetControlStrategy() { movement_domain = MovementDomain::lateral; }
-    // Doesn`t need configuration attributes. Controller keeps current lane offset on adding entity or update
+    KeepLaneOffsetControlStrategy()
+    {
+        movement_domain = MovementDomain::kLateral;
+        type = ControlStrategyType::kKeepLaneOffset;
+    }
+    // Doesn't need configuration attributes. Controller keeps current lane offset on adding entity or update
+};
+
+struct FollowHeadingSplineControlStrategy : public ControlStrategy
+{
+    FollowHeadingSplineControlStrategy()
+    {
+        movement_domain = MovementDomain::kLateral;
+        type = ControlStrategyType::kFollowHeadingSpline;
+    }
+
+    std::vector<mantle_api::SplineSection> heading_splines;
+    double default_value{0};
 };
 
-// Control strategy which sets the respective value y (heading/velocity) as a polynomial function y = P(x) of simulation
-// time x
-struct FollowSplineControlStrategy : public ControlStrategy
+struct FollowVelocitySplineControlStrategy : public ControlStrategy
+{
+    FollowVelocitySplineControlStrategy()
+    {
+        movement_domain = MovementDomain::kLongitudinal;
+        type = ControlStrategyType::kFollowVelocitySpline;
+    }
+
+    std::vector<mantle_api::SplineSection> velocity_splines;
+    double default_value{0};
+};
+
+inline bool operator==(const FollowVelocitySplineControlStrategy& lhs,
+                       const FollowVelocitySplineControlStrategy& rhs) noexcept
+{
+    return lhs.default_value == rhs.default_value && lhs.velocity_splines == rhs.velocity_splines;
+}
+
+inline bool operator!=(const FollowVelocitySplineControlStrategy& lhs,
+                       const FollowVelocitySplineControlStrategy& rhs) noexcept
+{
+    return !(lhs == rhs);
+}
+
+struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
 {
-    // movement_domain has to be set when used! Lat=heading, Lon=velocity
-    FollowSplineControlStrategy() {}
+    FollowLateralOffsetSplineControlStrategy()
+    {
+        movement_domain = MovementDomain::kLateral;
+        type = ControlStrategyType::kFollowLateralOffsetSpline;
+    }
 
-    std::vector<mantle_api::SplineSection> splines;
-    double default_value{0};  // when no spline section is defined for a certain simulation time
+    std::vector<mantle_api::SplineSection> lateral_offset_splines;
 };
 
 // TODO: Create new control strategy for following 3D trajectories with shapes NURBS, polyline, clothoid
 
 struct FollowRouteControlStrategy : public ControlStrategy
 {
-    FollowRouteControlStrategy() { movement_domain = MovementDomain::lateral; }
+    FollowRouteControlStrategy()
+    {
+        movement_domain = MovementDomain::kLateral;
+        type = ControlStrategyType::kFollowRoute;
+    }
 
     std::vector<mantle_api::Vec3d> waypoints;
 };
 
 enum class Dimension
 {
-    undefined = 0,
-    distance,
-    rate,
-    time
+    kUndefined = 0,
+    kDistance,
+    kRate,
+    kTime
 };
 
 enum class Shape
 {
-    undefined = 0,
-    cubic,
-    linear,
-    sinusoidal
+    kUndefined = 0,
+    kStep,
+    kCubic,
+    kLinear,
+    kSinusoidal
 };
 
 struct TransitionDynamics
 {
-    Dimension dimension{Dimension::undefined};
-    Shape shape{Shape::undefined};
+    Dimension dimension{Dimension::kUndefined};
+    Shape shape{Shape::kUndefined};
     double value{0};
 };
 
-struct AcquireVelocityControlStrategy : public ControlStrategy
-{
-    AcquireVelocityControlStrategy() { movement_domain = MovementDomain::longitudinal; }
-
-    double velocity_target;
-    TransitionDynamics transition_dynamics;
-};
-
 struct AcquireLaneOffsetControlStrategy : public ControlStrategy
 {
-    AcquireLaneOffsetControlStrategy() { movement_domain = MovementDomain::lateral; }
-
-    int road_id;
-    int lane_id;
-    double offset;
+    AcquireLaneOffsetControlStrategy()
+    {
+        movement_domain = MovementDomain::kLateral;
+        type = ControlStrategyType::kAcquireLaneOffset;
+    }
+
+    int road_id{};
+    int lane_id{};
+    double offset{};
     TransitionDynamics transition_dynamics;
 };
 

MantleAPI/include/MantleAPI/Traffic/entity_properties.h

@@ -122,6 +122,8 @@ struct VehicleProperties : public EntityProperties
     double front_wheel_diameter{0.0};
     double rear_wheel_diameter{0.0};
     bool is_host{false};
+    // TODO: remove, once external control for traffic is implemented through controllers
+    bool is_controlled_externally{false};
 };
 
 inline bool operator==(const VehicleProperties& lhs, const VehicleProperties& rhs) noexcept

MantleAPI/include/MantleAPI/Traffic/i_controller_config.h

@@ -21,6 +21,7 @@
 #include <MantleAPI/Common/vector.h>
 #include <MantleAPI/Map/i_lane_location_query_service.h>
 
+#include <algorithm>
 #include <memory>
 #include <vector>
 
@@ -29,19 +30,19 @@ namespace mantle_api
 
 struct IControllerConfig
 {
-    virtual ~IControllerConfig() = default;
-
-    // TODO: Remove copy constructor and then remove config as member of NoOpController and RouteController
     IControllerConfig() {}
     IControllerConfig(const IControllerConfig& controller_config)
         : map_query_service(controller_config.map_query_service), id(controller_config.id)
     {
-        for (const auto& control_strategy : controller_config.control_strategies)
-        {
-            control_strategies.push_back(std::make_unique<mantle_api::ControlStrategy>(*control_strategy));
-        }
+        std::transform(
+            controller_config.control_strategies.begin(),
+            controller_config.control_strategies.end(),
+            std::back_inserter(control_strategies),
+            [](auto& control_strategy) { return std::make_unique<mantle_api::ControlStrategy>(*control_strategy); });
     }
 
+    virtual ~IControllerConfig() = default;
+
     // TODO: Check why map_query_service is part of the interface because it is not set from engine side but only in the
     // environment on calling AddController()
     ILaneLocationQueryService* map_query_service{nullptr};
@@ -70,21 +71,6 @@ inline bool operator==(const IControllerConfig& lhs, const IControllerConfig& rh
     return lhs.id == rhs.id && control_strategies_equal;
 }
 
-// TODO: Remove and use FollowRoute (lat) and FollowSpline (lon) control strategy instead
-struct PathControllerConfig : public IControllerConfig
-{
-    std::vector<mantle_api::Vec3d> waypoints;
-    std::vector<mantle_api::SplineSection> velocity_splines;
-    double default_velocity;
-};
-
-// TODO: Remove and use FollowSpline (lat) and FollowSpline (lon) control strategy instead
-struct TrajectoryControllerConfig : public IControllerConfig
-{
-    std::vector<mantle_api::SplineSection> heading_splines;
-    std::vector<mantle_api::SplineSection> velocity_splines;
-};
-
 struct NoOpControllerConfig : public IControllerConfig
 {
 };

MantleAPI/include/MantleAPI/Traffic/i_entity.h

@@ -51,7 +51,6 @@ class IEntity : public IIdentifiable
     virtual void SetProperties(std::unique_ptr<mantle_api::EntityProperties> properties) = 0;
     virtual EntityProperties* GetProperties() const = 0;
 
-    // TODO: evaluate if this will be part of the final interface
     virtual void SetAssignedLaneIds(const std::vector<std::uint64_t>& assigned_lane_ids) = 0;
     virtual std::vector<std::uint64_t> GetAssignedLaneIds() const = 0;
 };

MantleAPI/test/MantleAPI/Test/test_utils.h

@@ -285,10 +285,6 @@ class MockEntityRepository : public mantle_api::IEntityRepository
 class MockEnvironment : public mantle_api::IEnvironment
 {
   public:
-    virtual void Init() override {}
-
-    virtual void Step() override {}
-
     MOCK_METHOD(void,
                 CreateMap,
                 (const std::string& file_path, const std::vector<mantle_api::Position>& map_region),
@@ -312,6 +308,11 @@ class MockEnvironment : public mantle_api::IEnvironment
                  std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies),
                 (override));
 
+    MOCK_METHOD(bool,
+                HasControlStrategyGoalBeenReached,
+                (std::uint64_t controller_id, mantle_api::ControlStrategyType type),
+                (const, override));
+
     const mantle_api::ILaneLocationQueryService& GetQueryService() const override { return query_service_; }
 
     const mantle_api::ICoordConverter* GetConverter() const override { return &converter_; }
@@ -331,6 +332,8 @@ class MockEnvironment : public mantle_api::IEnvironment
 
     mantle_api::DateTime GetDateTime() override { return mantle_api::DateTime(); }
 
+    mantle_api::SimulationTime GetSimulationTime() override { return mantle_api::SimulationTime(); }
+
   private:
     MockQueryService query_service_{};
     MockEntityRepository entity_repository_{};