Merge branch 'master' into mb_proposal_si_units

Additionally provided units for "Axle" and "Performance" structs.
Signed-off-by: Jupp Tscheak <jupp.tscheak@daimler.com>

.cmake-format

+line_width: 120
+tab_size: 2
+max_subgroups_hwrap: 2
+max_pargs_hwrap: 6
+separate_ctrl_name_with_space: false
+separate_fn_name_with_space: false
+dangle_parens: true
+dangle_align: prefix
+min_prefix_chars: 4
+max_prefix_chars: 10
+max_lines_hwrap: 2
+line_ending: unix
+command_case: canonical
+keyword_case: upper
+additional_commands:
+  configure_package_config_file:
+    flags:
+      - NO_SET_AND_CHECK_MACRO
+      - NO_CHECK_REQUIRED_COMPONENTS_MACRO
+    kwargs:
+      INSTALL_DESTINATION: "*"
+      PATH_VARS: "*"
+      INSTALL_PREFIX: "*"
+  find_dependency:
+    flags:
+      - CONFIG
+      - QUIET
+      - REQUIRED
+    kwargs:
+      COMPONENTS: "*"
+  gtest_discover_tests:
+    flags:
+      - NO_PRETTY_TYPES
+      - NO_PRETTY_VALUES
+    kwargs:
+      EXTRA_ARGS: "*"
+      WORKING_DIRECTORY: "*"
+      TEST_PREFIX: "*"
+      TEST_SUFFIX: "*"
+      PROPERTIES: "*"
+      TEST_LIST: "*"
+      DISCOVERY_TIMEOUT: "*"
+  setup_target_for_coverage_gcovr_html:
+    kwargs:
+      BASE_DIRECTORY: "*"
+      EXCLUDE: "*"
+      EXECUTABLE: "*"
+      EXECUTABLE_ARGS: "*"
+      DEPENDENCIES: "*"
+always_wrap: ["add_library", "find_package", "find_dependency", "target_include_directories"]
+enable_sort: true
+autosort: false
+hashruler_min_length: 10
+per_command: {}
+layout_passes: {}
+bullet_char: "*"
+enum_char: .
+enable_markup: true
+first_comment_is_literal: true
+literal_comment_pattern: null
+fence_pattern: ^\s*([`~]{3}[`~]*)(.*)$
+ruler_pattern: ^\s*[^\w\s]{3}.*[^\w\s]{3}$
+canonicalize_hashrulers: true
+emit_byteorder_mark: false
+input_encoding: utf-8
+output_encoding: utf-8

CMakeLists.txt

@@ -8,7 +8,7 @@
 # SPDX-License-Identifier: EPL-2.0
 ################################################################################
 
-cmake_minimum_required(VERSION 3.14.0 FATAL_ERROR)
+cmake_minimum_required(VERSION 3.15.0 FATAL_ERROR)
 
 # Add the custom CMake modules to CMake's module path
 list(PREPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -17,7 +17,8 @@ project(
   ScenarioAPI
   VERSION 0.1.0
   DESCRIPTION "Scenario API, an abstraction layer for environmental simulators"
-  LANGUAGES CXX)
+  LANGUAGES CXX
+)
 
 # Project setup only if this is the main project
 if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME)
@@ -30,8 +31,7 @@ endif()
 add_subdirectory(MantleAPI)
 
 # Should we build documentation for the project?
-if((CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND BUILD_DOCUMENTATION)
-   OR ScenarioAPI_BUILD_DOCUMENTATION)
+if((CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND BUILD_DOCUMENTATION) OR ScenarioAPI_BUILD_DOCUMENTATION)
   add_subdirectory(doc)
 endif()
 

MantleAPI/include/CMakeLists.txt

@@ -8,15 +8,22 @@
 # SPDX-License-Identifier: EPL-2.0
 ################################################################################
 
-find_package(units CONFIG REQUIRED)
+find_package(
+  units CONFIG REQUIRED
+)
 
 file(
   GLOB_RECURSE HEADERS
   RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
-  CONFIGURE_DEPENDS "*.h")
+  CONFIGURE_DEPENDS "*.h"
+)
 
-add_library(ScenarioAPI INTERFACE)
-add_library(ScenarioAPI::ScenarioAPI ALIAS ScenarioAPI)
+add_library(
+  ScenarioAPI INTERFACE
+)
+add_library(
+  ScenarioAPI::ScenarioAPI ALIAS ScenarioAPI
+)
 
 target_link_libraries(ScenarioAPI INTERFACE units)
 
@@ -24,21 +31,24 @@ include(GNUInstallDirs)
 set(INSTALL_CONFIG_DIR "${CMAKE_INSTALL_LIBDIR}/cmake/ScenarioAPI")
 
 target_include_directories(
-  ScenarioAPI INTERFACE $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+  ScenarioAPI
+  INTERFACE $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+)
 
 target_compile_features(ScenarioAPI INTERFACE cxx_std_17)
 
 include(CMakePackageConfigHelpers)
 configure_package_config_file(
-  "${PROJECT_SOURCE_DIR}/cmake/ScenarioAPIConfig.cmake.in"
-  "${CMAKE_CURRENT_BINARY_DIR}/ScenarioAPIConfig.cmake"
+  "${PROJECT_SOURCE_DIR}/cmake/ScenarioAPIConfig.cmake.in" "${CMAKE_CURRENT_BINARY_DIR}/ScenarioAPIConfig.cmake"
   INSTALL_DESTINATION ${INSTALL_CONFIG_DIR}
-  PATH_VARS CMAKE_INSTALL_INCLUDEDIR)
+  PATH_VARS CMAKE_INSTALL_INCLUDEDIR
+)
 
 write_basic_package_version_file(
   "${CMAKE_CURRENT_BINARY_DIR}/ScenarioAPIConfigVersion.cmake"
   VERSION ${PROJECT_VERSION}
-  COMPATIBILITY SameMajorVersion)
+  COMPATIBILITY SameMajorVersion
+)
 
 install(DIRECTORY MantleAPI TYPE INCLUDE)
 install(TARGETS ScenarioAPI EXPORT ScenarioAPITargets)
@@ -46,10 +56,12 @@ install(TARGETS ScenarioAPI EXPORT ScenarioAPITargets)
 install(
   EXPORT ScenarioAPITargets
   DESTINATION ${INSTALL_CONFIG_DIR}
-  NAMESPACE ScenarioAPI::)
+  NAMESPACE ScenarioAPI::
+)
 
 install(
   FILES "${CMAKE_CURRENT_BINARY_DIR}/ScenarioAPIConfig.cmake"
         "${CMAKE_CURRENT_BINARY_DIR}/ScenarioAPIConfigVersion.cmake"
   DESTINATION ${INSTALL_CONFIG_DIR}
-  COMPONENT dev)
+  COMPONENT dev
+)

MantleAPI/include/MantleAPI/Common/i_identifiable.h

@@ -21,6 +21,7 @@
 namespace mantle_api
 {
 using UniqueId = std::uint64_t;
+constexpr UniqueId InvalidId{std::numeric_limits<UniqueId>::max()};
 
 /// Common interface for all classes that can be referenced by an ID or name.
 class IIdentifiable

MantleAPI/include/MantleAPI/Common/poly_line.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  poly_line.h */
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_COMMON_POLY_LINE_H
+#define MANTLEAPI_COMMON_POLY_LINE_H
+
+#include <MantleAPI/Common/pose.h>
+#include <MantleAPI/Common/time_utils.h>
+
+#include <optional>
+#include <vector>
+
+namespace mantle_api
+{
+    
+struct PolyLinePoint
+{
+    Pose pose{};
+    std::optional<Time> time{};
+
+    bool operator== (const PolyLinePoint& other) const
+    {
+        return other.time == time
+                && other.pose == pose;
+    }
+
+    friend std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLinePoint);
+};
+
+inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLinePoint)
+{
+    os << polyLinePoint.pose;
+
+    if(polyLinePoint.time.has_value())
+    {
+        os << ", time in ms " << polyLinePoint.time.value().count();
+    }
+
+    return os;
+}
+
+using PolyLine = std::vector<PolyLinePoint>;
+
+} // namespace mantle_api
+
+#endif // MANTLEAPI_COMMON_POLY_LINE_H
\ No newline at end of file

MantleAPI/include/MantleAPI/Common/pose.h

@@ -17,8 +17,12 @@
 
 #include <MantleAPI/Common/orientation.h>
 #include <MantleAPI/Common/vector.h>
+
 #include <units.h>
 
+#include <iostream>
+
+
 namespace mantle_api
 {
 
@@ -26,8 +30,30 @@ struct Pose
 {
     Vec3<units::length::meter_t> position{};
     Orientation3<units::angle::radian_t> orientation{};
+
+    bool operator== (const Pose& other) const
+    {
+        return other.position == position
+                && other.orientation == orientation;
+    }
+
+    friend inline std::ostream& operator<<(std::ostream& os, const Pose& pose);
 };
 
+std::ostream& operator<<(std::ostream& os, const Pose& pose)
+{
+    os << "position (" 
+        << pose.position.x
+        << ", " << pose.position.y
+        << ", " << pose.position.z
+        << "), orientation (" << pose.orientation.yaw
+        << ", " << pose.orientation.pitch
+        << ", " << pose.orientation.roll
+        << ")";
+
+    return os;
+}
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_COMMON_POSE_H

MantleAPI/include/MantleAPI/Common/trajectory.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  trajectory.h */
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_COMMON_TRAJECTORY_H
+#define MANTLEAPI_COMMON_TRAJECTORY_H
+
+#include <string>
+#include <variant>
+
+#include <MantleAPI/Common/poly_line.h>
+
+namespace mantle_api
+{
+struct Trajectory
+{
+    std::string name;
+    std::variant<PolyLine> type;
+
+    friend std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory);
+};
+
+inline std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory)
+{
+    os << "Trajectory \"" << trajectory.name;
+    
+    if(std::holds_alternative<PolyLine>(trajectory.type))
+    {
+        const auto &polyLine = std::get<PolyLine>(trajectory.type);
+        for (const auto &polyLinePoint : polyLine)
+        {
+            os << polyLinePoint;
+        }
+    }
+    
+    os << "\"\n";
+
+    return os;
+}
+
+} // namespace mantle_api
+
+#endif // MANTLEAPI_COMMON_TRAJECTORY_H
\ No newline at end of file

MantleAPI/include/MantleAPI/Common/vector.h

@@ -73,6 +73,38 @@ inline Vec3<T> operator/(const Vec3<T>& lhs, double d) noexcept
     return {lhs.x / d, lhs.y / d, lhs.z / d};
 }
 
+inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept
+{
+    lhs.x += rhs.x;
+    lhs.y += rhs.y;
+    lhs.z += rhs.z;
+    return lhs;
+}
+
+inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept
+{
+    lhs.x -= rhs.x;
+    lhs.y -= rhs.y;
+    lhs.z -= rhs.z;
+    return lhs;
+}
+
+inline Vec3d operator+=(Vec3d& lhs, double d) noexcept
+{
+    lhs.x += d;
+    lhs.y += d;
+    lhs.z += d;
+    return lhs;
+}
+
+inline Vec3d operator-=(Vec3d& lhs, double d) noexcept
+{
+    lhs.x -= d;
+    lhs.y -= d;
+    lhs.z -= d;
+    return lhs;
+}
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_COMMON_VECTOR_H

MantleAPI/include/MantleAPI/Execution/i_environment.h

@@ -34,9 +34,9 @@ class IEnvironment
 
     /// 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
-    ///                  environment.
-    virtual void CreateMap(const std::string& file_path, const std::vector<Position>& map_region) = 0;
+    /// @param file_path map file path from the scenario file. If this path is not resolved by the engine, the
+    ///                  environment must do so.
+    virtual void CreateMap(const std::string& map_file_path, const std::vector<Position>& map_region) = 0;
 
     /// Creates a controller from the given config. A created controller can be assigned to multiple entities
     ///

MantleAPI/include/MantleAPI/Traffic/control_strategy.h

@@ -17,6 +17,7 @@
 
 #include <MantleAPI/Common/spline.h>
 #include <MantleAPI/Common/vector.h>
+#include <MantleAPI/Common/trajectory.h>
 
 #include <vector>
 
@@ -40,7 +41,8 @@ enum class ControlStrategyType
     kFollowLateralOffsetSpline,
     kFollowVelocitySpline,
     kFollowRoute,
-    kAcquireLaneOffset
+    kAcquireLaneOffset,
+    kFollowTrajectory
 };
 
 struct ControlStrategy
@@ -130,8 +132,6 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
     std::vector<mantle_api::SplineSection<units::length::meter_t>> lateral_offset_splines;
 };
 
-// TODO: Create new control strategy for following 3D trajectories with shapes NURBS, polyline, clothoid
-
 struct FollowRouteControlStrategy : public ControlStrategy
 {
     FollowRouteControlStrategy()
@@ -164,7 +164,7 @@ struct TransitionDynamics
 {
     Dimension dimension{Dimension::kUndefined};
     Shape shape{Shape::kUndefined};
-    double value{0};
+    double value{0.0};
 };
 
 struct AcquireLaneOffsetControlStrategy : public ControlStrategy
@@ -181,6 +181,33 @@ struct AcquireLaneOffsetControlStrategy : public ControlStrategy
     TransitionDynamics transition_dynamics;
 };
 
+enum class ReferenceContext
+{
+    kAbsolute = 0,
+    kRelative
+};
+
+struct FollowTrajectoryControlStrategy : public ControlStrategy
+{
+    // TODO: Extend the FollowTrajectoryControlStrategy to support shapes like NURBS and clothoid
+
+    struct TrajectoryTimeReference
+    {
+        ReferenceContext domainAbsoluteRelative;
+        double scale{0.0};
+        double offset{0.0};
+    };
+    
+    FollowTrajectoryControlStrategy()
+    {
+        movement_domain = MovementDomain::kBoth;
+        type = ControlStrategyType::kFollowTrajectory;
+    }
+
+    Trajectory trajectory;
+    std::optional<TrajectoryTimeReference> timeReference;
+};
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_TRAFFIC_CONTROLSTRATEGY_H

MantleAPI/include/MantleAPI/Traffic/entity_helper.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  entity_helper.h */
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_TRAFFIC_ENTITY_HELPER_H
+#define MANTLEAPI_TRAFFIC_ENTITY_HELPER_H
+
+#include <MantleAPI/Traffic/i_entity.h>
+
+#include <cmath>
+
+namespace mantle_api
+{
+
+void SetSpeed(mantle_api::IEntity* entity, double velocity)
+{
+    auto orientation = entity->GetOrientation();
+
+    double cos_elevation = std::cos(orientation.pitch);
+    mantle_api::Vec3d velocity_vector{velocity * std::cos(orientation.yaw) * cos_elevation,
+                                      velocity * std::sin(orientation.yaw) * cos_elevation,
+                                      velocity * -std::sin(orientation.pitch)};
+
+    entity->SetVelocity(velocity_vector);
+}
+
+}  // namespace mantle_api
+
+#endif  // MANTLEAPI_TRAFFIC_ENTITY_HELPER_H

MantleAPI/include/MantleAPI/Traffic/entity_properties.h

@@ -20,6 +20,7 @@
 #include <MantleAPI/Common/vector.h>
 
 #include <string>
+#include <map>
 
 namespace mantle_api
 {
@@ -44,6 +45,7 @@ struct EntityProperties
     BoundingBox bounding_box{};
     EntityType type{EntityType::kOther};
     std::string model{};
+    std::map<std::string, std::string> properties{};
 };
 
 inline bool operator==(const EntityProperties& lhs, const EntityProperties& rhs) noexcept
@@ -115,13 +117,45 @@ enum class ExternalControlState
     kLongitudinalOnly = 3
 };
 
+struct Performance
+{
+    units::velocity::meters_per_second_t max_speed{0.0};
+    units::acceleration::meters_per_second_squared_t max_acceleration{0.0};
+    units::acceleration::meters_per_second_squared_t max_deceleration{0.0};
+};
+
+inline bool operator==(const Performance& lhs, const Performance& rhs) noexcept
+{
+    return IsEqual(lhs.max_speed, rhs.max_speed) &&
+            IsEqual(lhs.max_acceleration, rhs.max_acceleration) &&
+            IsEqual(lhs.max_deceleration, rhs.max_deceleration);
+}
+
+struct Axle
+{
+    units::angle::radian_t max_steering{0.0};
+    units::length::meter_t wheel_diameter{0.0};
+    units::length::meter_t track_width{0.0};
+    Vec3<units::length::meter_t> bb_center_to_axle_center{};
+};
+
+inline bool operator==(const Axle& lhs, const Axle& rhs) noexcept
+{
+    return IsEqual(lhs.max_steering, rhs.max_steering) &&
+            IsEqual(lhs.wheel_diameter, rhs.wheel_diameter) &&
+            IsEqual(lhs.track_width, rhs.track_width) &&
+            lhs.bb_center_to_axle_center == rhs.bb_center_to_axle_center;
+}
+
 struct VehicleProperties : public EntityProperties
 {
     VehicleClass classification{VehicleClass::kOther};
-    Vec3<units::length::meter_t> bb_center_to_front{};
-    Vec3<units::length::meter_t> bb_center_to_rear{};
-    units::length::meter_t front_wheel_diameter{0.0};
-    units::length::meter_t rear_wheel_diameter{0.0};
+
+    Performance performance{};
+
+    Axle front_axle{};
+    Axle rear_axle{};
+
     bool is_host{false};
     // TODO: remove, once external control for traffic is implemented through controllers
     bool is_controlled_externally{false};
@@ -130,10 +164,9 @@ struct VehicleProperties : public EntityProperties
 inline bool operator==(const VehicleProperties& lhs, const VehicleProperties& rhs) noexcept
 {
     return lhs.bounding_box == rhs.bounding_box && lhs.type == rhs.type && lhs.model == rhs.model &&
-           lhs.classification == rhs.classification && lhs.bb_center_to_front == rhs.bb_center_to_front &&
-           lhs.bb_center_to_rear == rhs.bb_center_to_rear &&
-           IsEqual(lhs.front_wheel_diameter, rhs.front_wheel_diameter) &&
-           IsEqual(lhs.rear_wheel_diameter, rhs.rear_wheel_diameter) && lhs.is_host == rhs.is_host;
+           lhs.classification == rhs.classification && lhs.performance == rhs.performance &&
+           lhs.front_axle == rhs.front_axle && lhs.rear_axle == rhs.rear_axle &&
+           lhs.is_host == rhs.is_host;
 }
 
 struct PedestrianProperties : public EntityProperties

MantleAPI/include/MantleAPI/Traffic/i_controller_config.h

@@ -21,6 +21,7 @@
 #include <MantleAPI/Traffic/control_strategy.h>
 
 #include <algorithm>
+#include <map>
 #include <memory>
 #include <vector>
 
@@ -81,6 +82,7 @@ struct InternalControllerConfig : public IControllerConfig
 struct ExternalControllerConfig : public IControllerConfig
 {
     std::string name;
+    std::map<std::string, std::string> parameters;
 };
 
 }  // namespace mantle_api

MantleAPI/include/MantleAPI/Traffic/i_entity_repository.h

@@ -18,6 +18,8 @@
 #include <MantleAPI/Traffic/entity_properties.h>
 #include <MantleAPI/Traffic/i_entity.h>
 
+#include <functional>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -35,14 +37,18 @@ class IEntityRepository
     virtual IStaticObject& Create(UniqueId id, const std::string& name, const StaticObjectProperties& properties) = 0;
 
     virtual IVehicle& GetHost() = 0;
-    virtual IEntity& Get(const std::string& name) = 0;
-    virtual IEntity& Get(UniqueId id) = 0;
+    virtual std::optional<std::reference_wrapper<IEntity>> Get(const std::string& name) = 0;
+    virtual std::optional<std::reference_wrapper<IEntity>> Get(UniqueId id) = 0;
     virtual bool Contains(UniqueId id) const = 0;
 
     virtual void Delete(const std::string& name) = 0;
     virtual void Delete(UniqueId id) = 0;
 
     virtual const std::vector<std::unique_ptr<mantle_api::IEntity>>& GetEntities() const = 0;