コード例 #1
0
 /// <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);
     }
 }
コード例 #2
0
        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);
            }
        }
コード例 #3
0
        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);
        }