Merge branch 'fix/coordinate-conversion-in-parampoly3' into 'main'

Fix: coordinate conversion in parampoly3

See merge request !62

RoadLogicSuite/Internal/Geometry/param_poly_3.cpp

@@ -149,7 +149,7 @@ Vec2<units::length::meter_t> ParamPoly3::CalcDirectionAt(const units::length::me
 
     auto local_heading = Vec2<units::length::meter_t>{units::length::meter_t(u_poly.CalcDerivativeAt(distance_query)),
                                                       units::length::meter_t(v_poly.CalcDerivativeAt(distance_query))};
-    return local_heading.Rotate(hdg) / local_heading.Length().value();
+    return local_heading / local_heading.Length().value();
 }
 
 units::angle::radian_t ParamPoly3::GetHeadingAt(const units::length::meter_t& s) const

RoadLogicSuite/Internal/Geometry/param_poly_3_test.cpp

@@ -55,4 +55,22 @@ TEST(ParamPoly3Test,
     ASSERT_NEAR(result.value(), 1.570656, kAbsErrorMax);
 }
 
+TEST(ParamPoly3Test,
+     GivenSimpleParametricPolynomialRoad_WhenCalcDirectionAt_ThenCorrectDirectionAtFiftyMetersMustBeReturned)
+{
+    auto param_poly = ParamPoly3(units::length::meter_t(0),
+                                 Vec2<units::length::meter_t>(units::length::meter_t(0), units::length::meter_t(0)),
+                                 units::angle::radian_t(0),
+                                 units::length::meter_t(100),
+                                 "polynomial_road",
+                                 types::Poly3(0, 1, 0, 0),
+                                 types::Poly3(1.5, 1, -3.5, 1),
+                                 false);
+
+    const auto result = param_poly.CalcDirectionAt(units::length::meter_t(50));
+
+    ASSERT_NEAR(result.x.value(), 0.0001398, kAbsErrorMax);
+    ASSERT_NEAR(result.y.value(), 1.0, kAbsErrorMax);
+}
+
 }  // namespace road_logic_suite

RoadLogicSuite/Tests/Data/Maps/road_with_param_poly3_geometry.xodr

+<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
+<OpenDRIVE>
+    <header revMajor="1" revMinor="4" date="Thu Mar 26 14:00:00 CEST 2024" north="5000" south="-100" east="100" west="-1000">
+        <geoReference></geoReference>
+    </header>
+    <road name="Road0" length="100" id="0" junction="-1">
+        <link></link>
+        <type s="0.0" type="motorway" />
+        <planView>
+            <geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="100">
+                <paramPoly3 aU="0.000000000000e+00" bU="1.000000000000e+00" cU="-4.666602734948e-09" dU="-2.629787927644e-08" aV="0.000000000000e+00" bV="1.665334536938e-16" cV="-1.987729787588e-04" dV="-1.317158625579e-09" pRange="arcLength">
+                </paramPoly3>
+            </geometry>
+        </planView>
+        <lanes>
+            <laneSection s="0.0">
+                <center>
+                    <lane id="0" type="driving" level="true">
+                        <link />
+                        <roadMark sOffset="0.0" type="solid" weight="standard" color="standard" width="0.13" />
+                    </lane>
+                </center>
+                <right>
+                    <lane id="-1" type="driving" level="true">
+                        <width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0" />
+                        <roadMark sOffset="0.0" type="broken" weight="standard" color="standard" width="0.13" />
+                    </lane>
+                    <lane id="-2" type="driving" level="true">
+                        <width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0" />
+                        <roadMark sOffset="0.0" type="broken" weight="standard" color="standard" width="0.13" />
+                    </lane>
+                </right>
+            </laneSection>
+        </lanes>
+    </road>
+</OpenDRIVE>

RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/BUILD

@@ -118,3 +118,10 @@ cc_test(
     srcs = ["pedestrain_crossing_integration_test.cpp"],
     deps = ["//RoadLogicSuite/Tests/MantleMapConvertIntegrationTestFixture:mantle_map_convert_integration_test_fixture"],
 )
+
+cc_test(
+    name = "param_poly3_integration_test",
+    timeout = "short",
+    srcs = ["param_poly3_integration_test.cpp"],
+    deps = ["//RoadLogicSuite/Tests/MantleMapConvertIntegrationTestFixture:mantle_map_convert_integration_test_fixture"],
+)

RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/param_poly3_integration_test.cpp

+/*******************************************************************************
+ * Copyright (C) 2024, ANSYS, Inc.
+ *
+ * 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 "RoadLogicSuite/Tests/MantleMapConvertIntegrationTestFixture/mantle_map_convert_integration_test_fixture.h"
+
+#include <gmock/gmock.h>
+
+namespace road_logic_suite::test
+{
+using map_api::Lane;
+using map_api::LaneBoundary;
+using map_api::Map;
+using ::testing::ElementsAre;
+using units::literals::operator""_m;
+
+///////////////////////////////////////////////////////////
+/// @verbatim
+/// OpenDrive Map
+///  origin = (0.0, 0.0)
+///   |-----------------------| ref line
+///   | x x x (Lane -1) x x x |
+///   |-----------------------|
+///   | x x x (Lane -2) x x x |
+///   |-----------------------|
+///   ^                       ^
+/// s(0)                    (100)
+/// x(0)
+///
+/// IDs:
+/// |LaneGroup: 9                 |
+/// |-----------------------------|
+/// |Left LaneBoundary: 1         |
+/// |Lane: 3                      |
+/// |Right LaneBoundary: 6        |
+/// |-----------------------------|
+/// |Left LaneBoundary: 6         |
+/// |Lane: 4                      |
+/// |Right LaneBoundary:7         |
+/// |-----------------------------|
+///
+/// @endverbatim
+
+TEST_F(MapConvertIntegrationTestFixture,
+       GivenMapWithParametricPolynomialGeometry_WhenParsingMap_ThenConversionLaneAndLaneBoundariesAreCorrect)
+{
+
+    const std::string test_map = GetResolvedMapPathInternal("road_with_param_poly3_geometry.xodr");
+
+    const auto& map = GetMap(test_map);
+
+    ASSERT_EQ(map.lanes.size(), 2);
+    const auto& lane_3 = map.lanes[0];
+    const auto& lane_4 = map.lanes[1];
+
+    ASSERT_EQ(lane_3->left_lane_boundaries.size(), 1);
+    const auto& lane_boundary_1 = lane_3->left_lane_boundaries[0];
+    ASSERT_EQ(lane_3->right_lane_boundaries.size(), 1);
+    const auto& lane_boundary_6 = lane_3->right_lane_boundaries[0];
+    ASSERT_EQ(lane_4->right_lane_boundaries.size(), 1);
+    const auto& lane_boundary_7 = lane_4->right_lane_boundaries[0];
+
+    // Lane 3
+    EXPECT_THAT(lane_3->left_adjacent_lanes, ::testing::IsEmpty());
+    EXPECT_THAT(lane_3->right_adjacent_lanes, ElementsAre(*lane_4));
+    EXPECT_THAT(lane_3->centerline.front(), MantlePositionIsNear(MantlePosition{0.0_m, -1.75_m, 0.0_m}));
+    EXPECT_THAT(lane_3->centerline.back(), MantlePositionIsNear(MantlePosition{99.904_m, -3.73766_m, 0.0_m}));
+    ASSERT_THAT(lane_3->left_lane_boundaries, ElementsAre(lane_boundary_1));
+    ASSERT_THAT(lane_3->right_lane_boundaries, ElementsAre(lane_boundary_6));
+
+    // Lane 4
+    EXPECT_THAT(lane_4->left_adjacent_lanes, ElementsAre(*lane_3));
+    EXPECT_THAT(lane_4->centerline.front(), MantlePositionIsNear(MantlePosition{0.0_m, -5.25_m, 0.0_m}));
+    EXPECT_THAT(lane_4->centerline.back(), MantlePositionIsNear(MantlePosition{99.7647_m, -7.23489_m, 0.0_m}));
+    ASSERT_THAT(lane_4->left_lane_boundaries, ElementsAre(lane_boundary_6));
+    ASSERT_THAT(lane_4->right_lane_boundaries, ElementsAre(lane_boundary_7));
+
+    // Lane boundary 1
+    EXPECT_EQ(lane_boundary_1.get().type, LaneBoundary::Type::kSolidLine);
+    EXPECT_EQ(lane_boundary_1.get().color, LaneBoundary::Color::kStandard);
+    EXPECT_THAT(lane_boundary_1.get().boundary_line.front(),
+                LaneBoundaryIsEqual(
+                    LaneBoundary::BoundaryPoint{.position = {0.0_m, 0.0_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+    EXPECT_THAT(lane_boundary_1.get().boundary_line.back(),
+                LaneBoundaryIsEqual(LaneBoundary::BoundaryPoint{
+                    .position = {99.9737_m, -1.98905_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+
+    // Lane boundary 6
+    EXPECT_EQ(lane_boundary_6.get().type, LaneBoundary::Type::kDashedLine);
+    EXPECT_EQ(lane_boundary_6.get().color, LaneBoundary::Color::kStandard);
+    EXPECT_THAT(lane_boundary_6.get().boundary_line.front(),
+                LaneBoundaryIsEqual(
+                    LaneBoundary::BoundaryPoint{.position = {0.0_m, -3.5_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+    EXPECT_THAT(lane_boundary_6.get().boundary_line.back(),
+                LaneBoundaryIsEqual(LaneBoundary::BoundaryPoint{
+                    .position = {99.8344_m, -5.48627_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+
+    // Lane boundary 7
+    EXPECT_EQ(lane_boundary_7.get().type, LaneBoundary::Type::kDashedLine);
+    EXPECT_EQ(lane_boundary_7.get().color, LaneBoundary::Color::kStandard);
+    EXPECT_THAT(lane_boundary_7.get().boundary_line.front(),
+                LaneBoundaryIsEqual(
+                    LaneBoundary::BoundaryPoint{.position = {0.0_m, -7.0_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+    EXPECT_THAT(lane_boundary_7.get().boundary_line.back(),
+                LaneBoundaryIsEqual(LaneBoundary::BoundaryPoint{
+                    .position = {99.6951_m, -8.9835_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+}
+
+}  // namespace road_logic_suite::test