void BuildTestMap()
{
var lanes = new List <Lane>();
var overlaps = new List <Overlap>();
lanes.Add(new Lane()
{
id = new Id("2dap0_1_1"),
central_curve = new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = new List <Ros.PointENU>()
{
new Ros.PointENU(590700, 4140310.24),
new Ros.PointENU(590606, 4140310.24)
},
},
},
s = 0,
start_position = new Ros.PointENU(590700, 4140310.24),
length = 94
},
},
},
left_boundary = new LaneBoundary()
{
curve = new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = new List <Ros.PointENU>()
{
new Ros.PointENU(590700, 4140308.24),
new Ros.PointENU(590611, 4140308.24)
},
},
},
s = 0,
start_position = new Ros.PointENU(590700, 4140308.24),
length = 94,
},
},
},
length = 94,
@virtual = false,
boundary_type = new List <LaneBoundaryType>()
{
new LaneBoundaryType()
{
s = 0,
types = new List <LaneBoundaryType.Type>()
{
LaneBoundaryType.Type.DOUBLE_YELLOW,
},
},
},
},
});
overlaps.Add(new Overlap()
{
id = new Id("overlap_1"),
@object = new List <ObjectOverlapInfo>()
{
new ObjectOverlapInfo()
{
id = new Id("2dap0_1_2"),
overlap_info = new ObjectOverlapInfo.OverlapInfo_OneOf()
{
lane_overlap_info = new LaneOverlapInfo()
{
start_s = 0,
end_s = 0.1,
is_merge = false,
},
},
},
new ObjectOverlapInfo()
{
id = new Id("2505"),
overlap_info = new ObjectOverlapInfo.OverlapInfo_OneOf()
{
signal_overlap_info = new SignalOverlapInfo(),
},
},
}
});
overlaps.Add(new Overlap()
{
id = new Id("overlap_2"),
@object = new List <ObjectOverlapInfo>()
{
new ObjectOverlapInfo()
{
id = new Id("2dap0_2_2"),
overlap_info = new ObjectOverlapInfo.OverlapInfo_OneOf()
{
lane_overlap_info = new LaneOverlapInfo()
{
start_s = 0,
end_s = 0.1,
is_merge = false,
},
},
},
new ObjectOverlapInfo()
{
id = new Id("2506"),
overlap_info = new ObjectOverlapInfo.OverlapInfo_OneOf()
{
signal_overlap_info = new SignalOverlapInfo(),
},
},
}
});
hdmap = new HDMap()
{
header = new Map.Apollo.Header()
{
version = "1.500000",
date = "2018-03-23T13:27:54",
projection = new Projection()
{
proj = "+proj=utm +zone=10 +ellps=WGS84 +datum=WGS84 +units=m +no_defs",
},
district = "0",
rev_major = "1",
rev_minor = "0",
left = -121.982277,
top = 37.398079,
right = -121.971998,
bottom = 37.398079,
vendor = "Baidu",
},
lane = lanes,
overlap = overlaps,
};
}
bool Calculate()
{
//BuildTestMap();
hdmap = new Map.Apollo.HDMap();
//HD map top level elements
var lanes = new List <Lane>();
var signals = new List <Signal>();
var stop_signs = new List <StopSign>();
var overlaps = new List <Overlap>();
const float laneHalfWidth = 1.75f; //temp solution
const float stoplineWidth = 0.7f;
//list of target transforms
var targetList = new List <Transform>();
var noTarget = true;
foreach (var t in targets)
{
if (t != null)
{
noTarget = false;
targetList.Add(t);
}
}
if (noTarget)
{
targetList.Add(transform);
}
//initial collection
var segBldrs = new List <MapSegmentBuilder>();
var signalLights = new List <HDMapSignalLight>();
var stopSigns = new List <HDMapStopSign>();
foreach (var t in targetList)
{
if (t == null)
{
continue;
}
segBldrs.AddRange(t.GetComponentsInChildren <MapSegmentBuilder>());
signalLights.AddRange(t.GetComponentsInChildren <HDMapSignalLight>());
//stopSigns.AddRange(t.GetComponentsInChildren<HDMapStopSign>());
}
bool missingPoints = false;
var allSegs = new HashSet <MapSegment>(); //All segments regardless of segment actual type
//connect builder reference for each segment
foreach (var segBldr in segBldrs)
{
segBldr.segment.builder = segBldr;
allSegs.Add(segBldr.segment);
}
//Link before and after segment for each segment
foreach (var segment in allSegs)
{
//Make sure to clear unwanted leftover data from previous generation
segment.Clear();
segment.hdmapInfo = new HDMapSegmentInfo();
if ((segment.builder as MapLaneSegmentBuilder) == null) //consider only lane for now
{
continue;
}
//this is to avoid accidentally connect two nearby stoplines
if ((segment.builder as MapStopLineSegmentBuilder) != null)
{
continue;
}
//each segment must have at least 2 waypoints for calculation so complement to 2 waypoints as needed
while (segment.targetLocalPositions.Count < 2)
{
segment.targetLocalPositions.Add(Vector3.zero);
missingPoints = true;
}
var firstPt = segment.builder.transform.TransformPoint(segment.targetLocalPositions[0]);
var lastPt = segment.builder.transform.TransformPoint(segment.targetLocalPositions[segment.targetLocalPositions.Count - 1]);
foreach (var segment_cmp in allSegs)
{
if (segment_cmp.builder.GetType() != segment.builder.GetType()) //only connect same actual type
{
continue;
}
var firstPt_cmp = segment_cmp.builder.transform.TransformPoint(segment_cmp.targetLocalPositions[0]);
var lastPt_cmp = segment_cmp.builder.transform.TransformPoint(segment_cmp.targetLocalPositions[segment_cmp.targetLocalPositions.Count - 1]);
if ((firstPt - lastPt_cmp).magnitude < PROXIMITY / exportScaleFactor)
{
segment.befores.Add(segment_cmp);
}
if ((lastPt - firstPt_cmp).magnitude < PROXIMITY / exportScaleFactor)
{
segment.afters.Add(segment_cmp);
}
}
}
if (missingPoints)
{
Debug.Log("Some segment has less than 2 waypoints, complement it to 2");
}
var allLnSegs = new HashSet <MapSegment>();
foreach (var segment in allSegs)
{
var type = segment.builder.GetType();
if (type == typeof(MapLaneSegmentBuilder))
{
allLnSegs.Add(segment);
}
}
foreach (var lnSeg in allLnSegs)
{
foreach (var localPos in lnSeg.targetLocalPositions)
{
lnSeg.targetWorldPositions.Add(lnSeg.builder.transform.TransformPoint(localPos)); //Convert to world position
}
var lnBuilder = (MapLaneSegmentBuilder)(lnSeg.builder);
lnSeg.hdmapInfo.leftNeighborSegmentForward = lnBuilder.leftNeighborForward?.segment;
lnSeg.hdmapInfo.rightNeighborSegmentForward = lnBuilder.rightNeighborForward?.segment;
lnSeg.hdmapInfo.leftNeighborSegmentReverse = lnBuilder.leftNeighborReverse?.segment;
lnSeg.hdmapInfo.rightNeighborSegmentReverse = lnBuilder.rightNeighborReverse?.segment;
lnSeg.hdmapInfo.laneTurn = lnBuilder.laneTurn;
}
//build virtual connection lanes
var bridgeVirtualLnSegs = new List <MapSegment>();
foreach (var lnSeg in allLnSegs)
{
if (lnSeg.afters.Count > 0)
{
foreach (var aftrLn in lnSeg.afters)
{
bridgeVirtualLnSegs.Add(new MapSegment()
{
hdmapInfo = new HDMapSegmentInfo()
{
id = null
},
builder = null,
targetLocalPositions = null,
befores = new List <MapSegment>()
{
lnSeg
},
afters = new List <MapSegment>()
{
aftrLn
},
targetWorldPositions = new List <Vector3>()
{
lnSeg.targetWorldPositions[lnSeg.targetWorldPositions.Count - 1],
aftrLn.targetWorldPositions[0]
}
});
}
}
}
//lanes
//assign ids
int laneId = 0;
foreach (var lnSeg in allLnSegs)
{
lnSeg.hdmapInfo.id = $"lane_{laneId}";
++laneId;
}
//config lanes
foreach (var lnSeg in allLnSegs)
{
var centerPts = new List <Ros.PointENU>();
var lBndPts = new List <Ros.PointENU>();
var rBndPts = new List <Ros.PointENU>();
var worldPoses = lnSeg.targetWorldPositions;
var leftBoundPoses = new List <Vector3>();
var rightBoundPoses = new List <Vector3>();
float mLength = 0;
float lLength = 0;
float rLength = 0;
List <LaneSampleAssociation> associations = new List <LaneSampleAssociation>();
associations.Add(new LaneSampleAssociation()
{
s = 0,
width = laneHalfWidth,
});
for (int i = 0; i < worldPoses.Count; i++)
{
Vector3 curPt = worldPoses[i];
Vector3 tangFwd;
if (i == 0)
{
tangFwd = (worldPoses[1] - curPt).normalized;
}
else if (i == worldPoses.Count - 1)
{
tangFwd = (curPt - worldPoses[worldPoses.Count - 2]).normalized;
}
else
{
tangFwd = (((curPt - worldPoses[i - 1]) + (worldPoses[i + 1] - curPt)) * 0.5f).normalized;
}
Vector3 lPoint = -Vector3.Cross(tangFwd, Vector3.up) * laneHalfWidth + curPt;
Vector3 rPoint = Vector3.Cross(tangFwd, Vector3.up) * laneHalfWidth + curPt;
leftBoundPoses.Add(lPoint);
rightBoundPoses.Add(rPoint);
if (i > 0)
{
mLength += (curPt - worldPoses[i - 1]).magnitude;
associations.Add(new LaneSampleAssociation()
{
s = mLength,
width = laneHalfWidth,
});
}
centerPts.Add(GetApolloCoordinates(curPt, OriginEasting, OriginNorthing, false));
lBndPts.Add(GetApolloCoordinates(lPoint, OriginEasting, OriginNorthing, false));
rBndPts.Add(GetApolloCoordinates(rPoint, OriginEasting, OriginNorthing, false));
}
for (int i = 0; i < worldPoses.Count; i++)
{
if (i > 0)
{
lLength += (leftBoundPoses[i] - leftBoundPoses[i - 1]).magnitude;
rLength += (rightBoundPoses[i] - rightBoundPoses[i - 1]).magnitude;
}
}
var predecessor_ids = new List <Id>();
var successor_ids = new List <Id>();
foreach (var ls in lnSeg.befores)
{
predecessor_ids.Add(new Id(ls.hdmapInfo.id));
}
foreach (var ls in lnSeg.afters)
{
successor_ids.Add(new Id(ls.hdmapInfo.id));
}
lanes.Add(new Lane()
{
id = new Id(lnSeg.hdmapInfo.id),
central_curve = new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = centerPts
}
},
s = 0,
start_position = centerPts[0],
length = mLength
}
}
},
left_boundary = new LaneBoundary()
{
curve = new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = lBndPts
}
},
s = 0,
start_position = lBndPts[0],
length = lLength
},
},
},
length = lLength,
@virtual = true,
boundary_type = new List <LaneBoundaryType>()
{
new LaneBoundaryType()
{
s = 0,
types = new List <LaneBoundaryType.Type>()
{
LaneBoundaryType.Type.DOTTED_WHITE,
}
}
}
},
right_boundary = new LaneBoundary()
{
curve = new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = rBndPts
}
},
s = 0,
start_position = rBndPts[0],
length = rLength
},
},
},
length = rLength,
@virtual = true,
boundary_type = new List <LaneBoundaryType>()
{
new LaneBoundaryType()
{
s = 0,
types = new List <LaneBoundaryType.Type>()
{
LaneBoundaryType.Type.DOTTED_WHITE,
}
}
}
},
length = mLength,
speed_limit = 5,
predecessor_id = predecessor_ids.Count > 0 ? predecessor_ids : null,
successor_id = successor_ids.Count > 0 ? successor_ids : null,
type = Lane.LaneType.CITY_DRIVING,
turn = lnSeg.hdmapInfo.laneTurn,
direction = Lane.LaneDirection.FORWARD,
left_sample = associations,
right_sample = associations,
left_neighbor_forward_lane_id = lnSeg.hdmapInfo.leftNeighborSegmentForward == null ? null : new List <Id>()
{
lnSeg.hdmapInfo.leftNeighborSegmentForward.hdmapInfo.id
},
right_neighbor_forward_lane_id = lnSeg.hdmapInfo.rightNeighborSegmentForward == null ? null : new List <Id>()
{
lnSeg.hdmapInfo.rightNeighborSegmentForward.hdmapInfo.id
},
left_neighbor_reverse_lane_id = lnSeg.hdmapInfo.leftNeighborSegmentReverse == null ? null : new List <Id>()
{
lnSeg.hdmapInfo.leftNeighborSegmentReverse.hdmapInfo.id
},
right_neighbor_reverse_lane_id = lnSeg.hdmapInfo.rightNeighborSegmentReverse == null ? null : new List <Id>()
{
lnSeg.hdmapInfo.rightNeighborSegmentReverse.hdmapInfo.id
}
});
}
//for backtracking what overlaps are related to a specific lane
var laneIds2OverlapIdsMapping = new Dictionary <Id, List <Id> >();
//setup signals and lane_signal overlaps
foreach (var signalLight in signalLights)
{
//signal id
int signal_Id = signals.Count;
//construct boundry points
var bounds = signalLight.Get2DBounds();
List <Ros.PointENU> signalBoundPts = new List <Ros.PointENU>()
{
GetApolloCoordinates(bounds.Item1, OriginEasting, OriginNorthing),
GetApolloCoordinates(bounds.Item2, OriginEasting, OriginNorthing),
GetApolloCoordinates(bounds.Item3, OriginEasting, OriginNorthing),
GetApolloCoordinates(bounds.Item4, OriginEasting, OriginNorthing)
};
//sub signals
List <Subsignal> subsignals = null;
if (signalLight.signalDatas.Count > 0)
{
subsignals = new List <Subsignal>();
for (int i = 0; i < signalLight.signalDatas.Count; i++)
{
var lightData = signalLight.signalDatas[i];
subsignals.Add(new Subsignal()
{
id = i,
type = Subsignal.Type.CIRCLE,
location = GetApolloCoordinates(signalLight.transform.TransformPoint(lightData.localPosition), OriginEasting, OriginNorthing),
});
}
}
//keep track of all overlaps this signal created
List <Id> overlap_ids = new List <Id>();
//stopline points
List <Ros.PointENU> stoplinePts = null;
var stopline = signalLight.hintStopline;
if (stopline != null && stopline.segment.targetLocalPositions.Count > 1)
{
stoplinePts = new List <Ros.PointENU>();
List <MapSegment> lanesToInspec = new List <MapSegment>();
lanesToInspec.AddRange(allLnSegs);
lanesToInspec.AddRange(bridgeVirtualLnSegs);
if (!MakeStoplineLaneOverlaps(stopline, lanesToInspec, stoplineWidth, signal_Id, OverlapType.Signal_Stopline_Lane, ref stoplinePts, ref laneIds2OverlapIdsMapping, ref overlap_ids, ref overlaps))
{
return(false);
}
}
if (stoplinePts != null && stoplinePts.Count > 2)
{
signals.Add(new Signal()
{
id = $"signal_{signal_Id}",
boundary = new Polygon()
{
point = signalBoundPts,
},
subsignal = subsignals,
overlap_id = overlap_ids.Count > 1 ? overlap_ids : null, //backtrack and fill reverse link
type = Signal.Type.MIX_3_VERTICAL,
stop_line = new List <Curve>()
{
new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = stoplinePts,
}
}
}
}
}
}
});
}
}
//setup stopsigns and lane_stopsign overlaps
foreach (var stopSign in stopSigns)
{
//stopsign id
int stopsign_Id = stop_signs.Count;
//keep track of all overlaps this stopsign created
List <Id> overlap_ids = new List <Id>();
//stopline points
List <Ros.PointENU> stoplinePts = null;
var stopline = stopSign.stopline;
if (stopline != null && stopline.segment.targetLocalPositions.Count > 1)
{
stoplinePts = new List <Ros.PointENU>();
List <MapSegment> lanesToInspec = new List <MapSegment>();
lanesToInspec.AddRange(allLnSegs);
lanesToInspec.AddRange(bridgeVirtualLnSegs);
if (!MakeStoplineLaneOverlaps(stopline, lanesToInspec, stoplineWidth, stopsign_Id, OverlapType.Stopsign_Stopline_Lane, ref stoplinePts, ref laneIds2OverlapIdsMapping, ref overlap_ids, ref overlaps))
{
return(false);
}
}
if (stoplinePts != null && stoplinePts.Count > 2)
{
stop_signs.Add(new StopSign()
{
id = $"stopsign_{stopsign_Id}",
overlap_id = overlap_ids.Count > 1 ? overlap_ids : null, //backtrack and fill reverse link;
stop_line = new List <Curve>()
{
new Curve()
{
segment = new List <CurveSegment>()
{
new CurveSegment()
{
curve_type = new CurveSegment.CurveType_OneOf()
{
line_segment = new LineSegment()
{
point = stoplinePts,
}
}
}
}
}
}
});
}
}
//backtrack and fill missing information for lanes
for (int i = 0; i < lanes.Count; i++)
{
Id land_id = (Id)(lanes[i].id);
var oldLane = lanes[i];
oldLane.overlap_id = laneIds2OverlapIdsMapping.ContainsKey(land_id) ? laneIds2OverlapIdsMapping[(Id)(lanes[i].id)] : null;
lanes[i] = oldLane;
}
//integration
hdmap = new HDMap()
{
header = new Header()
{
version = "1.500000",
date = "2018-03-23T13:27:54",
projection = new Projection()
{
proj = "+proj=utm +zone=10 +ellps=WGS84 +datum=WGS84 +units=m +no_defs",
},
district = "0",
rev_major = "1",
rev_minor = "0",
left = -121.982277,
top = 37.398079,
right = -121.971998,
bottom = 37.398079,
vendor = "LGSVL",
},
lane = lanes.Count == 0 ? null : lanes,
signal = signals.Count == 0 ? null : signals,
overlap = overlaps.Count == 0 ? null : overlaps,
//stop_sign = stop_signs,
};
return(true);
}
