Skip to content

GitLab

Commit 8a7f09a5 authored by Christoph Kochendoerfer's avatar Christoph Kochendoerfer
Browse files

Fixed some merging issues regarding units

parent 4b445937
Hide whitespace changes
Inline Side-by-side
MantleAPI/include/MantleAPI/Common/floating_point_helper.h
View file @ 8a7f09a5
......@@ -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>>
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.
......
MantleAPI/include/MantleAPI/Common/poly_line.h
View file @ 8a7f09a5
......@@ -42,7 +42,7 @@ inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLineP
if (polyLinePoint.time.has_value())
{
os << ", time in ms " << polyLinePoint.time.value().count();
os << ", time in ms " << polyLinePoint.time.value();
}
return os;
......
MantleAPI/include/MantleAPI/Common/spline.h
View file @ 8a7f09a5
......@@ -21,7 +21,9 @@
#include <MantleAPI/Common/vector.h>
#include <units.h>
#include <array>
#include <tuple>
using namespace units;
namespace mantle_api
{
......@@ -34,29 +36,33 @@ struct SplineSegment
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
{
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
/// The tuple 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<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.
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
{
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 T& a, const T& b) { return IsEqual(a, b); });
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));
}
} // namespace mantle_api
......
MantleAPI/include/MantleAPI/Common/vector.h
View file @ 8a7f09a5
......@@ -28,7 +28,7 @@ struct Vec3
T y{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>
......@@ -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};
}
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.y += rhs.y;
......@@ -81,7 +82,8 @@ inline Vec3d operator+=(Vec3d& lhs, const Vec3d& rhs) noexcept
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.y -= rhs.y;
......@@ -89,15 +91,16 @@ inline Vec3d operator-=(Vec3d& lhs, const Vec3d& rhs) noexcept
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.y += d;
lhs.z += d;
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.y -= d;
......
MantleAPI/include/MantleAPI/Traffic/control_strategy.h
View file @ 8a7f09a5
......@@ -92,8 +92,8 @@ struct FollowHeadingSplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowHeadingSpline;
}
std::vector<mantle_api::SplineSection<units::angle::radian_t>> heading_splines;
double default_value{0};
std::vector<mantle_api::SplineSection<units::angle::radian>> heading_splines;
units::angle::radian_t default_value{0};
};
struct FollowVelocitySplineControlStrategy : public ControlStrategy
......@@ -104,8 +104,8 @@ struct FollowVelocitySplineControlStrategy : public ControlStrategy
type = ControlStrategyType::kFollowVelocitySpline;
}
std::vector<mantle_api::SplineSection<units::velocity::meters_per_second_t>> velocity_splines;
double default_value{0};
std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>> velocity_splines;
units::velocity::meters_per_second_t default_value{0};
};
inline bool operator==(const FollowVelocitySplineControlStrategy& lhs,
......@@ -128,7 +128,7 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
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
......@@ -194,7 +194,7 @@ struct FollowTrajectoryControlStrategy : public ControlStrategy
{
ReferenceContext domainAbsoluteRelative;
double scale{0.0};
double offset{0.0};
units::time::second_t offset{0.0};
};
FollowTrajectoryControlStrategy()
......
MantleAPI/include/MantleAPI/Traffic/entity_helper.h
View file @ 8a7f09a5
......@@ -21,14 +21,15 @@
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();
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)};
auto cos_elevation = units::math::cos(orientation.pitch);
mantle_api::Vec3<units::velocity::meters_per_second_t> velocity_vector{velocity * units::math::cos(orientation.yaw) * cos_elevation,
velocity * units::math::sin(orientation.yaw) * cos_elevation,
velocity * -units::math::sin(orientation.pitch)};
entity->SetVelocity(velocity_vector);
}
......
MantleAPI/test/MantleAPI/Test/test_utils.h
View file @ 8a7f09a5
......@@ -33,9 +33,9 @@ namespace mantle_api
class MockConverter : public mantle_api::ICoordConverter
{
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;
return mantle_api::Position{};
......@@ -50,26 +50,26 @@ public:
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override));
MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override));
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (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(mantle_api::Vec3d, GetVelocity, (), (const, override));
MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (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(mantle_api::Vec3d, GetAcceleration, (), (const, override));
MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (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(mantle_api::Orientation3d, GetOrientation, (), (const, override));
MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (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(mantle_api::Orientation3d, GetOrientationRate, (), (const, override));
MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void,
SetOrientationAcceleration,
(const mantle_api::Orientation3d& orientation_acceleration),
(const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(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(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
......@@ -91,22 +91,22 @@ private:
class MockQueryService : public mantle_api::ILaneLocationQueryService
{
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;
return {};
}
Vec3d GetUpwardsShiftedLanePosition(const Vec3d& position,
Vec3<units::length::meter_t> GetUpwardsShiftedLanePosition(const Vec3<units::length::meter_t>& position,
double upwards_shift,
bool allowed_to_leave_lane = false) const override
{
std::ignore = position;
std::ignore = upwards_shift;
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;
return false;
......@@ -124,26 +124,26 @@ public:
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override));
MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override));
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (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(mantle_api::Vec3d, GetVelocity, (), (const, override));
MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (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(mantle_api::Vec3d, GetAcceleration, (), (const, override));
MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (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(mantle_api::Orientation3d, GetOrientation, (), (const, override));
MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (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(mantle_api::Orientation3d, GetOrientationRate, (), (const, override));
MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void,
SetOrientationAcceleration,
(const mantle_api::Orientation3d& orientation_acceleration),
(const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(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(std::vector<std::uint64_t>, GetAssignedLaneIds, (), (const, override));
......@@ -167,26 +167,26 @@ public:
void SetName(const std::string& name) override { name_ = name; }
const std::string& GetName() const override { return name_; }
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3d& inert_pos), (override));
MOCK_METHOD(mantle_api::Vec3d, GetPosition, (), (const, override));
MOCK_METHOD(void, SetPosition, (const mantle_api::Vec3<units::length::meter_t>& inert_pos), (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(mantle_api::Vec3d, GetVelocity, (), (const, override));
MOCK_METHOD(void, SetVelocity, (const mantle_api::Vec3<units::velocity::meters_per_second_t>& velocity), (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(mantle_api::Vec3d, GetAcceleration, (), (const, override));
MOCK_METHOD(void, SetAcceleration, (const mantle_api::Vec3<units::acceleration::meters_per_second_squared_t>& acceleration), (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(mantle_api::Orientation3d, GetOrientation, (), (const, override));
MOCK_METHOD(void, SetOrientation, (const mantle_api::Orientation3<units::angle::radian_t>& orientation), (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(mantle_api::Orientation3d, GetOrientationRate, (), (const, override));
MOCK_METHOD(void, SetOrientationRate, (const mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>& orientation_rate), (override));
MOCK_METHOD(mantle_api::Orientation3<units::angular_velocity::radians_per_second_t>, GetOrientationRate, (), (const, override));
MOCK_METHOD(void,
SetOrientationAcceleration,
(const mantle_api::Orientation3d& orientation_acceleration),
(const mantle_api::Orientation3<units::angular_acceleration::radians_per_second_squared_t>& orientation_acceleration),
(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(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

Copyright © Eclipse Foundation, Inc. All Rights Reserved.     Privacy Policy | Terms of Use | Copyright Agent