Commit b69bbad3 authored by David Weiß's avatar David Weiß Committed by Reinhard Biegel
Browse files

mod(World_OSI): Add query for relative lane of other object



Signed-off-by: David Weiß's avatarWeiss David <david.weiss@in-tech.com>
parent 9873e01f
......@@ -127,6 +127,13 @@ public:
//! \return lanes relative to agent
virtual RelativeWorldView::Lanes GetRelativeLanes(double range, int relativeLane = 0) 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<int> 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
......
......@@ -553,7 +553,8 @@ 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 route route of the agent
//! \param roadGraph road network as viewed from agent
//! \param startNode position on roadGraph of agent
//! \param ownPosition position of the agent
//! \param otherPosition position of the other object
//! \param objectCorners corners of the other object
......@@ -563,8 +564,8 @@ public:
//! Returns all traffic signs valid for a lane inside the range
//!
//! \param route route to search along
//! \param roadId OpenDrive Id of the road
//! \param roadGraph road network as viewed from agent
//! \param startNode position on roadGraph of agent
//! \param laneId OpenDrive Id of the lane
//! \param startDistance s coordinate
//! \param searchRange range of search (can also be negative)
......@@ -574,8 +575,8 @@ public:
//! Returns all road markings valid for a lane inside the range
//!
//! \param route route to search along
//! \param roadId OpenDrive Id of the road
//! \param roadGraph road network as viewed from agent
//! \param startNode position on roadGraph of agent
//! \param laneId OpenDrive Id of the lane
//! \param startDistance s coordinate
//! \param searchRange range of search
......@@ -596,7 +597,8 @@ public:
//! Retrieves all lane markings on the given position on the given side of the lane inside the range
//!
//! \param roadId OpenDrive Id of the road
//! \param roadGraph road network as viewed from agent
//! \param startNode position on roadGraph of agent
//! \param laneId OpenDrive Id of the lane
//! \param startDistance s coordinate
//! \param range search range
......@@ -605,8 +607,8 @@ public:
//! Returns the relative distances (start and end) and the connecting road id of all junctions on the route in range
//!
//! \param route route of the agent
//! \param roadId OpenDrive Id of the road
//! \param roadGraph road network as viewed from agent
//! \param startNode position on roadGraph of agent
//! \param startDistance start s coordinate on the road
//! \param range range of search
//! \return information about all junctions in range
......@@ -617,8 +619,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 route route of the agent
//! \param roadId OpenDrive Id of the road
//! \param roadGraph road network as viewed from agent
//! \param startNode position on roadGraph of agent
//! \param laneId OpenDrive Id of the lane
//! \param distance start s coordinate on the road
//! \param range range of search
......@@ -626,6 +628,16 @@ public:
//! \return information about all lanes in range
virtual RouteQueryResult<RelativeWorldView::Lanes> 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<std::optional<int>> GetRelativeLaneId(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map<std::string, GlobalRoadPosition> 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
......
......@@ -345,6 +345,11 @@ public:
return implementation->GetRelativeLanes(roadGraph, startNode, laneId, distance, range, includeOncoming);
}
RouteQueryResult<std::optional<int>> GetRelativeLaneId(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map<std::string, GlobalRoadPosition> targetPosition) const override
{
return implementation->GetRelativeLaneId(roadGraph, startNode, laneId, distance, targetPosition);
}
std::vector<JunctionConnection> GetConnectionsOnJunction(std::string junctionId, std::string incomingRoadId) const override
{
return implementation->GetConnectionsOnJunction(junctionId, incomingRoadId);
......
......@@ -915,7 +915,7 @@ std::map<int, OWL::Id> WorldDataQuery::AddLanesOfSection(const OWL::Interfaces::
continue;
}
int relativeLaneId = inStreamDirection ? (laneId - currentOwnLaneId) : (currentOwnLaneId - laneId);
bool differentSigns = (currentOwnLaneId * laneId < 0) && currentOwnLaneId != 0;
bool differentSigns = currentOwnLaneId * laneId < 0;
if (differentSigns)
{
relativeLaneId += (relativeLaneId > 0) ? -1 : 1;
......@@ -990,6 +990,97 @@ RouteQueryResult<RelativeWorldView::Lanes> WorldDataQuery::GetRelativeLanes(cons
worldData);
}
RouteQueryResult<std::optional<int>> WorldDataQuery::GetRelativeLaneId(const RoadMultiStream &roadStream, double ownPosition, int ownLaneId, std::map<std::string, GlobalRoadPosition> targetPosition) const
{
std::optional<int> currentOwnLaneId;
std::optional<int> currentTargetLaneId;
return roadStream.Traverse<std::optional<int>, std::map<int, OWL::Id>>(
RoadMultiStream::TraversedFunction<std::optional<int>, std::map<int, OWL::Id>>{[&](const auto& road, const auto& previousResult, const auto& previousLaneIds)->std::tuple<std::optional<int>, std::map<int, OWL::Id>>
{
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<int, OWL::Id> 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<std::optional<double> > WorldDataQuery::GetLaneCurvature(const LaneMultiStream& laneStream, double position) const
{
return laneStream.Traverse<std::optional<double>>(LaneMultiStream::TraversedFunction<std::optional<double>>{[&](const auto& lane, const auto& previousResult)
......
......@@ -553,6 +553,16 @@ public:
//! \return information about all lanes in range
RouteQueryResult<RelativeWorldView::Lanes> 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<std::optional<int>> GetRelativeLaneId(const RoadMultiStream& roadStream, double ownPosition, int ownLaneId, std::map<std::string, GlobalRoadPosition> targetPosition) const;
RouteQueryResult<std::optional<double>> GetLaneCurvature (const LaneMultiStream& laneStream, double position) const;
RouteQueryResult<std::optional<double>> GetLaneWidth (const LaneMultiStream& laneStream, double position) const;
......
......@@ -334,6 +334,14 @@ RouteQueryResult<RelativeWorldView::Lanes> WorldImplementation::GetRelativeLanes
return worldDataQuery.GetRelativeLanes(*roadMultiStream, startDistanceOnStream, laneId, range, includeOncoming);
}
RouteQueryResult<std::optional<int> > WorldImplementation::GetRelativeLaneId(const RoadGraph &roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map<std::string, GlobalRoadPosition> targetPosition) const
{
const auto roadMultiStream = worldDataQuery.CreateRoadMultiStream(roadGraph, startNode);
double startDistanceOnStream = roadMultiStream->GetPositionByVertexAndS(startNode, distance);
return worldDataQuery.GetRelativeLaneId(*roadMultiStream, startDistanceOnStream, laneId, targetPosition);
}
RouteQueryResult<std::vector<const AgentInterface*> > 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);
......
......@@ -213,6 +213,8 @@ public:
RouteQueryResult<RelativeWorldView::Lanes> GetRelativeLanes(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, double range, bool includeOncoming = true) const override;
RouteQueryResult<std::optional<int>> GetRelativeLaneId(const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map<std::string, GlobalRoadPosition> targetPosition) const override;
std::vector<JunctionConnection> GetConnectionsOnJunction(std::string junctionId, std::string incomingRoadId) const override;
std::vector<IntersectingConnection> GetIntersectingConnections(std::string connectingRoadId) const override;
......
......@@ -159,6 +159,28 @@ RelativeWorldView::Lanes EgoAgent::GetRelativeLanes(double range, int relativeLa
range).at(0);
}
std::optional<int> EgoAgent::GetRelativeLaneId(const WorldObjectInterface *object, MeasurementPoint mp) const
{
std::map<std::string, GlobalRoadPosition> 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<const WorldObjectInterface*> EgoAgent::GetObjectsInRange(double backwardRange, double forwardRange, int relativeLane) const
{
auto objectsInRange = world->GetObjectsInRange(wayToTarget,
......
......@@ -46,6 +46,8 @@ public:
RelativeWorldView::Lanes GetRelativeLanes(double range, int relativeLane = 0) const override;
std::optional<int> GetRelativeLaneId(const WorldObjectInterface* object, MeasurementPoint mp) const override;
RelativeWorldView::Junctions GetRelativeJunctions(double range) const override;
std::vector<const WorldObjectInterface*> GetObjectsInRange (double backwardRange, double forwardRange, int relativeLane = 0) const override;
......
......@@ -27,6 +27,7 @@ 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_METHOD2(GetRelativeLanes, RelativeWorldView::Lanes (double range, int relativeLane));
MOCK_CONST_METHOD2(GetRelativeLaneId, std::optional<int> (const WorldObjectInterface* object, MeasurementPoint mp));
MOCK_CONST_METHOD1(GetRelativeJunctions, RelativeWorldView::Junctions (double range));
MOCK_CONST_METHOD3(GetObjectsInRange, std::vector<const WorldObjectInterface*> (double backwardRange, double forwardRange, int relativeLane));
MOCK_CONST_METHOD3(GetAgentsInRange, std::vector<const AgentInterface*> (double backwardRange, double forwardRange, int relativeLane));
......
......@@ -101,6 +101,7 @@ class FakeWorld : public WorldInterface
MOCK_CONST_METHOD5(GetDistanceBetweenObjects, RouteQueryResult<LongitudinalDistance> (const RoadGraph& roadGraph, RoadGraphVertex startNode, const ObjectPosition&, const std::optional<double>, const ObjectPosition&));
MOCK_CONST_METHOD4(GetRelativeJunctions, RouteQueryResult<RelativeWorldView::Junctions> (const RoadGraph& roadGraph, RoadGraphVertex startNode, double startDistance, double range));
MOCK_CONST_METHOD6(GetRelativeLanes, RouteQueryResult<RelativeWorldView::Lanes> (const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, double range, bool includeOncoming));
MOCK_CONST_METHOD5(GetRelativeLaneId, RouteQueryResult<std::optional<int>> (const RoadGraph& roadGraph, RoadGraphVertex startNode, int laneId, double distance, std::map<std::string, GlobalRoadPosition> targetPosition));
MOCK_CONST_METHOD2(GetRoadGraph, std::pair<RoadGraph, RoadGraphVertex>(const RouteElement& start, int maxDepth));
MOCK_CONST_METHOD1(GetEdgeWeights, std::map<RoadGraphEdge, double>(const RoadGraph& roadGraph));
};
......@@ -3044,7 +3044,7 @@ TEST(GetRelativeLanes, IdOfEgoLaneChanges_ReturnsLanesWithCorrectRelativeId)
EXPECT_THAT(result.at(1).lanes.at(2).successor, Eq(std::nullopt));
}
TEST(GetRelativeLanes, BranchingTree_ReturnsLanesOfWayToNode)
TEST(GetRelativeLane, BranchingTree_ReturnsLanesOfWayToNode)
{
OWL::Fakes::WorldData worldData;
FakeRoadMultiStream roadStream;
......@@ -3169,6 +3169,292 @@ TEST(GetRelativeLanes, 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<OWL::Id> 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<const OWL::Interfaces::Section*> sections{&section};
ON_CALL(*road, GetSections()).WillByDefault(ReturnRef(sections));
std::string idRoad = "RoadA";
ON_CALL(*road, GetId()).WillByDefault(ReturnRef(idRoad));
std::map<std::string, GlobalRoadPosition> targetPosition1{{idRoad, {idRoad, 1, 15., 0, 0}}};
std::map<std::string, GlobalRoadPosition> 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<OWL::Id> 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<OWL::Id> predecessors2_2{idLane1_1};
std::vector<OWL::Id> 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<const OWL::Interfaces::Section*> sections{&section1, &section2};
ON_CALL(*road, GetSections()).WillByDefault(ReturnRef(sections));
std::string idRoad = "RoadA";
ON_CALL(*road, GetId()).WillByDefault(ReturnRef(idRoad));
std::map<std::string, GlobalRoadPosition> targetPosition1{{idRoad, {idRoad, -1, 115., 0, 0}}};
std::map<std::string, GlobalRoadPosition> 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<OWL::Id> 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<OWL::Id> predecessors2_1{idLane1_1};
std::vector<OWL::Id> 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<OWL::Id> predecessors3_1{idLane1_1};
std::vector<OWL::Id> 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<const OWL::Interfaces::Section*> sections1{&section1};
ON_CALL(*road1, GetSections()).WillByDefault(ReturnRef(sections1));
std::list<const OWL::Interfaces::Section*> sections2{&section2};
ON_CALL(*road2, GetSections()).WillByDefault(ReturnRef(sections2));
std::list<const OWL::Interfaces::Section*> sections3{&section3};
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<std::string, GlobalRoadPosition> targetPosition1{{idRoad2, {idRoad2, -1, 115., 0, 0}}, {idRoad3, {idRoad3, 1, 115., 0, 0}}};
std::map<std::string, GlobalRoadPosition> 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<OWL::Id> 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<OWL::Id> predecessors2_2{idLane1_1};
std::vector<OWL::Id> 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<const OWL::Interfaces::Section*> sections{&section1, &section2};
ON_CALL(*road, GetSections()).WillByDefault(ReturnRef(sections));
std::string idRoad = "RoadA";
ON_CALL(*road, GetId()).WillByDefault(ReturnRef(idRoad));
std::map<std::string, GlobalRoadPosition> targetPosition1{{idRoad, {idRoad, -1, 15., 0, 0}}};
std::map<std