From db325a85da6805ed2ea0802fecd60e519fd36961 Mon Sep 17 00:00:00 2001
From: Arun Das <arun.das@bmw.de>
Date: Wed, 26 May 2021 07:47:23 +0200
Subject: [PATCH] - 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/Common/simulation_time.h | 24 +--
MantleAPI/include/MantleAPI/Common/spline.h | 8 +-
.../include/MantleAPI/Common/time_utils.h | 44 ++++++
MantleAPI/include/MantleAPI/Common/vector.h | 3 +
.../EnvironmentalConditions/date_time.h | 7 +-
.../MantleAPI/Execution/i_environment.h | 14 +-
.../MantleAPI/Execution/scenario_info.h | 4 +-
.../MantleAPI/Traffic/control_strategy.h | 140 +++++++++++++-----
.../MantleAPI/Traffic/entity_properties.h | 2 +
.../MantleAPI/Traffic/i_controller_config.h | 30 +---
.../include/MantleAPI/Traffic/i_entity.h | 1 -
MantleAPI/test/MantleAPI/Test/test_utils.h | 11 +-
12 files changed, 189 insertions(+), 99 deletions(-)
create mode 100644 MantleAPI/include/MantleAPI/Common/time_utils.h
diff --git a/MantleAPI/include/MantleAPI/Common/simulation_time.h b/MantleAPI/include/MantleAPI/Common/simulation_time.h
index 77200149..bd06dbaf 100644
--- a/MantleAPI/include/MantleAPI/Common/simulation_time.h
+++ b/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
diff --git a/MantleAPI/include/MantleAPI/Common/spline.h b/MantleAPI/include/MantleAPI/Common/spline.h
index 1fd7c9be..d12d4ddc 100644
--- a/MantleAPI/include/MantleAPI/Common/spline.h
+++ b/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.
diff --git a/MantleAPI/include/MantleAPI/Common/time_utils.h b/MantleAPI/include/MantleAPI/Common/time_utils.h
new file mode 100644
index 00000000..78b9b51a
--- /dev/null
+++ b/MantleAPI/include/MantleAPI/Common/time_utils.h
@@ -0,0 +1,44 @@
+/*******************************************************************************
+ * 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
diff --git a/MantleAPI/include/MantleAPI/Common/vector.h b/MantleAPI/include/MantleAPI/Common/vector.h
index d495f9dd..5127e5e0 100644
--- a/MantleAPI/include/MantleAPI/Common/vector.h
+++ b/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
diff --git a/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h b/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h
index f1005118..b879da03 100644
--- a/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h
+++ b/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
diff --git a/MantleAPI/include/MantleAPI/Execution/i_environment.h b/MantleAPI/include/MantleAPI/Execution/i_environment.h
index 337b8207..5fbbcbf9 100644
--- a/MantleAPI/include/MantleAPI/Execution/i_environment.h
+++ b/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;
};
diff --git a/MantleAPI/include/MantleAPI/Execution/scenario_info.h b/MantleAPI/include/MantleAPI/Execution/scenario_info.h
index 6e0dd3b4..72f0cede 100644
--- a/MantleAPI/include/MantleAPI/Execution/scenario_info.h
+++ b/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;
};
diff --git a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
index 68252a16..42a736d4 100644
--- a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
+++ b/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;
};
diff --git a/MantleAPI/include/MantleAPI/Traffic/entity_properties.h b/MantleAPI/include/MantleAPI/Traffic/entity_properties.h
index 7e37ccb5..8d9002ab 100644
--- a/MantleAPI/include/MantleAPI/Traffic/entity_properties.h
+++ b/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
diff --git a/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h b/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h
index 81d0ef00..3d2355bd 100644
--- a/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h
+++ b/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
{
};
diff --git a/MantleAPI/include/MantleAPI/Traffic/i_entity.h b/MantleAPI/include/MantleAPI/Traffic/i_entity.h
index 1d25f1bd..89b1e48a 100644
--- a/MantleAPI/include/MantleAPI/Traffic/i_entity.h
+++ b/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;
};
diff --git a/MantleAPI/test/MantleAPI/Test/test_utils.h b/MantleAPI/test/MantleAPI/Test/test_utils.h
index a72d175a..dc03ff8e 100644
--- a/MantleAPI/test/MantleAPI/Test/test_utils.h
+++ b/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_{};
--
GitLab