/// <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 PathRoadModel GetPathRoadEstimate(SimVehicleState state)
{
PathRoadModel ret = new PathRoadModel(new List <PathRoadModel.LaneEstimate>(), (double)SimulatorClient.GetCurrentTimestamp);
ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position);
LinePath path;
if (IsPartitionSameDirection(closestPartition, state.Heading))
{
path = BuildPath(closestPartition, true, 10, 40);
}
else
{
path = BuildPath(closestPartition, false, 10, 40);
}
Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan) * Matrix3.Translation(-state.Position.X, -state.Position.Y);
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate center = new PathRoadModel.LaneEstimate(path, closestPartition.Lane.Width, closestPartition.PartitionId.ToString());
ret.laneEstimates.Add(center);
// get the lane to the left
if (closestPartition.Lane.LaneOnLeft != null)
{
ArbiterLanePartition partition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(partition))
{
if (IsPartitionSameDirection(partition, state.Heading))
{
path = BuildPath(partition, true, 10, 25);
}
else
{
path = BuildPath(partition, false, 10, 25);
}
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate left = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString());
ret.laneEstimates.Add(left);
}
}
if (closestPartition.Lane.LaneOnRight != null)
{
ArbiterLanePartition partition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(partition))
{
if (IsPartitionSameDirection(partition, state.Heading))
{
path = BuildPath(partition, true, 10, 25);
}
else
{
path = BuildPath(partition, false, 10, 25);
}
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate right = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString());
ret.laneEstimates.Add(right);
}
}
return(ret);
}