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