public SmoothingResult PlanPath(LineList basePath, LineList targetPath, IList<LineList> leftBounds, IList<LineList> rightBounds, double initHeading, double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed)
{
// create the boundary list
List<Boundary> upperBound = new List<Boundary>();
foreach (LineList leftBound in leftBounds) {
Boundary ub0 = new Boundary();
ub0.Coords = leftBound;
ub0.DesiredSpacing = 0.5;
ub0.MinSpacing = 0.1;
ub0.Polygon = false;
upperBound.Add(ub0);
}
List<Boundary> lowerBound = new List<Boundary>();
foreach (LineList rightBound in rightBounds) {
Boundary lb0 = new Boundary();
lb0.Coords = rightBound;
lb0.DesiredSpacing = 0.5;
lb0.MinSpacing = 0.1;
lb0.Polygon = false;
lowerBound.Add(lb0);
}
return PlanPath(basePath, targetPath, upperBound, lowerBound, initHeading, maxSpeed, startSpeed, endingHeading, time, endingOffsetFixed);
}
public PathFollowingBehavior(Path basePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand)
{
this.basePath = basePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
}
public static Line[] SplitLine(LineList lines,int index)
{
Line line = lines[index];
var nextline = new Line();
if (lines.Count - 1 < index + 1)
{
nextline.StartTime.FromMilliSeconds(line.EndTime.ToMilliSeconds()+ 2000);
}
if (line == null || nextline == null) return null;
double duration = line.EndTime.ToMilliSeconds() - line.StartTime.ToMilliSeconds();
duration = duration / 2;
var line1 = new Line();
LineList.CopyTwoLineTimes(line.EndTime,line1.StartTime);
line.Duration = (duration/1000);
var line2 = new Line();
LineList.CopyTwoLineTimes(line1.EndTime, line2.StartTime);
line2.Duration = duration / 1000;
var linetext = StringTools.SplitText(line.Context);
line1.Context = linetext[0];
line1.ConstantContext = linetext[0];
line2.Context = linetext[1];
line2.ConstantContext = linetext[1];
return new Line[] { line1, line2 };
}
public PathSearchBehavior(LineList leftBound, LineList rightBound, Coordinates endOrientation, double endHeading, double endSpeed, double maxSpeed, PathSearchType searchType)
{
this.leftBound = leftBound;
this.rightBound = rightBound;
this.endOrientation = endOrientation;
this.endHeading = endHeading;
this.endSpeed = endSpeed;
this.maxSpeed = maxSpeed;
this.searchType = searchType;
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.ignorableObstacles = new List<int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List<int>();
this.decorators = new List<BehaviorDecorator>();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="interconnect"></param>
/// <param name="turn"></param>
public TurnState(ArbiterInterconnect interconnect, ArbiterTurnDirection turn, ArbiterLane target, LinePath endingPath, LineList left,
LineList right, SpeedCommand speed, SAUDILevel saudi, bool useTurnBounds)
{
this.Interconnect = interconnect;
this.turnDirection = turn;
this.TargetLane = target;
this.EndingPath = endingPath;
this.LeftBound = left;
this.RightBound = right;
this.SpeedCommand = speed;
this.Saudi = saudi;
this.UseTurnBounds = useTurnBounds;
}
public TurnBehavior(ArbiterLaneId targetLane, LinePath targetLanePath, LineList leftBound, LineList rightBound, SpeedCommand speedCommand, Polygon intersectionPolygon, IEnumerable<int> ignorableObstacles, ArbiterInterconnectId interconnectId)
{
this.targetLane = targetLane;
this.targetLanePath = targetLanePath;
this.leftBound = leftBound;
this.rightBound = rightBound;
this.speedCommand = speedCommand;
this.intersectionPolygon = intersectionPolygon;
this.ignorableObstacles = ignorableObstacles != null ? new List<int>(ignorableObstacles) : new List<int>();
this.interconnectId = interconnectId;
this.VehiclesToIgnore = new List<int>();
this.decorators = new List<BehaviorDecorator>();
}
public SmoothingResult PlanPath(LineList basePath, LineList targetPath,
List<Boundary> leftBounds, List<Boundary> rightBounds, double initHeading,
double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed)
{
PlanningSettings settings = new PlanningSettings();
settings.basePath = basePath;
settings.targetPaths = new List<Boundary>();
settings.targetPaths.Add(new Boundary(targetPath, 0, 0, settings.Options.alpha_w));
settings.leftBounds = leftBounds;
settings.rightBounds = rightBounds;
settings.initHeading = initHeading;
settings.maxSpeed = maxSpeed;
settings.startSpeed = startSpeed;
settings.endingHeading = endingHeading;
settings.timestamp = time;
settings.endingPositionFixed = endingOffsetFixed;
return PlanPath(settings);
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void TurnInfo(ArbiterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
if (entry.NextPartition != null)
{
double distance = entry.NextPartition.Length;
// get left bound
rightBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).RotateM90());
// get right bound
leftBound = GeneralToolkit.TranslateVector(entry.Position, entry.NextPartition.Final.Position,
entry.NextPartition.Vector().Normalize(entry.Lane.Width / 2.0).Rotate90());
ArbiterWaypoint current = entry.NextPartition.Final;
while (current.NextPartition != null && distance < 50)
{
distance += current.NextPartition.Length;
LineList rtTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).RotateM90());
rightBound.Add(rtTemp[rtTemp.Count - 1]);
LineList ltTemp = GeneralToolkit.TranslateVector(current.Position, current.NextPartition.Final.Position,
current.NextPartition.Vector().Normalize(current.Lane.Width / 2.0).Rotate90());
leftBound.Add(ltTemp[ltTemp.Count - 1]);
current = current.NextPartition.Final;
}
finalPath = entry.Lane.LanePath(entry, 50.0);
}
else
{
Coordinates final = entry.Position + entry.PreviousPartition.Vector().Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, final });
LinePath lB = finalPath.ShiftLateral(entry.Lane.Width / 2.0);
LinePath rB = finalPath.ShiftLateral(-entry.Lane.Width / 2.0);
leftBound = new LineList(lB);
rightBound = new LineList(rB);
}
}
public static void OnRoadBearing(CarTimestamp timestamp, double angle, double confidence)
{
if (currentConfidence == 0) {
currentAngle = angle;
currentConfidence = confidence;
}
else if (confidence == 0) {
currentConfidence *= 0.5;
}
else {
currentAngle = alpha*angle + (1-alpha)*currentAngle;
currentConfidence = confidence;
}
LineList roadBearing = new LineList(2);
roadBearing.Add(new Coordinates(2, 0).Rotate(currentAngle));
roadBearing.Add(new Coordinates(10, 0).Rotate(currentAngle));
Services.UIService.PushLineList(roadBearing, timestamp, "road bearing", true);
Services.Dataset.ItemAs<double>("road bearing confidence").Add(confidence, timestamp);
}
public void PushLineList(LineList list, CarTimestamp timestamp, string name, bool relative)
{
try {
if (relative) {
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
timestamp = absTransform.Timestamp;
list = list.Transform(absTransform);
}
Services.Dataset.ItemAs<LineList>(name).Add(list, timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send line data to ui: {0}", ex.Message);
}
}
private void ItalicTextCommand_Executed(object sender, ExecutedRoutedEventArgs e)
{
LineList lines = new LineList();
}
/// <summary>
/// Translate a vector
/// </summary>
/// <param name="vector"></param>
/// <param name="direction"></param>
/// <returns></returns>
public static LineList TranslateVector(Coordinates initial, Coordinates final, Coordinates direction)
{
LineList ll = new LineList();
ll.Add(initial + direction);
ll.Add(final + direction);
return ll;
}
public void DrawLine(Vector3 p0, Vector3 p1, Color color)
{
LineList.Add(new VertexPositionColor(p0, color));
LineList.Add(new VertexPositionColor(p1, color));
}
public SmoothingResult PlanPath(LineList basePath, LineList leftBound, LineList rightBound, double initHeading, double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time, bool endingOffsetFixed)
{
return PlanPath(basePath, null, new LineList[] { leftBound }, new LineList[] { rightBound }, initHeading, maxSpeed, startSpeed, endingHeading, time, endingOffsetFixed);
}
public void MessageArrived(string channelName, object message)
{
// check the method
if (message is LocalRoadEstimate) {
LocalRoadEstimate lre = (LocalRoadEstimate)message;
CarTimestamp ct = lre.timestamp;
// get the stop-line estimate
Services.Dataset.ItemAs<bool>("stopline found").Add(lre.stopLineEstimate.stopLineExists, ct);
Services.Dataset.ItemAs<double>("stopline distance").Add(lre.stopLineEstimate.distToStopline, ct);
Services.Dataset.ItemAs<double>("stopline variance").Add(lre.stopLineEstimate.distToStoplineVar, ct);
}
else if (message is PathRoadModel && Settings.UsePathRoadModel) {
PathRoadModel pathRoadModel = (PathRoadModel)message;
List<ILaneModel> models = new List<ILaneModel>();
foreach (PathRoadModel.LaneEstimate le in pathRoadModel.laneEstimates) {
models.Add(new PathLaneModel(pathRoadModel.timestamp, le));
}
Services.LaneModels.SetLaneModel(models);
}
else if (message is VehicleState) {
VehicleState vs = (VehicleState)message;
AbsolutePose pose = new AbsolutePose(vs.Position, vs.Heading.ArcTan, vs.Timestamp);
Services.AbsolutePosteriorPose.PushAbsolutePose(pose);
Services.Dataset.ItemAs<Coordinates>("posterior pose").Add(vs.Position, vs.Timestamp);
double heading = vs.Heading.ArcTan;
if (Services.PoseListener != null && Services.PoseListener.BiasEstimator != null) {
heading = Services.PoseListener.BiasEstimator.CorrectHeading(heading);
}
Services.Dataset.ItemAs<double>("posterior heading").Add(heading, vs.Timestamp);
TimeoutMonitor.MarkData(OperationalDataSource.PosteriorPose);
}
else if (message is SceneEstimatorUntrackedClusterCollection) {
// push to the obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived((SceneEstimatorUntrackedClusterCollection)message);
TimeoutMonitor.MarkData(OperationalDataSource.UntrackedClusters);
}
else if (message is SceneEstimatorTrackedClusterCollection) {
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
TimeoutMonitor.MarkData(OperationalDataSource.TrackedClusters);
}
//else if (message is LocalRoadModel) {
// LocalRoadModel roadModel = (LocalRoadModel)message;
// UrbanChallenge.Operational.Common.LocalLaneModel centerLaneModel =
// new UrbanChallenge.Operational.Common.LocalLaneModel(
// new LinePath(roadModel.LanePointsCenter), ArrayConvert(roadModel.LanePointsCenterVariance), roadModel.laneWidthCenter,
// roadModel.laneWidthCenterVariance, roadModel.probabilityCenterLaneExists);
// UrbanChallenge.Operational.Common.LocalLaneModel leftLaneModel =
// new UrbanChallenge.Operational.Common.LocalLaneModel(
// new LinePath(roadModel.LanePointsLeft), ArrayConvert(roadModel.LanePointsLeftVariance), roadModel.laneWidthLeft,
// roadModel.laneWidthLeftVariance, roadModel.probabilityLeftLaneExists);
// UrbanChallenge.Operational.Common.LocalLaneModel rightLaneModel =
// new UrbanChallenge.Operational.Common.LocalLaneModel(
// new LinePath(roadModel.LanePointsRight), ArrayConvert(roadModel.LanePointsRightVariance), roadModel.laneWidthRight,
// roadModel.laneWidthRightVariance, roadModel.probabilityRightLaneExists);
// UrbanChallenge.Operational.Common.LocalRoadModel localRoadModel = new UrbanChallenge.Operational.Common.LocalRoadModel(
// roadModel.timestamp, roadModel.probabilityRoadModelValid, centerLaneModel, leftLaneModel, rightLaneModel);
// //Services.RoadModelProvider.LocalRoadModel = localRoadModel;
// // clone the lane models so when we transform to absolute coordinates for the ui, the lane model we have
// // stored doesn't get modified
// centerLaneModel = centerLaneModel.Clone();
// leftLaneModel = leftLaneModel.Clone();
// rightLaneModel = rightLaneModel.Clone();
// AbsoluteTransformer trans = Services.StateProvider.GetAbsoluteTransformer(roadModel.timestamp).Invert();
// centerLaneModel.LanePath.TransformInPlace(trans);
// leftLaneModel.LanePath.TransformInPlace(trans);
// rightLaneModel.LanePath.TransformInPlace(trans);
// // send to ui
// Services.Dataset.ItemAs<UrbanChallenge.Operational.Common.LocalLaneModel>("center lane").Add(centerLaneModel, roadModel.timestamp);
// Services.Dataset.ItemAs<UrbanChallenge.Operational.Common.LocalLaneModel>("left lane").Add(leftLaneModel, roadModel.timestamp);
// Services.Dataset.ItemAs<UrbanChallenge.Operational.Common.LocalLaneModel>("right lane").Add(rightLaneModel, roadModel.timestamp);
// Services.Dataset.ItemAs<double>("lane model probability").Add(roadModel.probabilityRoadModelValid, roadModel.timestamp);
//}
else if (message is SideObstacles) {
Services.ObstaclePipeline.OnSideObstaclesReceived((SideObstacles)message);
}
else if (message is SideRoadEdge) {
SideRoadEdge edge = (SideRoadEdge)message;
RoadEdge.OnRoadEdge(edge);
LineList line = RoadEdge.GetEdgeLine(edge);
if (line == null)
line = new LineList();
string name = (edge.side == SideRoadEdgeSide.Driver) ? "left road edge" : "right road edge";
Services.UIService.PushLineList(line, edge.timestamp, name, true);
}
else if (message is SparseRoadBearing) {
SparseRoadBearing road = (SparseRoadBearing)message;
RoadBearing.OnRoadBearing(road.timestamp, road.Heading, road.Confidence);
}
}
private void SmoothIt()
{
Stopwatch s = Stopwatch.StartNew();
SmootherOptions opt = PathSmoother.GetDefaultOptions();
if (optIPOPT.Checked) {
opt.alg = SmoothingAlgorithm.Ipopt;
}
else {
opt.alg = SmoothingAlgorithm.Loqo;
}
opt.alpha_w = getW();
opt.alpha_d = getD1();
opt.set_init_velocity = true;
opt.init_velocity = getV();
opt.set_init_heading = true;
opt.init_heading = Math.Atan2(pathPoints[1].Y-pathPoints[0].Y, pathPoints[1].X-pathPoints[0].X);
opt.set_final_heading = true;
opt.final_heading = Math.Atan2(pathPoints[pathPoints.Count-1].Y-pathPoints[pathPoints.Count-2].Y, pathPoints[pathPoints.Count-1].X-pathPoints[pathPoints.Count-2].X);
opt.set_final_offset = true;
List<PathPoint> path = new List<PathPoint>();
Boundary ub = new Boundary();
ub.Coords = ubPoints;
ub.DesiredSpacing = 1;
ub.MinSpacing = 0.25;
ub.Polygon = false;
List<Boundary> ub_bounds = new List<Boundary>();
ub_bounds.Add(ub);
Boundary lb = new Boundary();
lb.Coords = lbPoints;
lb.DesiredSpacing = 0.5;
lb.MinSpacing = 0;
lb.Polygon = false;
List<Boundary> lb_bounds = new List<Boundary>();
lb_bounds.Add(lb);
LineList basePath = new LineList();
basePath.AddRange(pathPoints);
SmoothResult sr = SmoothResult.Error;
try {
sr = PathSmoother.SmoothPath(basePath, ub_bounds, lb_bounds, opt, path);
}
catch (Exception ex) {
MessageBox.Show("exception: " + ex.Message);
return;
}
smoothedPoints = path.ConvertAll<Coordinates>(delegate(PathPoint p) { return new Coordinates(p.x, p.y); });
s.Stop();
long ms = s.ElapsedMilliseconds;
if (sr != SmoothResult.Sucess) {
MessageBox.Show("Path smooth result: " + sr.ToString());
}
MessageBox.Show("Elapsed MS: " + ms);
picEntry.Invalidate();
}
public async Task GET_ALL_EDIT_SAVE()
{
var allLinesCount = -1;
bool linesCountStaysTheSame = false;
var text0 = "This is edited Text00000._save";
var text1 = "This is edited Text11111._save";
var containsText0 = false;
var containsText1 = false;
LineList allLines = null;
LineList savedLines = null;
var isAsyncComplete = false;
var hasError = false;
EnqueueConditional(() => isAsyncComplete);
await Setup();
try
{
allLines = await LineList.GetAllAsync();
allLines[0].Phrase.Text = text0;
allLines[1].Phrase.Text = text1;
allLinesCount = allLines.Count;
savedLines = await allLines.SaveAsync();
linesCountStaysTheSame = allLinesCount == savedLines.Count;
containsText0 = (from line in savedLines
where line.Phrase.Text == text0
select line).Count() > 0;
containsText1 = (from line in savedLines
where line.Phrase.Text == text1
select line).Count() > 0;
}
catch
{
hasError = true;
}
finally
{
EnqueueCallback(
() => Assert.IsFalse(hasError),
() => Assert.IsNotNull(allLines),
() => Assert.IsNotNull(savedLines),
() => Assert.IsTrue(containsText0),
() => Assert.IsTrue(containsText1),
() => Assert.IsTrue(linesCountStaysTheSame),
() => Assert.IsTrue(allLinesCount > 0),
//KEEP THIS LAST IN THE CALLBACKS
() => Teardown()
);
EnqueueTestComplete();
isAsyncComplete = true;
}
}
public async Task NEW_EDIT_BEGINSAVE_GET()
{
MultiLineTextEdit newMultiLineTextEdit = null;
MultiLineTextEdit savedMultiLineTextEdit = null;
MultiLineTextEdit gottenMultiLineTextEdit = null;
string mltTitle = "My Test MLT Title Here";
string lineAText = "MultiLineTextTests.neweditbeginsaveget.Test Line A Text. This is line A in English.";
string lineBText = "MultiLineTextTests.neweditbeginsaveget.Test Line BBBB Text. This is line B in English.";
//LineEdit lineA = null;
//LineEdit lineB = null;
bool gottenMultiLineTextLinesCountIsTwo = false;
bool gottenMultiLineTextContainsLineA = false;
bool gottenMultiLineTextContainsLineB = false;
var isAsyncComplete = false;
var hasError = false;
EnqueueConditional(() => isAsyncComplete);
await Setup();
try
{
//NEW UP THE MULTILINETEXT
newMultiLineTextEdit = await MultiLineTextEdit.NewMultiLineTextEditAsync();
//EDIT
//TITLE MLT
newMultiLineTextEdit.Title = mltTitle;
//CREATE LINES IN A LINELIST
//1) CREATE LINE INFO DICTIONARY
var lineInfoDictionary = new Dictionary <int, string>();
lineInfoDictionary.Add(0, lineAText);
lineInfoDictionary.Add(1, lineBText);
//2) LANGUAGE TEXT
var linesLanguageText = SeedData.Ton.EnglishText;
//3) CRITERIA
var criteria = new Business.Criteria.LineInfosCriteria(linesLanguageText, lineInfoDictionary);
//4) CREATE LINES
var lineList = await LineList.NewLineListAsync(criteria);
//5) ASSIGN LINES
newMultiLineTextEdit.Lines = lineList;
savedMultiLineTextEdit = await newMultiLineTextEdit.SaveAsync();
//GET (CONFIRM SAVE)
gottenMultiLineTextEdit =
await MultiLineTextEdit.GetMultiLineTextEditAsync(savedMultiLineTextEdit.Id);
gottenMultiLineTextLinesCountIsTwo = (gottenMultiLineTextEdit.Lines.Count == 2);
gottenMultiLineTextContainsLineA = (from line in gottenMultiLineTextEdit.Lines
where line.LineNumber == 0
select line).First().Phrase.Text == lineAText;
gottenMultiLineTextContainsLineB = (from line in gottenMultiLineTextEdit.Lines
where line.LineNumber == 1
select line).First().Phrase.Text == lineBText;
}
catch
{
hasError = true;
}
finally
{
EnqueueCallback(
() => Assert.IsFalse(hasError),
() => Assert.IsTrue(gottenMultiLineTextLinesCountIsTwo),
() => Assert.IsTrue(gottenMultiLineTextContainsLineA),
() => Assert.IsTrue(gottenMultiLineTextContainsLineB),
() => Assert.IsNotNull(newMultiLineTextEdit),
() => Assert.IsNotNull(savedMultiLineTextEdit),
() => Assert.IsNotNull(gottenMultiLineTextEdit),
() => Assert.AreEqual(savedMultiLineTextEdit.Id, gottenMultiLineTextEdit.Id)
);
EnqueueTestComplete();
Teardown();
isAsyncComplete = true;
}
}
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;
}
public void PushRelativePath(LineList list, CarTimestamp timestamp, string name)
{
try {
// convert to the current time
// get the absolute transform
AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(timestamp).Invert();
list = list.Transform(absTransform);
Services.Dataset.ItemAs<LineList>(name).Add(list, absTransform.Timestamp);
}
catch (Exception ex) {
OperationalLayer.Tracing.OperationalTrace.WriteWarning("could not send line data to ui: {0}", ex.Message);
}
}
public SmoothingResult PlanPath(LineList basePath, LineList leftBound, LineList rightBound, double initHeading, double maxSpeed, double startSpeed, double? endingHeading, CarTimestamp time)
{
return PlanPath(basePath, leftBound, rightBound, initHeading, maxSpeed, startSpeed, endingHeading, time, false);
}
public void PushAbsolutePath(LineList list, CarTimestamp timestamp, string name)
{
Services.Dataset.ItemAs<LineList>(name).Add(list, timestamp);
}
/// <summary>
/// Turn information
/// </summary>
/// <param name="entry"></param>
/// <param name="finalPath"></param>
/// <param name="leftBound"></param>
/// <param name="rightBound"></param>
public static void ZoneTurnInfo(ArbiterInterconnect ai, ArbiterPerimeterWaypoint entry, out LinePath finalPath, out LineList leftBound, out LineList rightBound)
{
//Coordinates centerVec = entry.Perimeter.PerimeterPolygon.CalculateBoundingCircle().center - entry.Position;
Coordinates centerVec = ai.InterconnectPath[1] - ai.InterconnectPath[0];
centerVec = centerVec.Normalize(TahoeParams.VL);
finalPath = new LinePath(new Coordinates[] { entry.Position, entry.Position + centerVec });
leftBound = finalPath.ShiftLateral(TahoeParams.T * 2.0);
rightBound = finalPath.ShiftLateral(-TahoeParams.T * 2.0);
}
