Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
Eclipse Projects
Eclipse simopenpass
opSimulation
Commits
b69bbad3
Commit
b69bbad3
authored
Jul 23, 2021
by
David Weiß
Committed by
Reinhard Biegel
Sep 02, 2021
Browse files
mod(World_OSI): Add query for relative lane of other object
Signed-off-by:
Weiss David
<
david.weiss@in-tech.com
>
parent
9873e01f
Changes
12
Hide whitespace changes
Inline
Side-by-side
sim/include/egoAgentInterface.h
View file @
b69bbad3
...
...
@@ -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
...
...
sim/include/worldInterface.h
View file @
b69bbad3
...
...
@@ -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 ro
ute
ro
ute to search along
//! \param
roadId OpenDrive Id of the road
//! \param ro
adGraph
ro
ad 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 ro
ute
ro
ute to search along
//! \param
roadId OpenDrive Id of the road
//! \param ro
adGraph
ro
ad 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 ro
ute
route of the
agent
//! \param
roadId OpenDrive Id of the road
//! \param ro
adGraph
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 ro
ute
route of the
agent
//! \param
roadId OpenDrive Id of the road
//! \param ro
adGraph
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
...
...
sim/src/core/slave/bindings/world.h
View file @
b69bbad3
...
...
@@ -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
);
...
...
sim/src/core/slave/modules/World_OSI/WorldDataQuery.cpp
View file @
b69bbad3
...
...
@@ -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
)
...
...
sim/src/core/slave/modules/World_OSI/WorldDataQuery.h
View file @
b69bbad3
...
...
@@ -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
;
...
...
sim/src/core/slave/modules/World_OSI/WorldImplementation.cpp
View file @
b69bbad3
...
...
@@ -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
);
...
...
sim/src/core/slave/modules/World_OSI/WorldImplementation.h
View file @
b69bbad3
...
...
@@ -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
;
...
...
sim/src/core/slave/modules/World_OSI/egoAgent.cpp
View file @
b69bbad3
...
...
@@ -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
,
...
...
sim/src/core/slave/modules/World_OSI/egoAgent.h
View file @
b69bbad3
...
...
@@ -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
;
...
...
sim/tests/fakes/gmock/fakeEgoAgent.h
View file @
b69bbad3
...
...
@@ -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
));
...
...
sim/tests/fakes/gmock/fakeWorld.h
View file @
b69bbad3
...
...
@@ -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
));
};
sim/tests/unitTests/core/slave/modules/World_OSI/worldQuery_Tests.cpp
View file @
b69bbad3
...
...
@@ -3044,7 +3044,7 @@ TEST(GetRelativeLanes, IdOfEgoLaneChanges_ReturnsLanesWithCorrectRelativeId)
EXPECT_THAT
(
result
.
at
(
1
).
lanes
.
at
(
2
).
successor
,
Eq
(
std
::
nullopt
));
}
TEST
(
GetRelativeLane
s
,
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