CatmullRomSubspline collision not being detected

[TauTheLepton]

Describe the bug
The CatmullRomSubspline member function getCollisionPointIn2D does not always return the correct result. In a simple case with one collision is works fine, but in more complicated scenarios with more collision points the function does not always return a collision point.
This is caused by the fact, that the CatmullRomSubspline calls the CatmullRomSpline member function getCollisionPointIn2D here:

scenario_simulator_v2/common/math/geometry/src/spline/catmull_rom_subspline.cpp

Line 28 in c4b7daa

auto s = spline_->getCollisionPointIn2D(polygon, search_backward);

which already filters out one collision point here (the first one found is returned):

scenario_simulator_v2/common/math/geometry/src/spline/catmull_rom_spline.cpp

Lines 298 to 320 in c4b7daa

	/// @note If the spline has three or more control points.
	const auto get_collision_point_2d_with_curve =
	[this](const auto & polygon, const auto search_backward) -> std::optional<double> {
	size_t n = curves_.size();
	if (search_backward) {
	for (size_t i = 0; i < n; i++) {
	auto s = curves_[n - 1 - i].getCollisionPointIn2D(polygon, search_backward);
	if (s) {
	return getSInSplineCurve(n - 1 - i, s.value());
	}
	}
	return std::optional<double>();
	} else {
	for (size_t i = 0; i < n; i++) {
	auto s = curves_[i].getCollisionPointIn2D(polygon, search_backward);
	if (s) {
	return std::optional<double>(getSInSplineCurve(i, s.value()));
	}
	}
	return std::optional<double>();
	}
	return std::optional<double>();
	};

This leads to incorrect situations, where a polygon has multiple collisions with the spline, the spline returns first (or last - depending on the search_backward argument) collision point, but this collision point is not part of the subspline. Some other collision point may be a part of the subspline, but this case is not checked, the information is lost and no collision point is returned from the CatmullRomSubspline member function getCollisionPointIn2D.

In my opinion there should exist a CatmullRomSpline member function that will return all collision points, so that from those collision points CatmullRomSubspline could choose the ones that are part of the spline and then return the first one (or the last one).
NOTE that this fix will require to modify the code that is affected by another bug (#1120) which should probably be fixed first.

Context:
I am adding unit tests for the geometry package and wanted to add tests for the getCollisionPointIn2D function, but it generates the wrong results in some cases (see To Reproduce).

To Reproduce
I have created an example in the test case to demonstrate the bug here
Steps to reproduce the behavior:

1. Clone this branch
2. Build
3. Run tests
4. See that the last spline test passes (which makes no sens, as the same spline with the same polygon does collide in one case and does not collide in other case)

Expected behavior
The collision should always be detected (provided it exists).

Screenshots
If applicable, add screenshots to help explain your problem.

Desktop (please complete the following information):

• OS: [e.g. iOS]: ubuntu
• Browser [e.g. chrome, safari]: Firefox
• Version [e.g. 22]: 22
• ROS 2 version: Humble
• DDS: CycloneDDS

[hakuturu583]

I understand, it is a bug.

[hakuturu583]

#1139 was merged