示例#1
0
        private Task <ActivityHeaderViewModel> CreateHeaderViewModelAsync(Activity activity)
        {
            return(Task.Run(
                       () =>
            {
                var lap = activity.Lap[0];

                double maxSpeed = lap.MaximumSpeed * 3.6;
                EffortComputer effortComputer = lap.AverageHeartRateBpm != null
                            ? HumanEffortComputer.ByHeartBeat
                            : HumanEffortComputer.BySpeed.OverrideDefaultMaxValue(maxSpeed);

                var dispersion = new SortedDictionary <double, IDispersionSpan>();
                Trackpoint previousPoint = lap.Track[0];
                DateTime startTime = lap.Track[0].Time;
                for (int index = 0; index < lap.Track.Count; index++)
                {
                    Trackpoint currentPoint = lap.Track[index];

                    TimeSpan elapsedTime = currentPoint.Time - startTime;

                    double?speed = null;
                    if (previousPoint != null && previousPoint.Position != null &&
                        previousPoint.DistanceMeters > 0 && currentPoint.Position != null &&
                        currentPoint.DistanceMeters > 0 && elapsedTime.TotalSeconds > 0)
                    {
                        double kilometersTraveled = GeoCalculation.HaversineDistance(
                            new LatLong(
                                previousPoint.Position.LatitudeDegrees,
                                previousPoint.Position.LongitudeDegrees),
                            new LatLong(
                                currentPoint.Position.LatitudeDegrees,
                                currentPoint.Position.LongitudeDegrees));
                        double hoursElapsed = (elapsedTime - (previousPoint.Time - startTime)).TotalHours;
                        speed = kilometersTraveled / hoursElapsed;
                    }

                    double?value = lap.AverageHeartRateBpm != null ? currentPoint.HeartRateBpm?.Value : speed;
                    if (value == null)
                    {
                        previousPoint = currentPoint;
                        continue;
                    }

                    var elapsedTimeSinceLastPoint = currentPoint.Time - previousPoint.Time;
                    EffortSpan effortSpan = effortComputer.GetSpan(value);
                    if (!dispersion.ContainsKey(effortSpan.Threshold))
                    {
                        dispersion.Add(effortSpan.Threshold, new DispersionSpan(effortSpan.Color, 0));
                    }

                    dispersion[effortSpan.Threshold]
                    .IncrementValue(elapsedTimeSinceLastPoint.TotalMilliseconds);

                    previousPoint = currentPoint;
                }

                return new ActivityHeaderViewModel(activity.ToActivityHeader(), dispersion.Values.ToList());
            }));
        }
示例#2
0
        public IHttpActionResult PutTrackpoint(Guid id, Trackpoint trackpoint)
        {
            if (!ModelState.IsValid)
            {
                return(BadRequest(ModelState));
            }

            if (id != trackpoint.Id)
            {
                return(BadRequest());
            }

            db.Entry(trackpoint).State = EntityState.Modified;

            try
            {
                db.SaveChanges();
            }
            catch (DbUpdateConcurrencyException)
            {
                if (!TrackpointExists(id))
                {
                    return(NotFound());
                }
                else
                {
                    throw;
                }
            }

            return(StatusCode(HttpStatusCode.NoContent));
        }
示例#3
0
        public IHttpActionResult PostTrackpoint(Trackpoint trackpoint)
        {
            if (!ModelState.IsValid)
            {
                return(BadRequest(ModelState));
            }

            db.Trackpoints.Add(trackpoint);

            try
            {
                db.SaveChanges();
            }
            catch (DbUpdateException)
            {
                if (TrackpointExists(trackpoint.Id))
                {
                    return(Conflict());
                }
                else
                {
                    throw;
                }
            }

            return(CreatedAtRoute("DefaultApi", new { id = trackpoint.Id }, trackpoint));
        }
示例#4
0
        public IHttpActionResult GetTrackpoint(Guid id)
        {
            Trackpoint trackpoint = db.Trackpoints.Find(id);

            if (trackpoint == null)
            {
                return(NotFound());
            }

            return(Ok(trackpoint));
        }
示例#5
0
        public void ToTrackpoint_GivenStartTime_ShouldAddToElapsedTime()
        {
            // Arrange
            DataRecord record = new DataRecord();

            record.ElapsedTime = 100;
            DateTime startTime = DateTime.Now;

            // Act
            Trackpoint trackpoint = record.ToTrackpoint(startTime);

            // Assert
            Assert.AreEqual(startTime.AddMilliseconds(record.ElapsedTime), trackpoint.Time);
        }
示例#6
0
        public IHttpActionResult DeleteTrackpoint(Guid id)
        {
            Trackpoint trackpoint = db.Trackpoints.Find(id);

            if (trackpoint == null)
            {
                return(NotFound());
            }

            db.Trackpoints.Remove(trackpoint);
            db.SaveChanges();

            return(Ok(trackpoint));
        }
示例#7
0
        public void ToTrackpoint_ShouldMapToPosition()
        {
            // Arrange
            DataRecord record = new DataRecord();

            record.Latitude  = 1;
            record.Longitude = 2;

            // Act
            Trackpoint trackpoint = record.ToTrackpoint(DateTime.Now);

            // Assert
            Assert.AreEqual(1, trackpoint.Position.LatitudeDegrees);
            Assert.AreEqual(2, trackpoint.Position.LongitudeDegrees);
        }
    //public String replace(String s, String one, String another) {
    //    // In a string replace one substring with another
    //    if (s.Equals(""))
    //        return "";
    //    String res = "";
    //    int i = s.IndexOf(one, 0);
    //    int lastpos = 0;
    //    while (i != -1) {
    //        res += s.Substring(lastpos, i) + another;
    //        lastpos = i + one.Length();
    //        i = s.IndexOf(one, lastpos);
    //    }
    //    res += s.Substring(lastpos); // the rest
    //    return res;
    //}

    /**
     * distance(p0, p1, p2) computes the distance between the point p0 and the
     * segment [p1,p2]. This could probably be replaced with something that is a
     * bit more numerically stable.
     *
     * @param p0
     * @param p1
     * @param p2
     * @return
     */
    public double distance(Trackpoint p0, Trackpoint p1, Trackpoint p2)
    {
        double u, result = 0.0;

        if (p1.getLatDouble() == p2.getLatDouble() &&
            p1.getLonDouble() == p2.getLonDouble())
        {
            result = Math.Sqrt(Math.Pow(p2.getLatDouble() - p0.getLatDouble(), 2)
                               + Math.Pow(p2.getLonDouble() - p0.getLonDouble(), 2));
        }
        else
        {
            u = ((p0.getLatDouble() - p1.getLatDouble())
                 * (p2.getLatDouble() - p1.getLatDouble()) + (p0
                                                              .getLonDouble() - p1.getLonDouble())
                 * (p2.getLonDouble() - p1.getLonDouble()))
                / (Math.Pow(p2.getLatDouble() - p1.getLatDouble(), 2) + Math
                   .Pow(p2.getLonDouble() - p1.getLonDouble(), 2));

            if (u <= 0)
            {
                result = Math.Sqrt(Math.Pow(p0.getLatDouble() - p1.getLatDouble(),
                                            2)
                                   + Math.Pow(p0.getLonDouble() - p1.getLonDouble(), 2));
            }
            if (u >= 1)
            {
                result = Math.Sqrt(Math.Pow(p0.getLatDouble() - p2.getLatDouble(),
                                            2)
                                   + Math.Pow(p0.getLonDouble() - p2.getLonDouble(), 2));
            }
            if (0 < u && u < 1)
            {
                result = Math.Sqrt(Math.Pow(p0.getLatDouble() - p1.getLatDouble()
                                            - u * (p2.getLatDouble() - p1.getLatDouble()), 2)
                                   + Math.Pow(p0.getLonDouble() - p1.getLonDouble() - u
                                              * (p2.getLonDouble() - p1.getLonDouble()), 2));
            }
        }
        return(result);
    }
示例#9
0
        public IHttpActionResult AddTrackpoint(string device, string lat, string lon)
        {
            var dev = db.Devices.Where(d => d.Name.Equals(device, StringComparison.OrdinalIgnoreCase)).FirstOrDefault();

            if (dev == null)
            {
                return(BadRequest("Device not registered!"));
            }

            var trackpoint = new Trackpoint()
            {
                Id              = Guid.NewGuid(),
                Device          = dev,
                LocalTimeOffset = 0,
                UtcTime         = DateTime.UtcNow,
                Location        =
                    DbGeography.FromText(String.Format(CultureInfo.InvariantCulture, "POINT({0} {1})", lon, lat))
            };

            db.Trackpoints.Add(trackpoint);
            try
            {
                db.SaveChanges();
            }
            catch (DbUpdateException)
            {
                if (TrackpointExists(trackpoint.Id))
                {
                    return(Conflict());
                }
                else
                {
                    throw;
                }
            }

            return(Ok(trackpoint));
        }
示例#10
0
        public Trackpoint ToTrackpoint(DateTime startTime)
        {
            Trackpoint trackpoint = new Trackpoint();

            trackpoint.Position = new Position()
            {
                LatitudeDegrees = Latitude, LongitudeDegrees = Longitude
            };
            trackpoint.Cadence      = (byte)StrokeRate;
            trackpoint.Time         = startTime.AddMilliseconds(ElapsedTime).ToUniversalTime();
            trackpoint.HeartRateBpm = new HeartRateInBeatsPerMinute()
            {
                Value = (byte)HeartRate
            };

            NKEmpowerTrackpointExtension_t extensionData;

            if (Power != 0)
            {
                extensionData = new NKEmpowerTrackpointExtension_t(SpeedGps, Power, CatchAngle, FinishAngle, MaxForceAngle, AvgHandleForce, MaxHandleForce, SlipAngle, WashAngle, WorkPerStroke);
            }
            else
            {
                extensionData = new NKEmpowerTrackpointExtension_t(SpeedGps);
            }

            XmlDocument doc = new XmlDocument();

            using (XmlWriter writer = doc.CreateNavigator().AppendChild())
            {
                new XmlSerializer(typeof(NKEmpowerTrackpointExtension_t)).Serialize(writer, extensionData);
            }

            trackpoint.Extensions.Any.Add(doc.DocumentElement);

            return(trackpoint);
        }
示例#11
0
 public void addTrackpoint(Trackpoint trkpt)
 {
     this.trackpoints.Add(trkpt);
 }
示例#12
0
        void Export()
        {
            string selectedFile = FileService.SaveFileDialog();

            if (!string.IsNullOrEmpty(selectedFile))
            {
                OutputFilePath = selectedFile;
            }

            if (!string.IsNullOrEmpty(OutputFilePath))
            {
                DbProviderFactory fact = DbProviderFactories.GetFactory("System.Data.SQLite");
                using (SQLiteConnection cnn = (SQLiteConnection)fact.CreateConnection())
                {
                    cnn.ConnectionString = "Data Source=" + FilePath;
                    cnn.Open();

                    SQLiteCommand command = new SQLiteCommand(
                        "SELECT PK_IntervalId, StartTime, TotalElapsedTime, TotalDistance, AverageStrokeRate, AverageHeartRate, AverageSpeed, TotalStrokeCount FROM Intervals" +
                        " WHERE FK_SessionId = @sessionId", cnn);
                    SQLiteParameter sessionParam = new SQLiteParameter("sessionId", System.Data.DbType.Int32);
                    sessionParam.Value = SelectedSession.Id;
                    command.Parameters.Add(sessionParam);

                    SQLiteCommand dataRecordCommand = new SQLiteCommand(
                        "SELECT dr.PK_DataRecordId, dr.ElapsedTime, dr.Latitude, dr.Longitude, dr.SpeedGps, dr.SpeedImpeller, dr.DistanceGps, dr.DistanceImpeller, dr.StrokeRate, dr.HeartRate," +
                        " osd.Power, osd.CatchAngle, osd.FinishAngle, osd.PositionOfMaxForce, osd.AverageHandleForce, osd.MaxHandleForce, osd.Slip, osd.Wash, osd.RealWorkPerStroke" +
                        " FROM DataRecords dr LEFT OUTER JOIN OarlockStrokeData osd ON osd.PK_OarlockStrokeRecordId = (SELECT PK_OarlockStrokeRecordId FROM OarlockStrokeData WHERE FK_DataRecordId = dr.PK_DataRecordId LIMIT 1)" +
                        " WHERE dr.FK_IntervalId = @intervalId AND dr.Type = 0", cnn); // Type filtered to 0 to just pick up strokes

                    SQLiteParameter intervalParam = new SQLiteParameter("intervalId", System.Data.DbType.Int32);
                    dataRecordCommand.Parameters.Add(intervalParam);

                    SQLiteDataReader reader = command.ExecuteReader();

                    while (reader.Read())
                    {
                        int      id               = (int)(long)reader[0];
                        DateTime startTime        = NKDateTimeConverter.NKToClr((long)reader[1]);
                        int      totalElapsedTime = (int)(long)reader[2];
                        int      totalDistance    = (int)(long)reader[3];
                        double   strokeRate       = Convert.ToDouble((long)reader[4]) / 2;
                        int      averageHeartRate = reader.IsDBNull(5) ? 0 : (int)(long)reader[5];
                        double   speed            = Convert.ToDouble((long)reader[6]) / 100;
                        int      totalStrokeCount = (int)(long)reader[7];

                        Interval interval = new Interval(id, startTime, totalElapsedTime, totalDistance, strokeRate, averageHeartRate, speed, totalStrokeCount);

                        intervalParam.Value = interval.Id;

                        SQLiteDataReader dataRecordReader = dataRecordCommand.ExecuteReader();

                        while (dataRecordReader.Read())
                        {
                            DataRecord record = new DataRecord();
                            record.ElapsedTime      = (int)(long)dataRecordReader[1];
                            record.Latitude         = (double)dataRecordReader[2];
                            record.Longitude        = (double)dataRecordReader[3];
                            record.SpeedGps         = Convert.ToDouble((long)dataRecordReader[4]) / 100;
                            record.SpeedImpeller    = Convert.ToDouble((long)dataRecordReader[5]) / 100;
                            record.DistanceGps      = (int)(long)dataRecordReader[6];
                            record.DistanceImpeller = (int)(long)dataRecordReader[7];
                            record.StrokeRate       = Convert.ToDouble((long)dataRecordReader[8]) / 2;
                            record.HeartRate        = (int)(long)dataRecordReader[9];
                            record.Power            = dataRecordReader.IsDBNull(10) ? 0 : Convert.ToDouble((long)dataRecordReader[10]) / 10;
                            record.CatchAngle       = dataRecordReader.IsDBNull(11) ? 0 : (int)(long)dataRecordReader[11];
                            record.FinishAngle      = dataRecordReader.IsDBNull(12) ? 0 : (int)(long)dataRecordReader[12];
                            record.MaxForceAngle    = dataRecordReader.IsDBNull(13) ? 0 : (int)(long)dataRecordReader[13];
                            record.AvgHandleForce   = dataRecordReader.IsDBNull(14) ? 0 : Convert.ToDouble((long)dataRecordReader[14]) / 10;
                            record.MaxHandleForce   = dataRecordReader.IsDBNull(15) ? 0 : Convert.ToDouble((long)dataRecordReader[15]) / 10;
                            record.SlipAngle        = dataRecordReader.IsDBNull(16) ? 0 : (int)(long)dataRecordReader[16];
                            record.WashAngle        = dataRecordReader.IsDBNull(17) ? 0 : (int)(long)dataRecordReader[17];
                            record.WorkPerStroke    = dataRecordReader.IsDBNull(18) ? 0 : Convert.ToDouble((long)dataRecordReader[18]) / 10;

                            interval.DataRecords.Add(record);
                        }

                        SelectedSession.Intervals.Add(interval);
                    }

                    reader.Close();
                }

                Activity activity = new Activity()
                {
                    Id = SelectedSession.StartTime, Sport = Sport.Other
                };
                activity.Creator = new Device()
                {
                    Name = "SpeedCoach GPS"
                };

                foreach (Interval interval in SelectedSession.Intervals)
                {
                    ActivityLap lap = interval.ToLap();

                    // TODO: Cadence/Stroke Rate (type conversion)
                    double maxSpeed     = 0;
                    byte   maxHeartRate = 0;

                    foreach (DataRecord record in interval.DataRecords)
                    {
                        Trackpoint trackpoint = record.ToTrackpoint(lap.StartTime);

                        if (trackpoint.HeartRateBpm.Value > maxHeartRate)
                        {
                            maxHeartRate = trackpoint.HeartRateBpm.Value;
                        }

                        switch (SelectedSession.SpeedInput)
                        {
                        case SpeedInput.Gps:
                            if (record.SpeedGps > maxSpeed)
                            {
                                maxSpeed = record.SpeedGps;
                            }
                            break;

                        case SpeedInput.Impeller:
                            if (record.SpeedImpeller > maxSpeed)
                            {
                                maxSpeed = record.SpeedImpeller;
                            }
                            break;
                        }
                        lap.Track.Add(trackpoint);
                    }

                    lap.MaximumSpeed        = maxSpeed;
                    lap.MaximumHeartRateBpm = new HeartRateInBeatsPerMinute()
                    {
                        Value = maxHeartRate
                    };

                    activity.Lap.Add(lap);
                }

                TrainingCenterDatabase tcd = new TrainingCenterDatabase();
                tcd.Activities.Activity.Add(activity);

                using (XmlWriter writer = XmlWriter.Create(OutputFilePath))
                {
                    (new XmlSerializer(typeof(TrainingCenterDatabase))).Serialize(writer, tcd);
                }
            }
        }
    private String createEncodings(List <Trackpoint> points, double[] dists)
    {
        StringBuilder encodedPoints = new StringBuilder();

        double maxlat = 0, minlat = 0, maxlon = 0, minlon = 0;

        int plat = 0;
        int plng = 0;

        for (int i = 0; i < points.Count; i++)
        {
            // determin bounds (max/min lat/lon)
            if (i == 0)
            {
                maxlat = minlat = points[i].getLatDouble();
                maxlon = minlon = points[i].getLonDouble();
            }
            else
            {
                if (points[i].getLatDouble() > maxlat)
                {
                    maxlat = points[i].getLatDouble();
                }
                else if (points[i].getLatDouble() < minlat)
                {
                    minlat = points[i].getLatDouble();
                }
                else if (points[i].getLonDouble() > maxlon)
                {
                    maxlon = points[i].getLonDouble();
                }
                else if (points[i].getLonDouble() < minlon)
                {
                    minlon = points[i].getLonDouble();
                }
            }

            if (dists[i] != 0 || i == 0 || i == points.Count - 1)
            {
                Trackpoint point = points[i];

                int late5 = floor1e5(point.getLatDouble());
                int lnge5 = floor1e5(point.getLonDouble());

                int dlat = late5 - plat;
                int dlng = lnge5 - plng;

                plat = late5;
                plng = lnge5;

                encodedPoints.Append(encodeSignedNumber(dlat));
                encodedPoints.Append(encodeSignedNumber(dlng));
            }
        }

        Dictionary <String, Double> bounds = new Dictionary <String, Double>();

        bounds.Add("maxlat", maxlat);
        bounds.Add("minlat", minlat);
        bounds.Add("maxlon", maxlon);
        bounds.Add("minlon", minlon);

        this.setBounds(bounds);
        return(encodedPoints.ToString());
    }
示例#14
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="ddg"></param>
        /// <param name="speaker"></param>
        /// <param name="trackFileName">can be null, for a saved track</param>
        public BehaviorRouteFollowing(IDriveGeometry ddg, ISpeaker speaker, string trackFileName, double powerFactor = 1.0d)
            : base(ddg)
        {
            this.speaker     = speaker;
            this.powerFactor = powerFactor;

            BehaviorActivateCondition = bd =>
            {
                return(nextWp != null);
            };

            BehaviorDeactivateCondition = bd =>
            {
                return(nextWp == null);
            };

            if (String.IsNullOrWhiteSpace(trackFileName))
            {
                //speaker.Speak("Loading saved track");
                try
                {
                    missionTrack = null;

                    // Load stored waypoints:
                    // on the PC, from   C:\Users\sergei\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState\MyTrack.xml
                    //            RPi:   \\172.16.1.175\c$\Data\Users\DefaultAccount\AppData\Local\Packages\RobotPluckyPackage_sjh4qacv6p1wm\LocalState
                    Track track = SerializableStorage <Track> .Load(savedTrackFileName).Result;

                    if (track != null)
                    {
                        missionTrack = track;
                        //speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");
                    }
                }
                catch (Exception ex)
                {
                    speaker.Speak("could not load saved track file");
                }

                if (missionTrack == null)
                {
                    speaker.Speak("failed to load saved track file");
                    missionTrack = new Track();
                }
                nextWp          = missionTrack.nextTargetWp;
                stopStartedTime = null;
            }
            else
            {
                speaker.Speak("Loading file " + trackFileName);

                missionTrack = new Track();

                try
                {
                    missionTrack.Init(trackFileName);

                    speaker.Speak("Loaded file " + missionTrack.Count + " trackpoints");

                    nextWp = missionTrack.nextTargetWp;
                }
                catch (Exception ex)
                {
                    speaker.Speak("could not load planned track file");
                }
            }
        }
示例#15
0
        /// <summary>
        /// Computes goalBearingDegrees based on current location and the next trackpoint
        /// </summary>
        /// <returns></returns>
        public override IEnumerator <ISubsumptionTask> Execute()
        {
            FiredOn = true;

            while (!MustExit && !MustTerminate)
            {
                nextWp = missionTrack.nextTargetWp;

                while (!MustActivate && !MustExit && !MustTerminate) // wait for activation
                {
                    yield return(RobotTask.Continue);                // dormant state - no need to calculate
                }

                if (MustExit || MustTerminate)
                {
                    break;
                }

                // we have next trackpoint to go to, enter the work loop:

                //int i = 0;
                while (!MustDeactivate && !MustExit && !MustTerminate)
                {
                    nextWp = missionTrack.nextTargetWp;

                    if (nextWp == null)
                    {
                        break;
                    }

                    Direction dirToWp  = nextWp.directionToWp(behaviorData.robotPose.geoPosition, behaviorData.robotPose.direction);
                    Distance  distToWp = nextWp.distanceToWp(behaviorData.robotPose.geoPosition);

                    if (distToWp.Meters < closeEnoughMeters)
                    {
                        nextWp.trackpointState = TrackpointState.Passed;            // trackpoint will be ignored on the next cycle
                        //speaker.Speak("Waypoint " + nextWp.number + " reached");

                        if (nextWp.headingDegrees.HasValue)
                        {
                            // if the trackpoint was saved with a heading, we need to turn to that heading:
                            speaker.Speak("turning to " + Math.Round(nextWp.headingDegrees.Value));

                            SetGrabByMe();
                            turnStartedTime = DateTime.Now;

                            // TODO: we overshoot a lot here. Maybe having a PID controller would help.
                            while (Math.Abs(behaviorData.robotPose.direction.heading.Value - nextWp.headingDegrees.Value) > trackpointHeadingToleranceDegrees &&
                                   (DateTime.Now - turnStartedTime).TotalSeconds < waitOnTurnSeconds)
                            {
                                double heading            = behaviorData.robotPose.direction.heading.Value; // robot's heading by SLAM
                                double desiredTurnDegrees = DirectionMath.to180(heading - nextWp.headingDegrees.Value);
                                setSpeedAndTurn(0.0d, -turnFactor * Math.Sign(desiredTurnDegrees));
                                yield return(RobotTask.Continue);    // let the chain work
                            }

                            if (Math.Abs(behaviorData.robotPose.direction.heading.Value - nextWp.headingDegrees.Value) > trackpointHeadingToleranceDegrees)
                            {
                                speaker.Speak("Error: turning to " + Math.Round(nextWp.headingDegrees.Value) + " failed");
                            }

                            ClearGrab();
                        }

                        // pause and declare to other behaviors that we reached trackpoing:
                        if (stopStartedTime == null)
                        {
                            speaker.Speak("waiting");
                            stopStartedTime = DateTime.Now;
                            while ((DateTime.Now - stopStartedTime.Value).TotalSeconds < waitOnTrackpointsSeconds)
                            {
                                goalBearingRelativeDegrees           = null;         // BehaviorGoToAngle will stop the robot
                                goalDistanceMeters                   = null;
                                getCoordinatorData().EnablingRequest = "trackpoint"; // let other behaviors know we are waiting on a trackpoint
                                yield return(RobotTask.Continue);                    // wait a bit on the trackpoint
                            }
                            stopStartedTime = null;
                        }

                        if (missionTrack.nextTargetWp != null)
                        {
                            speaker.Speak("proceeding");
                        }
                    }
                    else if (nextWp.estimatedTimeOfArrival.HasValue &&
                             (DateTime.Now - nextWp.estimatedTimeOfArrival.Value).TotalSeconds > waypointCantReachSeconds)
                    {
                        nextWp.trackpointState = TrackpointState.CouldNotReach;     // will be ignored on the next cycle
                        speaker.Speak("Waypoint " + nextWp.number + " could not reach");
                    }
                    else
                    {
                        goalBearingRelativeDegrees = dirToWp.bearingRelative.Value;       // update robotState
                        goalDistanceMeters         = distToWp.Meters;

                        Debug.WriteLine(string.Format("IP: Trackpoint {0}  abs bearing= {1}  distToWp.Meters= {2}", nextWp.number, dirToWp.bearing, distToWp.Meters));

                        if (!nextWp.estimatedTimeOfArrival.HasValue)
                        {
                            nextWp.trackpointState = TrackpointState.SelectedAsTarget;
                            double maxVelocityMSec = ToVelocity(behaviorData.robotState.powerLevelPercent * powerFactor);  // m/sec
                            double timeToReachSec  = distToWp.Meters / maxVelocityMSec;
                            nextWp.estimatedTimeOfArrival = DateTime.Now.AddSeconds(timeToReachSec);
                            speaker.Speak("Next " + Math.Round(distToWp.Meters) + " " + Math.Round(timeToReachSec));
                            //speaker.Speak("Next trackpoint " + Math.Round(distToWp.Meters) + " meters away. I expect reaching it in " + Math.Round(timeToReachSec) + " seconds");
                        }
                    }

                    yield return(RobotTask.Continue);
                }

                speaker.Speak("No more trackpoints");
                goalBearingRelativeDegrees = null;      // BehaviorGoToAngle will stop the robot
                goalDistanceMeters         = null;
            }

            FiredOn = false;

            Debug.WriteLine("BehaviorRouteFollowing: " + (MustExit ? "MustExit" : "completed"));

            yield break;
        }
示例#16
0
        public static Tour Parse(string raw)
        {
            var        tour = new Tour();
            Trackpoint currentTrackPoint = null;
            var        readingAltitude   = false;

            using (var rawReader = new StringReader(raw))
                using (var xmlReader = XmlReader.Create(rawReader))
                    while (xmlReader.Read())
                    {
                        switch (xmlReader.NodeType)
                        {
                        case XmlNodeType.Element:
                            switch (xmlReader.Name)
                            {
                            case "trkpt":
                                currentTrackPoint = new Trackpoint();
                                currentTrackPoint.SetLatitude(Convert.ToDouble(xmlReader.GetAttribute("lat"), CultureInfo.InvariantCulture));
                                currentTrackPoint.SetLongitude(Convert.ToDouble(xmlReader.GetAttribute("lon"), CultureInfo.InvariantCulture));
                                break;

                            case "ele":
                                readingAltitude = true;
                                break;
                            }

                            break;

                        case XmlNodeType.Text:
                            if (!readingAltitude)
                            {
                                break;
                            }
                            if (currentTrackPoint == null)
                            {
                                throw new Exception("Malformed data");
                            }
                            currentTrackPoint.SetAltitude(Convert.ToDouble(xmlReader.Value, CultureInfo.InvariantCulture));
                            break;

                        case XmlNodeType.EndElement:
                            switch (xmlReader.Name)
                            {
                            case "trkpt":
                                if (currentTrackPoint == null)
                                {
                                    throw new Exception("Malformed data");
                                }
                                tour.Add(currentTrackPoint);
                                currentTrackPoint = null;
                                break;

                            case "ele":
                                if (!readingAltitude)
                                {
                                    throw new Exception("Malformed data");
                                }
                                readingAltitude = false;
                                break;
                            }

                            break;
                        }
                    }

            return(tour);
        }