From 17e93d8cf69c252918cd508e6086c437be85a714 Mon Sep 17 00:00:00 2001 From: Reinhard Biegel Date: Mon, 13 Sep 2021 11:43:23 +0200 Subject: [PATCH] Revert "mod(World_OSI): Add query for relative lane of other object" This reverts commit b69bbad30029d3565282a6b04258a0140ea75918. --- sim/include/egoAgentInterface.h | 7 - sim/include/worldInterface.h | 32 +- sim/src/core/slave/bindings/world.h | 5 - .../modules/World_OSI/WorldDataQuery.cpp | 93 +----- .../slave/modules/World_OSI/WorldDataQuery.h | 10 - .../modules/World_OSI/WorldImplementation.cpp | 8 - .../modules/World_OSI/WorldImplementation.h | 2 - .../core/slave/modules/World_OSI/egoAgent.cpp | 22 -- .../core/slave/modules/World_OSI/egoAgent.h | 2 - sim/tests/fakes/gmock/fakeEgoAgent.h | 1 - sim/tests/fakes/gmock/fakeWorld.h | 1 - .../modules/World_OSI/worldQuery_Tests.cpp | 288 +----------------- 12 files changed, 12 insertions(+), 459 deletions(-) diff --git a/sim/include/egoAgentInterface.h b/sim/include/egoAgentInterface.h index 1afd792b..b07bdb8a 100644 --- a/sim/include/egoAgentInterface.h +++ b/sim/include/egoAgentInterface.h @@ -127,13 +127,6 @@ public: //! \return lanes relative to agent virtual RelativeWorldView::Lanes GetRelativeLanes(double range, int relativeLane = 0, bool includeOncoming = true) const = 0; - //! Returns the relative lane of the ReferencePoint or MainLocatePoint of another object - //! - //! \param object other object - //! \param mp either Front for MainLocatePoint or Reference (Rear not supported) - //! \return relative lane id - virtual std::optional GetRelativeLaneId(const WorldObjectInterface* object, MeasurementPoint mp) const = 0; - virtual RelativeWorldView::Junctions GetRelativeJunctions(double range) const = 0; //! Returns all WorldObjects around the agent inside the specified range on the specified diff --git a/sim/include/worldInterface.h b/sim/include/worldInterface.h index 71220ded..dc8b32f3 100644 --- a/sim/include/worldInterface.h +++ b/sim/include/worldInterface.h @@ -553,8 +553,7 @@ public: //! Calculates the obstruction of an agent with another object i.e. how far to left or the right the object is from my position //! For more information see the [markdown documentation](\ref dev_framework_modules_world_getobstruction) //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent + //! \param route route of the agent //! \param ownPosition position of the agent //! \param otherPosition position of the other object //! \param objectCorners corners of the other object @@ -564,8 +563,8 @@ public: //! Returns all traffic signs valid for a lane inside the range //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent + //! \param route route to search along + //! \param roadId OpenDrive Id of the road //! \param laneId OpenDrive Id of the lane //! \param startDistance s coordinate //! \param searchRange range of search (can also be negative) @@ -575,8 +574,8 @@ public: //! Returns all road markings valid for a lane inside the range //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent + //! \param route route to search along + //! \param roadId OpenDrive Id of the road //! \param laneId OpenDrive Id of the lane //! \param startDistance s coordinate //! \param searchRange range of search @@ -597,8 +596,7 @@ public: //! Retrieves all lane markings on the given position on the given side of the lane inside the range //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent + //! \param roadId OpenDrive Id of the road //! \param laneId OpenDrive Id of the lane //! \param startDistance s coordinate //! \param range search range @@ -607,8 +605,8 @@ public: //! Returns the relative distances (start and end) and the connecting road id of all junctions on the route in range //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent + //! \param route route of the agent + //! \param roadId OpenDrive Id of the road //! \param startDistance start s coordinate on the road //! \param range range of search //! \return information about all junctions in range @@ -619,8 +617,8 @@ public: //! driving direction of the lane is the same as the direction of the route. If the ego lane prematurely ends, then //! the further lane ids are relative to the middle of the road. //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent + //! \param route route of the agent + //! \param roadId OpenDrive Id of the road //! \param laneId OpenDrive Id of the lane //! \param distance start s coordinate on the road //! \param range range of search @@ -628,16 +626,6 @@ public: //! \return information about all lanes in range virtual RouteQueryResult GetRelativeLanes(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, double range, bool includeOncoming = true) const = 0; - //! Returns the relative lane id of the located position of a point relative to the given position - //! - //! \param roadGraph road network as viewed from agent - //! \param startNode position on roadGraph of agent - //! \param laneId OpenDrive Id of the lane - //! \param distance own s coordinate - //! \param targetPosition position of queried point - //! \return lane id relative to own position - virtual RouteQueryResult> GetRelativeLaneId(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map targetPosition) const = 0; - //! Returns all possible connections on the junction, that an agent has when coming from the specified road //! //! \param junctionId OpenDrive Id of the junction diff --git a/sim/src/core/slave/bindings/world.h b/sim/src/core/slave/bindings/world.h index 0545462f..003c3aee 100644 --- a/sim/src/core/slave/bindings/world.h +++ b/sim/src/core/slave/bindings/world.h @@ -345,11 +345,6 @@ public: return implementation->GetRelativeLanes(roadGraph, startNode, laneId, distance, range, includeOncoming); } - RouteQueryResult> GetRelativeLaneId(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map targetPosition) const override - { - return implementation->GetRelativeLaneId(roadGraph, startNode, laneId, distance, targetPosition); - } - std::vector GetConnectionsOnJunction(std::string junctionId, std::string incomingRoadId) const override { return implementation->GetConnectionsOnJunction(junctionId, incomingRoadId); diff --git a/sim/src/core/slave/modules/World_OSI/WorldDataQuery.cpp b/sim/src/core/slave/modules/World_OSI/WorldDataQuery.cpp index 15d99248..d4d57ca2 100644 --- a/sim/src/core/slave/modules/World_OSI/WorldDataQuery.cpp +++ b/sim/src/core/slave/modules/World_OSI/WorldDataQuery.cpp @@ -915,7 +915,7 @@ std::map WorldDataQuery::AddLanesOfSection(const OWL::Interfaces:: continue; } int relativeLaneId = inStreamDirection ? (laneId - currentOwnLaneId) : (currentOwnLaneId - laneId); - bool differentSigns = currentOwnLaneId * laneId < 0; + bool differentSigns = (currentOwnLaneId * laneId < 0) && currentOwnLaneId != 0; if (differentSigns) { relativeLaneId += (relativeLaneId > 0) ? -1 : 1; @@ -990,97 +990,6 @@ RouteQueryResult WorldDataQuery::GetRelativeLanes(cons worldData); } -RouteQueryResult> WorldDataQuery::GetRelativeLaneId(const RoadMultiStream &roadStream, double ownPosition, int ownLaneId, std::map targetPosition) const -{ - std::optional currentOwnLaneId; - std::optional currentTargetLaneId; - return roadStream.Traverse, std::map>( - RoadMultiStream::TraversedFunction, std::map>{[&](const auto& road, const auto& previousResult, const auto& previousLaneIds)->std::tuple, std::map> - { - if (previousResult.has_value()) - { - return std::make_tuple(previousResult, previousLaneIds); - } - const auto& roadId = road.element->GetId(); - auto positionOnRoad = helper::map::query(targetPosition, roadId); - auto streamPosition = road.GetStreamPosition(positionOnRoad->roadPosition.s); - auto sections = road().GetSections(); - if (!road.inStreamDirection) - { - sections.reverse(); - } - std::map previousSectionLaneIds{previousLaneIds}; - for (const auto& section : sections) - { - const double sectionStart = road.GetStreamPosition(section->GetSOffset() + (road.inStreamDirection ? 0 : section->GetLength())); - const double sectionEnd = road.GetStreamPosition(section->GetSOffset() + (road.inStreamDirection ? section->GetLength() : 0)); - bool onSection = positionOnRoad.has_value() && sectionStart <= streamPosition && sectionEnd >= streamPosition; - const auto& lanesOnSection = section->GetLanes(); - if (sectionStart <= ownPosition && sectionEnd >= ownPosition) - { - if (currentTargetLaneId.has_value()) - { - currentTargetLaneId = FindNextEgoLaneId(lanesOnSection, road.inStreamDirection, previousSectionLaneIds); - } - else - { - currentOwnLaneId = ownLaneId; - } - } - else - { - if (onSection) - { - currentTargetLaneId = positionOnRoad->laneId; - } - else if (currentTargetLaneId.has_value()) - { - currentTargetLaneId = FindNextEgoLaneId(lanesOnSection, road.inStreamDirection, previousSectionLaneIds); - } - if (currentOwnLaneId.has_value()) - { - currentOwnLaneId = FindNextEgoLaneId(lanesOnSection, road.inStreamDirection, previousSectionLaneIds); - } - } - if (!currentOwnLaneId && !currentTargetLaneId) - { - continue; - } - previousSectionLaneIds = {}; - for (auto lane : section->GetLanes()) - { - const auto& laneId = lane->GetOdId(); - const auto currentId = currentOwnLaneId.has_value() ? currentOwnLaneId.value() : currentTargetLaneId.value(); - int relativeLaneId = road.inStreamDirection ? (laneId - currentId) : (currentId - laneId); - bool differentSigns = currentId * laneId < 0; - if (differentSigns) - { - relativeLaneId += (relativeLaneId > 0) ? -1 : 1; - } - if (currentOwnLaneId.has_value()) - { - if (onSection && positionOnRoad->laneId == laneId) - { - return std::make_tuple(relativeLaneId, previousLaneIds); - } - } - else - { - if (sectionStart <= ownPosition && sectionEnd >= ownPosition && ownLaneId == laneId) - { - return std::make_tuple(-relativeLaneId, previousLaneIds); - } - } - previousSectionLaneIds.insert({relativeLaneId, lane->GetId()}); - } - } - return std::make_tuple(std::nullopt, previousSectionLaneIds); - }}, - {}, - {}, - worldData); -} - RouteQueryResult > WorldDataQuery::GetLaneCurvature(const LaneMultiStream& laneStream, double position) const { return laneStream.Traverse>(LaneMultiStream::TraversedFunction>{[&](const auto& lane, const auto& previousResult) diff --git a/sim/src/core/slave/modules/World_OSI/WorldDataQuery.h b/sim/src/core/slave/modules/World_OSI/WorldDataQuery.h index 5216ba02..9fa1cbd7 100644 --- a/sim/src/core/slave/modules/World_OSI/WorldDataQuery.h +++ b/sim/src/core/slave/modules/World_OSI/WorldDataQuery.h @@ -553,16 +553,6 @@ public: //! \return information about all lanes in range RouteQueryResult GetRelativeLanes(const RoadMultiStream& roadStream, double startPosition, int startLaneId, double range, bool includeOncoming) const; - //! Returns the relative lane id of the located position of a point relative to the given position - //! - //! \param roadStream road stream to search - //! \param startPosition start search position on the road stream - //! \param ownPosition own position on stream - //! \param ownLaneId id of own lane - //! \param targetPosition position of queried point - //! \return lane id relative to own position - RouteQueryResult> GetRelativeLaneId(const RoadMultiStream& roadStream, double ownPosition, int ownLaneId, std::map targetPosition) const; - RouteQueryResult> GetLaneCurvature (const LaneMultiStream& laneStream, double position) const; RouteQueryResult> GetLaneWidth (const LaneMultiStream& laneStream, double position) const; diff --git a/sim/src/core/slave/modules/World_OSI/WorldImplementation.cpp b/sim/src/core/slave/modules/World_OSI/WorldImplementation.cpp index a810b4b8..b63e8d15 100644 --- a/sim/src/core/slave/modules/World_OSI/WorldImplementation.cpp +++ b/sim/src/core/slave/modules/World_OSI/WorldImplementation.cpp @@ -334,14 +334,6 @@ RouteQueryResult WorldImplementation::GetRelativeLanes return worldDataQuery.GetRelativeLanes(*roadMultiStream, startDistanceOnStream, laneId, range, includeOncoming); } -RouteQueryResult > WorldImplementation::GetRelativeLaneId(const RoadGraph &roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map targetPosition) const -{ - const auto roadMultiStream = worldDataQuery.CreateRoadMultiStream(roadGraph, startNode); - double startDistanceOnStream = roadMultiStream->GetPositionByVertexAndS(startNode, distance); - - return worldDataQuery.GetRelativeLaneId(*roadMultiStream, startDistanceOnStream, laneId, targetPosition); -} - RouteQueryResult > WorldImplementation::GetAgentsInRange(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double startDistance, double backwardRange, double forwardRange) const { const auto laneMultiStream = worldDataQuery.CreateLaneMultiStream(roadGraph, startNode, laneId, startDistance); diff --git a/sim/src/core/slave/modules/World_OSI/WorldImplementation.h b/sim/src/core/slave/modules/World_OSI/WorldImplementation.h index 62338f49..348cb01a 100644 --- a/sim/src/core/slave/modules/World_OSI/WorldImplementation.h +++ b/sim/src/core/slave/modules/World_OSI/WorldImplementation.h @@ -213,8 +213,6 @@ public: RouteQueryResult GetRelativeLanes(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, double range, bool includeOncoming = true) const override; - RouteQueryResult> GetRelativeLaneId(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map targetPosition) const override; - std::vector GetConnectionsOnJunction(std::string junctionId, std::string incomingRoadId) const override; std::vector GetIntersectingConnections(std::string connectingRoadId) const override; diff --git a/sim/src/core/slave/modules/World_OSI/egoAgent.cpp b/sim/src/core/slave/modules/World_OSI/egoAgent.cpp index df151913..af169f77 100644 --- a/sim/src/core/slave/modules/World_OSI/egoAgent.cpp +++ b/sim/src/core/slave/modules/World_OSI/egoAgent.cpp @@ -160,28 +160,6 @@ RelativeWorldView::Lanes EgoAgent::GetRelativeLanes(double range, int relativeLa includeOncoming).at(0); } -std::optional EgoAgent::GetRelativeLaneId(const WorldObjectInterface *object, MeasurementPoint mp) const -{ - std::map objectPosition; - if (mp == MeasurementPoint::Front) - { - objectPosition = object->GetObjectPosition().mainLocatePoint; - } - else if (mp == MeasurementPoint::Reference) - { - objectPosition = object->GetObjectPosition().referencePoint; - } - else - { - throw std::runtime_error("MeasurementPoint not supported for RelativeLaneId"); - } - return world->GetRelativeLaneId(wayToTarget, - rootOfWayToTargetGraph, - GetMainLocatePosition().laneId, - GetMainLocatePosition().roadPosition.s, - objectPosition).at(0); -} - std::vector EgoAgent::GetObjectsInRange(double backwardRange, double forwardRange, int relativeLane) const { auto objectsInRange = world->GetObjectsInRange(wayToTarget, diff --git a/sim/src/core/slave/modules/World_OSI/egoAgent.h b/sim/src/core/slave/modules/World_OSI/egoAgent.h index 0b4973d0..6d730575 100644 --- a/sim/src/core/slave/modules/World_OSI/egoAgent.h +++ b/sim/src/core/slave/modules/World_OSI/egoAgent.h @@ -46,8 +46,6 @@ public: RelativeWorldView::Lanes GetRelativeLanes(double range, int relativeLane = 0, bool includeOncoming = true) const override; - std::optional GetRelativeLaneId(const WorldObjectInterface* object, MeasurementPoint mp) const override; - RelativeWorldView::Junctions GetRelativeJunctions(double range) const override; std::vector GetObjectsInRange (double backwardRange, double forwardRange, int relativeLane = 0) const override; diff --git a/sim/tests/fakes/gmock/fakeEgoAgent.h b/sim/tests/fakes/gmock/fakeEgoAgent.h index 1c211a15..eaf6a8f1 100644 --- a/sim/tests/fakes/gmock/fakeEgoAgent.h +++ b/sim/tests/fakes/gmock/fakeEgoAgent.h @@ -27,7 +27,6 @@ class FakeEgoAgent : public EgoAgentInterface MOCK_CONST_METHOD2(GetDistanceToEndOfLane, double (double range, int relativeLane)); MOCK_CONST_METHOD3(GetDistanceToEndOfLane, double (double range, int relativeLane, const LaneTypes& acceptableLaneTypes)); MOCK_CONST_METHOD3(GetRelativeLanes, RelativeWorldView::Lanes (double range, int relativeLane, bool includeOncoming)); - MOCK_CONST_METHOD2(GetRelativeLaneId, std::optional (const WorldObjectInterface* object, MeasurementPoint mp)); MOCK_CONST_METHOD1(GetRelativeJunctions, RelativeWorldView::Junctions (double range)); MOCK_CONST_METHOD3(GetObjectsInRange, std::vector (double backwardRange, double forwardRange, int relativeLane)); MOCK_CONST_METHOD3(GetAgentsInRange, std::vector (double backwardRange, double forwardRange, int relativeLane)); diff --git a/sim/tests/fakes/gmock/fakeWorld.h b/sim/tests/fakes/gmock/fakeWorld.h index c9c53a0f..fec3b1d0 100644 --- a/sim/tests/fakes/gmock/fakeWorld.h +++ b/sim/tests/fakes/gmock/fakeWorld.h @@ -101,7 +101,6 @@ class FakeWorld : public WorldInterface MOCK_CONST_METHOD5(GetDistanceBetweenObjects, RouteQueryResult (const RoadGraph& roadGraph, RoadGraphVertex startNode, const ObjectPosition&, const std::optional, const ObjectPosition&)); MOCK_CONST_METHOD4(GetRelativeJunctions, RouteQueryResult (const RoadGraph& roadGraph, RoadGraphVertex startNode, double startDistance, double range)); MOCK_CONST_METHOD6(GetRelativeLanes, RouteQueryResult (const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, double range, bool includeOncoming)); - MOCK_CONST_METHOD5(GetRelativeLaneId, RouteQueryResult> (const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map targetPosition)); MOCK_CONST_METHOD2(GetRoadGraph, std::pair(const RouteElement& start, int maxDepth)); MOCK_CONST_METHOD1(GetEdgeWeights, std::map(const RoadGraph& roadGraph)); }; diff --git a/sim/tests/unitTests/core/slave/modules/World_OSI/worldQuery_Tests.cpp b/sim/tests/unitTests/core/slave/modules/World_OSI/worldQuery_Tests.cpp index 2d40d932..04018126 100644 --- a/sim/tests/unitTests/core/slave/modules/World_OSI/worldQuery_Tests.cpp +++ b/sim/tests/unitTests/core/slave/modules/World_OSI/worldQuery_Tests.cpp @@ -3044,7 +3044,7 @@ TEST(GetRelativeLanes, IdOfEgoLaneChanges_ReturnsLanesWithCorrectRelativeId) EXPECT_THAT(result.at(1).lanes.at(2).successor, Eq(std::nullopt)); } -TEST(GetRelativeLane, BranchingTree_ReturnsLanesOfWayToNode) +TEST(GetRelativeLanes, BranchingTree_ReturnsLanesOfWayToNode) { OWL::Fakes::WorldData worldData; FakeRoadMultiStream roadStream; @@ -3169,292 +3169,6 @@ TEST(GetRelativeLane, BranchingTree_ReturnsLanesOfWayToNode) EXPECT_THAT(result3.at(1).lanes.at(1).successor, Eq(std::nullopt)); } -TEST(GetRelativeLaneId, OnlySectionInDrivingDirection_ReturnsCorrectRelativeId) -{ - OWL::Fakes::WorldData worldData; - FakeRoadMultiStream roadStream; - - auto [node, road] = roadStream.AddRoot(100.0, true); - OWL::Fakes::Lane lane1; - OWL::Fakes::Lane lane2; - OWL::Fakes::Lane lane3; - OWL::Id idLane1 = 1, idLane2 = 2, idLane3 = 3; - ON_CALL(lane1, GetId()).WillByDefault(Return(idLane1)); - ON_CALL(lane2, GetId()).WillByDefault(Return(idLane2)); - ON_CALL(lane3, GetId()).WillByDefault(Return(idLane3)); - ON_CALL(lane1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane2, GetOdId()).WillByDefault(Return(-2)); - ON_CALL(lane3, GetOdId()).WillByDefault(Return(1)); - std::vector emptyIds{}; - ON_CALL(lane1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane2, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane3, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - OWL::Fakes::Section section; - OWL::Interfaces::Lanes lanes{{&lane1, &lane2, &lane3}}; - ON_CALL(section, GetLanes()).WillByDefault(ReturnRef(lanes)); - ON_CALL(section, GetLength()).WillByDefault(Return(100.0)); - std::list sections{§ion}; - ON_CALL(*road, GetSections()).WillByDefault(ReturnRef(sections)); - std::string idRoad = "RoadA"; - ON_CALL(*road, GetId()).WillByDefault(ReturnRef(idRoad)); - - std::map targetPosition1{{idRoad, {idRoad, 1, 15., 0, 0}}}; - std::map targetPosition2{{idRoad, {idRoad, -2, 15., 0, 0}}}; - - WorldDataQuery wdQuery{worldData}; - auto result1 = wdQuery.GetRelativeLaneId(roadStream.Get(), 10.0, -1, targetPosition1).at(node->roadGraphVertex); - auto result2 = wdQuery.GetRelativeLaneId(roadStream.Get(), 10.0, -1, targetPosition2).at(node->roadGraphVertex); - - EXPECT_THAT(result1, Eq(1)); - EXPECT_THAT(result2, Eq(-1)); -} - -TEST(GetRelativeLaneId, IdOfEgoLaneChanges_ReturnsCorrectRelativeId) -{ - OWL::Fakes::WorldData worldData; - FakeRoadMultiStream roadStream; - - auto [node, road] = roadStream.AddRoot(100.0, true); - OWL::Fakes::Lane lane1_1; - OWL::Fakes::Lane lane1_2; - OWL::Id idLane1_1 = 1, idLane1_2 = 2; - ON_CALL(lane1_1, GetId()).WillByDefault(Return(idLane1_1)); - ON_CALL(lane1_2, GetId()).WillByDefault(Return(idLane1_2)); - ON_CALL(lane1_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane1_2, GetOdId()).WillByDefault(Return(-2)); - std::vector emptyIds{}; - ON_CALL(lane1_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane1_2, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - OWL::Fakes::Section section1; - OWL::Interfaces::Lanes lanes1{{&lane1_1, &lane1_2}}; - ON_CALL(section1, GetLanes()).WillByDefault(ReturnRef(lanes1)); - ON_CALL(section1, GetLength()).WillByDefault(Return(100.0)); - OWL::Fakes::Lane lane2_1; - OWL::Fakes::Lane lane2_2; - OWL::Fakes::Lane lane2_3; - OWL::Id idLane2_1 = 3, idLane2_2 = 4, idLane2_3 = 5; - ON_CALL(lane2_1, GetId()).WillByDefault(Return(idLane2_1)); - ON_CALL(lane2_2, GetId()).WillByDefault(Return(idLane2_2)); - ON_CALL(lane2_3, GetId()).WillByDefault(Return(idLane2_3)); - ON_CALL(lane2_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane2_2, GetOdId()).WillByDefault(Return(-2)); - ON_CALL(lane2_3, GetOdId()).WillByDefault(Return(-3)); - std::vector predecessors2_2{idLane1_1}; - std::vector predecessors2_3{idLane1_2}; - ON_CALL(lane2_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane2_2, GetPrevious()).WillByDefault(ReturnRef(predecessors2_2)); - ON_CALL(lane2_3, GetPrevious()).WillByDefault(ReturnRef(predecessors2_3)); - OWL::Fakes::Section section2; - OWL::Interfaces::Lanes lanes2{{&lane2_1, &lane2_2, &lane2_3}}; - ON_CALL(section2, GetLanes()).WillByDefault(ReturnRef(lanes2)); - ON_CALL(section2, GetSOffset()).WillByDefault(Return(100.0)); - ON_CALL(section2, GetLength()).WillByDefault(Return(200.0)); - std::list sections{§ion1, §ion2}; - ON_CALL(*road, GetSections()).WillByDefault(ReturnRef(sections)); - std::string idRoad = "RoadA"; - ON_CALL(*road, GetId()).WillByDefault(ReturnRef(idRoad)); - - std::map targetPosition1{{idRoad, {idRoad, -1, 115., 0, 0}}}; - std::map targetPosition2{{idRoad, {idRoad, -2, 115., 0, 0}}}; - - WorldDataQuery wdQuery{worldData}; - auto result1 = wdQuery.GetRelativeLaneId(roadStream.Get(), 10.0, -1, targetPosition1).at(node->roadGraphVertex); - auto result2 = wdQuery.GetRelativeLaneId(roadStream.Get(), 10.0, -1, targetPosition2).at(node->roadGraphVertex); - - EXPECT_THAT(result1, Eq(1)); - EXPECT_THAT(result2, Eq(0)); -} - -TEST(GetRelativeLaneId, BranchingTree_ReturnsCorrectRelativeId) -{ - OWL::Fakes::WorldData worldData; - FakeRoadMultiStream roadStream; - - auto [node1, road1] = roadStream.AddRoot(100.0, true); - auto [node2, road2] = roadStream.AddRoad(150.0, true, *node1); - auto [node3, road3] = roadStream.AddRoad(200.0, false, *node1); - OWL::Fakes::Lane lane1_1; - OWL::Fakes::Lane lane1_2; - OWL::Id idLane1_1 = 1, idLane1_2 = 2; - ON_CALL(lane1_1, GetId()).WillByDefault(Return(idLane1_1)); - ON_CALL(lane1_2, GetId()).WillByDefault(Return(idLane1_2)); - ON_CALL(lane1_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane1_2, GetOdId()).WillByDefault(Return(-2)); - std::vector emptyIds{}; - ON_CALL(lane1_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane1_2, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - OWL::Fakes::Section section1; - OWL::Interfaces::Lanes lanes1{{&lane1_1, &lane1_2}}; - ON_CALL(section1, GetLanes()).WillByDefault(ReturnRef(lanes1)); - ON_CALL(section1, GetLength()).WillByDefault(Return(100.0)); - OWL::Fakes::Lane lane2_1; - OWL::Fakes::Lane lane2_2; - OWL::Id idLane2_1 = 3, idLane2_2 = 4; - ON_CALL(lane2_1, GetId()).WillByDefault(Return(idLane2_1)); - ON_CALL(lane2_2, GetId()).WillByDefault(Return(idLane2_2)); - ON_CALL(lane2_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane2_2, GetOdId()).WillByDefault(Return(-2)); - std::vector predecessors2_1{idLane1_1}; - std::vector predecessors2_2{idLane1_2}; - ON_CALL(lane2_1, GetPrevious()).WillByDefault(ReturnRef(predecessors2_1)); - ON_CALL(lane2_2, GetPrevious()).WillByDefault(ReturnRef(predecessors2_2)); - OWL::Fakes::Section section2; - OWL::Interfaces::Lanes lanes2{{&lane2_1, &lane2_2}}; - ON_CALL(section2, GetLanes()).WillByDefault(ReturnRef(lanes2)); - ON_CALL(section2, GetSOffset()).WillByDefault(Return(0.0)); - ON_CALL(section2, GetLength()).WillByDefault(Return(150.0)); - OWL::Fakes::Lane lane3_1; - OWL::Fakes::Lane lane3_2; - OWL::Id idLane3_1 = 5, idLane3_2 = 6; - ON_CALL(lane3_1, GetId()).WillByDefault(Return(idLane3_1)); - ON_CALL(lane3_2, GetId()).WillByDefault(Return(idLane3_2)); - ON_CALL(lane3_1, GetOdId()).WillByDefault(Return(1)); - ON_CALL(lane3_2, GetOdId()).WillByDefault(Return(2)); - std::vector predecessors3_1{idLane1_1}; - std::vector predecessors3_2{idLane1_2}; - ON_CALL(lane3_1, GetNext()).WillByDefault(ReturnRef(predecessors3_1)); - ON_CALL(lane3_2, GetNext()).WillByDefault(ReturnRef(predecessors3_2)); - OWL::Fakes::Section section3; - OWL::Interfaces::Lanes lanes3{{&lane3_1, &lane3_2}}; - ON_CALL(section3, GetLanes()).WillByDefault(ReturnRef(lanes3)); - ON_CALL(section3, GetSOffset()).WillByDefault(Return(0.0)); - ON_CALL(section3, GetLength()).WillByDefault(Return(200.0)); - std::list sections1{§ion1}; - ON_CALL(*road1, GetSections()).WillByDefault(ReturnRef(sections1)); - std::list sections2{§ion2}; - ON_CALL(*road2, GetSections()).WillByDefault(ReturnRef(sections2)); - std::list sections3{§ion3}; - ON_CALL(*road3, GetSections()).WillByDefault(ReturnRef(sections3)); - - std::string idRoad1 = "RoadA"; - std::string idRoad2 = "RoadB"; - std::string idRoad3 = "RoadC"; - ON_CALL(*road1, GetId()).WillByDefault(ReturnRef(idRoad1)); - ON_CALL(*road2, GetId()).WillByDefault(ReturnRef(idRoad2)); - ON_CALL(*road3, GetId()).WillByDefault(ReturnRef(idRoad3)); - - std::map targetPosition1{{idRoad2, {idRoad2, -1, 115., 0, 0}}, {idRoad3, {idRoad3, 1, 115., 0, 0}}}; - std::map targetPosition2{{idRoad2, {idRoad2, -2, 115., 0, 0}}, {idRoad3, {idRoad3, 2, 115., 0, 0}}}; - - WorldDataQuery wdQuery{worldData}; - auto result1 = wdQuery.GetRelativeLaneId(roadStream.Get(), 10.0, -1, targetPosition1).at(node3->roadGraphVertex); - auto result2 = wdQuery.GetRelativeLaneId(roadStream.Get(), 10.0, -1, targetPosition2).at(node3->roadGraphVertex); - - EXPECT_THAT(result1, Eq(0)); - EXPECT_THAT(result2, Eq(-1)); -} - -TEST(GetRelativeLaneId, TargetOnPreviousSection_ReturnsCorrectRelativeId) -{ - OWL::Fakes::WorldData worldData; - FakeRoadMultiStream roadStream; - - auto [node, road] = roadStream.AddRoot(100.0, true); - OWL::Fakes::Lane lane1_1; - OWL::Fakes::Lane lane1_2; - OWL::Id idLane1_1 = 1, idLane1_2 = 2; - ON_CALL(lane1_1, GetId()).WillByDefault(Return(idLane1_1)); - ON_CALL(lane1_2, GetId()).WillByDefault(Return(idLane1_2)); - ON_CALL(lane1_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane1_2, GetOdId()).WillByDefault(Return(-2)); - std::vector emptyIds{}; - ON_CALL(lane1_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane1_2, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - OWL::Fakes::Section section1; - OWL::Interfaces::Lanes lanes1{{&lane1_1, &lane1_2}}; - ON_CALL(section1, GetLanes()).WillByDefault(ReturnRef(lanes1)); - ON_CALL(section1, GetLength()).WillByDefault(Return(100.0)); - OWL::Fakes::Lane lane2_1; - OWL::Fakes::Lane lane2_2; - OWL::Fakes::Lane lane2_3; - OWL::Id idLane2_1 = 3, idLane2_2 = 4, idLane2_3 = 5; - ON_CALL(lane2_1, GetId()).WillByDefault(Return(idLane2_1)); - ON_CALL(lane2_2, GetId()).WillByDefault(Return(idLane2_2)); - ON_CALL(lane2_3, GetId()).WillByDefault(Return(idLane2_3)); - ON_CALL(lane2_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane2_2, GetOdId()).WillByDefault(Return(-2)); - ON_CALL(lane2_3, GetOdId()).WillByDefault(Return(-3)); - std::vector predecessors2_2{idLane1_1}; - std::vector predecessors2_3{idLane1_2}; - ON_CALL(lane2_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane2_2, GetPrevious()).WillByDefault(ReturnRef(predecessors2_2)); - ON_CALL(lane2_3, GetPrevious()).WillByDefault(ReturnRef(predecessors2_3)); - OWL::Fakes::Section section2; - OWL::Interfaces::Lanes lanes2{{&lane2_1, &lane2_2, &lane2_3}}; - ON_CALL(section2, GetLanes()).WillByDefault(ReturnRef(lanes2)); - ON_CALL(section2, GetSOffset()).WillByDefault(Return(100.0)); - ON_CALL(section2, GetLength()).WillByDefault(Return(200.0)); - std::list sections{§ion1, §ion2}; - ON_CALL(*road, GetSections()).WillByDefault(ReturnRef(sections)); - std::string idRoad = "RoadA"; - ON_CALL(*road, GetId()).WillByDefault(ReturnRef(idRoad)); - - std::map targetPosition1{{idRoad, {idRoad, -1, 15., 0, 0}}}; - std::map targetPosition2{{idRoad, {idRoad, -2, 15., 0, 0}}}; - - WorldDataQuery wdQuery{worldData}; - auto result1 = wdQuery.GetRelativeLaneId(roadStream.Get(), 110.0, -2, targetPosition1).at(node->roadGraphVertex); - auto result2 = wdQuery.GetRelativeLaneId(roadStream.Get(), 110.0, -2, targetPosition2).at(node->roadGraphVertex); - - EXPECT_THAT(result1, Eq(0)); - EXPECT_THAT(result2, Eq(-1)); -} - - -TEST(GetRelativeLaneId, TargetOnPreviousRoad_ReturnsCorrectRelativeId) -{ - OWL::Fakes::WorldData worldData; - FakeRoadMultiStream roadStream; - - auto [node1, road1] = roadStream.AddRoot(100.0, true); - auto [node2, road2] = roadStream.AddRoad(150.0, true, *node1); - OWL::Fakes::Lane lane1_1; - OWL::Fakes::Lane lane1_2; - OWL::Id idLane1_1 = 1, idLane1_2 = 2; - ON_CALL(lane1_1, GetId()).WillByDefault(Return(idLane1_1)); - ON_CALL(lane1_2, GetId()).WillByDefault(Return(idLane1_2)); - ON_CALL(lane1_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane1_2, GetOdId()).WillByDefault(Return(-2)); - std::vector emptyIds{}; - ON_CALL(lane1_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane1_2, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - OWL::Fakes::Section section1; - OWL::Interfaces::Lanes lanes1{{&lane1_1, &lane1_2}}; - ON_CALL(section1, GetLanes()).WillByDefault(ReturnRef(lanes1)); - ON_CALL(section1, GetLength()).WillByDefault(Return(100.0)); - OWL::Fakes::Lane lane2_1; - OWL::Fakes::Lane lane2_2; - OWL::Id idLane2_1 = 3, idLane2_2 = 4; - ON_CALL(lane2_1, GetId()).WillByDefault(Return(idLane2_1)); - ON_CALL(lane2_2, GetId()).WillByDefault(Return(idLane2_2)); - ON_CALL(lane2_1, GetOdId()).WillByDefault(Return(-1)); - ON_CALL(lane2_2, GetOdId()).WillByDefault(Return(-2)); - std::vector predecessors2_2{idLane1_1}; - ON_CALL(lane2_1, GetPrevious()).WillByDefault(ReturnRef(emptyIds)); - ON_CALL(lane2_2, GetPrevious()).WillByDefault(ReturnRef(predecessors2_2)); - OWL::Fakes::Section section2; - OWL::Interfaces::Lanes lanes2{{&lane2_1, &lane2_2}}; - ON_CALL(section2, GetLanes()).WillByDefault(ReturnRef(lanes2)); - ON_CALL(section2, GetSOffset()).WillByDefault(Return(0.0)); - ON_CALL(section2, GetLength()).WillByDefault(Return(150.0)); - std::list sections1{§ion1}; - ON_CALL(*road1, GetSections()).WillByDefault(ReturnRef(sections1)); - std::list sections2{§ion2}; - ON_CALL(*road2, GetSections()).WillByDefault(ReturnRef(sections2)); - - std::string idRoad1 = "RoadA"; - std::string idRoad2 = "RoadB"; - ON_CALL(*road1, GetId()).WillByDefault(ReturnRef(idRoad1)); - ON_CALL(*road2, GetId()).WillByDefault(ReturnRef(idRoad2)); - - std::map targetPosition{{idRoad1, {idRoad1, -1, 15., 0, 0}}}; - - WorldDataQuery wdQuery{worldData}; - auto result = wdQuery.GetRelativeLaneId(roadStream.Get(), 110.0, -2, targetPosition).at(node2->roadGraphVertex); - - EXPECT_THAT(result, Eq(0)); -} - TEST(GetLaneCurvature, OnLaneStreamReturnsCorrectCurvature) { OWL::Fakes::WorldData worldData; -- GitLab