diff --git a/MantleAPI/include/MantleAPI/Common/bounding_box.h b/MantleAPI/include/MantleAPI/Common/bounding_box.h
index 7be0db8e7b98282494b32f0cbe76c40ff09b1b37..e67ebbd225ab6ff87d98e83120b87be5e0e239e0 100644
--- a/MantleAPI/include/MantleAPI/Common/bounding_box.h
+++ b/MantleAPI/include/MantleAPI/Common/bounding_box.h
@@ -17,22 +17,26 @@
#include <MantleAPI/Common/dimension.h>
#include <MantleAPI/Common/vector.h>
+#include <units.h>
namespace mantle_api
{
+
/// Bounding box is defined in local entity coordinate system.
-/// The origin of the entity coordinate system is defined in relation to the geometric center.
+/// The origin of the entity coordinate system is defined in relation to the
+/// geometric center.
struct BoundingBox
{
- Vec3d geometric_center{0.0, 0.0, 0.0};
- Dimension3d dimension{0.0, 0.0, 0.0};
+ Vec3<units::length::meter_t> geometric_center{};
+ Dimension3 dimension{};
};
inline bool operator==(const BoundingBox& lhs, const BoundingBox& rhs) noexcept
{
- return lhs.geometric_center == rhs.geometric_center && lhs.dimension == rhs.dimension;
+ return lhs.geometric_center == rhs.geometric_center &&
+ lhs.dimension == rhs.dimension;
}
-} // namespace mantle_api
+} // namespace mantle_api
-#endif // MANTLEAPI_COMMON_BOUNDINGBOX_H
+#endif // MANTLEAPI_COMMON_BOUNDINGBOX_H
diff --git a/MantleAPI/include/MantleAPI/Common/dimension.h b/MantleAPI/include/MantleAPI/Common/dimension.h
index 64b97e29cf8b238bc44959858d2cfd91163fc5c9..a36d07d34003688ea995fab0097b2cbb913d2ebe 100644
--- a/MantleAPI/include/MantleAPI/Common/dimension.h
+++ b/MantleAPI/include/MantleAPI/Common/dimension.h
@@ -16,27 +16,24 @@
#define MANTLEAPI_COMMON_DIMENSION_H
#include <MantleAPI/Common/floating_point_helper.h>
-#include <MantleAPI/Common/vector.h>
+#include <units.h>
namespace mantle_api
{
-template <typename T>
struct Dimension3
{
- T length{};
- T width{};
- T height{};
+ units::length::meter_t length{0};
+ units::length::meter_t width{0};
+ units::length::meter_t height{0};
};
-using Dimension3d = Dimension3<double>;
-
-inline bool operator==(const Dimension3d& lhs, const Dimension3d& rhs) noexcept
+inline bool operator==(const Dimension3& lhs, const Dimension3& rhs) noexcept
{
return IsEqual(lhs.length, rhs.length) && IsEqual(lhs.width, rhs.width) && IsEqual(lhs.height, rhs.height);
}
-inline bool operator!=(const Dimension3d& lhs, const Dimension3d& rhs) noexcept
+inline bool operator!=(const Dimension3& lhs, const Dimension3& rhs) noexcept
{
return !(lhs == rhs);
}
diff --git a/MantleAPI/include/MantleAPI/Common/floating_point_helper.h b/MantleAPI/include/MantleAPI/Common/floating_point_helper.h
index ea93a5fd56bc55baea2ae1a6d16bdf2288a66124..66eff41ca93021e5c450f1707c7baa64f691dbd0 100644
--- a/MantleAPI/include/MantleAPI/Common/floating_point_helper.h
+++ b/MantleAPI/include/MantleAPI/Common/floating_point_helper.h
@@ -15,6 +15,8 @@
#ifndef MANTLEAPI_COMMON_FLOATING_POINT_HELPER_H
#define MANTLEAPI_COMMON_FLOATING_POINT_HELPER_H
+#include <units.h>
+
#include <cmath>
#include <limits>
#include <type_traits>
@@ -36,12 +38,31 @@ namespace mantle_api
/// @param rhs Right-hand operand
/// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
/// @return True if both numbers are equal within the given precision.
-template <typename T, typename std::enable_if_t<std::is_floating_point_v<T>>* = nullptr>
-bool IsEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
+inline bool IsEqual(double lhs, double rhs, double precision = std::numeric_limits<double>::epsilon())
{
return std::abs(lhs - rhs) < precision;
}
+/// @brief Compares two floating point numbers for equality.
+///
+/// **Attention** No approximation of the correct precision for the actual value is done!
+///
+/// This function uses a default precision of `std::numeric_limts<T>::epsilon()`, which means that without providing the
+/// precision yourself, it is only suited for
+/// * bit-wise equal numbers
+/// * numbers around 1 or smaller
+///
+/// @tparam T Floating point type, ensured by SFINAE
+/// @param lhs Left-hand operand
+/// @param rhs Right-hand operand
+/// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
+/// @return True if both numbers are equal within the given precision.
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+bool IsEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
+{
+ return units::math::abs(lhs - rhs) < precision;
+}
+
/// @brief Compares two floating point numbers for equality.
///
/// **Attention** No approximation of the correct precision for the actual value is done!
@@ -56,8 +77,8 @@ bool IsEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
/// @param rhs Right-hand operand
/// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
/// @return True if lhs is greater than rhs or both numbers are equal within the given precision.
-template <typename T, typename std::enable_if_t<std::is_floating_point_v<T>>* = nullptr>
-bool GreaterOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+bool GreaterOrEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
{
if (lhs > rhs)
{
@@ -80,8 +101,8 @@ bool GreaterOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon(
/// @param rhs Right-hand operand
/// @param precision Given precision, defaults to `std::numeric_limits<T>::epsilon()`.
/// @return True if lhs is less than rhs or both numbers are equal within the given precision.
-template <typename T, typename std::enable_if_t<std::is_floating_point_v<T>>* = nullptr>
-bool LessOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+bool LessOrEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
{
if (lhs < rhs)
{
@@ -89,6 +110,7 @@ bool LessOrEqual(T lhs, T rhs, T precision = std::numeric_limits<T>::epsilon())
}
return IsEqual(lhs, rhs, precision);
}
+
} // namespace mantle_api
#endif // MANTLEAPI_COMMON_FLOATING_POINT_HELPER_H
diff --git a/MantleAPI/include/MantleAPI/Common/orientation.h b/MantleAPI/include/MantleAPI/Common/orientation.h
index ab2038fbc833209a1dd79c2d55dee40d0ceaa0a6..f8575a5dc2eb7963a0634f4d275337cd2428e20b 100644
--- a/MantleAPI/include/MantleAPI/Common/orientation.h
+++ b/MantleAPI/include/MantleAPI/Common/orientation.h
@@ -16,41 +16,62 @@
#define MANTLEAPI_COMMON_ORIENTATION_H
#include <MantleAPI/Common/floating_point_helper.h>
+#include <units.h>
+
+namespace units
+{
+
+UNIT_ADD(angular_acceleration,
+ radians_per_second_squared,
+ radians_per_second_squared,
+ rad_per_s_sq,
+ compound_unit<angle::radians, inverse<squared<time::seconds>>>)
+
+namespace category
+{
+typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-2>, std::ratio<1>> angular_acceleration_unit;
+}
+
+UNIT_ADD_CATEGORY_TRAIT(angular_acceleration)
+
+} // namespace units
namespace mantle_api
{
-template <typename T>
+template <typename T,
+ class = typename std::enable_if_t<units::traits::is_angle_unit<T>::value ||
+ units::traits::is_angular_velocity_unit<T>::value ||
+ units::traits::is_angular_acceleration_unit<T>::value>>
struct Orientation3
{
- Orientation3() = default;
- Orientation3(T yaw, T pitch, T roll) : yaw{yaw}, pitch{pitch}, roll{roll} {}
-
T yaw{};
T pitch{};
T roll{};
};
-using Orientation3d = Orientation3<double>;
-
-inline bool operator==(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline bool operator==(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
return IsEqual(lhs.yaw, rhs.yaw) && IsEqual(lhs.pitch, rhs.pitch) && IsEqual(lhs.roll, rhs.roll);
}
-inline bool operator!=(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline bool operator!=(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
return !(lhs == rhs);
}
-inline Orientation3d operator+(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline Orientation3<T> operator+(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
- return Orientation3d{lhs.yaw + rhs.yaw, lhs.pitch + rhs.pitch, lhs.roll + rhs.roll};
+ return Orientation3<T>{lhs.yaw + rhs.yaw, lhs.pitch + rhs.pitch, lhs.roll + rhs.roll};
}
-inline Orientation3d operator-(const Orientation3d& lhs, const Orientation3d& rhs) noexcept
+template <typename T>
+inline Orientation3<T> operator-(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
- return Orientation3d{lhs.yaw - rhs.yaw, lhs.pitch - rhs.pitch, lhs.roll - rhs.roll};
+ return Orientation3<T>{lhs.yaw - rhs.yaw, lhs.pitch - rhs.pitch, lhs.roll - rhs.roll};
}
} // namespace mantle_api
diff --git a/MantleAPI/include/MantleAPI/Common/pose.h b/MantleAPI/include/MantleAPI/Common/pose.h
index 85779cb8b2dfc739fa0ef9e50cf7c8e8f4e0b377..43ee627f120256d4ff8500a9213ba7f39dcd7fab 100644
--- a/MantleAPI/include/MantleAPI/Common/pose.h
+++ b/MantleAPI/include/MantleAPI/Common/pose.h
@@ -17,14 +17,19 @@
#include <MantleAPI/Common/orientation.h>
#include <MantleAPI/Common/vector.h>
+
+#include <units.h>
+
#include <iostream>
+
namespace mantle_api
{
+
struct Pose
{
- Vec3d position{};
- Orientation3d orientation{};
+ Vec3<units::length::meter_t> position{};
+ Orientation3<units::angle::radian_t> orientation{};
bool operator== (const Pose& other) const
{
diff --git a/MantleAPI/include/MantleAPI/Common/position.h b/MantleAPI/include/MantleAPI/Common/position.h
index 029e375add9efd3f925a9dc8899b516858ba7760..c25e18cef10d8afa1e64c92274fa637d1907d519 100644
--- a/MantleAPI/include/MantleAPI/Common/position.h
+++ b/MantleAPI/include/MantleAPI/Common/position.h
@@ -17,6 +17,7 @@
#include <MantleAPI/Common/floating_point_helper.h>
#include <MantleAPI/Common/vector.h>
+#include <units.h>
#include <cmath>
#include <cstdint>
@@ -24,6 +25,7 @@
namespace mantle_api
{
+
struct ReferencedObject
{
std::int32_t road{0};
@@ -35,20 +37,20 @@ struct OpenDrivePosition
ReferencedObject referenced_object{};
/// @brief Offset in s direction, unit: [m]
- double s_offset{0.0};
+ units::length::meter_t s_offset{0.0};
/// @brief Offset in t direction, unit: [m]
- double t_offset{0.0};
+ units::length::meter_t t_offset{0.0};
};
struct LatLonPosition
{
/// @brief GPS latitude, unit: [rad]
- double latitude{0.0};
+ units::angle::radian_t latitude{0.0};
/// @brief GPS longitude, unit: [rad]
- double longitude{0.0};
+ units::angle::radian_t longitude{0.0};
};
-using Position = std::variant<OpenDrivePosition, LatLonPosition, Vec3d>;
+using Position = std::variant<OpenDrivePosition, LatLonPosition, Vec3<units::length::meter_t>>;
/// @brief Equality comparison for OpenDrivePosition.
///
diff --git a/MantleAPI/include/MantleAPI/Common/simulation_time.h b/MantleAPI/include/MantleAPI/Common/simulation_time.h
index bd06dbaf31c7ec6828b99d46e2d87e34f75d8d38..66e9d8596610518c587ab4fb4ef0615275bdea56 100644
--- a/MantleAPI/include/MantleAPI/Common/simulation_time.h
+++ b/MantleAPI/include/MantleAPI/Common/simulation_time.h
@@ -15,15 +15,15 @@
#ifndef MANTLEAPI_COMMON_SIMULATION_TIME_H
#define MANTLEAPI_COMMON_SIMULATION_TIME_H
-#include "time_utils.h"
+#include <MantleAPI/Common/time_utils.h>
namespace mantle_api
{
struct SimulationTime
{
- mantle_api::Time current_sim_time_ms{0};
- mantle_api::Time last_delta_time_ms{0};
+ Time current_sim_time{0};
+ Time last_delta_time{0};
};
} // namespace mantle_api
diff --git a/MantleAPI/include/MantleAPI/Common/spline.h b/MantleAPI/include/MantleAPI/Common/spline.h
index e498e2229069c59ff415cc111dacf7437a347ce3..7e6f5d700b335738bd4488008855d2258c421c46 100644
--- a/MantleAPI/include/MantleAPI/Common/spline.h
+++ b/MantleAPI/include/MantleAPI/Common/spline.h
@@ -19,40 +19,45 @@
#include <MantleAPI/Common/floating_point_helper.h>
#include <MantleAPI/Common/time_utils.h>
#include <MantleAPI/Common/vector.h>
+#include <units.h>
#include <array>
namespace mantle_api
{
+
+template <typename T>
struct SplineSegment
{
- mantle_api::Vec3d a;
- mantle_api::Vec3d b;
- mantle_api::Vec3d c;
- mantle_api::Vec3d d;
+ Vec3<T> a;
+ Vec3<T> b;
+ Vec3<T> c;
+ Vec3<T> d;
};
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
struct SplineSection
{
- mantle_api::Time start_time{0};
- mantle_api::Time end_time{0};
+ Time start_time{0};
+ 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{0, 0, 0, 0};
+ std::array<T, 4> polynomial{0, 0, 0, 0};
};
/// @brief Equality comparison for SplineSection.
-inline bool operator==(const SplineSection& lhs, const SplineSection& rhs) noexcept
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
+inline bool operator==(const SplineSection<T>& lhs, const SplineSection<T>& rhs) noexcept
{
return lhs.start_time == rhs.start_time && lhs.end_time == rhs.end_time &&
std::equal(lhs.polynomial.begin(),
lhs.polynomial.end(),
rhs.polynomial.begin(),
- [](const double a, const double b) { return IsEqual(a, b); });
+ [](const T& a, const T& b) { return IsEqual(a, b); });
}
} // namespace mantle_api
diff --git a/MantleAPI/include/MantleAPI/Common/time_utils.h b/MantleAPI/include/MantleAPI/Common/time_utils.h
index 78b9b51a96ad6cef5f699666fce256b87e1537c7..73a727229bc06ba53d03d3c1f42ab9a9154ddf9c 100644
--- a/MantleAPI/include/MantleAPI/Common/time_utils.h
+++ b/MantleAPI/include/MantleAPI/Common/time_utils.h
@@ -15,11 +15,11 @@
#ifndef MANTLEAPI_COMMON_TIME_UTILS_H
#define MANTLEAPI_COMMON_TIME_UTILS_H
-#include <chrono>
+#include <units.h>
namespace mantle_api
{
-using Time = std::chrono::duration<std::int64_t, std::milli>;
+using Time = units::time::millisecond_t;
/// @brief Converts input in [s] to @ref Time.
/// @tparam T Input type, eg. `double`.
@@ -28,7 +28,7 @@ using Time = std::chrono::duration<std::int64_t, std::milli>;
template <typename T>
inline Time SecondsToTime(T duration)
{
- return std::chrono::duration_cast<Time>(std::chrono::duration<T>{duration});
+ return units::convert<units::time::seconds, Time>(duration);
}
/// @brief Converts input @ref Time to [s].
@@ -36,7 +36,7 @@ inline Time SecondsToTime(T duration)
/// @return Duration in seconds representing the passed in @ref Time.
inline double TimeToSeconds(const Time& time)
{
- return static_cast<double>(time.count()) / 1000.0;
+ return units::time::second_t{time}.value();
}
} // namespace mantle_api
diff --git a/MantleAPI/include/MantleAPI/Common/vector.h b/MantleAPI/include/MantleAPI/Common/vector.h
index 498cbdc715327d9247c5459298ce3e429162ecdf..f4165966f27eeb680e01578dd3bd2748f30e85e3 100644
--- a/MantleAPI/include/MantleAPI/Common/vector.h
+++ b/MantleAPI/include/MantleAPI/Common/vector.h
@@ -21,53 +21,54 @@
namespace mantle_api
{
-
-template <typename T>
+template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>>
struct Vec3
{
- Vec3() = default;
- Vec3(T x, T y, T z) : x{x}, y{y}, z{z} {}
-
- T x{};
- T y{};
- T z{};
+ T x{0};
+ T y{0};
+ T z{0};
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
+template <typename T>
+inline bool operator==(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{
return IsEqual(lhs.x, rhs.x) && IsEqual(lhs.y, rhs.y) && IsEqual(lhs.z, rhs.z);
}
-inline bool operator!=(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline bool operator!=(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{
return !(lhs == rhs);
}
-inline Vec3d operator-(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator-(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{
return {lhs.x - rhs.x, lhs.y - rhs.y, lhs.z - rhs.z};
}
-inline Vec3d operator+(const Vec3d& lhs, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator+(const Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{
return {lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z};
}
-inline Vec3d operator*(const Vec3d& lhs, double d) noexcept
+template <typename T>
+inline Vec3<T> operator*(const Vec3<T>& lhs, double d) noexcept
{
return {lhs.x * d, lhs.y * d, lhs.z * d};
}
-inline Vec3d operator*(double d, const Vec3d& rhs) noexcept
+template <typename T>
+inline Vec3<T> operator*(double d, const Vec3<T>& rhs) noexcept
{
return rhs * d;
}
-inline Vec3d operator/(const Vec3d& lhs, double d) noexcept
+template <typename T>
+inline Vec3<T> operator/(const Vec3<T>& lhs, double d) noexcept
{
return {lhs.x / d, lhs.y / d, lhs.z / d};
}
diff --git a/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h b/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h
index b879da032a435cc6430ebf376aa2d9f6f308b995..6f9dbccbbac51e6b931c52e0d3e6ba17af650758 100644
--- a/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h
+++ b/MantleAPI/include/MantleAPI/EnvironmentalConditions/date_time.h
@@ -17,15 +17,13 @@
#include <MantleAPI/Common/time_utils.h>
-#include <chrono>
-
namespace mantle_api
{
// TODO: Delete this struct and use Time directly in Get/SetDateTime once the move to the MantleAPI is complete
struct [[deprecated]] DateTime
{
- Time date_time_ms{0};
+ Time date_time{0};
};
} // namespace mantle_api
diff --git a/MantleAPI/include/MantleAPI/EnvironmentalConditions/road_condition.h b/MantleAPI/include/MantleAPI/EnvironmentalConditions/road_condition.h
index 147ab828f0c6f4e4214cf0e4f5836ee15831b2c2..4464c8417ebb4da0eb106e87c9c24146a7305c2f 100644
--- a/MantleAPI/include/MantleAPI/EnvironmentalConditions/road_condition.h
+++ b/MantleAPI/include/MantleAPI/EnvironmentalConditions/road_condition.h
@@ -16,19 +16,21 @@
#define MANTLEAPI_ENVIRONMENTALCONDITIONS_ROADCONDITION_H
#include <MantleAPI/Common/position.h>
+#include <units.h>
+
namespace mantle_api
{
struct Rectangle
{
- Position bottom_left;
- Position top_right;
+ Position bottom_left{};
+ Position top_right{};
};
struct FrictionPatch
{
- Rectangle bounding_box;
- double friction;
+ Rectangle bounding_box{};
+ units::concentration::percent_t friction{100.0};
};
} // namespace mantle_api
#endif // MANTLEAPI_ENVIRONMENTALCONDITIONS_ROADCONDITION_H
diff --git a/MantleAPI/include/MantleAPI/EnvironmentalConditions/weather.h b/MantleAPI/include/MantleAPI/EnvironmentalConditions/weather.h
index aa11aa9f2d95ad59a4323223dec8023efbcad4c3..7a1b7f15e288c0972c28fa68cc85fbe20da10acd 100644
--- a/MantleAPI/include/MantleAPI/EnvironmentalConditions/weather.h
+++ b/MantleAPI/include/MantleAPI/EnvironmentalConditions/weather.h
@@ -15,10 +15,11 @@
#ifndef MANTLEAPI_ENVIRONMENTALCONDITIONS_WEATHER_H
#define MANTLEAPI_ENVIRONMENTALCONDITIONS_WEATHER_H
-#include <chrono>
+#include <units.h>
namespace mantle_api
{
+
enum class Precipitation
{
kUnknown,
@@ -66,10 +67,11 @@ struct Weather
Fog fog{Fog::kExcellentVisibility};
Precipitation precipitation{Precipitation::kNone};
Illumination illumination{Illumination::kOther};
- double humidity_percent{0.0};
- double temperature_kelvin{0.0};
- double atmospheric_pressure_pascal{0.0};
+ units::concentration::percent_t humidity{0.0};
+ units::temperature::kelvin_t temperature{0.0};
+ units::pressure::pascal_t atmospheric_pressure{0.0};
};
+
} // namespace mantle_api
#endif // MANTLEAPI_ENVIRONMENTALCONDITIONS_WEATHER_H
diff --git a/MantleAPI/include/MantleAPI/Execution/i_environment.h b/MantleAPI/include/MantleAPI/Execution/i_environment.h
index bbd668623bd6b52eecabe34b2579f3febb15304f..520525a2dd81063732f8ed5f35e8ad02630f2e47 100644
--- a/MantleAPI/include/MantleAPI/Execution/i_environment.h
+++ b/MantleAPI/include/MantleAPI/Execution/i_environment.h
@@ -27,7 +27,6 @@
namespace mantle_api
{
-
class IEnvironment
{
public:
@@ -50,24 +49,22 @@ class IEnvironment
///
/// @param entity The entity to be manipulated by the specified controller.
/// @param controller_id Identifies the controller to manipulate the entity.
- virtual void AddEntityToController(IEntity& entity, std::uint64_t controller_id) = 0;
+ virtual void AddEntityToController(IEntity& entity, UniqueId controller_id) = 0;
- virtual void RemoveControllerFromEntity(std::uint64_t entity_id) = 0;
+ virtual void RemoveControllerFromEntity(UniqueId entity_id) = 0;
/// Updates the control strategies for an entity.
///
/// @param entity_id Specifies the entity to be updated
/// @param control_strategies Specifies the desired movement behaviour for the entity
virtual void UpdateControlStrategies(
- std::uint64_t entity_id,
- std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
+ UniqueId entity_id, std::vector<std::unique_ptr<mantle_api::ControlStrategy>>& control_strategies) = 0;
/// Checks, if a control strategy of a certain type for a specific entity has been fulfilled
///
/// @param entity_id The entity to check
/// @param type The control strategy type
- virtual bool HasControlStrategyGoalBeenReached(std::uint64_t entity_id,
- mantle_api::ControlStrategyType type) const = 0;
+ virtual bool HasControlStrategyGoalBeenReached(UniqueId entity_id, mantle_api::ControlStrategyType type) const = 0;
virtual const ILaneLocationQueryService& GetQueryService() const = 0;
virtual const ICoordConverter* GetConverter() const = 0;
diff --git a/MantleAPI/include/MantleAPI/Map/i_coord_converter.h b/MantleAPI/include/MantleAPI/Map/i_coord_converter.h
index 87892dde35993437e7721693f271aab299ca9d4c..e29bc76c7868b7c9b1933a13e8ce14132dfe903a 100644
--- a/MantleAPI/include/MantleAPI/Map/i_coord_converter.h
+++ b/MantleAPI/include/MantleAPI/Map/i_coord_converter.h
@@ -17,6 +17,7 @@
#include <MantleAPI/Common/position.h>
#include <MantleAPI/Common/vector.h>
+#include <units.h>
namespace mantle_api
{
@@ -28,11 +29,12 @@ class ICoordConverter
public:
virtual ~ICoordConverter() = default;
/// Converts a track position to its corresponding inertial position.
- virtual Vec3d Convert(Position position) const = 0;
+ virtual Vec3<units::length::meter_t> Convert(Position position) const = 0;
/// Converts alane position to its corresponding inertial position.
- virtual Position Convert(Vec3d) const = 0;
+ virtual Position Convert(const Vec3<units::length::meter_t>& inert_pos) const = 0;
};
+
} // namespace mantle_api
#endif // MANTLEAPI_MAP_ICOORDCONVERTER_H
diff --git a/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h b/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h
index 5cf1884b36bfa2afb1996e0b3f223f25b4bfaa97..543782f3846bbab7e74cd1f08acf1b5bd57ff970 100644
--- a/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h
+++ b/MantleAPI/include/MantleAPI/Map/i_lane_location_query_service.h
@@ -18,6 +18,7 @@
#include <MantleAPI/Common/i_identifiable.h>
#include <MantleAPI/Common/orientation.h>
#include <MantleAPI/Common/vector.h>
+#include <units.h>
namespace mantle_api
{
@@ -35,12 +36,14 @@ class ILaneLocationQueryService
/// would add a rat-tail of more interfaces we need to define (ILaneLocation, ILane, ...?)
// virtual const IIdentifiable& GetMapObjectById(UniqueId id) = 0;
- virtual Orientation3d GetLaneOrientation(const Vec3d& position) const = 0;
- virtual Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position,
- double upwards_shift,
- bool allow_invalid_positions = false) const = 0;
- virtual bool IsPositionOnLane(const Vec3d& position) const = 0;
+ virtual Orientation3<units::angle::radian_t> GetLaneOrientation(
+ const Vec3<units::length::meter_t>& position) const = 0;
+ virtual Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
+ double upwards_shift,
+ bool allow_invalid_positions = false) const = 0;
+ virtual bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const = 0;
};
+
} // namespace mantle_api
#endif // MANTLEAPI_MAP_ILANELOCATIONQUERYSERVICE_H
diff --git a/MantleAPI/include/MantleAPI/Map/i_route.h b/MantleAPI/include/MantleAPI/Map/i_route.h
index 8079861f46fc4a3691b2147e0a9c0e76c9c069ce..1c685925e2341e4d2b41964eeaa5d93410f72e1a 100644
--- a/MantleAPI/include/MantleAPI/Map/i_route.h
+++ b/MantleAPI/include/MantleAPI/Map/i_route.h
@@ -17,9 +17,11 @@
#include <MantleAPI/Common/i_identifiable.h>
#include <MantleAPI/Common/vector.h>
+#include <units.h>
namespace mantle_api
{
+
enum class LaneId
{
kLeft9,
@@ -46,14 +48,18 @@ enum class LaneId
class IRoute : public virtual IIdentifiable
{
public:
- virtual IRoute& AddWaypoint(const Vec3d& inert_pos) = 0;
- virtual IRoute& AddWaypoint(Vec3d&& inert_pos) = 0;
- virtual Vec3d GetInertPos(double route_pos, LaneId lane_id, double lane_offset = 0) const = 0;
- virtual double GetLaneWidth(double route_pos, LaneId lane_id) const = 0;
- virtual LaneId GetLaneId(const Vec3d& inert_pos) const = 0;
- virtual double GetDistanceFromStartTo(const Vec3d& inert_pos) const = 0;
- virtual double GetLength() const = 0;
+ virtual IRoute& AddWaypoint(const Vec3<units::length::meter_t>& inert_pos) = 0;
+ virtual IRoute& AddWaypoint(Vec3<units::length::meter_t>&& inert_pos) = 0;
+ virtual Vec3<units::length::meter_t> GetInertPos(units::length::meter_t route_pos,
+ LaneId lane_id,
+ units::length::meter_t lane_offset = units::length::meter_t{
+ 0.0}) const = 0;
+ virtual units::length::meter_t GetLaneWidth(units::length::meter_t route_pos, LaneId lane_id) const = 0;
+ virtual LaneId GetLaneId(const Vec3<units::length::meter_t>& inert_pos) const = 0;
+ virtual units::length::meter_t GetDistanceFromStartTo(const Vec3<units::length::meter_t>& inert_pos) const = 0;
+ virtual units::length::meter_t GetLength() const = 0;
};
+
} // namespace mantle_api
#endif // MANTLEAPI_MAP_IROUTE_H
diff --git a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
index 22dffa888a368cb6653c04ba181029a494c3ea77..cdd4b0ebcb67e2e3aad6704f4ae4ef1fe225dc57 100644
--- a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
+++ b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
@@ -93,7 +93,7 @@ struct FollowHeadingSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowHeadingSpline;
}
- std::vector<mantle_api::SplineSection> heading_splines;
+ std::vector<mantle_api::SplineSection<units::angle::radian_t>> heading_splines;
double default_value{0};
};
@@ -105,7 +105,7 @@ struct FollowVelocitySplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowVelocitySpline;
}
- std::vector<mantle_api::SplineSection> velocity_splines;
+ std::vector<mantle_api::SplineSection<units::velocity::meters_per_second_t>> velocity_splines;
double default_value{0};
};
@@ -129,7 +129,7 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowLateralOffsetSpline;
}
- std::vector<mantle_api::SplineSection> lateral_offset_splines;
+ std::vector<mantle_api::SplineSection<units::length::meter_t>> lateral_offset_splines;
};
struct FollowRouteControlStrategy : public ControlStrategy
@@ -140,7 +140,7 @@ struct FollowRouteControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowRoute;
}
- std::vector<mantle_api::Vec3d> waypoints;
+ std::vector<mantle_api::Vec3<units::length::meter_t>> waypoints;
};
enum class Dimension
@@ -177,7 +177,7 @@ struct AcquireLaneOffsetControlStrategy : public ControlStrategy
int road_id{};
int lane_id{};
- double offset{};
+ units::length::meter_t offset{};
TransitionDynamics transition_dynamics;
};
diff --git a/MantleAPI/include/MantleAPI/Traffic/entity_properties.h b/MantleAPI/include/MantleAPI/Traffic/entity_properties.h
index 12e75fddd86349ad2faba0b4f2fffb5fb6af3de0..9d99dab77d1fe819fc6ad914748328b6e9afd9d4 100644
--- a/MantleAPI/include/MantleAPI/Traffic/entity_properties.h
+++ b/MantleAPI/include/MantleAPI/Traffic/entity_properties.h
@@ -24,6 +24,7 @@
namespace mantle_api
{
+
enum class EntityType
{
// Other (unspecified but known) type of moving object.
@@ -41,7 +42,7 @@ struct EntityProperties
{
virtual ~EntityProperties() = default;
- BoundingBox bounding_box{Vec3d{}, Dimension3d{}};
+ BoundingBox bounding_box{};
EntityType type{EntityType::kOther};
std::string model{};
std::map<std::string, std::string> properties{};
@@ -118,12 +119,9 @@ enum class ExternalControlState
struct Performance
{
- // in m/s
- double max_speed{0.0};
- // in m/s²
- double max_acceleration{0.0};
- // in m/s²
- double max_deceleration{0.0};
+ 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
@@ -135,14 +133,10 @@ inline bool operator==(const Performance& lhs, const Performance& rhs) noexcept
struct Axle
{
- // in radian
- double max_steering{0.0};
- // in m
- double wheel_diameter{0.0};
- // in m
- double track_width{0.0};
- // in m
- Vec3d bb_center_to_axle_center{};
+ 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
diff --git a/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h b/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h
index c6e23d7eaf349131d678302af053004f257e20c0..f5cf665f7c77d1042c26bb9ba06bbd179b492f56 100644
--- a/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h
+++ b/MantleAPI/include/MantleAPI/Traffic/i_controller_config.h
@@ -15,11 +15,10 @@
#ifndef MANTLEAPI_TRAFFIC_ICONTROLLERCONFIG_H
#define MANTLEAPI_TRAFFIC_ICONTROLLERCONFIG_H
-#include "control_strategy.h"
-
#include <MantleAPI/Common/spline.h>
#include <MantleAPI/Common/vector.h>
#include <MantleAPI/Map/i_lane_location_query_service.h>
+#include <MantleAPI/Traffic/control_strategy.h>
#include <algorithm>
#include <map>
@@ -35,11 +34,11 @@ struct IControllerConfig
IControllerConfig(const IControllerConfig& controller_config)
: map_query_service(controller_config.map_query_service), id(controller_config.id)
{
- 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); });
+ 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;
diff --git a/MantleAPI/include/MantleAPI/Traffic/i_entity.h b/MantleAPI/include/MantleAPI/Traffic/i_entity.h
index 89b1e48aff7fb91bb3a07a7b3aa7fda4c1896af2..162bf7427e0142df7985aeca05166de910303a9c 100644
--- a/MantleAPI/include/MantleAPI/Traffic/i_entity.h
+++ b/MantleAPI/include/MantleAPI/Traffic/i_entity.h
@@ -26,27 +26,31 @@
namespace mantle_api
{
+
/// Base interface for all static and dynamic scenario entities.
class IEntity : public IIdentifiable
{
public:
- virtual void SetPosition(const Vec3d& inert_pos) = 0;
- virtual Vec3d GetPosition() const = 0;
+ virtual void SetPosition(const Vec3<units::length::meter_t>& inert_pos) = 0;
+ virtual Vec3<units::length::meter_t> GetPosition() const = 0;
- virtual void SetVelocity(const Vec3d& velocity) = 0;
- virtual Vec3d GetVelocity() const = 0;
+ virtual void SetVelocity(const Vec3<units::velocity::meters_per_second_t>& velocity) = 0;
+ virtual Vec3<units::velocity::meters_per_second_t> GetVelocity() const = 0;
- virtual void SetAcceleration(const Vec3d& acceleration) = 0;
- virtual Vec3d GetAcceleration() const = 0;
+ virtual void SetAcceleration(const Vec3<units::acceleration::meters_per_second_squared_t>& acceleration) = 0;
+ virtual Vec3<units::acceleration::meters_per_second_squared_t> GetAcceleration() const = 0;
- virtual void SetOrientation(const Orientation3d& orientation) = 0;
- virtual Orientation3d GetOrientation() const = 0;
+ virtual void SetOrientation(const Orientation3<units::angle::radian_t>& orientation) = 0;
+ virtual Orientation3<units::angle::radian_t> GetOrientation() const = 0;
- virtual void SetOrientationRate(const Orientation3d& orientation_rate) = 0;
- virtual Orientation3d GetOrientationRate() const = 0;
+ virtual void SetOrientationRate(
+ const Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate) = 0;
+ virtual Orientation3<units::angular_velocity::radians_per_second_t> GetOrientationRate() const = 0;
- virtual void SetOrientationAcceleration(const Orientation3d& orientation_acceleration) = 0;
- virtual Orientation3d GetOrientationAcceleration() const = 0;
+ virtual void SetOrientationAcceleration(
+ const Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration) = 0;
+ virtual Orientation3<units::angular_acceleration::radians_per_second_squared_t> GetOrientationAcceleration()
+ const = 0;
virtual void SetProperties(std::unique_ptr<mantle_api::EntityProperties> properties) = 0;
virtual EntityProperties* GetProperties() const = 0;