diff --git a/BUILD.bazel b/BUILD.bazel
index 1241afd48a2b8ca915d26657c4eacf2b20c3dded..d26e88c2fef44b3cf0d6899b8f14a7405c6dceb3 100644
--- a/BUILD.bazel
+++ b/BUILD.bazel
@@ -18,7 +18,7 @@ cc_library(
cc_library(
name = "test_utils",
- hdrs = ["test/MantleAPI/Test/test_utils.h"],
+ hdrs = glob(["test/**/*.h"]),
includes = ["test"],
visibility = ["//visibility:public"],
deps = [
@@ -30,6 +30,6 @@ cc_library(
cc_test(
name = "interface_test",
timeout = "short",
- srcs = ["test/interface_test.cpp"],
+ srcs = ["test/interface_test.cpp"] + glob(["test/**/*.cc"]),
deps = [":test_utils"],
)
diff --git a/include/MantleAPI/Common/clothoid_spline.h b/include/MantleAPI/Common/clothoid_spline.h
new file mode 100644
index 0000000000000000000000000000000000000000..48a922799deec1b93582e8cbe11e1cc10ce83597
--- /dev/null
+++ b/include/MantleAPI/Common/clothoid_spline.h
@@ -0,0 +1,130 @@
+/*******************************************************************************
+ * Copyright (c) 2025, 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 clothoid_spline.h
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_COMMON_CLOTHOID_SPLINE_H
+#define MANTLEAPI_COMMON_CLOTHOID_SPLINE_H
+
+#include <MantleAPI/Common/pose.h>
+#include <MantleAPI/Common/unit_definitions.h>
+#include <units.h>
+
+#include <optional>
+#include <tuple>
+#include <variant>
+#include <vector>
+
+namespace mantle_api
+{
+
+/// This struct represents one segment of a clothoid spline
+struct ClothoidSplineSegment
+{
+ units::curve::curvature_t curvature_end; ///< End curvature of the clothoid segment. Unit: [1/m]. Range: ]-inf..inf[.
+ units::curve::curvature_t curvature_start; ///< Start curvature of the clothoid segment. Unit: [1/m]. Range: ]-inf..inf[.
+ units::angle::radian_t h_offset; ///< Heading offset relative to end of the previous segment or to position_start if present. Unit: [rad]. Range: ]-pi..pi[.
+ units::length::meter_t length; ///< Length of the clothoid segment. Unit: [m]. Range: ]0..inf[.
+ std::optional<Pose> position_start; ///< Optional start position of the clothoid segment.
+};
+
+/// Compare two objects of ClothoidSplineSegment.
+///
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true if the values of lhs exactly equals to rhs values
+constexpr bool operator==(const ClothoidSplineSegment& lhs, const ClothoidSplineSegment& rhs) noexcept
+{
+ return std::tie(lhs.curvature_end, lhs.curvature_start, lhs.h_offset, lhs.length, lhs.position_start) ==
+ std::tie(rhs.curvature_end, rhs.curvature_start, rhs.h_offset, rhs.length, rhs.position_start);
+};
+
+/// Print a human-readable representation of 'ClothoidSplineSegment' to 'os'.
+///
+/// @param os The output stream
+/// @param clothoid_spline_segment ClothoidSplineSegment to be printed
+/// @returns 'clothoid_spline_segment' in a human-readable format
+inline std::ostream& operator<<(std::ostream& os, const ClothoidSplineSegment& clothoid_spline_segment)
+{
+ os << "ClothoidSplineSegment("
+ << ".curvatureEnd=" << clothoid_spline_segment.curvature_end
+ << ", .curvatureStart=" << clothoid_spline_segment.curvature_start
+ << ", .hOffset=" << clothoid_spline_segment.h_offset
+ << ", .length=" << clothoid_spline_segment.length;
+
+ if (clothoid_spline_segment.position_start.has_value())
+ {
+ os << ", .position_start=" << clothoid_spline_segment.position_start.value();
+ }
+ os << ")";
+ return os;
+};
+
+/// Representation of a single clothoid or a sequence of concatenated clothoids forming a spline
+struct ClothoidSpline
+{
+ using segments_type = std::vector<ClothoidSplineSegment>; ///< Clothoid spline segments type
+ using iterator = segments_type::iterator; ///< Clothoid spline segments iterator
+ using const_iterator = segments_type::const_iterator; ///< Clothoid spline segments const iterator
+
+ segments_type segments; ///< Clothoid spline segments
+
+ /// Return an iterator to the beginning.
+ ///
+ /// @returns Return an iterator to the first element of the segments
+ iterator begin() { return segments.begin(); };
+
+ /// Return an iterator to the end.
+ ///
+ /// @returns Return an iterator to the element next after the last element of the segments
+ iterator end() { return segments.end(); };
+
+ /// Return the const iterator to the beginning.
+ ///
+ /// @returns Return the const iterator to the first element of the segments
+ [[nodiscard]] const_iterator cbegin() const { return segments.cbegin(); };
+
+ /// Return the const iterator to the end.
+ ///
+ /// @returns Return the const iterator to the element next after the last element of the segments
+ [[nodiscard]] const_iterator cend() const { return segments.cend(); };
+};
+
+/// Compare two objects of ClothoidSpline.
+///
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true if the segments values of lhs exactly equals to rhs segments values
+inline bool operator==(const ClothoidSpline& lhs, const ClothoidSpline& rhs) noexcept
+{
+ return lhs.segments == rhs.segments;
+};
+
+/// Print a human-readable representation of 'ClothoidSpline' to 'os'.
+///
+/// @param os The output stream
+/// @param clothoid_spline ClothoidSpline to be printed
+/// @returns 'clothoid_spline' in a human-readable format
+inline std::ostream& operator<<(std::ostream& os, const ClothoidSpline& clothoid_spline)
+{
+ os << "ClothoidSpline(";
+ for (std::size_t i = 0; i < clothoid_spline.segments.size(); i++)
+ {
+ os << (i == 0 ? "[" : ", [") << std::to_string(i) << "]=" << clothoid_spline.segments[i];
+ }
+ os << ")";
+ return os;
+};
+
+} // namespace mantle_api
+
+#endif // MANTLEAPI_COMMON_CLOTHOID_SPLINE_H
diff --git a/include/MantleAPI/Common/poly_line.h b/include/MantleAPI/Common/poly_line.h
index c28c40a1c432d14820f372704490ed60773f9cb1..ab3292e0e9b7a3dc7639369a137b515fa9a092bd 100644
--- a/include/MantleAPI/Common/poly_line.h
+++ b/include/MantleAPI/Common/poly_line.h
@@ -1,5 +1,6 @@
/*******************************************************************************
* Copyright (c) 2021-2023, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2024, Mercedes-Benz Tech Innovation GmbH
*
* This program and the accompanying materials are made
* available under the terms of the Eclipse Public License 2.0
@@ -18,53 +19,74 @@
#include <MantleAPI/Common/pose.h>
#include <MantleAPI/Common/time_utils.h>
+#include <iosfwd>
#include <optional>
+#include <string>
+#include <tuple>
#include <vector>
namespace mantle_api
{
-/// This struct represents the point of a polygonal chain (polyline) trajectory specification
+/// One point of a polygonal chain (polyline) trajectory specification.
struct PolyLinePoint
{
- Pose pose{}; ///< Pose of the PolyLinePoint
- std::optional<Time> time{}; ///< Time specification of the PolyLinePoint
+ Pose pose; ///< Pose of the PolyLinePoint
+ std::optional<Time> time; ///< Time specification of the PolyLinePoint
+};
- /// @brief Equality comparison for PolyLinePoint.
- ///
- /// @param[in] other The left-hand side value for the comparison
- /// @returns true if the values of `this` exactly equal to the values of other.
- bool operator==(const PolyLinePoint& other) const
- {
- return other.time == time && other.pose == pose;
- }
+/// Compare the values of two PolyLinePoints.
+///
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true, if the values of the two PolyLinePoints are equal, false otherwise
+constexpr bool operator==(const PolyLinePoint& lhs, const PolyLinePoint& rhs) noexcept
+{
+ return std::tie(lhs.pose, lhs.time) == std::tie(rhs.pose, rhs.time);
+}
- /// @brief Prints a human-readable representation of 'polyLinePoint' to 'os'.
- /// @param os The output stream
- /// @param polyLinePoint The point of a polygonal chain (polyline) trajectory specification
- /// @returns 'polyLinePoint' in a human-readable format
- friend std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLinePoint);
-};
+/// Compare the values of two PolyLinePoints.
+///
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true, if the values of the two PolyLinePoints are not equal, false otherwise
+constexpr bool operator!=(const PolyLinePoint& lhs, const PolyLinePoint& rhs) noexcept
+{
+ return !(lhs == rhs);
+}
-/// @brief Prints a human-readable representation of 'polyLinePoint' to 'os'.
-/// @param os The output stream
-/// @param polyLinePoint The point of a polygonal chain (polyline) trajectory specification
-/// @returns 'polyLinePoint' in a human-readable format
-inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLinePoint)
+/// The list of polyline points.
+using PolyLine = std::vector<PolyLinePoint>;
+
+/// Output stream operator for PolyLinePoint.
+///
+/// @param[in] os output stream
+/// @param[in] point PolyLinePoint to be printed
+/// @returns output stream with printed PolyLinePoint
+///
+inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& point)
{
- os << polyLinePoint.pose;
+ os << "PolyLinePoint(.pose=" << point.pose << ", .time=" << point.time.value_or(Time{}) << ')';
+ return os;
+}
- if (polyLinePoint.time.has_value())
+/// Output stream operator for PolyLine.
+///
+/// @param[in] os output stream
+/// @param[in] poly_line PolyLine to be printed
+/// @returns output stream with printed PolyLine
+///
+inline std::ostream& operator<<(std::ostream& os, const PolyLine& poly_line)
+{
+ os << "PolyLine(";
+ for (auto idx = 0U; idx < poly_line.size(); ++idx)
{
- os << ", time in ms " << polyLinePoint.time.value();
+ os << (idx == 0U ? "[" : ", [") << std::to_string(idx) << "]=" << poly_line.at(idx);
}
-
+ os << ')';
return os;
}
-/// The list of polyline points
-using PolyLine = std::vector<PolyLinePoint>;
-
} // namespace mantle_api
#endif // MANTLEAPI_COMMON_POLY_LINE_H
diff --git a/include/MantleAPI/Common/trajectory.h b/include/MantleAPI/Common/trajectory.h
index 0dbb0c880e7815cbf0fb27d6eda10fb13e657401..e4598bef778f4cc4c07466a5f9e10bf18b2cada3 100644
--- a/include/MantleAPI/Common/trajectory.h
+++ b/include/MantleAPI/Common/trajectory.h
@@ -1,5 +1,5 @@
/*******************************************************************************
- * Copyright (c) 2021-2023, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ * Copyright (c) 2021-2025, 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
@@ -15,45 +15,65 @@
#ifndef MANTLEAPI_COMMON_TRAJECTORY_H
#define MANTLEAPI_COMMON_TRAJECTORY_H
+#include <MantleAPI/Common/clothoid_spline.h>
#include <MantleAPI/Common/poly_line.h>
+#include <cstdint>
+#include <optional>
#include <string>
+#include <tuple>
#include <variant>
namespace mantle_api
{
+/// Defines which reference point of the object is used for the trajectory.
+enum class TrajectoryReferencePoint : std::uint8_t
+{
+ kUndefined = 0, ///< Type of trajectory reference point is underfined
+ kRearAxle, ///< Reference point is the rear axle of the object
+ kFrontAxle, ///< Reference point is the front axle of the object
+ kCenterOfMass, ///< Reference point is the center of mass of the object
+ kBoundingBoxCenter ///< Reference point is the center of the bounding box of the object
+};
+
/// Definition of a trajectory type in terms of shape
struct Trajectory
{
- std::string name; ///< Name of the trajectory type
- std::variant<PolyLine> type; ///< Trajectory type in terms of shape
-
- /// @brief Prints a human-readable representation of 'trajectory' to 'os'.
- /// @param os The output stream
- /// @param trajectory Open scenario trajectory
- /// @returns 'trajectory' in a human-readable format
- friend std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory);
+ std::string name; ///< Name of the trajectory type
+ std::variant<PolyLine, ClothoidSpline> type; ///< Trajectory type in terms of shape
+ TrajectoryReferencePoint reference; ///< Reference point of object, which is used in trajectory
};
-/// @brief Prints a human-readable representation of 'trajectory' to 'os'.
+/// Compare two objects of Trajectory.
+///
+/// @param[in] lhs left-hand side value for the comparison
+/// @param[in] rhs right-hand side value for the comparison
+/// @returns true if the values of lhs exactly equals to rhs values
+constexpr bool operator==(const Trajectory& lhs, const Trajectory& rhs) noexcept
+{
+ return std::tie(lhs.name, lhs.type, lhs.reference) == std::tie(rhs.name, rhs.type, rhs.reference);
+}
+
+/// Print a human-readable representation of 'trajectory' to 'os'.
+///
/// @param os The output stream
/// @param trajectory Open scenario trajectory
/// @returns 'trajectory' in a human-readable format
inline std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory)
{
- os << "Trajectory \"" << trajectory.name;
+ os << "Trajectory(.name=" << trajectory.name << ", .type=";
if (std::holds_alternative<PolyLine>(trajectory.type))
{
- const auto& polyLine = std::get<PolyLine>(trajectory.type);
- for (const auto& polyLinePoint : polyLine)
- {
- os << polyLinePoint;
- }
+ os << std::get<PolyLine>(trajectory.type);
+ }
+ else if (std::holds_alternative<ClothoidSpline>(trajectory.type))
+ {
+ os << std::get<ClothoidSpline>(trajectory.type);
}
- os << "\"\n";
+ os << ", reference=" << static_cast<int>(trajectory.reference) << ")";
return os;
}
diff --git a/include/MantleAPI/Common/unit_definitions.h b/include/MantleAPI/Common/unit_definitions.h
index c3b6693197b7f01e7b88d635e0ccad31bf1f4668..f4cb0f7d6d8b97632400bb6333d2d11a9b06b0ae 100644
--- a/include/MantleAPI/Common/unit_definitions.h
+++ b/include/MantleAPI/Common/unit_definitions.h
@@ -84,6 +84,23 @@ using jerk_acceleration_unit = base_unit<detail::meter_ratio<1>, std::ratio<0>,
UNIT_ADD_CATEGORY_TRAIT(jerk_acceleration)
+/// Definition of inverse meter [1/m]
+UNIT_ADD(
+ curve,
+ curvature,
+ curvatures,
+ curv,
+ unit<std::ratio<1>, inverse<length::meter>>)
+
+namespace category
+{
+
+using curve_unit = base_unit<std::ratio<1>, inverse<length::meter>>;
+
+} // namespace category
+
+UNIT_ADD_CATEGORY_TRAIT(curve)
+
} // namespace units
#endif // MANTLEAPI_COMMON_UNIT_DEFINITIONS_H
diff --git a/test/MantleAPI/Common/CMakeLists.txt b/test/MantleAPI/Common/CMakeLists.txt
index 0bf4b4e39e65886709a435f23eb03bb1ec2bd469..9920800bd3dbf30b6b20f750bede30a74655355f 100644
--- a/test/MantleAPI/Common/CMakeLists.txt
+++ b/test/MantleAPI/Common/CMakeLists.txt
@@ -9,7 +9,7 @@
################################################################################
add_executable(CommonTest)
-target_sources(CommonTest PUBLIC floating_point_helper_test.cc logger_test.cc log_utils_test.cc)
+target_sources(CommonTest PUBLIC floating_point_helper_test.cc logger_test.cc log_utils_test.cc clothoid_spline_test.cc trajectory_test.cc)
target_include_directories(CommonTest PRIVATE $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/test>)
target_link_libraries(CommonTest PUBLIC MantleAPI::MantleAPI GTest::gmock_main)
diff --git a/test/MantleAPI/Common/clothoid_spline_test.cc b/test/MantleAPI/Common/clothoid_spline_test.cc
new file mode 100644
index 0000000000000000000000000000000000000000..dfc637d128f7d1cfcafcd3609be1188d7b337c86
--- /dev/null
+++ b/test/MantleAPI/Common/clothoid_spline_test.cc
@@ -0,0 +1,191 @@
+/*******************************************************************************
+ * Copyright (c) 2025, 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
+ *******************************************************************************/
+
+#include <MantleAPI/Common/clothoid_spline.h>
+#include <MantleAPI/Common/pose.h>
+#include <gtest/gtest.h>
+
+#include <functional>
+#include <iostream>
+#include <optional>
+#include <sstream>
+
+using units::literals::operator""_m;
+using units::literals::operator""_rad;
+using units::literals::operator""_curv;
+
+using mantle_api::ClothoidSpline;
+using mantle_api::ClothoidSplineSegment;
+
+namespace
+{
+class ClothoidSplineTest : public ::testing::Test
+{
+public:
+ ClothoidSplineTest()
+ {
+ clothoid_spline_left_.segments.push_back({0.0_curv,
+ 6.6_curv,
+ 0.4_rad,
+ 1.0_m,
+ std::nullopt});
+
+ clothoid_spline_left_.segments.push_back(
+ {
+ 7.0_curv,
+ -7.6_curv,
+ 0.5_rad,
+ 0.2_m,
+ mantle_api::Pose{{96.8_m, -4.9_m, 1.6_m}, {-0.3_rad, 2.8_rad, -2.0_rad}},
+ });
+ clothoid_spline_right_ = clothoid_spline_left_;
+ }
+
+protected:
+ ClothoidSpline clothoid_spline_left_;
+ ClothoidSpline clothoid_spline_right_;
+};
+
+TEST_F(ClothoidSplineTest, GivenClothoidSplineSegments_WhenSegmentsHaveEqualParameters_ThenClothoidSplineSegmentEqualOperatorReturnsTrue)
+{
+ ClothoidSplineSegment segment_left = clothoid_spline_left_.segments.back();
+ ClothoidSplineSegment segment_rigt = clothoid_spline_left_.segments.back();
+
+ EXPECT_TRUE(segment_left == segment_rigt);
+}
+
+using ChangeParam = std::function<void(ClothoidSplineSegment&)>;
+
+void ChangeCurvatureEnd(ClothoidSplineSegment& segment)
+{
+ segment.curvature_end = 6.7_curv;
+}
+
+void ChangeCurvatureStart(ClothoidSplineSegment& segment)
+{
+ segment.curvature_start = 5.7_curv;
+}
+
+void ChangeHOffset(ClothoidSplineSegment& segment)
+{
+ segment.h_offset = 188.0_rad;
+}
+
+void ChangeLength(ClothoidSplineSegment& segment)
+{
+ segment.length = 256.0_m;
+}
+
+void ChangePositionStart(ClothoidSplineSegment& segment)
+{
+ if (segment.position_start.has_value())
+ {
+ segment.position_start.value().position.x = 6.8_m;
+ }
+}
+
+class ClothoidSplineTestParam : public ::testing::WithParamInterface<ChangeParam>, public ClothoidSplineTest
+{
+};
+
+INSTANTIATE_TEST_SUITE_P(ClothoidSplineTestInstParam,
+ ClothoidSplineTestParam,
+ ::testing::Values(
+ ChangeCurvatureEnd,
+ ChangeCurvatureStart,
+ ChangeHOffset,
+ ChangeLength,
+ ChangePositionStart));
+
+TEST_P(ClothoidSplineTestParam, GivenClothoidSplineSegments_WhenSegmentsHaveDifferentParameters_ThenClothoidSplineSegmentEqualOperatorReturnsFalse)
+{
+ ClothoidSplineSegment segment_left = clothoid_spline_left_.segments.back();
+ ClothoidSplineSegment segment_right = clothoid_spline_left_.segments.back();
+
+ GetParam()(segment_right);
+
+ EXPECT_FALSE(segment_left == segment_right);
+}
+
+TEST_F(ClothoidSplineTest, GivenClothoidSplines_WhenSegmentsHaveEqualParameters_ThenClothoidSplineEqualOperatorReturnsTrue)
+{
+ EXPECT_TRUE(clothoid_spline_left_ == clothoid_spline_right_);
+}
+
+TEST_F(ClothoidSplineTest, GivenClothoidSplines_WhenSegmentsHaveDifferentParameters_ThenClothoidSplineEqualOperatorReturnsFalse)
+{
+ clothoid_spline_right_.segments.back().curvature_end = 6.7_curv;
+ EXPECT_FALSE(clothoid_spline_left_ == clothoid_spline_right_);
+}
+
+TEST_F(ClothoidSplineTest, GivenClothoidSplines_WhenSegmentsHaveDifferentSize_ThenClothoidSplineEqualOperatorReturnsFalse)
+{
+ clothoid_spline_left_.segments.push_back(ClothoidSplineSegment());
+ EXPECT_FALSE(clothoid_spline_left_ == clothoid_spline_right_);
+}
+
+TEST_F(ClothoidSplineTest, GivenClothoidSpline_WhenOutputToStream_ThenNoExceptionAndOutputNotEmpty)
+{
+ auto output_to_stream = [](std::ostream& os, ClothoidSpline& clothoid_spline)
+ {
+ os << clothoid_spline;
+ };
+ std::ostringstream actual_os;
+ ASSERT_NO_THROW(output_to_stream(actual_os, clothoid_spline_left_));
+ EXPECT_GT(actual_os.str().size(), 0);
+}
+
+TEST_F(ClothoidSplineTest, GivenClothoidSpline_WhenIterate_ThenIterationIsCorrect)
+{
+ auto actual_it{clothoid_spline_left_.begin()};
+ auto expected_it{clothoid_spline_left_.segments.begin()};
+
+ EXPECT_EQ(clothoid_spline_left_.begin(), clothoid_spline_left_.segments.begin());
+ EXPECT_EQ(clothoid_spline_left_.end(), clothoid_spline_left_.segments.end());
+
+ while (actual_it != clothoid_spline_left_.end())
+ {
+ // Test if increment is correct
+ EXPECT_EQ(actual_it, expected_it);
+
+ // Test if dereferencing is correct
+ EXPECT_EQ(*actual_it, *expected_it);
+
+ ++actual_it;
+ ++expected_it;
+ }
+
+ EXPECT_EQ(actual_it, clothoid_spline_left_.segments.end());
+}
+
+TEST_F(ClothoidSplineTest, GivenConstClothoidSpline_WhenIterate_ThenIterationIsCorrect)
+{
+ const ClothoidSpline clothoid_spline{clothoid_spline_left_};
+ auto actual_it{clothoid_spline.cbegin()};
+ auto expected_it{clothoid_spline.segments.cbegin()};
+
+ EXPECT_EQ(clothoid_spline.cbegin(), clothoid_spline.segments.cbegin());
+ EXPECT_EQ(clothoid_spline.cend(), clothoid_spline.segments.cend());
+
+ while (actual_it != clothoid_spline.cend())
+ {
+ // Test if increment is correct
+ EXPECT_EQ(actual_it, expected_it);
+
+ // Test if dereferencing is correct
+ EXPECT_EQ(*actual_it, *expected_it);
+
+ ++actual_it;
+ ++expected_it;
+ }
+
+ EXPECT_EQ(actual_it, clothoid_spline.segments.cend());
+}
+} // namespace
diff --git a/test/MantleAPI/Common/trajectory_test.cc b/test/MantleAPI/Common/trajectory_test.cc
new file mode 100644
index 0000000000000000000000000000000000000000..684315815fc8b59d863971d4f151587a5c77e115
--- /dev/null
+++ b/test/MantleAPI/Common/trajectory_test.cc
@@ -0,0 +1,158 @@
+/*******************************************************************************
+ * Copyright (c) 2025, 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
+ *******************************************************************************/
+
+#include <MantleAPI/Common/trajectory.h>
+#include <gtest/gtest.h>
+
+#include <functional>
+#include <iostream>
+#include <sstream>
+#include <variant>
+
+using units::literals::operator""_m;
+using units::literals::operator""_rad;
+using units::literals::operator""_curv;
+using units::literals::operator""_s;
+
+using mantle_api::ClothoidSpline;
+using mantle_api::PolyLine;
+using mantle_api::Trajectory;
+
+namespace
+{
+class TrajectoryTest : public ::testing::Test
+{
+public:
+ TrajectoryTest()
+ {
+ clothoid_spline_.segments.push_back({0.0_curv,
+ 6.6_curv,
+ 0.4_rad,
+ 1.0_m,
+ std::nullopt});
+
+ clothoid_spline_.segments.push_back({
+ 7.0_curv,
+ -7.6_curv,
+ 0.5_rad,
+ 0.2_m,
+ mantle_api::Pose{{96.8_m, -4.9_m, 1.6_m}, {-0.3_rad, 2.8_rad, -2.0_rad}},
+ });
+
+ polyline_.push_back({{{1.0_m, 51.0_m, 8.3_m}, {0.8_rad, 0.5_rad, 0.4_rad}}, 0.5_s});
+ polyline_.push_back({{{1.0_m, 51.0_m, 8.3_m}, {-0.8_rad, 0.5_rad, 0.4_rad}}, 0.6_s});
+
+ trajectory_left_.name = "Trajectory";
+ trajectory_left_.reference = mantle_api::TrajectoryReferencePoint::kCenterOfMass;
+ }
+
+protected:
+ ClothoidSpline clothoid_spline_;
+ PolyLine polyline_;
+ Trajectory trajectory_left_;
+ Trajectory trajectory_right_;
+};
+
+TEST_F(TrajectoryTest, GivenTrajectories_WhenTrajectoriesHaveEqualParameters_ThenTrajectoryEqualOperatorReturnsTrue)
+{
+ trajectory_left_.type = clothoid_spline_;
+ trajectory_right_ = trajectory_left_;
+ EXPECT_TRUE(trajectory_left_ == trajectory_right_);
+
+ trajectory_left_.type = polyline_;
+ trajectory_right_ = trajectory_left_;
+ EXPECT_TRUE(trajectory_left_ == trajectory_right_);
+}
+
+using ChangeParam = std::function<void(Trajectory&)>;
+
+void ChangeName(Trajectory& trajectory)
+{
+ trajectory.name = "New name";
+}
+
+void ChangeType(Trajectory& trajectory)
+{
+ if (std::holds_alternative<PolyLine>(trajectory.type))
+ {
+ auto& polyline = std::get<PolyLine>(trajectory.type);
+ polyline.front().pose.position.x = 100_m;
+ }
+ else if (std::holds_alternative<ClothoidSpline>(trajectory.type))
+ {
+ auto& clothoid = std::get<ClothoidSpline>(trajectory.type);
+ clothoid.segments.front().curvature_end = 100.0_curv;
+ }
+}
+
+void ChangeReference(Trajectory& trajectory)
+{
+ trajectory.reference = mantle_api::TrajectoryReferencePoint::kRearAxle;
+}
+
+class TrajectoryTestParam : public ::testing::WithParamInterface<ChangeParam>, public TrajectoryTest
+{
+};
+
+INSTANTIATE_TEST_SUITE_P(TrajectoryTestInstParam,
+ TrajectoryTestParam,
+ ::testing::Values(
+ ChangeName,
+ ChangeType,
+ ChangeReference));
+
+TEST_P(TrajectoryTestParam, GivenTrajectories_WhenTrajectoriesHaveDifferentParameters_ThenTrajectoryEqualOperatorReturnsFalse)
+{
+ trajectory_left_.type = polyline_;
+ trajectory_right_ = trajectory_left_;
+ GetParam()(trajectory_right_);
+ EXPECT_FALSE(trajectory_left_ == trajectory_right_);
+
+ trajectory_left_.type = clothoid_spline_;
+ trajectory_right_ = trajectory_left_;
+ GetParam()(trajectory_right_);
+ EXPECT_FALSE(trajectory_left_ == trajectory_right_);
+}
+
+TEST_F(TrajectoryTest, GivenTrajectoryWithNoType_WhenOutputToStream_ThenNoExceptionAndOutputNotEmpty)
+{
+ std::stringstream actual_os;
+ auto output_to_stream = [](std::ostream& os, Trajectory& trajectory)
+ {
+ os << trajectory;
+ };
+ ASSERT_NO_THROW(output_to_stream(actual_os, trajectory_left_));
+ EXPECT_GT(actual_os.str().size(), 0);
+}
+
+TEST_F(TrajectoryTest, GivenTrajectoryWithPolylineType_WhenOutputToStream_ThenNoExceptionAndOutputNotEmpty)
+{
+ std::stringstream actual_os;
+ trajectory_left_.type = polyline_;
+ auto output_to_stream = [](std::ostream& os, Trajectory& trajectory)
+ {
+ os << trajectory;
+ };
+ ASSERT_NO_THROW(output_to_stream(actual_os, trajectory_left_));
+ EXPECT_GT(actual_os.str().size(), 0);
+}
+
+TEST_F(TrajectoryTest, GivenTrajectoryWithClothoidSplineType_WhenOutputToStream_ThenNoExceptionAndOutputNotEmpty)
+{
+ std::stringstream actual_os;
+ trajectory_left_.type = clothoid_spline_;
+ auto output_to_stream = [](std::ostream& os, Trajectory& trajectory)
+ {
+ os << trajectory;
+ };
+ ASSERT_NO_THROW(output_to_stream(actual_os, trajectory_left_));
+ EXPECT_GT(actual_os.str().size(), 0);
+}
+} // namespace