Skip to content
Snippets Groups Projects
Commit 8a7f09a5 authored by Christoph Kochendoerfer's avatar Christoph Kochendoerfer
Browse files

Fixed some merging issues regarding units

parent 4b445937
No related branches found
No related tags found
1 merge request!17Fixed some merging issues regarding units
Hide whitespace changes
Inline Side-by-side
Showing
with 81 additions and 71 deletions
MantleAPI/include/MantleAPI/Common/floating_point_helper.h
+ 1
− 1
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
+ 1
− 1
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
+ 16
− 10
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
+ 9
− 6
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
+ 6
− 6
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
+ 6
− 5
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
+ 42
− 42
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));
......
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