Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
S
simulator
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Requirements
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Locked files
Build
Pipelines
Jobs
Pipeline schedules
Test cases
Artifacts
Deploy
Releases
Package Registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Code review analytics
Issue analytics
Insights
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to JiHu GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Chenhao Ma
simulator
Commits
7c695b92
Commit
7c695b92
authored
5 years ago
by
Qiang Lu
Committed by
Martins Mozeiko
5 years ago
Browse files
Options
Downloads
Patches
Plain Diff
Add junctions to opendrive map exporter
parent
0e809810
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
Assets/Scripts/Editor/OpenDriveMapExporter.cs
+111
-53
111 additions, 53 deletions
Assets/Scripts/Editor/OpenDriveMapExporter.cs
with
111 additions
and
53 deletions
Assets/Scripts/Editor/OpenDriveMapExporter.cs
+
111
−
53
View file @
7c695b92
...
...
@@ -7,9 +7,11 @@
using
UnityEngine
;
using
Simulator.Map
;
using
System
;
using
System.IO
;
using
System.Linq
;
using
System.Xml.Serialization
;
using
System.Xml
;
using
Schemas
;
using
System.Collections.Generic
;
using
Utility
=
Simulator
.
Utilities
.
Utility
;
...
...
@@ -439,6 +441,35 @@ namespace Simulator.Editor
}
}
Map
.
junction
=
AddJunctions
(
roadId
);
// Update road links and lane links
foreach
(
var
NLSLIdxList
in
neighborLaneSectionLanesIdx
)
{
var
roadBeforeLanes
=
new
HashSet
<
MapLane
>();
// lanes before current road
var
roadAfterLanes
=
new
HashSet
<
MapLane
>();
// One way road
if
(
NLSLIdxList
.
Count
==
1
)
{
UpdateLaneLink
(
ref
roadBeforeLanes
,
ref
roadAfterLanes
,
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
0
]]);
}
// Two way Roads
else
{
UpdateLaneLink
(
ref
roadBeforeLanes
,
ref
roadAfterLanes
,
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
0
]]);
UpdateLaneLink
(
ref
roadBeforeLanes
,
ref
roadAfterLanes
,
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
1
]]);
}
var
curRoadId
=
Lane2RoadId
[
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
0
]][
0
]];
UpdateRoadLink
(
roadBeforeLanes
,
roadAfterLanes
,
curRoadId
);
}
Map
.
road
=
Roads
;
}
OpenDRIVEJunction
[]
AddJunctions
(
uint
roadId
)
{
uint
firstJunctionId
=
roadId
;
uint
junctionId
=
firstJunctionId
;
var
junctions
=
new
OpenDRIVEJunction
[
Intersections
.
Count
];
...
...
@@ -453,44 +484,78 @@ namespace Simulator.Editor
var
intersectionLanes
=
mapIntersection
.
transform
.
GetComponentsInChildren
<
MapLane
>();
var
updatedRoadIds
=
new
HashSet
<
uint
>();
Debug
.
LogWarning
(
$"intersectionLanes length
{
intersectionLanes
.
Length
}
"
);
// Tuple: (incomingRoadId, connectingRoadId, contactPoint)
var
connections2LaneLink
=
new
Dictionary
<
Tuple
<
uint
,
uint
,
contactPoint
>,
List
<
OpenDRIVEJunctionConnectionLaneLink
>>();
foreach
(
var
lane
in
intersectionLanes
)
{
Lane2JunctionId
[
lane
]
=
junctionId
;
var
connectingRoadId
=
Lane2RoadId
[
lane
];
var
connectingLaneId
=
Lane2LaneId
[
lane
];
var
incomingLanes
=
lane
.
befores
;
// Check whether lane has same direction with its road
if
(
connectingLaneId
>
0
)
{
incomingLanes
=
lane
.
afters
;
}
foreach
(
var
incomingLane
in
incomingLanes
)
{
var
incomingRoadId
=
Lane2RoadId
[
incomingLane
];
var
key
=
Tuple
.
Create
(
incomingRoadId
,
connectingRoadId
,
GetContactPoint
(
incomingRoadId
,
lane
));
if
(
connections2LaneLink
.
ContainsKey
(
key
))
{
connections2LaneLink
[
key
].
Add
(
new
OpenDRIVEJunctionConnectionLaneLink
()
{
from
=
Lane2LaneId
[
incomingLane
],
to
=
connectingLaneId
,
fromSpecified
=
true
,
toSpecified
=
true
,
}
);
}
else
{
connections2LaneLink
[
key
]
=
new
List
<
OpenDRIVEJunctionConnectionLaneLink
>()
{
new
OpenDRIVEJunctionConnectionLaneLink
()
{
from
=
Lane2LaneId
[
incomingLane
],
to
=
connectingLaneId
,
fromSpecified
=
true
,
toSpecified
=
true
,
}
};
}
}
// Update corresponding road header's junctionId
roadId
=
Lane2RoadId
[
lane
];
if
(
updatedRoadIds
.
Contains
(
roadId
))
continue
;
Roads
[
roadId
].
junction
=
junctionId
.
ToString
();
}
junctions
[
junctionId
-
firstJunctionId
]
=
junction
;
junctionId
+=
1
;
}
Map
.
junction
=
junctions
;
// Update road links and lane links
foreach
(
var
NLSLIdxList
in
neighborLaneSectionLanesIdx
)
{
var
roadBeforeLanes
=
new
HashSet
<
MapLane
>();
// lanes before current road
var
roadAfterLanes
=
new
HashSet
<
MapLane
>();
// One way road
if
(
NLSLIdxList
.
Count
==
1
)
var
connections
=
new
OpenDRIVEJunctionConnection
[
connections2LaneLink
.
Keys
.
Count
];
var
index
=
0
;
foreach
(
var
entry
in
connections2LaneLink
)
{
UpdateLaneLink
(
ref
roadBeforeLanes
,
ref
roadAfterLanes
,
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
0
]]);
}
// Two way Roads
else
{
UpdateLaneLink
(
ref
roadBeforeLanes
,
ref
roadAfterLanes
,
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
0
]]);
UpdateLaneLink
(
ref
roadBeforeLanes
,
ref
roadAfterLanes
,
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
1
]]);
var
laneLinks
=
entry
.
Value
.
ToArray
();
connections
[
index
++]
=
new
OpenDRIVEJunctionConnection
()
{
laneLink
=
laneLinks
,
id
=
index
.
ToString
(),
incomingRoad
=
entry
.
Key
.
Item1
.
ToString
(),
connectingRoad
=
entry
.
Key
.
Item2
.
ToString
(),
contactPoint
=
entry
.
Key
.
Item3
,
contactPointSpecified
=
true
,
};
}
var
curRoadId
=
Lane2RoadId
[
neighborForwardLaneSectionLanes
[
NLSLIdxList
[
0
]][
0
]]
;
UpdateRoadLink
(
roadBeforeLanes
,
roadAfterLanes
,
curRoadId
)
;
// TODO update road link for junction and road header association with junction.
junction
.
connection
=
connections
;
junctions
[
junctionId
-
firstJunctionId
]
=
junction
;
junctionId
+=
1
;
}
Map
.
road
=
Road
s
;
return
junction
s
;
}
void
UpdateRoadLink
(
HashSet
<
MapLane
>
roadBeforeLanes
,
HashSet
<
MapLane
>
roadAfterLanes
,
uint
curRoadId
)
...
...
@@ -510,11 +575,8 @@ namespace Simulator.Editor
sucRoadIds
.
Add
(
Lane2RoadId
[
afterLane
]);
}
var
roadStartPoint
=
new
Vector3
((
float
)
Roads
[
curRoadId
].
planView
[
0
].
x
,
(
float
)
Roads
[
curRoadId
].
elevationProfile
.
elevation
[
0
].
a
,
(
float
)
Roads
[
curRoadId
].
planView
[
0
].
y
);
uint
?
preRoadId
=
null
;
uint
?
sucRoadId
=
null
;
contactPoint
preContact
=
contactPoint
.
start
;
contactPoint
sucContact
=
contactPoint
.
start
;
var
roadPredecessor
=
new
OpenDRIVERoadLinkPredecessor
();
var
roadSuccessor
=
new
OpenDRIVERoadLinkSuccessor
();
...
...
@@ -535,22 +597,11 @@ namespace Simulator.Editor
{
preRoadId
=
Lane2RoadId
[
curBeforeLane
];
// Use curBeforeLane to compute contactPoint
var
preRoadPositions
=
curBeforeLane
.
mapWorldPositions
;
if
(
Lane2LaneId
[
curBeforeLane
]
>
0
)
{
// if the lane is a left lane in the road, reference line is opposite with the lane
preRoadPositions
.
Reverse
();
}
if
((
roadStartPoint
-
preRoadPositions
.
First
()).
magnitude
>
(
roadStartPoint
-
preRoadPositions
.
Last
()).
magnitude
)
{
preContact
=
contactPoint
.
end
;
}
roadPredecessor
=
new
OpenDRIVERoadLinkPredecessor
()
{
elementType
=
elementType
.
road
,
elementId
=
preRoadId
.
ToString
(),
contactPoint
=
pre
Contact
,
contactPoint
=
Get
Contact
Point
(
curRoadId
,
curBeforeLane
)
,
elementTypeSpecified
=
true
,
contactPointSpecified
=
true
,
};
...
...
@@ -577,22 +628,11 @@ namespace Simulator.Editor
{
sucRoadId
=
Lane2RoadId
[
curAfterLane
];
// use curAfterLane positions to represent successor road compute contactPoint
var
sucRoadPositions
=
curAfterLane
.
mapWorldPositions
;
if
(
Lane2LaneId
[
curAfterLane
]
>
0
)
{
sucRoadPositions
.
Reverse
();
}
if
((
roadStartPoint
-
sucRoadPositions
.
First
()).
magnitude
>
(
roadStartPoint
-
sucRoadPositions
.
Last
()).
magnitude
)
{
sucContact
=
contactPoint
.
end
;
}
roadSuccessor
=
new
OpenDRIVERoadLinkSuccessor
()
{
elementType
=
elementType
.
road
,
elementId
=
sucRoadId
.
ToString
(),
contactPoint
=
suc
Contact
,
contactPoint
=
Get
Contact
Point
(
curRoadId
,
curAfterLane
)
,
elementTypeSpecified
=
true
,
contactPointSpecified
=
true
,
};
...
...
@@ -626,6 +666,23 @@ namespace Simulator.Editor
}
}
// Given current road and any lane of predecessor/successor road, get contactPoint of the predecessor/successor road
contactPoint
GetContactPoint
(
uint
curRoadId
,
MapLane
linkRoadLane
)
{
var
roadStartPoint
=
new
Vector3
((
float
)
Roads
[
curRoadId
].
planView
[
0
].
x
,
(
float
)
Roads
[
curRoadId
].
elevationProfile
.
elevation
[
0
].
a
,
(
float
)
Roads
[
curRoadId
].
planView
[
0
].
y
);
var
positions
=
linkRoadLane
.
mapWorldPositions
;
if
(
Lane2LaneId
[
linkRoadLane
]
>
0
)
{
// if the lane is a left lane in the road, reference line is opposite with the lane
positions
.
Reverse
();
}
if
((
roadStartPoint
-
positions
.
First
()).
magnitude
>
(
roadStartPoint
-
positions
.
Last
()).
magnitude
)
{
return
contactPoint
.
end
;
}
return
contactPoint
.
start
;
}
string
GetJunctionId
(
HashSet
<
uint
>
roadIds
)
{
var
junctionIds
=
new
HashSet
<
string
>();
...
...
@@ -1083,9 +1140,10 @@ namespace Simulator.Editor
{
var
serializer
=
new
XmlSerializer
(
typeof
(
OpenDRIVE
));
using
(
var
fs
=
File
.
Create
(
filePath
))
using
(
var
writer
=
new
StreamWriter
(
filePath
))
using
(
var
xmlWriter
=
XmlWriter
.
Create
(
writer
,
new
XmlWriterSettings
{
Indent
=
true
,
IndentChars
=
" "
}))
{
serializer
.
Serialize
(
fs
,
Map
);
serializer
.
Serialize
(
xmlWriter
,
Map
);
}
}
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment