Commit 6e78b450 authored by Christoph Kochendoerfer's avatar Christoph Kochendoerfer
Browse files

Merge branch 'bugfix/unit_usage' into 'master'

Fixed some merging issues regarding units

See merge request !17
parents 4b445937 8a7f09a5
...@@ -59,7 +59,7 @@ inline bool IsEqual(double lhs, double rhs, double precision = std::numeric_limi ...@@ -59,7 +59,7 @@ inline bool IsEqual(double lhs, double rhs, double precision = std::numeric_limi
template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>> 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()) bool IsEqual(T lhs, T rhs, double precision = std::numeric_limits<double>::epsilon())
{ {
return units::math::abs(lhs - rhs) < precision; return units::unit_cast<double>(units::math::abs(lhs - rhs)) < precision;
} }
/// @brief Compares two floating point numbers for equality. /// @brief Compares two floating point numbers for equality.
......
...@@ -42,7 +42,7 @@ inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLineP ...@@ -42,7 +42,7 @@ inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLineP
if (polyLinePoint.time.has_value()) if (polyLinePoint.time.has_value())
{ {
os << ", time in ms " << polyLinePoint.time.value().count(); os << ", time in ms " << polyLinePoint.time.value();
} }
return os; return os;
......
...@@ -21,7 +21,9 @@ ...@@ -21,7 +21,9 @@
#include <MantleAPI/Common/vector.h> #include <MantleAPI/Common/vector.h>
#include <units.h> #include <units.h>
#include <array> #include <tuple>
using namespace units;
namespace mantle_api namespace mantle_api
{ {
...@@ -34,29 +36,33 @@ struct SplineSegment ...@@ -34,29 +36,33 @@ struct SplineSegment
Vec3<T> d; Vec3<T> d;
}; };
template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>> template <typename T, class = typename std::enable_if_t<traits::is_unit<T>::value>>
struct SplineSection struct SplineSection
{ {
Time start_time{0}; Time start_time{0};
Time end_time{0}; Time end_time{0};
/// @brief Represents the polynomial. /// @brief Represents the polynomial.
/// ///
/// The array stores in format \f$[a_3, a_2, a_1, a_0]\f$ for a polynomial in form /// The tuple stores in format \f$[a_3, a_2, a_1, a_0]\f$ for a polynomial in form
/// \f[ /// \f[
/// P(x) = \sum_{i=0}^{3} a_{i} x^{i} = a_3 x^3 + a_2 x^2 + a_1 x + a_0 /// P(x) = \sum_{i=0}^{3} a_{i} x^{i} = a_3 x^3 + a_2 x^2 + a_1 x + a_0
/// \f] /// \f]
std::array<T, 4> polynomial{0, 0, 0, 0}; std::tuple<unit_t<compound_unit<T, inverse<cubed<time::second>>>>,
unit_t<compound_unit<T, inverse<squared<time::second>>>>,
unit_t<compound_unit<T, inverse<time::second>>>,
unit_t<T>>
polynomial{
unit_t<compound_unit<T, inverse<cubed<time::second>>>>(0),
unit_t<compound_unit<T, inverse<squared<time::second>>>>(0),
unit_t<compound_unit<T, inverse<time::second>>>(0),
unit_t<T>(0)};
}; };
/// @brief Equality comparison for SplineSection. /// @brief Equality comparison for SplineSection.
template <typename T, class = typename std::enable_if_t<units::traits::is_unit_t<T>::value>> template <typename T, class = typename std::enable_if_t<units::traits::is_unit<T>::value>>
inline bool operator==(const SplineSection<T>& lhs, const SplineSection<T>& rhs) noexcept 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 && return lhs.start_time == rhs.start_time && lhs.end_time == rhs.end_time && IsEqual(std::get<0>(lhs.polynomial), std::get<0>(rhs.polynomial)) && IsEqual(std::get<1>(lhs.polynomial), std::get<1>(rhs.polynomial)) && IsEqual(std::get<2>(lhs.polynomial), std::get<2>(rhs.polynomial)) && IsEqual(std::get<3>(lhs.polynomial), std::get<3>(rhs.polynomial));
std::equal(lhs.polynomial.begin(),
lhs.polynomial.end(),
rhs.polynomial.begin(),
[](const T& a, const T& b) { return IsEqual(a, b); });
} }
} // namespace mantle_api } // namespace mantle_api
......
...@@ -28,7 +28,7 @@ struct Vec3 ...@@ -28,7 +28,7 @@ struct Vec3
T y{0}; T y{0};
T z{0}; T z{0};
inline T Length() const { return sqrt((x * x) + (y * y) + (z * z)); } inline T Length() const { return units::math::sqrt((x * x) + (y * y) + (z * z)); }
}; };
template <typename T> template <typename T>
...@@ -73,7 +73,8 @@ inline Vec3<T> operator/(const Vec3<T>& lhs, double d) noexcept ...@@ -73,7 +73,8 @@ inline Vec3<T> operator/(const Vec3<T>& lhs, double d) noexcept
return {lhs.x / d, lhs.y / d, lhs.z / d}; return {lhs.x / d, lhs.y / d, lhs.z / d};
} }
inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept template <typename T>
inline Vec3<T> operator+=(Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{ {
lhs.x += rhs.x; lhs.x += rhs.x;
lhs.y += rhs.y; lhs.y += rhs.y;
...@@ -81,7 +82,8 @@ inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept ...@@ -81,7 +82,8 @@ inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept
return lhs; return lhs;
} }
inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept template <typename T>
inline Vec3<T> operator-=(Vec3<T>& lhs, const Vec3<T>& rhs) noexcept
{ {
lhs.x -= rhs.x; lhs.x -= rhs.x;
lhs.y -= rhs.y; lhs.y -= rhs.y;
...@@ -89,15 +91,16 @@ inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept ...@@ -89,15 +91,16 @@ inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept
return lhs; return lhs;
} }
inline Vec3d operator+=(Vec3d& lhs, double d) noexcept template <typename T>
inline Vec3<T> operator+=(Vec3<T>& lhs, double d) noexcept
{ {
lhs.x += d; lhs.x += d;
lhs.y += d; lhs.y += d;
lhs.z += d; lhs.z += d;
return lhs; return lhs;
} }
template <typename T>
inline Vec3d operator-=(Vec3d& lhs, double d) noexcept inline Vec3<T> operator-=(Vec3<T>& lhs, double d) noexcept
{ {
lhs.x -= d; lhs.x -= d;
lhs.y -= d; lhs.y -= d;
......
...@@ -92,8 +92,8 @@ struct FollowHeadingSplineControlStrategy : public ControlStrategy ...@@ -92,8 +92,8 @@ struct FollowHeadingSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowHeadingSpline; type = ControlStrategyType::kFollowHeadingSpline;
} }
std::vector<mantle_api::SplineSection<units::angle::radian_t>> heading_splines; std::vector<mantle_api::SplineSection<units::angle::radian>> heading_splines;
double default_value{0}; units::angle::radian_t default_value{0};
}; };
struct FollowVelocitySplineControlStrategy : public ControlStrategy struct FollowVelocitySplineControlStrategy : public ControlStrategy
...@@ -104,8 +104,8 @@ struct FollowVelocitySplineControlStrategy : public ControlStrategy ...@@ -104,8 +104,8 @@ struct FollowVelocitySplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowVelocitySpline; type = ControlStrategyType::kFollowVelocitySpline;
} }
std::vector<mantle_api::SplineSection<units::velocity::meters_per_second_t>> velocity_splines; std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>> velocity_splines;
double default_value{0}; units::velocity::meters_per_second_t default_value{0};
}; };
inline bool operator==(const FollowVelocitySplineControlStrategy& lhs, inline bool operator==(const FollowVelocitySplineControlStrategy& lhs,
...@@ -128,7 +128,7 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy ...@@ -128,7 +128,7 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowLateralOffsetSpline; type = ControlStrategyType::kFollowLateralOffsetSpline;
} }
std::vector<mantle_api::SplineSection<units::length::meter_t>> lateral_offset_splines; std::vector<mantle_api::SplineSection<units::length::meter>> lateral_offset_splines;
}; };
struct FollowRouteControlStrategy : public ControlStrategy struct FollowRouteControlStrategy : public ControlStrategy
...@@ -194,7 +194,7 @@ struct FollowTrajectoryControlStrategy : public ControlStrategy ...@@ -194,7 +194,7 @@ struct FollowTrajectoryControlStrategy : public ControlStrategy
{ {
ReferenceContext domainAbsoluteRelative; ReferenceContext domainAbsoluteRelative;
double scale{0.0}; double scale{0.0};
double offset{0.0}; units::time::second_t offset{0.0};
}; };
FollowTrajectoryControlStrategy() FollowTrajectoryControlStrategy()
......
...@@ -21,14 +21,15 @@ ...@@ -21,14 +21,15 @@
namespace mantle_api namespace mantle_api
{ {
inline void SetSpeed(mantle_api::IEntity* entity, double velocity)
inline void SetSpeed(mantle_api::IEntity* entity, const units::velocity::meters_per_second_t &velocity)
{ {
auto orientation = entity->GetOrientation(); auto orientation = entity->GetOrientation();
double cos_elevation = std::cos(orientation.pitch); auto cos_elevation = units::math::cos(orientation.pitch);
mantle_api::Vec3d velocity_vector{velocity * std::cos(orientation.yaw) * cos_elevation, mantle_api::Vec3<units::velocity::meters_per_second_t> velocity_vector{velocity * units::math::cos(orientation.yaw) * cos_elevation,
velocity * std::sin(orientation.yaw) * cos_elevation, velocity * units::math::sin(orientation.yaw) * cos_elevation,
velocity * -std::sin(orientation.pitch)}; velocity * -units::math::sin(orientation.pitch)};
entity->SetVelocity(velocity_vector); entity->SetVelocity(velocity_vector);
} }
......
...@@ -33,9 +33,9 @@ namespace mantle_api ...@@ -33,9 +33,9 @@ namespace mantle_api
class MockConverter : public mantle_api::ICoordConverter class MockConverter : public mantle_api::ICoordConverter
{ {
public: public:
MOCK_METHOD(mantle_api::Vec3d, Convert, (mantle_api::Position position), (const override)); MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, Convert, (mantle_api::Position position), (const override));
mantle_api::Position Convert(mantle_api::Vec3d vec) const override mantle_api::Position Convert(mantle_api::Vec3<units::length::meter_t> vec) const override
{ {
std::ignore = vec; std::ignore = vec;
return mantle_api::Position{}; return mantle_api::Position{};
...@@ -50,26 +50,26 @@ public: ...@@ -50,26 +50,26 @@ public:
void SetName(const std::string& name) override { name_ = name; } void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; } const std::string& GetName() const override { return name_; }
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override)); MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (override));
MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, GetPosition, (), (const, override));
MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3d& velocity), (override)); MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (override));
MOCK_METHOD(mantle_api::Vec3d, GetVelocity, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::velocity::meters_per_second_t>, GetVelocity, (), (const, override));
MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3d& acceleration), (override)); MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (override));
MOCK_METHOD(mantle_api::Vec3d, GetAcceleration, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>, GetAcceleration, (), (const, override));
MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3d& orientation), (override)); MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientation, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angle::radian_t>, GetOrientation, (), (const, override));
MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3d& orientation_rate), (override)); MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientationRate, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void, MOCK_METHOD(void,
SetOrientationAcceleration, SetOrientationAcceleration,
(const mantle_api::Orientation3d& orientation_acceleration), (const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(override)); (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientationAcceleration, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>, GetOrientationAcceleration, (), (const, override));
MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override)); MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override));
MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override)); MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
...@@ -91,22 +91,22 @@ private: ...@@ -91,22 +91,22 @@ private:
class MockQueryService : public mantle_api::ILaneLocationQueryService class MockQueryService : public mantle_api::ILaneLocationQueryService
{ {
public: public:
Orientation3d GetLaneOrientation(const Vec3d& position) const override Orientation3<units::angle::radian_t> GetLaneOrientation(const Vec3<units::length::meter_t>& position) const override
{ {
std::ignore = position; std::ignore = position;
return {}; return {};
} }
Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position, Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
double upwards_shift, double upwards_shift,
bool allowed_to_leave_lane = false) const override bool allowed_to_leave_lane = false) const override
{ {
std::ignore = position; std::ignore = position;
std::ignore = upwards_shift; std::ignore = upwards_shift;
std::ignore = allowed_to_leave_lane; std::ignore = allowed_to_leave_lane;
return mantle_api::Vec3d(); return mantle_api::Vec3<units::length::meter_t>();
} }
bool IsPositionOnLane(const Vec3d& position) const override bool IsPositionOnLane(const Vec3<units::length::meter_t>& position) const override
{ {
std::ignore = position; std::ignore = position;
return false; return false;
...@@ -124,26 +124,26 @@ public: ...@@ -124,26 +124,26 @@ public:
void SetName(const std::string& name) override { name_ = name; } void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; } const std::string& GetName() const override { return name_; }
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override)); MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (override));
MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, GetPosition, (), (const, override));
MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3d& velocity), (override)); MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (override));
MOCK_METHOD(mantle_api::Vec3d, GetVelocity, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::velocity::meters_per_second_t>, GetVelocity, (), (const, override));
MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3d& acceleration), (override)); MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (override));
MOCK_METHOD(mantle_api::Vec3d, GetAcceleration, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>, GetAcceleration, (), (const, override));
MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3d& orientation), (override)); MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientation, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angle::radian_t>, GetOrientation, (), (const, override));
MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3d& orientation_rate), (override)); MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientationRate, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void, MOCK_METHOD(void,
SetOrientationAcceleration, SetOrientationAcceleration,
(const mantle_api::Orientation3d& orientation_acceleration), (const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(override)); (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientationAcceleration, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>, GetOrientationAcceleration, (), (const, override));
MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override)); MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override));
MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override)); MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
...@@ -167,26 +167,26 @@ public: ...@@ -167,26 +167,26 @@ public:
void SetName(const std::string& name) override { name_ = name; } void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; } const std::string& GetName() const override { return name_; }
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override)); MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (override));
MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::length::meter_t>, GetPosition, (), (const, override));
MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3d& velocity), (override)); MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (override));
MOCK_METHOD(mantle_api::Vec3d, GetVelocity, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::velocity::meters_per_second_t>, GetVelocity, (), (const, override));
MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3d& acceleration), (override)); MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (override));
MOCK_METHOD(mantle_api::Vec3d, GetAcceleration, (), (const, override)); MOCK_METHOD(mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>, GetAcceleration, (), (const, override));
MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3d& orientation), (override)); MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientation, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angle::radian_t>, GetOrientation, (), (const, override));
MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3d& orientation_rate), (override)); MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientationRate, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void, MOCK_METHOD(void,
SetOrientationAcceleration, SetOrientationAcceleration,
(const mantle_api::Orientation3d& orientation_acceleration), (const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(override)); (override));
MOCK_METHOD(mantle_api::Orientation3d, GetOrientationAcceleration, (), (const, override)); MOCK_METHOD(mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>, GetOrientationAcceleration, (), (const, override));
MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override)); MOCK_METHOD(void, SetAssignedLaneIds, (const std::vector<std::uint64_t>& ids), (override));
MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override)); MOCK_METHOD(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
......
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