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);
}
}
