Esempio n. 1
0
        public OperationalVehicleState GetVehicleState()
        {
            DatasetSource ds = Services.Dataset;

            return(new OperationalVehicleState(
                       ds.ItemAs <double>("speed").CurrentValue,
                       ds.ItemAs <double>("actual steering").CurrentValue,
                       ds.ItemAs <TransmissionGear>("transmission gear").CurrentValue,
                       ds.ItemAs <double>("pitch").CurrentValue,
                       ds.ItemAs <double>("brake pressure").CurrentValue,
                       ds.ItemAs <double>("engine torque").CurrentValue,
                       ds.ItemAs <double>("rpm").CurrentValue,
                       ds.ItemAs <double>("pedal position").CurrentValue,
                       ds.ItemAs <double>("actual steering").CurrentTime));
        }
        void client_PoseAbsReceived(object sender, PoseAbsReceivedEventArgs e)
        {
            packetCount++;

            Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);

            //OperationalTrace.WriteVerbose("got absolute pose for time {0}", e.PoseAbsData.timestamp);

            if (lastPacketTime.ts > e.PoseAbsData.timestamp.ts)
            {
                lastPacketTime = new CarTimestamp(-10000);
            }

            if (e.PoseAbsData.timestamp.ts - lastPacketTime.ts >= 0.02)
            {
                DatasetSource ds  = Services.Dataset;
                PoseAbsData   d   = e.PoseAbsData;
                CarTimestamp  now = e.PoseAbsData.timestamp;

                if (Services.Projection != null)
                {
                    Coordinates xy = Services.Projection.ECEFtoXY(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
                    ds.ItemAs <Coordinates>("xy").Add(xy, now);

                    AbsolutePose absPose = new AbsolutePose(xy, d.yaw, now);
                    Services.AbsolutePose.PushAbsolutePose(absPose);
                }

                if (!Settings.UseWheelSpeed)
                {
                    ds.ItemAs <double>("speed").Add(d.veh_vx, now);
                }

                ds.ItemAs <double>("vel - y").Add(d.veh_vy, now);
                ds.ItemAs <double>("heading").Add(BiasEstimator.CorrectHeading(d.yaw), now);
                ds.ItemAs <double>("pitch").Add(d.pitch + 1.25 * Math.PI / 180.0, now);
                ds.ItemAs <double>("roll").Add(d.roll, now);
                ds.ItemAs <double>("ba - x").Add(d.bax, now);
                ds.ItemAs <double>("ba - y").Add(d.bay, now);
                ds.ItemAs <double>("ba - z").Add(d.baz, now);
                ds.ItemAs <double>("bw - x").Add(d.bwx, now);
                ds.ItemAs <double>("bw - y").Add(d.bwy, now);
                ds.ItemAs <double>("bw - z").Add(d.bwz, now);
                ds.ItemAs <PoseCorrectionMode>("correction mode").Add(d.correction_mode, now);

                LLACoord lla = WGS84.ECEFtoLLA(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
                ds.ItemAs <double>("altitude").Add(lla.alt, now);

                if (Services.Projection != null)
                {
                    ds.ItemAs <Coordinates>("gps xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.gps_px, d.gps_py, d.gps_pz)), now);
                    ds.ItemAs <Coordinates>("hp xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.hp_px, d.hp_py, d.hp_pz)), now);
                }

                ds.ItemAs <double>("sep heading").Add(d.sep_heading, now);
                ds.ItemAs <double>("sep pitch").Add(d.sep_pitch, now);
                ds.ItemAs <double>("sep roll").Add(d.sep_roll, now);

                if (!double.IsNaN(lastYawRate) && lastPacketTime.ts > 0)
                {
                    // get the enu velocity
                    Vector3 pECEF     = new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz);
                    Vector3 vECEF     = new Vector3(d.ecef_vx, d.ecef_vy, d.ecef_vz);
                    Matrix3 Recef2enu = Geocentric.Recef2enu(pECEF);
                    Vector3 vENU      = Recef2enu * vECEF;
                    BiasEstimator.Update(lastYawRate, d.yaw, vENU, now.ts - lastPacketTime.ts, now);

                    Services.Dataset.ItemAs <double>("heading bias").Add(BiasEstimator.HeadingBias, now);
                }

                lastPacketTime = now;
            }
        }
        void IChannelListener.MessageArrived(string channelName, object message)
        {
            if (message is OperationalSimVehicleState)
            {
                OperationalTrace.ThreadTraceSource = TraceSource;
                Trace.CorrelationManager.StartLogicalOperation("simstate callback");
                try {
                    OperationalSimVehicleState state = (OperationalSimVehicleState)message;

                    DatasetSource ds = Services.Dataset;

                    OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp);

                    Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);

                    CarTimestamp now = state.Timestamp;
                    ds.ItemAs <Coordinates>("xy").Add(state.Position, now);
                    ds.ItemAs <double>("speed").Add(state.Speed, now);
                    ds.ItemAs <double>("heading").Add(state.Heading, now);
                    ds.ItemAs <double>("actual steering").Add(state.SteeringAngle, now);
                    ds.ItemAs <TransmissionGear>("transmission gear").Add(state.TransmissionGear, now);
                    ds.ItemAs <double>("engine torque").Add(state.EngineTorque, now);
                    ds.ItemAs <double>("brake pressure").Add(state.BrakePressure, now);
                    ds.ItemAs <double>("rpm").Add(state.EngineRPM, now);

                    if (state.TransmissionGear != lastRecvTransGear)
                    {
                        ds.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, now);
                        lastRecvTransGear = state.TransmissionGear;
                    }

                    lastCarMode = state.CarMode;
                    if (CarModeChanged != null)
                    {
                        CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now));
                    }

                    // build the current relative pose matrix
                    Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0) * Matrix4.YPR(state.Heading, 0, 0);
                    relativePose = relativePose.Inverse();

                    // push on to the relative pose stack
                    Services.RelativePose.PushTransform(now, relativePose);

                    // push the current absolute pose entry
                    AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now);
                    Services.AbsolutePose.PushAbsolutePose(absPose);
                }
                finally {
                    Trace.CorrelationManager.StopLogicalOperation();
                    OperationalTrace.ThreadTraceSource = null;
                }
            }
            else if (message is SceneEstimatorUntrackedClusterCollection)
            {
                // push to obstacle pipeline
                Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message));
            }
            else if (message is SceneEstimatorTrackedClusterCollection)
            {
                Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
            }
        }