public PathLaneModel(CarTimestamp timestamp, PathRoadModel.LaneEstimate lane)
{
this.path = lane.path;
int dotIndex = lane.paritionID.LastIndexOf('.');
if (dotIndex == -1) {
this.laneID = lane.paritionID + ".not a lane";
}
else {
this.laneID = lane.paritionID.Substring(0, dotIndex);
}
this.width = lane.width;
if (this.width < TahoeParams.T + 2) {
this.width = TahoeParams.T + 2;
}
this.timestamp = timestamp;
}
/// <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 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;
}