private void InitializeParking()
{
// get the corners from sergei's algorithm
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
Coordinates[] rectPoints = null;
Utilities.ObstacleUtilities.FindParkingInterval(GetObstacles(Services.RelativePose.CurrentTimestamp), parkingSpotLine, zonePerimeter, pose.xy, pose.heading, ref rectPoints);
Polygon parkingRect = new Polygon(rectPoints);
Services.UIService.PushPolygon(parkingRect, curTimestamp, "uturn polygon", false);
// initialize parker
ParkingSpaceParams spaceParams = new ParkingSpaceParams();
// bottom left
spaceParams.FrontWallPoint = rectPoints[0];
spaceParams.BackWallPoint = rectPoints[2];
spaceParams.ParkPoint = parkingSpotLine.P1 + parkingSpotLine.UnitVector.Normalize(parkingSpotExtraDist);
spaceParams.ParkVector = parkingSpotLine.UnitVector;
spaceParams.PulloutPoint = pulloutPoint;
parker = new ParkerWrapper(spaceParams);
Services.UIService.PushPoint(spaceParams.FrontWallPoint, curTimestamp, "front left point", false);
Services.UIService.PushPoint(spaceParams.BackWallPoint, curTimestamp, "rear right point", false);
Services.UIService.PushPoint(spaceParams.ParkPoint, curTimestamp, "uturn stop point", false);
LineList backLine = new LineList();
backLine.Add(spaceParams.BackWallPoint + parkingSpotLine.UnitVector.Rotate90() * 30);
backLine.Add(spaceParams.BackWallPoint - parkingSpotLine.UnitVector.Rotate90() * 30);
Services.UIService.PushLineList(backLine, curTimestamp, "original path2", false);
stopTimestamp = CarTimestamp.Invalid;
passCompleted = true;
finalPass = false;
}
private void InitializeParking()
{
// get the corners from sergei's algorithm
AbsolutePose pose = Services.StateProvider.GetAbsolutePose();
Coordinates[] rectPoints = null;
Utilities.ObstacleUtilities.FindParkingInterval(GetObstacles(Services.RelativePose.CurrentTimestamp), parkingSpotLine, zonePerimeter, pose.xy, pose.heading, ref rectPoints);
Polygon parkingRect = new Polygon(rectPoints);
Services.UIService.PushPolygon(parkingRect, curTimestamp, "uturn polygon", false);
// initialize parker
ParkingSpaceParams spaceParams = new ParkingSpaceParams();
// bottom left
spaceParams.FrontWallPoint = rectPoints[0];
spaceParams.BackWallPoint = rectPoints[2];
spaceParams.ParkPoint = parkingSpotLine.P1 + parkingSpotLine.UnitVector.Normalize(parkingSpotExtraDist);
spaceParams.ParkVector = parkingSpotLine.UnitVector;
spaceParams.PulloutPoint = pulloutPoint;
parker = new ParkerWrapper(spaceParams);
Services.UIService.PushPoint(spaceParams.FrontWallPoint, curTimestamp, "front left point", false);
Services.UIService.PushPoint(spaceParams.BackWallPoint, curTimestamp, "rear right point", false);
Services.UIService.PushPoint(spaceParams.ParkPoint, curTimestamp, "uturn stop point", false);
LineList backLine = new LineList();
backLine.Add(spaceParams.BackWallPoint + parkingSpotLine.UnitVector.Rotate90() * 30);
backLine.Add(spaceParams.BackWallPoint - parkingSpotLine.UnitVector.Rotate90() * 30);
Services.UIService.PushLineList(backLine, curTimestamp, "original path2", false);
stopTimestamp = CarTimestamp.Invalid;
passCompleted = true;
finalPass = false;
}