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 static double FindBestCurvature(double prevCurvature, Coordinates goalPoint) { CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp; AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(); Coordinates relativeGoalPoint = absTransform.TransformPoint(goalPoint); // get a list of obstacles ObstacleCollection obstacles = Services.ObstaclePipeline.GetProcessedObstacles(curTimestamp, UrbanChallenge.Behaviors.SAUDILevel.None); List <Polygon> obstaclePolygons = new List <Polygon>(); foreach (Obstacle obs in obstacles.obstacles) { obstaclePolygons.Add(obs.cspacePolygon); } // get the side obstacles SideObstacle leftSideObstacle = Services.ObstaclePipeline.GetLeftSideObstacle(); SideObstacle rightSideObstacle = Services.ObstaclePipeline.GetRightSideObstacle(); double?leftDist = null, rightDist = null; if (leftSideObstacle != null) { leftDist = leftSideObstacle.distance; } if (rightSideObstacle != null) { rightDist = rightSideObstacle.distance; } double roll = Services.Dataset.ItemAs <double>("roll").CurrentValue; double roadBearing, roadConfidence; RoadBearing.GetCurrentData(out roadBearing, out roadConfidence); List <ArcResults> arcs = new List <ArcResults>(); double maxUtility = double.MinValue; ArcResults selectedArc = null; // recalculate weights double totalWeights = obstacle_weight + hysteresis_weight + straight_weight + goal_weight + side_obs_weight + roll_weight + road_weight; double obstacleWeight = obstacle_weight / totalWeights; double hysteresisWeight = hysteresis_weight / totalWeights; double straightWeight = straight_weight / totalWeights; double goalWeight = goal_weight / totalWeights; double sideObsWeight = side_obs_weight / totalWeights; double rollWeight = roll_weight / totalWeights; double roadWeight = road_weight / totalWeights; int start = num_arcs / 2; double curvatureStep = max_curvature / start; for (int i = -start; i <= start; i++) { double curvature = i * curvatureStep; double collisionDist, clearanceDist, collisionUtility; bool vetoed; EvaluateObstacleUtility(curvature, 20, obstaclePolygons, out collisionDist, out clearanceDist, out collisionUtility, out vetoed); double hystersisUtility = EvaluateHysteresisUtility(curvature, prevCurvature); double straightUtility = EvaluateStraightUtility(curvature); double goalUtility = EvaluateGoalUtility(curvature, relativeGoalPoint); double sideObstacleUtility = EvalualteSideObstacleUtility(curvature, leftDist, rightDist); double rollUtility = EvaluateRollUtility(curvature, roll); double roadUtility = EvaluateRoadBearingUtility(curvature, roadBearing, roadConfidence); double totalUtility = collisionUtility * obstacleWeight + hystersisUtility * hysteresisWeight + straightUtility * straightWeight + goalUtility * goalWeight + sideObstacleUtility * sideObsWeight + rollUtility * rollWeight + roadUtility * roadWeight; ArcResults result = new ArcResults(); result.curvature = curvature; result.vetoed = vetoed; result.totalUtility = totalUtility; result.obstacleHitDistance = collisionDist; result.obstacleClearanceDistance = clearanceDist; result.obstacleUtility = collisionUtility; result.hysteresisUtility = hystersisUtility; result.straightUtility = straightUtility; result.goalUtility = goalUtility; result.sideObstacleUtility = sideObstacleUtility; result.rollUtility = rollUtility; result.roadUtility = roadUtility; arcs.Add(result); if (!vetoed && totalUtility > maxUtility) { maxUtility = totalUtility; selectedArc = result; } } ArcVotingResults results = new ArcVotingResults(); results.arcResults = arcs; results.selectedArc = selectedArc; Services.Dataset.ItemAs <ArcVotingResults>("arc voting results").Add(results, LocalCarTimeProvider.LocalNow); if (selectedArc == null) { return(double.NaN); } else { return(selectedArc.curvature); } }