Commit db325a85 authored by Arun Das's avatar Arun Das
Browse files

- 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's avatarArun Das <arun.das@bmw.de>
parent 0f5245fc
......@@ -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
......@@ -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.
......
/*******************************************************************************
* 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
......@@ -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
......
......@@ -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
......
......@@ -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;
};
......
......@@ -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;
};
......
......@@ -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;
};
......
......@@ -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
......
......@@ -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
{
};
......
......@@ -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;
};
......
......@@ -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_{};
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment