/// <summary>
/// Update sensor states
/// </summary>
/// <param name="vehicleState"></param>
/// <param name="vehicles"></param>
/// <param name="obstacles"></param>
public void Update(VehicleState vehicleState, SceneEstimatorTrackedClusterCollection vehicles, SceneEstimatorUntrackedClusterCollection obstacles, double speed, LocalRoadEstimate lre, PathRoadModel prm)
{
this.vehicleStateChannel.PublishUnreliably(vehicleState);
this.observedVehicleChannel.PublishUnreliably(vehicles);
this.observedObstacleChannel.PublishUnreliably(obstacles);
this.vehicleSpeedChannel.PublishUnreliably(speed);
this.localRoadChannel.PublishUnreliably(lre);
if (prm != null)
{
this.localRoadChannel.PublishUnreliably(prm);
}
}
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);
}
}
public LocalRoadEstimate GetLocalRoadEstimate(SimVehicleState state)
{
LocalRoadEstimate lre = new LocalRoadEstimate();
/*ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position);
*
* LinePath path;
*
* if (IsPartitionSameDirection(closestPartition, state.Heading)) {
* path = BuildPath(closestPartition, true, 25, 25);
* }
* else {
* path = BuildPath(closestPartition, false, 25, 25);
* }
*
* Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan)*Matrix3.Translation(-state.Position.X, -state.Position.Y);
* path.TransformInPlace(absTransform);
*
* // get the closest point
* LinePath.PointOnPath p0 = path.ZeroPoint;
*
* List<Coordinates> points = new List<Coordinates>(5);
* List<double> dists = new List<double>();
* dists.Add(0);
* points.Add(path.GetPoint(p0));
* // iterate through and add points every five meteres
* LinePath.PointOnPath pt = p0;
* double sumDist = 0;
* for (int i = 0; i < 3; i++) {
* double dist = 5;
* pt = path.AdvancePoint(pt, ref dist);
*
* if (dist != 5) {
* sumDist += (5-dist);
*
* dists.Add(sumDist);
* points.Add(path.GetPoint(pt));
* }
*
* if (dist > 0)
* break;
* }
*
* pt = p0;
* sumDist = 0;
* for (int i = 0; i < 2; i++) {
* double dist = -5;
* pt = path.AdvancePoint(pt, ref dist);
*
* if (dist != -5) {
* sumDist += (5+dist);
*
* dists.Insert(0, sumDist);
* points.Insert(0, path.GetPoint(pt));
* }
*
* if (dist < 0)
* break;
* }
*
* // do a least squares fit over the points
* Matrix X = new Matrix(points.Count, 3);
* Matrix W = new Matrix(points.Count, points.Count);
* Matrix Y = new Matrix(points.Count, 1);
* for (int i = 0; i < points.Count; i++) {
* X[i, 0] = 1;
* X[i, 1] = points[i].X;
* X[i, 2] = points[i].X*points[i].X;
* W[i, i] = 1/(dists[i] + 1);
* Y[i, 0] = points[i].Y;
* }
*
* Matrix beta = null;
* try {
* Matrix X_T = X.Transpose();
* Matrix inv = (X_T*W*X).Inverse;
* beta = inv*X_T*W*Y;
* }
* catch (Exception) {
* }
*
* if (beta != null) {
* lre.isModelValid = true;
* lre.roadCurvature = beta[2, 0];
* lre.roadCurvatureVar = double.MaxValue;
* lre.roadHeading = beta[1, 0];
* lre.roadHeadingVar = 0;
*
* lre.centerLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate();
* lre.centerLaneEstimate.center = beta[0, 0];
* lre.centerLaneEstimate.centerVar = 0;
* lre.centerLaneEstimate.exists = true;
* lre.centerLaneEstimate.id = closestPartition.PartitionId.ToString();
* lre.centerLaneEstimate.width = closestPartition.Lane.Width;
* lre.centerLaneEstimate.widthVar = 0;
*
* lre.leftLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate();
* lre.leftLaneEstimate.exists = false;
* if (closestPartition.Lane.LaneOnLeft != null) {
* ArbiterLanePartition leftParition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position);
* if (closestPartition.NonLaneAdjacentPartitions.Contains(leftParition)) {
* double dist;
*
* if (IsPartitionSameDirection(leftParition, state.Heading)) {
* dist = (state.Position-leftParition.Initial.Position).Cross(leftParition.Vector().Normalize());
* }
* else {
* dist = (state.Position-leftParition.Final.Position).Cross(leftParition.Vector().Rotate180().Normalize());
* }
*
* lre.leftLaneEstimate.center = dist;
* lre.leftLaneEstimate.centerVar = 0;
* lre.leftLaneEstimate.exists = true;
* lre.leftLaneEstimate.id = leftParition.PartitionId.ToString();
* lre.leftLaneEstimate.width = leftParition.Lane.Width;
* lre.leftLaneEstimate.widthVar = 0;
* }
* }
*
* if (closestPartition.Lane.LaneOnRight != null) {
* ArbiterLanePartition rightPartition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position);
* if (closestPartition.NonLaneAdjacentPartitions.Contains(rightPartition)) {
* double dist;
* if (IsPartitionSameDirection(rightPartition, state.Heading)) {
* dist = (state.Position-rightPartition.Initial.Position).Cross(rightPartition.Vector().Normalize());
* }
* else {
* dist = (state.Position-rightPartition.Final.Position).Cross(rightPartition.Vector().Rotate180().Normalize());
* }
*
* lre.rightLaneEstimate.center = dist;
* lre.rightLaneEstimate.centerVar = 0;
* lre.rightLaneEstimate.exists = true;
* lre.rightLaneEstimate.id = rightPartition.PartitionId.ToString();
* lre.rightLaneEstimate.width = rightPartition.Lane.Width;
* lre.rightLaneEstimate.widthVar = 0;
* }
* }
* }
* else {
* lre.isModelValid = false;
* }*/
lre.centerLaneEstimate.exists = false;
lre.leftLaneEstimate.exists = false;
lre.rightLaneEstimate.exists = false;
lre.stopLineEstimate = new StopLineEstimate();
double?distToStopline = DistanceToNextStop(state);
lre.stopLineEstimate.stopLineExists = distToStopline.HasValue;
lre.stopLineEstimate.distToStopline = distToStopline.GetValueOrDefault(double.MaxValue);
lre.stopLineEstimate.distToStoplineVar = 0;
lre.timestamp = (double)SimulatorClient.GetCurrentTimestamp;
return(lre);
}
