From 99bd9f7a27f1a424f9c7bdff8c1219a267a6dc2c Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 16 Oct 2016 17:01:41 -0400 Subject: [PATCH 001/149] added new localizer analysis code --- offline/localizer/get_logs.py | 24 ++ .../localizer/latlonutm/Codes/matlab/ll2utm.m | 225 +++++++++++ .../localizer/latlonutm/Codes/matlab/utm2ll.m | 191 ++++++++++ offline/localizer/latlonutm/license.txt | 24 ++ offline/localizer/localizer.java | 355 ++++++++++++++++++ offline/localizer/localizer.m | 164 ++++++++ 6 files changed, 983 insertions(+) create mode 100644 offline/localizer/get_logs.py create mode 100644 offline/localizer/latlonutm/Codes/matlab/ll2utm.m create mode 100644 offline/localizer/latlonutm/Codes/matlab/utm2ll.m create mode 100644 offline/localizer/latlonutm/license.txt create mode 100644 offline/localizer/localizer.java create mode 100644 offline/localizer/localizer.m diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py new file mode 100644 index 00000000..b21f26f2 --- /dev/null +++ b/offline/localizer/get_logs.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +import json +import numpy as np +import scipy.io + +d = None +file = '../rolls_logs/2016-10-09-06-17-15/sensors_2016-10-09-06-17-15.txt' +# file = '../rolls_logs/sample.txt' +with open(file) as json_data: + d = json.load(json_data) + +if d is None: + print('No log file found') + exit() + +messages = d['sensor_data'] +for msg in messages: + if msg['VERSION_ID'] == '': + print(msg['VERSION_ID']) + + +arr = np.array([1, 2, 3]) +scipy.io.savemat('./roll_logs.mat', mdict={'logs': arr}) diff --git a/offline/localizer/latlonutm/Codes/matlab/ll2utm.m b/offline/localizer/latlonutm/Codes/matlab/ll2utm.m new file mode 100644 index 00000000..0e26214b --- /dev/null +++ b/offline/localizer/latlonutm/Codes/matlab/ll2utm.m @@ -0,0 +1,225 @@ +function [x,y,f]=ll2utm(varargin) +%LL2UTM Lat/Lon to UTM coordinates precise conversion. +% [X,Y]=LL2UTM2(LAT,LON) or LL2UTM([LAT,LON]) converts coordinates +% LAT,LON (in degrees) to UTM X and Y (in meters). Default datum is WGS84. +% +% LAT and LON can be scalars, vectors or matrix. Outputs X and Y will +% have the same size as inputs. +% +% LL2UTM(...,DATUM) uses specific DATUM for conversion. DATUM can be one +% of the following char strings: +% 'wgs84': World Geodetic System 1984 (default) +% 'nad27': North American Datum 1927 +% 'clk66': Clarke 1866 +% 'nad83': North American Datum 1983 +% 'grs80': Geodetic Reference System 1980 +% 'int24': International 1924 / Hayford 1909 +% or DATUM can be a 2-element vector [A,F] where A is semimajor axis (in +% meters) and F is flattening of the user-defined ellipsoid. +% +% LL2UTM(...,ZONE) forces the UTM ZONE (scalar integer or same size as +% LAT and LON) instead of automatic set. +% +% [X,Y,ZONE]=LL2UTM(...) returns also the computed UTM ZONE (negative +% value for southern hemisphere points). +% +% +% XY=LL2UTM(...) or without any output argument returns a 2-column +% matrix [X,Y]. +% +% Note: +% - LL2UTM does not perform cross-datum conversion. +% - precision is near a millimeter. +% +% +% Reference: +% I.G.N., Projection cartographique Mercator Transverse: Algorithmes, +% Notes Techniques NT/G 76, janvier 1995. +% +% Acknowledgments: Mathieu, Frederic Christen. +% +% +% Author: Francois Beauducel, +% Created: 2003-12-02 +% Updated: 2015-01-29 + + +% Copyright (c) 2001-2015, François Beauducel, covered by BSD License. +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are +% met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * Redistributions in binary form must reproduce the above copyright +% notice, this list of conditions and the following disclaimer in +% the documentation and/or other materials provided with the distribution +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE. + +% Available datums +datums = [ ... + { 'wgs84', 6378137.0, 298.257223563 }; + { 'nad83', 6378137.0, 298.257222101 }; + { 'grs80', 6378137.0, 298.257222101 }; + { 'nad27', 6378206.4, 294.978698214 }; + { 'int24', 6378388.0, 297.000000000 }; + { 'clk66', 6378206.4, 294.978698214 }; +]; + +% constants +D0 = 180/pi; % conversion rad to deg +K0 = 0.9996; % UTM scale factor +X0 = 500000; % UTM false East (m) + +% defaults +datum = 'wgs84'; +zone = []; + +if nargin < 1 + error('Not enough input arguments.') +end + +if nargin > 1 && isnumeric(varargin{1}) && isnumeric(varargin{2}) && all(size(varargin{1})==size(varargin{2})) + lat = varargin{1}; + lon = varargin{2}; + v = 2; +elseif isnumeric(varargin{1}) && size(varargin{1},2) == 2 + lat = varargin{1}(:,1); + lon = varargin{1}(:,2); + v = 1; +else + error('Single input argument must be a 2-column matrix [LAT,LON].') +end + +if all([numel(lat),numel(lon)] > 1) && any(size(lat) ~= size(lon)) + error('LAT and LON must be the same size or scalars.') +end + +for n = (v+1):nargin + % LL2UTM(...,DATUM) + if ischar(varargin{n}) || (isnumeric(varargin{n}) && numel(varargin{n})==2) + datum = varargin{n}; + % LL2UTM(...,ZONE) + elseif isnumeric(varargin{n}) && (isscalar(varargin{n}) || all(size(varargin{n})==size(lat))) + zone = round(varargin{n}); + else + error('Unknown argument #%d. See documentation.',n) + end +end + +if ischar(datum) + % LL2UTM(...,DATUM) with DATUM as char + if ~any(strcmpi(datum,datums(:,1))) + error('Unkown DATUM name "%s"',datum); + end + k = find(strcmpi(datum,datums(:,1))); + A1 = datums{k,2}; + F1 = datums{k,3}; +else + % LL2UTM(...,DATUM) with DATUM as [A,F] user-defined + A1 = datum(1); + F1 = datum(2); +end + +p1 = lat/D0; % Phi = Latitude (rad) +l1 = lon/D0; % Lambda = Longitude (rad) + +% UTM zone automatic setting +if isempty(zone) + F0 = round((l1*D0 + 183)/6); +else + F0 = zone; +end + +B1 = A1*(1 - 1/F1); +E1 = sqrt((A1*A1 - B1*B1)/(A1*A1)); +P0 = 0/D0; +L0 = (6*F0 - 183)/D0; % UTM origin longitude (rad) +Y0 = 1e7*(p1 < 0); % UTM false northern (m) +N = K0*A1; + +C = coef(E1,0); +B = C(1)*P0 + C(2)*sin(2*P0) + C(3)*sin(4*P0) + C(4)*sin(6*P0) + C(5)*sin(8*P0); +YS = Y0 - N*B; + +C = coef(E1,2); +L = log(tan(pi/4 + p1/2).*(((1 - E1*sin(p1))./(1 + E1*sin(p1))).^(E1/2))); +z = complex(atan(sinh(L)./cos(l1 - L0)),log(tan(pi/4 + asin(sin(l1 - L0)./cosh(L))/2))); +Z = N.*C(1).*z + N.*(C(2)*sin(2*z) + C(3)*sin(4*z) + C(4)*sin(6*z) + C(5)*sin(8*z)); +xs = imag(Z) + X0; +ys = real(Z) + YS; + +% outputs zone if needed: scalar value if unique, or vector/matrix of the +% same size as x/y in case of crossed zones +if nargout > 2 + f = F0.*sign(lat); + fu = unique(f); + if isscalar(fu) + f = fu; + end +end + +if nargout < 2 + x = [xs(:),ys(:)]; +else + x = xs; + y = ys; +end + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function c = coef(e,m) +%COEF Projection coefficients +% COEF(E,M) returns a vector of 5 coefficients from: +% E = first ellipsoid excentricity +% M = 0 for transverse mercator +% M = 1 for transverse mercator reverse coefficients +% M = 2 for merdian arc + + +if nargin < 2 + m = 0; +end + +switch m + case 0 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -105/4096, 0, -45/1024, 0, -3/32, 0, -3/8, 0, 0; + 525/16384, 0, 45/1024, 0, 15/256, 0, 0, 0, 0; + -175/12288, 0, -35/3072, 0, 0, 0, 0, 0, 0; + 315/131072, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 1 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + 1/61440, 0, 7/2048, 0, 1/48, 0, 1/8, 0, 0; + 559/368640, 0, 3/1280, 0, 1/768, 0, 0, 0, 0; + 283/430080, 0, 17/30720, 0, 0, 0, 0, 0, 0; + 4397/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 2 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -901/184320, 0, -9/1024, 0, -1/96, 0, 1/8, 0, 0; + -311/737280, 0, 17/5120, 0, 13/768, 0, 0, 0, 0; + 899/430080, 0, 61/15360, 0, 0, 0, 0, 0, 0; + 49561/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + +end +c = zeros(size(c0,1),1); + +for i = 1:size(c0,1) + c(i) = polyval(c0(i,:),e); +end diff --git a/offline/localizer/latlonutm/Codes/matlab/utm2ll.m b/offline/localizer/latlonutm/Codes/matlab/utm2ll.m new file mode 100644 index 00000000..24057ef0 --- /dev/null +++ b/offline/localizer/latlonutm/Codes/matlab/utm2ll.m @@ -0,0 +1,191 @@ +function [lat,lon]=utm2ll(x,y,f,datum,varargin) +%UTM2LL UTM to Lat/Lon coordinates precise conversion. +% [LAT,LON]=UTM2LL(X,Y,ZONE) converts UTM coordinates X,Y (in meters) +% defined in the UTM ZONE (integer) to latitude LAT and longitude LON +% (in degrees). Default datum is WGS84. +% +% X, Y and F can be scalars, vectors or matrix. Outputs LAT and LON will +% have the same size as inputs. +% +% For southern hemisphere points, use negative zone -ZONE. +% +% UTM2LL(X,Y,ZONE,DATUM) uses specific DATUM for conversion. DATUM can be +% a string in the following list: +% 'wgs84': World Geodetic System 1984 (default) +% 'nad27': North American Datum 1927 +% 'clk66': Clarke 1866 +% 'nad83': North American Datum 1983 +% 'grs80': Geodetic Reference System 1980 +% 'int24': International 1924 / Hayford 1909 +% or DATUM can be a 2-element vector [A,F] where A is semimajor axis (in +% meters) and F is flattening of the user-defined ellipsoid. +% +% Notice: +% - UTM2LL does not perform cross-datum conversion. +% - precision is near a millimeter. +% +% +% Reference: +% I.G.N., Projection cartographique Mercator Transverse: Algorithmes, +% Notes Techniques NT/G 76, janvier 1995. +% +% Author: Francois Beauducel, +% Created: 2001-08-23 +% Updated: 2015-01-29 + + +% Copyright (c) 2001-2015, François Beauducel, covered by BSD License. +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are +% met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * Redistributions in binary form must reproduce the above copyright +% notice, this list of conditions and the following disclaimer in +% the documentation and/or other materials provided with the distribution +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% POSSIBILITY OF SUCH DAMAGE. + +% Available datums +datums = [ ... + { 'wgs84', 6378137.0, 298.257223563 }; + { 'nad83', 6378137.0, 298.257222101 }; + { 'grs80', 6378137.0, 298.257222101 }; + { 'nad27', 6378206.4, 294.978698214 }; + { 'int24', 6378388.0, 297.000000000 }; + { 'clk66', 6378206.4, 294.978698214 }; +]; + +if nargin < 3 + error('Not enough input arguments.') +end + +if all([numel(x),numel(y)] > 1) && any(size(x) ~= size(y)) + error('X and Y must be the same size or scalars.') +end + +if ~isnumeric(f) || any(f ~= round(f)) || (~isscalar(f) && any(size(f) ~= size(x))) + error('ZONE must be integer value, scalar or same size as X and/or Y.') +end + +if nargin < 4 + datum = 'wgs84'; +end + +if ischar(datum) + if ~any(strcmpi(datum,datums(:,1))) + error('Unkown DATUM name "%s"',datum); + end + k = find(strcmpi(datum,datums(:,1))); + A1 = datums{k,2}; + F1 = datums{k,3}; +else + if numel(datum) ~= 2 + error('User defined DATUM must be a vector [A,F].'); + end + A1 = datum(1); + F1 = datum(2); +end + +% constants +D0 = 180/pi; % conversion rad to deg +maxiter = 100; % maximum iteration for latitude computation +eps = 1e-11; % minimum residue for latitude computation + +K0 = 0.9996; % UTM scale factor +X0 = 500000; % UTM false East (m) +Y0 = 1e7*(f < 0); % UTM false North (m) +P0 = 0; % UTM origin latitude (rad) +L0 = (6*abs(f) - 183)/D0; % UTM origin longitude (rad) +E1 = sqrt((A1^2 - (A1*(1 - 1/F1))^2)/A1^2); % ellpsoid excentricity +N = K0*A1; + +% computing parameters for Mercator Transverse projection +C = coef(E1,0); +YS = Y0 - N*(C(1)*P0 + C(2)*sin(2*P0) + C(3)*sin(4*P0) + C(4)*sin(6*P0) + C(5)*sin(8*P0)); + +C = coef(E1,1); +zt = complex((y - YS)/N/C(1),(x - X0)/N/C(1)); +z = zt - C(2)*sin(2*zt) - C(3)*sin(4*zt) - C(4)*sin(6*zt) - C(5)*sin(8*zt); +L = real(z); +LS = imag(z); + +l = L0 + atan(sinh(LS)./cos(L)); +p = asin(sin(L)./cosh(LS)); + +L = log(tan(pi/4 + p/2)); + +% calculates latitude from the isometric latitude +p = 2*atan(exp(L)) - pi/2; +p0 = NaN; +n = 0; +while any(isnan(p0) | abs(p - p0) > eps) && n < maxiter + p0 = p; + es = E1*sin(p0); + p = 2*atan(((1 + es)./(1 - es)).^(E1/2).*exp(L)) - pi/2; + n = n + 1; +end + +if nargout < 2 + lat = D0*[p(:),l(:)]; +else + lat = p*D0; + lon = l*D0; +end + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function c = coef(e,m) +%COEF Projection coefficients +% COEF(E,M) returns a vector of 5 coefficients from: +% E = first ellipsoid excentricity +% M = 0 for transverse mercator +% M = 1 for transverse mercator reverse coefficients +% M = 2 for merdian arc + + +if nargin < 2 + m = 0; +end + +switch m + case 0 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -105/4096, 0, -45/1024, 0, -3/32, 0, -3/8, 0, 0; + 525/16384, 0, 45/1024, 0, 15/256, 0, 0, 0, 0; + -175/12288, 0, -35/3072, 0, 0, 0, 0, 0, 0; + 315/131072, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 1 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + 1/61440, 0, 7/2048, 0, 1/48, 0, 1/8, 0, 0; + 559/368640, 0, 3/1280, 0, 1/768, 0, 0, 0, 0; + 283/430080, 0, 17/30720, 0, 0, 0, 0, 0, 0; + 4397/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + + case 2 + c0 = [-175/16384, 0, -5/256, 0, -3/64, 0, -1/4, 0, 1; + -901/184320, 0, -9/1024, 0, -1/96, 0, 1/8, 0, 0; + -311/737280, 0, 17/5120, 0, 13/768, 0, 0, 0, 0; + 899/430080, 0, 61/15360, 0, 0, 0, 0, 0, 0; + 49561/41287680, 0, 0, 0, 0, 0, 0, 0, 0]; + +end +c = zeros(size(c0,1),1); + +for i = 1:size(c0,1) + c(i) = polyval(c0(i,:),e); +end diff --git a/offline/localizer/latlonutm/license.txt b/offline/localizer/latlonutm/license.txt new file mode 100644 index 00000000..0355b586 --- /dev/null +++ b/offline/localizer/latlonutm/license.txt @@ -0,0 +1,24 @@ +Copyright (c) 2015, François Beauducel +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the distribution + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/offline/localizer/localizer.java b/offline/localizer/localizer.java new file mode 100644 index 00000000..c696bba7 --- /dev/null +++ b/offline/localizer/localizer.java @@ -0,0 +1,355 @@ +package com.roboclub.robobuggy.nodes.localizers; + +import Jama.Matrix; +import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.messages.SteeringMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.ros.Message; +import com.roboclub.robobuggy.ros.MessageListener; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +import java.util.Date; + + +/** + * localizes using a kalman filter + */ +public class KfLocalizer extends PeriodicNode { + private Publisher posePub; + private Matrix state; + private double lastEncoderReading; + private long lastEncoderReadingTime; + private Matrix covariance; + private UTMTuple lastGPS; + private Matrix motionMatrix; + private Date mostRecentUpdateTime; + private Matrix predictCovariance; + private double wheelBase; + + /** + * Constructor for the kfLocalizer, setup and starts the kf running at a + * fixed period + * + * @param period the period in milliseconds between pose outputs of the kf + */ + public KfLocalizer(int period) { + super(new BuggyBaseNode(NodeChannel.POSE), period, "KF"); + posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + wheelBase = 1.13; // meters + + double[][] startCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_b + { 0, 0, 0, 1, 0, 0, 0 }, // y_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + covariance = new Matrix(startCovariance); + // state [x,y,x_b_dot,y_b_dot,th,th_dot,gamma] + LocTuple startLatLng = new LocTuple(40.441670, -79.9416362); + UTMTuple startUTM = LocalizerUtil.deg2UTM(startLatLng); + lastGPS = startUTM; + lastEncoderReadingTime = new Date().getTime(); + mostRecentUpdateTime = new Date(); + double startAngle = 250; + + double[][] start = { { startUTM.getEasting() }, // X meters 0 + { startUTM.getNorthing() }, // Y meters 1 + { 0 }, // x_b_dot 2 + { 0 }, // y_b_dot 3 + { startAngle }, // th degree 4 + { 0 }, // th_dot degrees/second 5 + { 0 } // heading degree 6 + }; + state = new Matrix(start); + + double[][] predictCovarianceArray = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_b + { 0, 0, 0, 1, 0, 0, 0 }, // y_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + predictCovariance = new Matrix(predictCovarianceArray); + + // Initialize subscriber to GPS measurements + new Subscriber("htGpsLoc", NodeChannel.GPS.getMsgPath(), + new MessageListener() { + @Override + public synchronized void actionPerformed(String topicName, Message m) { + GpsMeasurement newGPSData = (GpsMeasurement) m; + LocTuple gpsLatLng = new LocTuple(newGPSData + .getLatitude(), newGPSData.getLongitude()); + UTMTuple gpsUTM = LocalizerUtil.deg2UTM(gpsLatLng); + double dx = gpsUTM.getEasting() - lastGPS.getEasting(); + double dy = gpsUTM.getNorthing() - lastGPS.getNorthing(); + double th = Math.toDegrees(Math.atan2(dy, dx)); + lastGPS = gpsUTM; + + double[][] observationModel = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 0 } // Heading + }; + + // don't update angle if we did not move a lot + if (Math.sqrt(dx * dx + dy * dy) < .5) {// || + // Math.abs(th- + // state.get(4, + // 0)) > 90){ + observationModel[4][4] = 0; + } + + if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) + + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { + th = startAngle; + } + + double[][] meassurement = { { gpsUTM.getEasting() }, + { gpsUTM.getNorthing() }, { 0 }, { 0 }, { th }, + { 0 }, { 0 } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), new Matrix( + meassurement), new Matrix(updateCovariance)); + } + }); + + /* + new Subscriber( + "HighTrustGpsLoc", + NodeChannel.IMU_ANG_POS.getMsgPath(), + ((topicName, m) -> { + + IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); + // double y = mes.getRot()[0][1]; + // double x = mes.getRot()[0][0]; + double[][] xVar = { { 1 }, { 0 }, { 0 } }; + double[][] yVar = { { 0 }, { 1 }, { 0 } }; + Matrix xMat = new Matrix(xVar); + Matrix yMat = new Matrix(yVar); + Matrix rot = new Matrix(mes.getRot()); + double x = rot.times(xMat).get(0, 0); + double y = rot.times(yMat).get(0, 0); + double th = -(Math.toDegrees(Math.atan2(y, x)) - 90); + double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x + { 0, 0, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 0 } // Heading + }; + + double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, + { th }, { 0 }, { 0 } }; + double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + // updateStep(new Matrix(observationModel),new + // Matrix(meassurement),new Matrix(updateCovariance)); + + })); + */ + + // TODO note that we will probably run into precision errors since the + // changes are so small + // would be good to batch up the encoder updates until we get a margin + // that we know can be represented proeprly + new Subscriber("htGpsLoc", NodeChannel.ENCODER.getMsgPath(), + new MessageListener() { + @Override + public synchronized void actionPerformed(String topicName, + Message m) { + EncoderMeasurement measurement = (EncoderMeasurement) m; + + // convert the feet from the last message into a delta + // degree, and update our position + double currentEncoderMeasurement = measurement + .getDistance(); + double deltaDistance = currentEncoderMeasurement + - lastEncoderReading; + long currentTime = new Date().getTime(); + long dt = currentTime - lastEncoderReadingTime; + if (dt > 1) { // to remove numeric instability + double bodySpeed = deltaDistance / (dt / 1000.0); + lastEncoderReadingTime = currentTime; + lastEncoderReading = currentEncoderMeasurement; + + double[][] observationModel = { + { 0, 0, 0, 0, 0, 0, 0 }, // x + { 0, 0, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 0 } // Heading + }; + + double[][] meassurement = { { 0 }, { 0 }, + { bodySpeed }, { 0 }, { 0 }, { 0 }, { 0 } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), + new Matrix(meassurement), new Matrix( + updateCovariance)); + } + } + }); + + new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), + new MessageListener() { + + @Override + public synchronized void actionPerformed(String topicName, + Message m) { + SteeringMeasurement steerM = (SteeringMeasurement) m; + double[][] observationModel = { + { 0, 0, 0, 0, 0, 0, 0 }, // x + { 0, 0, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // Heading + }; + double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, + { 0 }, { 0 }, { steerM.getAngle() } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), new Matrix( + meassurement), new Matrix(updateCovariance)); + } + }); + resume(); + + } + + private synchronized void updateStep(Matrix observationMatrix, + Matrix measurement, Matrix updateCovariance) { + predictStep(); + + // Matrix update = observationMatrix.times(state); + Matrix inovation = measurement.minus(observationMatrix.times(state)); + + for (int i = 4; i < 7; i++) { + if (inovation.get(i, 0) > 180) { + inovation.set(i, 0, -360 + inovation.get(i, 0)); + } else if (inovation.get(i, 0) < -180) { + inovation.set(i, 0, 360 + inovation.get(i, 0)); + } + } + + // Matrix innovation2 = + // observationMatrix.minus(measurement.times(state)); + // Matrix inovation = (innovation.normF() > innovation2.normF()) ? + // innovation2 : innovation; + Matrix innovationCovariance = observationMatrix.times(covariance) + .times(observationMatrix.transpose()).plus(updateCovariance); + Matrix kalmanGain = covariance.times(observationMatrix.transpose()) + .times(innovationCovariance.inverse()); + state = state.plus(kalmanGain.times(inovation)); + covariance = (Matrix.identity(covariance.getRowDimension(), + covariance.getColumnDimension()).minus(kalmanGain + .times(observationMatrix))); + + } + + @Override + protected synchronized void update() { + predictStep(); + // publish state + UTMTuple currentLatLng = new UTMTuple(17, 'T', state.get(0, 0), + state.get(1, 0)); + LocTuple latLng = LocalizerUtil.utm2Deg(currentLatLng); + posePub.publish(new GPSPoseMessage(new Date(), latLng.getLatitude(), + latLng.getLongitude(), state.get(4, 0))); + } + + @Override + protected boolean startDecoratorNode() { + // TODO Auto-generated method stub + return false; + } + + @Override + protected boolean shutdownDecoratorNode() { + // TODO Auto-generated method stub + return false; + } + + /** + * TODO + */ + // TODO + private synchronized void predictStep() { + Date now = new Date(); + double diff = (now.getTime() - mostRecentUpdateTime.getTime()) / 1000.0; + mostRecentUpdateTime = now; + double th = Math.toRadians(state.get(4, 0)); + double heading = Math.toRadians(state.get(6, 0)); + double[][] motionModel = { + // x y x_b y_b th th_dot heading + { 1, 0, Math.cos(th) * diff, -Math.sin(th) * diff, 0, 0, 0 }, // x + { 0, 1, Math.sin(th) * diff, Math.cos(th) * diff, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_b + { 0, 0, 0, 1, 0, 0, 0 }, // y_b + { 0, 0, 0, 0, 1, diff, 0 }, // th + { 0, 0, 180 / (Math.PI * (wheelBase / Math.sin(heading))), 0, + 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + motionMatrix = new Matrix(motionModel); + state = motionMatrix.times(state); + covariance = predictCovariance.times(covariance).times( + predictCovariance.transpose()); + for (int i = 4; i < 7; i++) { + while (state.get(i, 0) < -180) { + state.set(i, 0, state.get(i, 0) + 360); + } + while (state.get(i, 0) > 180) { + state.set(i, 0, state.get(i, 0) - 360); + } + } + } + +} diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m new file mode 100644 index 00000000..e8529178 --- /dev/null +++ b/offline/localizer/localizer.m @@ -0,0 +1,164 @@ +% localizer code, as seen in java codebase +% Bereket Abraham + +% TODO: change dt?, graph map, compare to raw latlon, fix mistakes + +addpath('./latlonutm/Codes/matlab'); + +% constants +dt = (1/10) * 1000; % 10 Hz in ms +wheel_base = 1.13; +utm_zone = 17; +Q = eye(7); % model noise covariance +R = eye(7); % measurement noise covariance + +% globals +first_theta = 250; +lat_long = [40.441670, -79.9416362]; +[x, y, zone] = ll2utm(lat_long(1), lat_long(2)); +first_gps = [x y]; +last_gps = [x y]; +utm_zone = zone; % fixed +last_encoder = 0; +last_encoder_time = 0; + +function llog = log_line(x) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + llog = [lat, lon, x(5)]'; +end + +function x = scrubAngles(x) + for i = 5:7 + while (x(i) < -180) + x(i) = x(i) + 360; + end + while (x(i) > 180) + x(i) = x(i) - 360; + end + end +end + +function A = model(x) + % dynamic model + theta = deg2rad(x(3)); + heading = deg2rad(x(7)); + A = [1, 0, dt*cos(theta), -dt*sin(theta), 0, 0, 0; + 0, 1, dt*sin(theta), dt*cos(theta), 0, 0, 0; + 0, 0, 1, 0, 0, 0, 0; + 0, 0, 0, 1, 0, 0, 0; + 0, 0, 0, 0, 1, dt, 0; + 0, 0, 180/(pi*(wheel_base/sin(heading))), 0, 0, 0, 0; + 0, 0, 0, 0, 0, 0, 1]; +end + +function [P_pre, x_pre] = predict_step(P, x) + A = model(x); + x_pre = A * x; + P_pre = eye(7) * P * eye(7)'; % !error! P_pre = A * P * A' + R; + x_pre = scrubAngles(x_pre); +end + +function [P, x] = update_step(C, z, P_pre, x_pre) + residual = z - (C * x_pre); + residual = scrubAngles(residual); + K = P_pre * C' * inv((C * P_pre * C') + Q); % gain + x = x_pre + (K * residual); + P = (eye(7) - (K * C)); % !error! P = (eye(7) - (K * C)) * P_pre; +end + +function [C, z] = gps_meaurement(lat, long) + [x, y, ~] = ll2utm(lat, long); + dx = x - last_gps(1); + dy = y - last_gps(2); + last_gps = [x y]; + theta = rad2deg(atan2(dy, dx)); + + C = zeros(7, 7); + C(1, 1) = 1; + C(2, 2) = 1; + C(5, 5) = 1; + % ignore small strides + if (norm([dx dy]) < 0.5) + C(5, 5) = 0; + end + % close the loop + d = abs(x - first_gps(1)) + abs(y - first_gps(2)); + if (d < 10) + theta = first_theta; + end + + z = [x, y, 0, 0, theta, 0 , 0]'; +end + +% TODO possible precision errors since changes are so small +% maybe batch encoder updates until we get a decent margin +function [C, z] = encoder_meaurement(encoder_dist, encoder_time) + dx = encoder_dist - last_encoder; + dt = encoder_time - last_encoder_time; + C = zeros(7, 7); + z = zeros(7, 1); + + if (dt <= 1) + return; + end + + last_encoder = encoder_dist; + last_encoder_time = encoder_time; + C(3, 3) = 1; + z(3) = dx / (dt / 1000); +end + +function [C, z] = steering_measurement(heading) + C = zeros(7, 7); + C(7, 7) = 1; + z = zeros(7, 1); + z(7) = heading; +end + +function [C, z] = imu_measurement(R) + x = R(1, 1); + x = R(1, 2); + theta = 90 - rad2deg(atan2(y, x)); + + C = zeros(7, 7); + C(5, 5) = 1; + z = zeros(7, 1); + z(5) = theta; +end + + +% initialize Kalman filter +P = eye(7); % covariance + +X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + 0; % d_Xb, body velocity + 0; % d_Yb + first_theta; % theta, deg, global + 0; % d_theta, deg/s + 0]; % heading, deg, body orientation + +trajectory = [X; norm(P)]; + +load('./roll_logs.mat'); + +for i = 1:size(logs, 2) + llog = logs(:, i); + if llog(1) == 'sensors/gps' + time = llog(2); + [C, z] = gps_meaurement(llog(3), llog(4)); + elseif llog(1) == 'sensors/encoder' + time = llog(2); + [C, z] = encoder_meaurement(llog(3), time) + elseif llog(1) == 'sensors/steering' + time = llog(2); + [C, z] = steering_measurement(llog(3)) + end + + [P_pre, X_pre] = predict_step(P, X); + [P, X] = update_step(C, z, P_pre, X_pre) + trajectory = [trajectory, [X; norm(P)]]; +end + +% graph results + From 72c0a1006de7780cb6c37431501d96b432321c88 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 16 Oct 2016 18:22:17 -0400 Subject: [PATCH 002/149] added matlab lib to plot map --- .../zoharby-plot_google_map/.gitattributes | 22 + .../zoharby-plot_google_map/.gitignore | 215 ++++++ .../zoharby-plot_google_map/README.md | 26 + .../zoharby-plot_google_map/license.txt | 24 + .../zoharby-plot_google_map/plot_google_map.m | 647 ++++++++++++++++++ 5 files changed, 934 insertions(+) create mode 100755 offline/localizer/zoharby-plot_google_map/.gitattributes create mode 100755 offline/localizer/zoharby-plot_google_map/.gitignore create mode 100755 offline/localizer/zoharby-plot_google_map/README.md create mode 100755 offline/localizer/zoharby-plot_google_map/license.txt create mode 100755 offline/localizer/zoharby-plot_google_map/plot_google_map.m diff --git a/offline/localizer/zoharby-plot_google_map/.gitattributes b/offline/localizer/zoharby-plot_google_map/.gitattributes new file mode 100755 index 00000000..412eeda7 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/.gitattributes @@ -0,0 +1,22 @@ +# Auto detect text files and perform LF normalization +* text=auto + +# Custom for Visual Studio +*.cs diff=csharp +*.sln merge=union +*.csproj merge=union +*.vbproj merge=union +*.fsproj merge=union +*.dbproj merge=union + +# Standard to msysgit +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain diff --git a/offline/localizer/zoharby-plot_google_map/.gitignore b/offline/localizer/zoharby-plot_google_map/.gitignore new file mode 100755 index 00000000..b9d6bd92 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/.gitignore @@ -0,0 +1,215 @@ +################# +## Eclipse +################# + +*.pydevproject +.project +.metadata +bin/ +tmp/ +*.tmp +*.bak +*.swp +*~.nib +local.properties +.classpath +.settings/ +.loadpath + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + + +################# +## Visual Studio +################# + +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. + +# User-specific files +*.suo +*.user +*.sln.docstates + +# Build results + +[Dd]ebug/ +[Rr]elease/ +x64/ +build/ +[Bb]in/ +[Oo]bj/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +*_i.c +*_p.c +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*.log +*.vspscc +*.vssscc +.builds +*.pidb +*.log +*.scc + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf +*.cachefile + +# Visual Studio profiler +*.psess +*.vsp +*.vspx + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# NCrunch +*.ncrunch* +.*crunch*.local.xml + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.Publish.xml +*.pubxml + +# NuGet Packages Directory +## TODO: If you have NuGet Package Restore enabled, uncomment the next line +#packages/ + +# Windows Azure Build Output +csx +*.build.csdef + +# Windows Store app package directory +AppPackages/ + +# Others +sql/ +*.Cache +ClientBin/ +[Ss]tyle[Cc]op.* +~$* +*~ +*.dbmdl +*.[Pp]ublish.xml +*.pfx +*.publishsettings + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file to a newer +# Visual Studio version. Backup files are not needed, because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm + +# SQL Server files +App_Data/*.mdf +App_Data/*.ldf + +############# +## Windows detritus +############# + +# Windows image file caches +Thumbs.db +ehthumbs.db + +# Folder config file +Desktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Mac crap +.DS_Store + + +############# +## Python +############# + +*.py[co] + +# Packages +*.egg +*.egg-info +dist/ +build/ +eggs/ +parts/ +var/ +sdist/ +develop-eggs/ +.installed.cfg + +# Installer logs +pip-log.txt + +# Unit test / coverage reports +.coverage +.tox + +#Translations +*.mo + +#Mr Developer +.mr.developer.cfg diff --git a/offline/localizer/zoharby-plot_google_map/README.md b/offline/localizer/zoharby-plot_google_map/README.md new file mode 100755 index 00000000..8c2a5e16 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/README.md @@ -0,0 +1,26 @@ +# plot_google_map +#### MATLAB function for plotting a Google map on the background of a figure + +plot_google_map.m uses the Google Maps API to plot a map in the background of the current figure. +It assumes the coordinates of the current figure are in the WGS84 datum, and uses a conversion code to convert and project the image from the coordinate system used by Google into WGS84 coordinates. +The zoom level of the map is automatically determined to cover the entire area of the figure. Additionally, it has the option to auto-refresh the map upon zooming in the figure, revealing more details as one zooms in. +The following code produces the screenshot: + +``` +lat = [48.8708 51.5188 41.9260 40.4312 52.523 37.982]; +lon = [2.4131 -0.1300 12.4951 -3.6788 13.415 23.715]; +plot(lon,lat,'.r','MarkerSize',20) +plot_google_map +``` + +Known Issues: + 1. The static maps API is limited to 1000 requests per day when used with no API key. If you use this function a lot, you can obtain an API key and easily set the function to use it (see help for more details) + + 2. Saving the map with an image/matrix overlay drawn on top of it (especially a semi-transparent one) can sometimes cause unexpected results (map not showing etc.). If you're encountering such problems, it's recommended to use the export_fig submission: + http://www.mathworks.com/matlabcentral/fileexchange/23629-exportfig + The combination that seems to work best: + ``` + set(gcf,'renderer','zbuffer') + export_fig('out.jpg') + ``` + diff --git a/offline/localizer/zoharby-plot_google_map/license.txt b/offline/localizer/zoharby-plot_google_map/license.txt new file mode 100755 index 00000000..52dbe68f --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/license.txt @@ -0,0 +1,24 @@ +Copyright (c) 2015, Zohar Bar-Yehuda +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the distribution + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/offline/localizer/zoharby-plot_google_map/plot_google_map.m b/offline/localizer/zoharby-plot_google_map/plot_google_map.m new file mode 100755 index 00000000..645392a6 --- /dev/null +++ b/offline/localizer/zoharby-plot_google_map/plot_google_map.m @@ -0,0 +1,647 @@ +function varargout = plot_google_map(varargin) +% function h = plot_google_map(varargin) +% Plots a google map on the current axes using the Google Static Maps API +% +% USAGE: +% h = plot_google_map(Property, Value,...) +% Plots the map on the given axes. Used also if no output is specified +% +% Or: +% [lonVec latVec imag] = plot_google_map(Property, Value,...) +% Returns the map without plotting it +% +% PROPERTIES: +% Axis - Axis handle. If not given, gca is used. +% Height (640) - Height of the image in pixels (max 640) +% Width (640) - Width of the image in pixels (max 640) +% Scale (2) - (1/2) Resolution scale factor. Using Scale=2 will +% double the resulotion of the downloaded image (up +% to 1280x1280) and will result in finer rendering, +% but processing time will be longer. +% Resize (1) - (recommended 1-2) Resolution upsampling factor. +% Increases image resolution using imresize(). This results +% in a finer image but it needs the image processing +% toolbox and processing time will be longer. +% MapType - ('roadmap') Type of map to return. Any of [roadmap, +% satellite, terrain, hybrid]. See the Google Maps API for +% more information. +% Alpha (1) - (0-1) Transparency level of the map (0 is fully +% transparent). While the map is always moved to the +% bottom of the plot (i.e. will not hide previously +% drawn items), this can be useful in order to increase +% readability if many colors are plotted +% (using SCATTER for example). +% ShowLabels (1) - (0/1) Controls whether to display city/street textual labels on the map +% Style - (string) A style configuration string. See: +% https://developers.google.com/maps/documentation/static-maps/?csw=1#StyledMaps +% http://instrument.github.io/styled-maps-wizard/ +% Language - (string) A 2 letter ISO 639-1 language code for displaying labels in a +% local language instead of English (where available). +% For example, for Chinese use: +% plot_google_map('language','zh') +% For the list of codes, see: +% http://en.wikipedia.org/wiki/List_of_ISO_639-1_codes +% Marker - The marker argument is a text string with fields +% conforming to the Google Maps API. The +% following are valid examples: +% '43.0738740,-70.713993' (default midsize orange marker) +% '43.0738740,-70.713993,blue' (midsize blue marker) +% '43.0738740,-70.713993,yellowa' (midsize yellow +% marker with label "A") +% '43.0738740,-70.713993,tinyredb' (tiny red marker +% with label "B") +% Refresh (1) - (0/1) defines whether to automatically refresh the +% map upon zoom/pan action on the figure. +% AutoAxis (1) - (0/1) defines whether to automatically adjust the axis +% of the plot to avoid the map being stretched. +% This will adjust the span to be correct +% according to the shape of the map axes. +% FigureResizeUpdate (1) - (0/1) defines whether to automatically refresh the +% map upon resizing the figure. This will ensure map +% isn't stretched after figure resize. +% APIKey - (string) set your own API key which you obtained from Google: +% http://developers.google.com/maps/documentation/staticmaps/#api_key +% This will enable up to 25,000 map requests per day, +% compared to a few hundred requests without a key. +% To set the key, use: +% plot_google_map('APIKey','SomeLongStringObtaindFromGoogle') +% You need to do this only once to set the key. +% To disable the use of a key, use: +% plot_google_map('APIKey','') +% +% OUTPUT: +% h - Handle to the plotted map +% +% lonVect - Vector of Longidute coordinates (WGS84) of the image +% latVect - Vector of Latidute coordinates (WGS84) of the image +% imag - Image matrix (height,width,3) of the map +% +% EXAMPLE - plot a map showing some capitals in Europe: +% lat = [48.8708 51.5188 41.9260 40.4312 52.523 37.982]; +% lon = [2.4131 -0.1300 12.4951 -3.6788 13.415 23.715]; +% plot(lon,lat,'.r','MarkerSize',20) +% plot_google_map +% +% References: +% http://www.mathworks.com/matlabcentral/fileexchange/24113 +% http://www.maptiler.org/google-maps-coordinates-tile-bounds-projection/ +% http://developers.google.com/maps/documentation/staticmaps/ +% +% Acknowledgements: +% Val Schmidt for his submission of get_google_map.m +% +% Author: +% Zohar Bar-Yehuda +% +% Version 1.8 - 25/04/2016 - By Hannes Diethelm +% - Add resize parameter to resize image using imresize() +% - Fix scale parameter +% Version 1.7 - 14/04/2016 +% - Add custom style support +% Version 1.6 - 12/11/2015 +% - Use system temp folder for writing image files (with fallback to current dir if missing write permissions) +% Version 1.5 - 20/11/2014 +% - Support for MATLAB R2014b +% - several fixes for complex layouts: several maps in one figure, +% map inside a panel, specifying axis handle as input (thanks to Luke Plausin) +% Version 1.4 - 25/03/2014 +% - Added the language parameter for showing labels in a local language +% - Display the URL on error to allow easier debugging of API errors +% Version 1.3 - 06/10/2013 +% - Improved functionality of AutoAxis, which now handles any shape of map axes. +% Now also updates the extent of the map if the figure is resized. +% - Added the showLabels parameter which allows hiding the textual labels on the map. +% Version 1.2 - 16/06/2012 +% - Support use of the "scale=2" parameter by default for finer rendering (set scale=1 if too slow). +% - Auto-adjust axis extent so the map isn't stretched. +% - Set and use an API key which enables a much higher usage volume per day. +% Version 1.1 - 25/08/2011 + +persistent apiKey useTemp +if isnumeric(apiKey) + % first run, check if API key file exists + if exist('api_key.mat','file') + load api_key + else + apiKey = ''; + end +end + +if isempty(useTemp) + % first run, check we we have wrtie access to the temp folder + try + tempfilename = tempname; + fid = fopen(tempfilename, 'w'); + if fid > 0 + fclose(fid); + useTemp = true; + delete(tempfilename); + else + % Don't have write access to temp folder or it doesn't exist, fallback to current dir + useTemp = false; + end + catch + % in case tempname fails for some reason + useTemp = false; + end +end + +hold on + +% Default parametrs +axHandle = gca; +height = 640; +width = 640; +scale = 2; +resize = 1; +maptype = 'roadmap'; +alphaData = 1; +autoRefresh = 1; +figureResizeUpdate = 1; +autoAxis = 1; +showLabels = 1; +language = ''; +markeridx = 1; +markerlist = {}; +style = ''; + +% Handle input arguments +if nargin >= 2 + for idx = 1:2:length(varargin) + switch lower(varargin{idx}) + case 'axis' + axHandle = varargin{idx+1}; + case 'height' + height = varargin{idx+1}; + case 'width' + width = varargin{idx+1}; + case 'scale' + scale = round(varargin{idx+1}); + if scale < 1 || scale > 2 + error('Scale must be 1 or 2'); + end + case 'resize' + resize = varargin{idx+1}; + case 'maptype' + maptype = varargin{idx+1}; + case 'alpha' + alphaData = varargin{idx+1}; + case 'refresh' + autoRefresh = varargin{idx+1}; + case 'showlabels' + showLabels = varargin{idx+1}; + case 'figureresizeupdate' + figureResizeUpdate = varargin{idx+1}; + case 'language' + language = varargin{idx+1}; + case 'marker' + markerlist{markeridx} = varargin{idx+1}; + markeridx = markeridx + 1; + case 'autoaxis' + autoAxis = varargin{idx+1}; + case 'apikey' + apiKey = varargin{idx+1}; % set new key + % save key to file + funcFile = which('plot_google_map.m'); + pth = fileparts(funcFile); + keyFile = fullfile(pth,'api_key.mat'); + save(keyFile,'apiKey') + case 'style' + style = varargin{idx+1}; + otherwise + error(['Unrecognized variable: ' varargin{idx}]) + end + end +end +if height > 640 + height = 640; +end +if width > 640 + width = 640; +end + +% Store paramters in axis handle (for auto refresh callbacks) +ud = get(axHandle, 'UserData'); +if isempty(ud) + % explicitly set as struct to avoid warnings + ud = struct; +end +ud.gmap_params = varargin; +set(axHandle, 'UserData', ud); + +curAxis = axis(axHandle); +if max(abs(curAxis)) > 500 + return; +end + +% Enforce Latitude constraints of EPSG:900913 +if curAxis(3) < -85 + curAxis(3) = -85; +end +if curAxis(4) > 85 + curAxis(4) = 85; +end +% Enforce longitude constrains +if curAxis(1) < -180 + curAxis(1) = -180; +end +if curAxis(1) > 180 + curAxis(1) = 0; +end +if curAxis(2) > 180 + curAxis(2) = 180; +end +if curAxis(2) < -180 + curAxis(2) = 0; +end + +if isequal(curAxis,[0 1 0 1]) % probably an empty figure + % display world map + curAxis = [-200 200 -85 85]; + axis(curAxis) +end + + +if autoAxis + % adjust current axis limit to avoid strectched maps + [xExtent,yExtent] = latLonToMeters(curAxis(3:4), curAxis(1:2) ); + xExtent = diff(xExtent); % just the size of the span + yExtent = diff(yExtent); + % get axes aspect ratio + drawnow + org_units = get(axHandle,'Units'); + set(axHandle,'Units','Pixels') + ax_position = get(axHandle,'position'); + set(axHandle,'Units',org_units) + aspect_ratio = ax_position(4) / ax_position(3); + + if xExtent*aspect_ratio > yExtent + centerX = mean(curAxis(1:2)); + centerY = mean(curAxis(3:4)); + spanX = (curAxis(2)-curAxis(1))/2; + spanY = (curAxis(4)-curAxis(3))/2; + + % enlarge the Y extent + spanY = spanY*xExtent*aspect_ratio/yExtent; % new span + if spanY > 85 + spanX = spanX * 85 / spanY; + spanY = spanY * 85 / spanY; + end + curAxis(1) = centerX-spanX; + curAxis(2) = centerX+spanX; + curAxis(3) = centerY-spanY; + curAxis(4) = centerY+spanY; + elseif yExtent > xExtent*aspect_ratio + + centerX = mean(curAxis(1:2)); + centerY = mean(curAxis(3:4)); + spanX = (curAxis(2)-curAxis(1))/2; + spanY = (curAxis(4)-curAxis(3))/2; + % enlarge the X extent + spanX = spanX*yExtent/(xExtent*aspect_ratio); % new span + if spanX > 180 + spanY = spanY * 180 / spanX; + spanX = spanX * 180 / spanX; + end + + curAxis(1) = centerX-spanX; + curAxis(2) = centerX+spanX; + curAxis(3) = centerY-spanY; + curAxis(4) = centerY+spanY; + end + % Enforce Latitude constraints of EPSG:900913 + if curAxis(3) < -85 + curAxis(3:4) = curAxis(3:4) + (-85 - curAxis(3)); + end + if curAxis(4) > 85 + curAxis(3:4) = curAxis(3:4) + (85 - curAxis(4)); + end + axis(axHandle, curAxis); % update axis as quickly as possible, before downloading new image + drawnow +end + +% Delete previous map from plot (if exists) +if nargout <= 1 % only if in plotting mode + curChildren = get(axHandle,'children'); + map_objs = findobj(curChildren,'tag','gmap'); + bd_callback = []; + for idx = 1:length(map_objs) + if ~isempty(get(map_objs(idx),'ButtonDownFcn')) + % copy callback properties from current map + bd_callback = get(map_objs(idx),'ButtonDownFcn'); + end + end + delete(map_objs) + +end + +% Calculate zoom level for current axis limits +[xExtent,yExtent] = latLonToMeters(curAxis(3:4), curAxis(1:2) ); +minResX = diff(xExtent) / width; +minResY = diff(yExtent) / height; +minRes = max([minResX minResY]); +tileSize = 256; +initialResolution = 2 * pi * 6378137 / tileSize; % 156543.03392804062 for tileSize 256 pixels +zoomlevel = floor(log2(initialResolution/minRes)); + +% Enforce valid zoom levels +if zoomlevel < 0 + zoomlevel = 0; +end +if zoomlevel > 19 + zoomlevel = 19; +end + +% Calculate center coordinate in WGS1984 +lat = (curAxis(3)+curAxis(4))/2; +lon = (curAxis(1)+curAxis(2))/2; + +% Construct query URL +preamble = 'http://maps.googleapis.com/maps/api/staticmap'; +location = ['?center=' num2str(lat,10) ',' num2str(lon,10)]; +zoomStr = ['&zoom=' num2str(zoomlevel)]; +sizeStr = ['&scale=' num2str(scale) '&size=' num2str(width) 'x' num2str(height)]; +maptypeStr = ['&maptype=' maptype ]; +if ~isempty(apiKey) + keyStr = ['&key=' apiKey]; +else + keyStr = ''; +end +markers = '&markers='; +for idx = 1:length(markerlist) + if idx < length(markerlist) + markers = [markers markerlist{idx} '%7C']; + else + markers = [markers markerlist{idx}]; + end +end + +if showLabels == 0 + if ~isempty(style) + style(end+1) = '|'; + end + style = [style 'feature:all|element:labels|visibility:off']; +end + +if ~isempty(language) + languageStr = ['&language=' language]; +else + languageStr = ''; +end + +if ismember(maptype,{'satellite','hybrid'}) + filename = 'tmp.jpg'; + format = '&format=jpg'; + convertNeeded = 0; +else + filename = 'tmp.png'; + format = '&format=png'; + convertNeeded = 1; +end +sensor = '&sensor=false'; + +if ~isempty(style) + styleStr = ['&style=' style]; +else + styleStr = ''; +end + +url = [preamble location zoomStr sizeStr maptypeStr format markers languageStr sensor keyStr styleStr]; + +% Get the image +if useTemp + filepath = fullfile(tempdir, filename); +else + filepath = filename; +end + +try + urlwrite(url,filepath); +catch % error downloading map + warning(['Unable to download map form Google Servers.\n' ... + 'Matlab error was: %s\n\n' ... + 'Possible reasons: missing write permissions, no network connection, quota exceeded, or some other error.\n' ... + 'Consider using an API key if quota problems persist.\n\n' ... + 'To debug, try pasting the following URL in your browser, which may result in a more informative error:\n%s'], lasterr, url); + varargout{1} = []; + varargout{2} = []; + varargout{3} = []; + return +end +[M Mcolor] = imread(filepath); +M = cast(M,'double'); +delete(filepath); % delete temp file +width = size(M,2); +height = size(M,1); + +% We now want to convert the image from a colormap image with an uneven +% mesh grid, into an RGB truecolor image with a uniform grid. +% This would enable displaying it with IMAGE, instead of PCOLOR. +% Advantages are: +% 1) faster rendering +% 2) makes it possible to display together with other colormap annotations (PCOLOR, SCATTER etc.) + +% Convert image from colormap type to RGB truecolor (if PNG is used) +if convertNeeded + imag = zeros(height,width,3); + for idx = 1:3 + imag(:,:,idx) = reshape(Mcolor(M(:)+1+(idx-1)*size(Mcolor,1)),height,width); + end +else + imag = M/255; +end +% Resize if needed +if resize ~= 1 + imag = imresize(imag, resize, 'bilinear'); +end + +% Calculate a meshgrid of pixel coordinates in EPSG:900913 +width = size(imag,2); +height = size(imag,1); +centerPixelY = round(height/2); +centerPixelX = round(width/2); +[centerX,centerY] = latLonToMeters(lat, lon ); % center coordinates in EPSG:900913 +curResolution = initialResolution / 2^zoomlevel / scale / resize; % meters/pixel (EPSG:900913) +xVec = centerX + ((1:width)-centerPixelX) * curResolution; % x vector +yVec = centerY + ((height:-1:1)-centerPixelY) * curResolution; % y vector +[xMesh,yMesh] = meshgrid(xVec,yVec); % construct meshgrid + +% convert meshgrid to WGS1984 +[lonMesh,latMesh] = metersToLatLon(xMesh,yMesh); + +% Next, project the data into a uniform WGS1984 grid +uniHeight = round(height*resize); +uniWidth = round(width*resize); +latVect = linspace(latMesh(1,1),latMesh(end,1),uniHeight); +lonVect = linspace(lonMesh(1,1),lonMesh(1,end),uniWidth); +[uniLonMesh,uniLatMesh] = meshgrid(lonVect,latVect); +uniImag = zeros(uniHeight,uniWidth,3); + +% old version (projection using INTERP2) +% for idx = 1:3 +% % 'nearest' method is the fastest. difference from other methods is neglible +% uniImag(:,:,idx) = interp2(lonMesh,latMesh,imag(:,:,idx),uniLonMesh,uniLatMesh,'nearest'); +% end +uniImag = myTurboInterp2(lonMesh,latMesh,imag,uniLonMesh,uniLatMesh); + +if nargout <= 1 % plot map + % display image + hold(axHandle, 'on'); + cax = caxis; + h = image(lonVect,latVect,uniImag, 'Parent', axHandle); + caxis(cax); % Preserve caxis that is sometimes changed by the call to image() + set(axHandle,'YDir','Normal') + set(h,'tag','gmap') + set(h,'AlphaData',alphaData) + + % add a dummy image to allow pan/zoom out to x2 of the image extent + h_tmp = image(lonVect([1 end]),latVect([1 end]),zeros(2),'Visible','off', 'Parent', axHandle); + set(h_tmp,'tag','gmap') + + % older version (display without conversion to uniform grid) + % h =pcolor(lonMesh,latMesh,(M)); + % colormap(Mcolor) + % caxis([0 255]) + % warning off % to avoid strange rendering warnings + % shading flat + + uistack(h,'bottom') % move map to bottom (so it doesn't hide previously drawn annotations) + axis(axHandle, curAxis) % restore original zoom + if nargout == 1 + varargout{1} = h; + end + + % if auto-refresh mode - override zoom callback to allow autumatic + % refresh of map upon zoom actions. + figHandle = axHandle; + while ~strcmpi(get(figHandle, 'Type'), 'figure') + % Recursively search for parent figure in case axes are in a panel + figHandle = get(figHandle, 'Parent'); + end + + zoomHandle = zoom(axHandle); + panHandle = pan(figHandle); % This isn't ideal, doesn't work for contained axis + if autoRefresh + set(zoomHandle,'ActionPostCallback',@update_google_map); + set(panHandle, 'ActionPostCallback', @update_google_map); + else % disable zoom override + set(zoomHandle,'ActionPostCallback',[]); + set(panHandle, 'ActionPostCallback',[]); + end + + % set callback for figure resize function, to update extents if figure + % is streched. + if figureResizeUpdate &&isempty(get(figHandle, 'ResizeFcn')) + % set only if not already set by someone else + set(figHandle, 'ResizeFcn', @update_google_map_fig); + end + + % set callback properties + set(h,'ButtonDownFcn',bd_callback); +else % don't plot, only return map + varargout{1} = lonVect; + varargout{2} = latVect; + varargout{3} = uniImag; +end + + +% Coordinate transformation functions + +function [lon,lat] = metersToLatLon(x,y) +% Converts XY point from Spherical Mercator EPSG:900913 to lat/lon in WGS84 Datum +originShift = 2 * pi * 6378137 / 2.0; % 20037508.342789244 +lon = (x ./ originShift) * 180; +lat = (y ./ originShift) * 180; +lat = 180 / pi * (2 * atan( exp( lat * pi / 180)) - pi / 2); + +function [x,y] = latLonToMeters(lat, lon ) +% Converts given lat/lon in WGS84 Datum to XY in Spherical Mercator EPSG:900913" +originShift = 2 * pi * 6378137 / 2.0; % 20037508.342789244 +x = lon * originShift / 180; +y = log(tan((90 + lat) * pi / 360 )) / (pi / 180); +y = y * originShift / 180; + + +function ZI = myTurboInterp2(X,Y,Z,XI,YI) +% An extremely fast nearest neighbour 2D interpolation, assuming both input +% and output grids consist only of squares, meaning: +% - uniform X for each column +% - uniform Y for each row +XI = XI(1,:); +X = X(1,:); +YI = YI(:,1); +Y = Y(:,1); + +xiPos = nan*ones(size(XI)); +xLen = length(X); +yiPos = nan*ones(size(YI)); +yLen = length(Y); +% find x conversion +xPos = 1; +for idx = 1:length(xiPos) + if XI(idx) >= X(1) && XI(idx) <= X(end) + while xPos < xLen && X(xPos+1)= Y(end) + while yPos < yLen && Y(yPos+1)>YI(idx) + yPos = yPos + 1; + end + diffs = abs(Y(yPos:yPos+1)-YI(idx)); + if diffs(1) < diffs(2) + yiPos(idx) = yPos; + else + yiPos(idx) = yPos + 1; + end + end +end +ZI = Z(yiPos,xiPos,:); + + +function update_google_map(obj,evd) +% callback function for auto-refresh +drawnow; +try + axHandle = evd.Axes; +catch ex + % Event doesn't contain the correct axes. Panic! + axHandle = gca; +end +ud = get(axHandle, 'UserData'); +if isfield(ud, 'gmap_params') + params = ud.gmap_params; + plot_google_map(params{:}); +end + + +function update_google_map_fig(obj,evd) +% callback function for auto-refresh +drawnow; +axes_objs = findobj(get(gcf,'children'),'type','axes'); +for idx = 1:length(axes_objs) + if ~isempty(findobj(get(axes_objs(idx),'children'),'tag','gmap')); + ud = get(axes_objs(idx), 'UserData'); + if isfield(ud, 'gmap_params') + params = ud.gmap_params; + else + params = {}; + end + + % Add axes to inputs if needed + if ~sum(strcmpi(params, 'Axis')) + params = [params, {'Axis', axes_objs(idx)}]; + end + plot_google_map(params{:}); + end +end From bb4aaf13d43df3feb87a0af4c11c9b5272884d76 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Wed, 19 Oct 2016 01:58:04 -0400 Subject: [PATCH 003/149] new localizer code --- offline/localizer/get_logs.py | 95 ++++++++++++++++++++++++++++++++--- offline/localizer/localizer.m | 6 +++ 2 files changed, 95 insertions(+), 6 deletions(-) diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py index b21f26f2..336f868c 100644 --- a/offline/localizer/get_logs.py +++ b/offline/localizer/get_logs.py @@ -4,8 +4,9 @@ import numpy as np import scipy.io +combined = True d = None -file = '../rolls_logs/2016-10-09-06-17-15/sensors_2016-10-09-06-17-15.txt' +file = '../../../rolls_logs/2016-10-09-06-17-15/sensors_2016-10-09-06-17-15.txt' # file = '../rolls_logs/sample.txt' with open(file) as json_data: d = json.load(json_data) @@ -15,10 +16,92 @@ exit() messages = d['sensor_data'] -for msg in messages: - if msg['VERSION_ID'] == '': - print(msg['VERSION_ID']) +map_names = ['imu', 'imu_ang_pos', 'imu_ang_vel', + 'gps', 'encoder', 'steering'] +map_names = np.asarray(map_names, dtype='object') +if combined: + logs = [] + for msg in messages: + name = msg['topicName'] + name = name[8:] + ind = map_names.index(name) -arr = np.array([1, 2, 3]) -scipy.io.savemat('./roll_logs.mat', mdict={'logs': arr}) + if name == 'imu_linear_no_grav': + logs.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z'], 0, 0, 0, 0, 0, 0]) + elif name == 'imu_ang_pos': + line = [ind, msg['timestamp']] + line.extend(msg['rot']) + logs.append(line) + elif name == 'imu_ang_vel': + logs.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z'], 0, 0, 0, 0, 0, 0]) + elif name == 'gps': + logs.append([ + ind, msg['timestamp'], + msg['latitude'], msg['longitude']], + msg['antennaAltitude', 0, 0, 0, 0, 0, 0]) + elif name == 'encoder': + logs.append([ + ind, msg['timestamp'], + msg['distance'], msg['velocity'], 0, 0, 0, 0, 0, 0, 0]) + elif name == 'steering': + logs.append([ + ind, msg['timestamp'], + msg['angle'], 0, 0, 0, 0, 0, 0, 0, 0]) + + logs = np.array(logs) + vars_map = {'map_names': map_names, 'logs': logs} + scipy.io.savemat('./roll_logs_combined.mat', mdict=vars_map) + +else: + imu = [] + imu_ang_pos = [] + imu_ang_vel = [] + gps = [] + encoder = [] + steering = [] + + for msg in messages: + name = msg['topicName'] + name = name[8:] + ind = map_names.index(name) + + if name == 'imu_linear_no_grav': + imu.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z']]) + elif name == 'imu_ang_pos': + line = [ind, msg['timestamp']] + line.extend(msg['rot']) + imu_ang_pos.append(line) + elif name == 'imu_ang_vel': + imu_ang_vel.append([ + ind, msg['timestamp'], + msg['x'], msg['y'], msg['z']]) + elif name == 'gps': + gps.append([ + ind, msg['timestamp'], + msg['latitude'], msg['longitude']], + msg['antennaAltitude']) + elif name == 'encoder': + encoder.append([ + ind, msg['timestamp'], + msg['distance'], msg['velocity']]) + elif name == 'steering': + steering.append([ + ind, msg['timestamp'], + msg['angle']]) + + imu = np.array(imu) + gps = np.array(gps) + encoder = np.array(encoder) + steering = np.array(steering) + + vars_map = {'map_names': map_names, 'imu': imu, + 'imu_ang_pos': imu_ang_pos, 'imu_ang_vel': imu_ang_vel, + 'gps': gps, 'encoder': encoder, 'steering': steering} + scipy.io.savemat('./roll_logs_separate.mat', mdict=vars_map) diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m index e8529178..20409451 100644 --- a/offline/localizer/localizer.m +++ b/offline/localizer/localizer.m @@ -4,6 +4,7 @@ % TODO: change dt?, graph map, compare to raw latlon, fix mistakes addpath('./latlonutm/Codes/matlab'); +addpath('./zoharby-plot_google_map') % constants dt = (1/10) * 1000; % 10 Hz in ms @@ -161,4 +162,9 @@ end % graph results +lat = [48.8708 51.5188 41.9260 40.4312 52.523 37.982]; +lon = [2.4131 -0.1300 12.4951 -3.6788 13.415 23.715]; +plot(lon, lat, '.r', 'MarkerSize', 20) +plot_google_map + From 88a8c3149514c4b18834b5be3159a1d6bc27d2c6 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Thu, 20 Oct 2016 18:56:46 -0400 Subject: [PATCH 004/149] finished localizer code and added analysis script --- offline/localizer/.gitignore | 4 + .../localizer/altmany-export_fig/.gitignore | 5 + .../altmany-export_fig/ImageSelection.java | 38 + offline/localizer/altmany-export_fig/LICENSE | 27 + .../localizer/altmany-export_fig/README.md | 270 ++++ .../altmany-export_fig/append_pdfs.m | 81 ++ .../localizer/altmany-export_fig/copyfig.m | 51 + .../altmany-export_fig/crop_borders.m | 152 ++ .../localizer/altmany-export_fig/eps2pdf.m | 189 +++ .../localizer/altmany-export_fig/export_fig.m | 1282 +++++++++++++++++ .../localizer/altmany-export_fig/fix_lines.m | 151 ++ .../altmany-export_fig/ghostscript.m | 196 +++ offline/localizer/altmany-export_fig/im2gif.m | 186 +++ .../altmany-export_fig/isolate_axes.m | 130 ++ .../localizer/altmany-export_fig/pdf2eps.m | 51 + .../localizer/altmany-export_fig/pdftops.m | 150 ++ .../altmany-export_fig/print2array.m | 244 ++++ .../localizer/altmany-export_fig/print2eps.m | 529 +++++++ .../read_write_entire_textfile.m | 37 + .../altmany-export_fig/user_string.m | 105 ++ .../localizer/altmany-export_fig/using_hg2.m | 36 + offline/localizer/get_logs.py | 27 +- offline/localizer/localizer.m | 158 +- offline/localizer/notes.txt | 52 + offline/localizer/trajectory_analysis.m | 33 + 25 files changed, 4098 insertions(+), 86 deletions(-) create mode 100644 offline/localizer/.gitignore create mode 100755 offline/localizer/altmany-export_fig/.gitignore create mode 100755 offline/localizer/altmany-export_fig/ImageSelection.java create mode 100755 offline/localizer/altmany-export_fig/LICENSE create mode 100755 offline/localizer/altmany-export_fig/README.md create mode 100755 offline/localizer/altmany-export_fig/append_pdfs.m create mode 100755 offline/localizer/altmany-export_fig/copyfig.m create mode 100755 offline/localizer/altmany-export_fig/crop_borders.m create mode 100755 offline/localizer/altmany-export_fig/eps2pdf.m create mode 100755 offline/localizer/altmany-export_fig/export_fig.m create mode 100755 offline/localizer/altmany-export_fig/fix_lines.m create mode 100755 offline/localizer/altmany-export_fig/ghostscript.m create mode 100755 offline/localizer/altmany-export_fig/im2gif.m create mode 100755 offline/localizer/altmany-export_fig/isolate_axes.m create mode 100755 offline/localizer/altmany-export_fig/pdf2eps.m create mode 100755 offline/localizer/altmany-export_fig/pdftops.m create mode 100755 offline/localizer/altmany-export_fig/print2array.m create mode 100755 offline/localizer/altmany-export_fig/print2eps.m create mode 100755 offline/localizer/altmany-export_fig/read_write_entire_textfile.m create mode 100755 offline/localizer/altmany-export_fig/user_string.m create mode 100755 offline/localizer/altmany-export_fig/using_hg2.m create mode 100644 offline/localizer/notes.txt create mode 100644 offline/localizer/trajectory_analysis.m diff --git a/offline/localizer/.gitignore b/offline/localizer/.gitignore new file mode 100644 index 00000000..99a185c2 --- /dev/null +++ b/offline/localizer/.gitignore @@ -0,0 +1,4 @@ +*.mat +*.jpg +*.png + diff --git a/offline/localizer/altmany-export_fig/.gitignore b/offline/localizer/altmany-export_fig/.gitignore new file mode 100755 index 00000000..68c9c928 --- /dev/null +++ b/offline/localizer/altmany-export_fig/.gitignore @@ -0,0 +1,5 @@ +/.ignore +*.txt +*.asv +*~ +*.mex* diff --git a/offline/localizer/altmany-export_fig/ImageSelection.java b/offline/localizer/altmany-export_fig/ImageSelection.java new file mode 100755 index 00000000..7ec32fd3 --- /dev/null +++ b/offline/localizer/altmany-export_fig/ImageSelection.java @@ -0,0 +1,38 @@ +/* + * Based on code snippet from + * http://java.sun.com/developer/technicalArticles/releases/data/ + * + * Copyright © 2008, 2010 Oracle and/or its affiliates. All rights reserved. Use is subject to license terms. + */ + +import java.awt.image.BufferedImage; +import java.awt.datatransfer.*; + +public class ImageSelection implements Transferable { + + private static final DataFlavor flavors[] = + {DataFlavor.imageFlavor}; + + private BufferedImage image; + + public ImageSelection(BufferedImage image) { + this.image = image; + } + + // Transferable + public Object getTransferData(DataFlavor flavor) throws UnsupportedFlavorException { + if (flavor.equals(flavors[0]) == false) { + throw new UnsupportedFlavorException(flavor); + } + return image; + } + + public DataFlavor[] getTransferDataFlavors() { + return flavors; + } + + public boolean isDataFlavorSupported(DataFlavor + flavor) { + return flavor.equals(flavors[0]); + } +} \ No newline at end of file diff --git a/offline/localizer/altmany-export_fig/LICENSE b/offline/localizer/altmany-export_fig/LICENSE new file mode 100755 index 00000000..1a058308 --- /dev/null +++ b/offline/localizer/altmany-export_fig/LICENSE @@ -0,0 +1,27 @@ +Copyright (c) 2014, Oliver J. Woodford, Yair M. Altman +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the {organization} nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/offline/localizer/altmany-export_fig/README.md b/offline/localizer/altmany-export_fig/README.md new file mode 100755 index 00000000..c6e68de7 --- /dev/null +++ b/offline/localizer/altmany-export_fig/README.md @@ -0,0 +1,270 @@ +export_fig +========== + +A toolbox for exporting figures from MATLAB to standard image and document formats nicely. + +### Overview +Exporting a figure from MATLAB the way you want it (hopefully the way it looks on screen), can be a real headache for the unitiated, thanks to all the settings that are required, and also due to some eccentricities (a.k.a. features and bugs) of functions such as `print`. The first goal of export_fig is to make transferring a plot from screen to document, just the way you expect (again, assuming that's as it appears on screen), a doddle. + +The second goal is to make the output media suitable for publication, allowing you to publish your results in the full glory that you originally intended. This includes embedding fonts, setting image compression levels (including lossless), anti-aliasing, cropping, setting the colourspace, alpha-blending and getting the right resolution. + +Perhaps the best way to demonstrate what export_fig can do is with some examples. + +### Examples +**Visual accuracy** - MATLAB's exporting functions, namely `saveas` and `print`, change many visual properties of a figure, such as size, axes limits and ticks, and background colour, in unexpected and unintended ways. Export_fig aims to faithfully reproduce the figure as it appears on screen. For example: +```Matlab +plot(cos(linspace(0, 7, 1000))); +set(gcf, 'Position', [100 100 150 150]); +saveas(gcf, 'test.png'); +export_fig test2.png +``` +generates the following: + +| Figure: | test.png: | test2.png: | +|:-------:|:---------:|:----------:| +|![](https://farm6.staticflickr.com/5616/15589249291_16e485c29a_o_d.png)|![](https://farm4.staticflickr.com/3944/15406302850_4d2e1c7afa_o_d.png)|![](https://farm6.staticflickr.com/5607/15568225476_8ce9bd5f6b_o_d.png)| + +Note that the size and background colour of test2.png (the output of export_fig) are the same as those of the on screen figure, in contrast to test.png. Of course, if you want the figure background to be white (or any other colour) in the exported file then you can set this prior to exporting using: +```Matlab +set(gcf, 'Color', 'w'); +``` + +Notice also that export_fig crops and anti-aliases (smooths, for bitmaps only) the output by default. However, these options can be disabled; see the Tips section below for details. + +**Resolution** - by default, export_fig exports bitmaps at screen resolution. However, you may wish to save them at a different resolution. You can do this using either of two options: `-m`, where is a positive real number, magnifies the figure by the factor for export, e.g. `-m2` produces an image double the size (in pixels) of the on screen figure; `-r`, again where is a positive real number, specifies the output bitmap to have pixels per inch, the dimensions of the figure (in inches) being those of the on screen figure. For example, using: +```Matlab +export_fig test.png -m2.5 +``` +on the figure from the example above generates: + +![](https://farm4.staticflickr.com/3937/15591910915_dc7040c477_o_d.png) + +Sometimes you might have a figure with an image in. For example: +```Matlab +imshow(imread('cameraman.tif')) +hold on +plot(0:255, sin(linspace(0, 10, 256))*127+128); +set(gcf, 'Position', [100 100 150 150]); +``` +generates this figure: + +![](https://farm4.staticflickr.com/3942/15589249581_ff87a56a3f_o_d.png) + +Here the image is displayed in the figure at resolution lower than its native resolution. However, you might want to export the figure at a resolution such that the image is output at its native (i.e. original) size (in pixels). Ordinarily this would require some non-trivial computation to work out what that resolution should be, but export_fig has an option to do this for you. Using: +```Matlab +export_fig test.png -native +``` +produces: + +![](https://farm6.staticflickr.com/5604/15589249591_da2b2652e4_o_d.png) + +with the image being the size (in pixels) of the original image. Note that if you want an image to be a particular size, in pixels, in the output (other than its original size) then you can resize it to this size and use the `-native` option to achieve this. + +All resolution options (`-m`, `-q` and `-native`) correctly set the resolution information in PNG and TIFF files, as if the image were the dimensions of the on screen figure. + +**Shrinking dots & dashes** - when exporting figures with dashed or dotted lines using either the ZBuffer or OpenGL (default for bitmaps) renderers, the dots and dashes can appear much shorter, even non-existent, in the output file, especially if the lines are thick and/or the resolution is high. For example: +```Matlab +plot(sin(linspace(0, 10, 1000)), 'b:', 'LineWidth', 4); +hold on +plot(cos(linspace(0, 7, 1000)), 'r--', 'LineWidth', 3); +grid on +export_fig test.png +``` +generates: + +![](https://farm4.staticflickr.com/3956/15592747732_f943d4aa0a_o_d.png) + +This problem can be overcome by using the painters renderer. For example: +```Matlab +export_fig test.png -painters +``` +used on the same figure generates: + +![](https://farm4.staticflickr.com/3945/14971168504_77692f11f5_o_d.png) + +Note that not only are the plot lines correct, but the grid lines are too. + +**Transparency** - sometimes you might want a figure and axes' backgrounds to be transparent, so that you can see through them to a document (for example a presentation slide, with coloured or textured background) that the exported figure is placed in. To achieve this, first (optionally) set the axes' colour to 'none' prior to exporting, using: +```Matlab +set(gca, 'Color', 'none'); % Sets axes background +``` + +then use export_fig's `-transparent` option when exporting: +```Matlab +export_fig test.png -transparent +``` + +This will make the background transparent in PDF, EPS and PNG outputs. You can additionally save fully alpha-blended semi-transparent patch objects to the PNG format. For example: + +```Matlab +logo; +alpha(0.5); +``` + +generates a figure like this: + +![](https://farm4.staticflickr.com/3933/15405290339_b08de33528_o_d.png) + +If you then export this to PNG using the `-transparent` option you can then put the resulting image into, for example, a presentation slide with fancy, textured background, like so: + +![](https://farm6.staticflickr.com/5599/15406302920_59beaefff1_o_d.png) + +and the image blends seamlessly with the background. + +**Image quality** - when publishing images of your results, you want them to look as good as possible. By default, when outputting to lossy file formats (PDF, EPS and JPEG), export_fig uses a high quality setting, i.e. low compression, for images, so little information is lost. This is in contrast to MATLAB's print and saveas functions, whose default quality settings are poor. For example: +```Matlab +A = im2double(imread('peppers.png')); +B = randn(ceil(size(A, 1)/6), ceil(size(A, 2)/6), 3) * 0.1; +B = cat(3, kron(B(:,:,1), ones(6)), kron(B(:,:,2), ones(6)), kron(B(:,:,3), ones(6))); +B = A + B(1:size(A, 1),1:size(A, 2),:); +imshow(B); +print -dpdf test.pdf +``` +generates a PDF file, a sub-window of which looks (when zoomed in) like this: + +![](https://farm6.staticflickr.com/5613/15405290309_881b2774d6_o_d.png) + +while the command + +```Matlab +export_fig test.pdf +``` +on the same figure produces this: + +![](https://farm4.staticflickr.com/3947/14971168174_687473133f_o_d.png) + +While much better, the image still contains some compression artifacts (see the low level noise around the edge of the pepper). You may prefer to export with no artifacts at all, i.e. lossless compression. Alternatively, you might need a smaller file, and be willing to accept more compression. Either way, export_fig has an option that can suit your needs: `-q`, where is a number from 0-100, will set the level of lossy image compression (again in PDF, EPS and JPEG outputs only; other formats are lossless), from high compression (0) to low compression/high quality (100). If you want lossless compression in any of those formats then specify a greater than 100. For example: +```Matlab +export_fig test.pdf -q101 +``` +again on the same figure, produces this: + +![](https://farm6.staticflickr.com/5608/15405803908_934512c1fe_o_d.png) + +Notice that all the noise has gone. + +### Tips +**Anti-aliasing** - the anti-aliasing which export_fig applies to bitmap outputs by default makes the images look nice, but it can also blur images and increase exporting time and memory requirements, so you might not always want it. You can set the level of anti-aliasing by using the `-a` option, where is 1 (no anti-aliasing), 2, 3 (default) or 4 (maximum anti-aliasing). + +**Cropping** - by default, export_fig crops its output to minimize the amount of empty space around the figure. If you'd prefer the figure to be uncropped, and instead have the same appearance (in terms of border width) as the on screen figure, then use the `-nocrop` option. + +**Colourspace** - by default, export_fig generates files in the RGB [colourspace](https://en.wikipedia.org/wiki/Color_space). However, you can also export in greyscale or the CMYK colourspace, using the `-grey` (or `-gray`) and `-cmyk` options respectively. The CMYK option is useful for publishers who require documents in this colourspace, but the option is only supported for PDF, EPS and TIFF files. + +**Specifying a target directory** - you can get export_fig to save output files to any directory (for which you have write permission), simply by specifying the full or relative path in the filename. For example: +```Matlab +export_fig ../subdir/fig.png; +export_fig('C:/Users/Me/Documents/figures/myfig', '-pdf', '-png'); +``` + +**Variable file names** - often you might want to save a series of figures in a for loop, each with a different name. For this you can use the functional form of input arguments, i.e. `export_fig(arg1, arg2)`, and construct the filename string in a variable. Here's an example of this: +```Matlab +for a = 1:5 + plot(rand(5, 2)); + export_fig(sprintf('plot%d.png', a)); +end +``` +When using the functional form like this, be sure to put string variables in quotes: +```Matlab +export_fig(sprintf('plot%d', a), '-a1', '-pdf', '-png'); +``` + +**Specifying the figure/axes** - if you have mutiple figures open you can specify which figure to export using its handle: +```Matlab +export_fig(figure_handle, filename); +``` +Equally, if your figure contains several subplots then you can export just one of them by giving export_fig the handle to the relevant axes: +```Matlab +export_fig(axes_handle, filename); +``` + +**Multiple formats** - save time by exporting to multiple formats simultaneously. E.g.: +```Matlab +export_fig filename -pdf -eps -png -jpg -tiff +``` + +**Other file formats** - if you'd like to save your figure to a bitmap format that is not supported by export_fig, e.g. animated GIF, PPM file or a frame in a movie, then you can use export_fig to output the image, and optionally an alpha-matte, to the workspace. E.g.: +```Matlab +frame = export_fig; +``` +or +```Matlab +[frame, alpha] = export_fig; +``` +These variables can then be saved to other image formats using other functions, such as imwrite. + +**Appending to a file** - you can use the `-append` option to append the figure to the end of an image/document, if it already exists. This is supported for PDF and TIFF files only. Note that if you wish to append a lot of figures consecutively to a PDF, it can be more efficient to save all the figures to PDF separately then append them all in one go at the end (e.g. using [append_pdfs](http://www.mathworks.com/matlabcentral/fileexchange/31215-appendpdfs)). + +**Output to clipboard** - you can use the `-clipboard` option to copy the specified figure or axes to the system clipboard, for easy paste into other documents (e.g., Word or PowerPoint). Note that the image is copied in bitmap (not vector) format. + +**Font size** - if you want to place an exported figure in a document with the font a particular size then you need to set the font to that size in the figure, and not resize the output of export_fig in the document. To avoid resizing, simply make sure that the on screen figure is the size you want the output to be in the document before exporting. + +**Renderers** - MATLAB has three renderers for displaying and exporting figures: painters, OpenGL and ZBuffer. The different renderers have different [features](http://www.mathworks.com/access/helpdesk/help/techdoc/creating_plots/f3-84337.html#f3-102410), so if you aren't happy with the result from one renderer try another. By default, vector formats (i.e. PDF and EPS outputs) use the painters renderer, while other formats use the OpenGL renderer. Non-default renderers can be selected by using one of these three export_fig input options: `-painters`, `-opengl`, `-zbuffer`: +```Matlab +export_fig test.png -painters +``` + +**Artifacts** - sometimes the output that you get from export_fig is not what you expected. If an output file contains artifacts that aren't in the on screen figure then make sure that the renderer used for rendering the figure on screen is the same as that used for exporting. To set the renderer used to display the figure, use: +```Matlab +set(figure_handle, 'Renderer', 'opengl'); +``` +After matching the two renderers, if the artifact appears in the on screen figure then you'll need to fix that before exporting. Alternatively you can try changing the renderer used by export_fig. Finally check that it isn't one of the known issues mentioned in the section below. + +**Smoothed/interpolated images in output PDF** - if you produce a PDF using export_fig and images in the PDF look overly smoothed or interpolated, this is because the software you are using to view the PDF is smoothing or interpolating the image data. The image is not smoothed in the PDF file itself. If the software has an option to disable this feature, you should select it. Alternatively, use another PDF viewer that doesn't exhibit this problem. + +**Locating Ghostscript/pdftops** - You may find a dialogue box appears when using export_fig, asking you to locate either [Ghostscript](http://www.ghostscript.com) or [pdftops](http://www.foolabs.com/xpdf). These are separate applications which export_fig requires to perform certain functions. If such a dialogue appears it is because export_fig can't find the application automatically. This is because you either haven't installed it, or it isn't in the normal place. Make sure you install the applications correctly first. They can be downloaded from the following places: + 1. Ghostscript: [www.ghostscript.com](http://www.ghostscript.com) + 2. pdftops (install the Xpdf package): [www.foolabs.com/xpdf](http://www.foolabs.com/xpdf) + +If you choose to install them in a non-default location then point export_fig +to this location using the dialogue box. + +**Undefined function errors** - If you download and run export_fig and get an error similar to this: +``` +??? Undefined function or method 'print2array' for input arguments of type 'double'. +``` +then you are missing one or more of the files that come in the export_fig package. Make sure that you click the "Get from GitHub" button at the top-right of the download [page](http://www.mathworks.co.uk/matlabcentral/fileexchange/23629-exportfig), then extract all the files in the zip file to the same directory. You should then have all the necessary files. + +### Known issues +There are lots of problems with MATLAB's exporting functions, especially `print`. Export_fig is simply a glorified wrapper for MATLAB's `print` function, and doesn't solve all of its bugs (yet?). Some of the problems I know about are: + +**Fonts** - when using the painters renderer, MATLAB can only export a small number of fonts, details of which can be found [here](http://www.mathworks.com/help/releases/R2014a/matlab/creating_plots/choosing-a-printer-driver.html#f3-96545). Export_fig attempts to correct font names in the resulting EPS file (up to a maximum of 11 different fonts in one figure), but this is not always guaranteed to work. In particular, the text positions will be affected. It also does not work for text blocks where the 'Interpreter' property is set to 'latex'. + +Also, when using the painters renderer, ghostscript will sometimes throw an error such as `Error: /undefined in /findfont`. This suggests that ghostscript could not find a definition file for one of your fonts. One possible fix for this is to make sure the file `EXPORT_FIG_PATH/.ignore/gs_font_path.txt` exists and contains a list of paths to the folder(s) containing the necessary font definitions (make sure that they are TrueType definitions!), separated by a semicolon. + +**RGB color data not yet supported in Painter's mode** - you will see this as a warning if you try to export a figure which contains patch objects whose face or vertex colors are specified as an RGB colour, rather than an index into the colormap, using the painters renderer (the default renderer for vector output). This problem can arise if you use `pcolor`, for example. This is a problem with MATLAB's painters renderer, which also affects `print`; there is currently no fix available in export_fig (other than to export to bitmap). The suggested workaround is to avoid colouring patches using RGB. First, try to use colours in the figure's colourmap (instructions [here](http://www.mathworks.co.uk/support/solutions/en/data/1-6OTPQE/)) - change the colourmap, if necessary. If you are using `pcolor`, try using [uimagesc](http://www.mathworks.com/matlabcentral/fileexchange/11368) (on the file exchange) instead. + +**Dashed contour lines appear solid** - when using the painters renderer, MATLAB cannot generate dashed lines using the `contour` function (either on screen or in exported PDF and EPS files). Details can be found [here](http://www.mathworks.com/support/solutions/en/data/1-14PPHB/?solution=1-14PPHB). + +**Text size** - when using the OpenGL or ZBuffer renderers, large text can be resized relative to the figure when exporting at non-screen-resolution (including using anti-alising at screen resolution). This is a feature of MATLAB's `print `function. In this case, try using the `-painters` option. + +**Lighting and transparency** - when using the painters renderer, transparency and lighting effects are not supported. Sorry, but this is an inherent feature of MATLAB's painters renderer. To find out more about the capabilities of each rendering method, see [here](http://www.mathworks.com/access/helpdesk/help/techdoc/creating_plots/f3-84337.html#f3-102410). You can still export transparent objects to vector format (SVG) using the excellent [plot2svg](http://www.mathworks.com/matlabcentral/fileexchange/7401) package, then convert this to PDF, for example using [Inkscape](http://inkscape.org/). However, it can't handle lighting. + +**Lines in patch objects** - when exporting patch objects to PDF using the painters renderer (default), sometimes the output can appear to have lines across the middle of rectangular patches; these lines are the colour of the background, as if there is a crack in the patch, allowing you to see through. This appears to be due to bugs in MATLAB's internal vector rendering code. These lines can often be removed from the PDF using software such as [InkScape](https://inkscape.org). Sometimes disabling anti-aliasing in the PDF-reader software can get rid of the lines ([discussion](https://github.com/altmany/export_fig/issues/44)). + +**Out of memory** - if you run into memory issues when using export_fig, some ways to get round this are: + 1. Reduce the level of anti-aliasing. + 2. Reduce the size of the figure. + 3. Reduce the export resolution (dpi). + 4. Change the renderer to painters or ZBuffer. + +**Errors** - the other common type of errors people get with export_fig are OpenGL errors. This isn't a fault of export_fig, but either a bug in MATLAB's `print`, or your graphics driver getting itself into a state. Always make sure your graphics driver is up-to-date. If it still doesn't work, try using the ZBuffer renderer. + +### Raising issues +If you think you have found a genuine error or issue with export_fig **that is not listed above**, first ensure that the figure looks correct on screen when rendered using the renderer that export_fig is set to use (e.g. if exporting to PDF or EPS, does the figure look correct on screen using the painters renderer, or if exporting to bitmap, does the figure look correct on screen using the OpenGL renderer?). If it looks wrong then the problem is there, and I cannot help (other than to suggest you try exporting using a different renderer). + +Secondly, if exporting to bitmap, do try all the renderers (i.e. try the options `-opengl`, `-zbuffer` and `-painters` separately), to see if one of them does produce an acceptable output, and if so, use that. + +If this still does not help, then ensure that you are using the latest version of export_fig, which is available [here](https://github.com/altmany/export_fig/archive/master.zip). + +If the figure looks correct on screen, but an error exists in the exported output (which cannot be solved using a different renderer) then please feel free to raise an [issue](https://github.com/altmany/export_fig/issues). Please be sure to include the .fig file, the export_fig command you use, the output you get, and a description of what you expected. I can't promise anything, but if it's easy to fix I may indeed do it. Often I will find that the error is due to a bug in MATLAB's `print` function, in which case I will suggest you submit it as a bug to TheMathWorks, and inform me of any fix they suggest. Also, if there's a feature you'd like that isn't supported please tell me what it is and I'll consider implementing it. + +### And finally... + +![](https://farm4.staticflickr.com/3956/15591911455_b9008bd77e_o_d.jpg) + +If you've ever wondered what's going on in the logo on the export_fig download page (reproduced here), then this explanantion is for you. The logo is designed to demonstrate as many of export_fig's features as possible: + +Given a figure containing a translucent mesh (top right), export_fig can export to pdf (bottom centre), which allows the figure to be zoomed-in without losing quality (because it's a vector graphic), but isn't able to reproduce the translucency. Also, depending on the PDF viewer program, small gaps appear between the patches, which are seen here as thin white lines. + +By contrast, when exporting to png (top left), translucency is preserved (see how the graphic below shows through), and the figure is anti-aliased. However, zooming-in does not reveal more detail since png is a bitmap format. Also, lines appear less sharp than in the pdf output. + diff --git a/offline/localizer/altmany-export_fig/append_pdfs.m b/offline/localizer/altmany-export_fig/append_pdfs.m new file mode 100755 index 00000000..9d050e71 --- /dev/null +++ b/offline/localizer/altmany-export_fig/append_pdfs.m @@ -0,0 +1,81 @@ +%APPEND_PDFS Appends/concatenates multiple PDF files +% +% Example: +% append_pdfs(output, input1, input2, ...) +% append_pdfs(output, input_list{:}) +% append_pdfs test.pdf temp1.pdf temp2.pdf +% +% This function appends multiple PDF files to an existing PDF file, or +% concatenates them into a PDF file if the output file doesn't yet exist. +% +% This function requires that you have ghostscript installed on your +% system. Ghostscript can be downloaded from: http://www.ghostscript.com +% +% IN: +% output - string of output file name (including the extension, .pdf). +% If it exists it is appended to; if not, it is created. +% input1 - string of an input file name (including the extension, .pdf). +% All input files are appended in order. +% input_list - cell array list of input file name strings. All input +% files are appended in order. + +% Copyright: Oliver Woodford, 2011 + +% Thanks to Reinhard Knoll for pointing out that appending multiple pdfs in +% one go is much faster than appending them one at a time. + +% Thanks to Michael Teo for reporting the issue of a too long command line. +% Issue resolved on 5/5/2011, by passing gs a command file. + +% Thanks to Martin Wittmann for pointing out the quality issue when +% appending multiple bitmaps. +% Issue resolved (to best of my ability) 1/6/2011, using the prepress +% setting + +% 26/02/15: If temp dir is not writable, use the output folder for temp +% files when appending (Javier Paredes); sanity check of inputs + +function append_pdfs(varargin) + +if nargin < 2, return; end % sanity check + +% Are we appending or creating a new file +append = exist(varargin{1}, 'file') == 2; +output = [tempname '.pdf']; +try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(output,'w'); + fwrite(fid,1); + fclose(fid); + delete(output); + isTempDirOk = true; +catch + % Temp dir is not writable, so use the output folder + [dummy,fname,fext] = fileparts(output); %#ok + fpath = fileparts(varargin{1}); + output = fullfile(fpath,[fname fext]); + isTempDirOk = false; +end +if ~append + output = varargin{1}; + varargin = varargin(2:end); +end +% Create the command file +if isTempDirOk + cmdfile = [tempname '.txt']; +else + cmdfile = fullfile(fpath,[fname '.txt']); +end +fh = fopen(cmdfile, 'w'); +fprintf(fh, '-q -dNOPAUSE -dBATCH -sDEVICE=pdfwrite -dPDFSETTINGS=/prepress -sOutputFile="%s" -f', output); +fprintf(fh, ' "%s"', varargin{:}); +fclose(fh); +% Call ghostscript +ghostscript(['@"' cmdfile '"']); +% Delete the command file +delete(cmdfile); +% Rename the file if needed +if append + movefile(output, varargin{1}); +end +end diff --git a/offline/localizer/altmany-export_fig/copyfig.m b/offline/localizer/altmany-export_fig/copyfig.m new file mode 100755 index 00000000..1746bbdb --- /dev/null +++ b/offline/localizer/altmany-export_fig/copyfig.m @@ -0,0 +1,51 @@ +function fh = copyfig(fh) +%COPYFIG Create a copy of a figure, without changing the figure +% +% Examples: +% fh_new = copyfig(fh_old) +% +% This function will create a copy of a figure, but not change the figure, +% as copyobj sometimes does, e.g. by changing legends. +% +% IN: +% fh_old - The handle of the figure to be copied. Default: gcf. +% +% OUT: +% fh_new - The handle of the created figure. + +% Copyright (C) Oliver Woodford 2012 + +% 26/02/15: If temp dir is not writable, use the dest folder for temp +% destination files (Javier Paredes) +% 15/04/15: Suppress warnings during copyobj (Dun Kirk comment on FEX page 2013-10-02) + + % Set the default + if nargin == 0 + fh = gcf; + end + % Is there a legend? + if isempty(findall(fh, 'Type', 'axes', 'Tag', 'legend')) + % Safe to copy using copyobj + oldWarn = warning('off'); %#ok %Suppress warnings during copyobj (Dun Kirk comment on FEX page 2013-10-02) + fh = copyobj(fh, 0); + warning(oldWarn); + else + % copyobj will change the figure, so save and then load it instead + tmp_nam = [tempname '.fig']; + try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); % cleanup + catch + % Temp dir is not writable, so use the current folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = pwd; + tmp_nam = fullfile(fpath,[fname fext]); + end + hgsave(fh, tmp_nam); + fh = hgload(tmp_nam); + delete(tmp_nam); + end +end diff --git a/offline/localizer/altmany-export_fig/crop_borders.m b/offline/localizer/altmany-export_fig/crop_borders.m new file mode 100755 index 00000000..21524960 --- /dev/null +++ b/offline/localizer/altmany-export_fig/crop_borders.m @@ -0,0 +1,152 @@ +function [A, vA, vB, bb_rel] = crop_borders(A, bcol, padding, crop_amounts) +%CROP_BORDERS Crop the borders of an image or stack of images +% +% [B, vA, vB, bb_rel] = crop_borders(A, bcol, [padding]) +% +%IN: +% A - HxWxCxN stack of images. +% bcol - Cx1 background colour vector. +% padding - scalar indicating how much padding to have in relation to +% the cropped-image-size (0<=padding<=1). Default: 0 +% crop_amounts - 4-element vector of crop amounts: [top,right,bottom,left] +% where NaN/Inf indicate auto-cropping, 0 means no cropping, +% and any other value mean cropping in pixel amounts. +% +%OUT: +% B - JxKxCxN cropped stack of images. +% vA - coordinates in A that contain the cropped image +% vB - coordinates in B where the cropped version of A is placed +% bb_rel - relative bounding box (used for eps-cropping) + +%{ +% 06/03/15: Improved image cropping thanks to Oscar Hartogensis +% 08/06/15: Fixed issue #76: case of transparent figure bgcolor +% 21/02/16: Enabled specifying non-automated crop amounts +% 04/04/16: Fix per Luiz Carvalho for old Matlab releases +%} + + if nargin < 3 + padding = 0; + end + if nargin < 4 + crop_amounts = nan(1,4); % =auto-cropping + end + crop_amounts(end+1:4) = NaN; % fill missing values with NaN + + [h, w, c, n] = size(A); + if isempty(bcol) % case of transparent bgcolor + bcol = A(ceil(end/2),1,:,1); + end + if isscalar(bcol) + bcol = bcol(ones(c, 1)); + end + + % Crop margin from left + if ~isfinite(crop_amounts(4)) + bail = false; + for l = 1:w + for a = 1:c + if ~all(col(A(:,l,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + l = 1 + abs(crop_amounts(4)); + end + + % Crop margin from right + if ~isfinite(crop_amounts(2)) + bcol = A(ceil(end/2),w,:,1); + bail = false; + for r = w:-1:l + for a = 1:c + if ~all(col(A(:,r,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + r = w - abs(crop_amounts(2)); + end + + % Crop margin from top + if ~isfinite(crop_amounts(1)) + bcol = A(1,ceil(end/2),:,1); + bail = false; + for t = 1:h + for a = 1:c + if ~all(col(A(t,:,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + t = 1 + abs(crop_amounts(1)); + end + + % Crop margin from bottom + bcol = A(h,ceil(end/2),:,1); + if ~isfinite(crop_amounts(3)) + bail = false; + for b = h:-1:t + for a = 1:c + if ~all(col(A(b,:,a,:)) == bcol(a)) + bail = true; + break; + end + end + if bail + break; + end + end + else + b = h - abs(crop_amounts(3)); + end + + if padding == 0 % no padding + if ~isequal([t b l r], [1 h 1 w]) % Check if we're actually croppping + padding = 1; % Leave one boundary pixel to avoid bleeding on resize + end + elseif abs(padding) < 1 % pad value is a relative fraction of image size + padding = sign(padding)*round(mean([b-t r-l])*abs(padding)); % ADJUST PADDING + else % pad value is in units of 1/72" points + padding = round(padding); % fix cases of non-integer pad value + end + + if padding > 0 % extra padding + % Create an empty image, containing the background color, that has the + % cropped image size plus the padded border + B = repmat(bcol,[(b-t)+1+padding*2,(r-l)+1+padding*2,1,n]); % Fix per Luiz Carvalho + % vA - coordinates in A that contain the cropped image + vA = [t b l r]; + % vB - coordinates in B where the cropped version of A will be placed + vB = [padding+1, (b-t)+1+padding, padding+1, (r-l)+1+padding]; + % Place the original image in the empty image + B(vB(1):vB(2), vB(3):vB(4), :, :) = A(vA(1):vA(2), vA(3):vA(4), :, :); + A = B; + else % extra cropping + vA = [t-padding b+padding l-padding r+padding]; + A = A(vA(1):vA(2), vA(3):vA(4), :, :); + vB = [NaN NaN NaN NaN]; + end + + % For EPS cropping, determine the relative BoundingBox - bb_rel + bb_rel = [l-1 h-b-1 r+1 h-t+1]./[w h w h]; +end + +function A = col(A) + A = A(:); +end diff --git a/offline/localizer/altmany-export_fig/eps2pdf.m b/offline/localizer/altmany-export_fig/eps2pdf.m new file mode 100755 index 00000000..6c109642 --- /dev/null +++ b/offline/localizer/altmany-export_fig/eps2pdf.m @@ -0,0 +1,189 @@ +function eps2pdf(source, dest, crop, append, gray, quality, gs_options) +%EPS2PDF Convert an eps file to pdf format using ghostscript +% +% Examples: +% eps2pdf source dest +% eps2pdf(source, dest, crop) +% eps2pdf(source, dest, crop, append) +% eps2pdf(source, dest, crop, append, gray) +% eps2pdf(source, dest, crop, append, gray, quality) +% eps2pdf(source, dest, crop, append, gray, quality, gs_options) +% +% This function converts an eps file to pdf format. The output can be +% optionally cropped and also converted to grayscale. If the output pdf +% file already exists then the eps file can optionally be appended as a new +% page on the end of the eps file. The level of bitmap compression can also +% optionally be set. +% +% This function requires that you have ghostscript installed on your +% system. Ghostscript can be downloaded from: http://www.ghostscript.com +% +% Inputs: +% source - filename of the source eps file to convert. The filename is +% assumed to already have the extension ".eps". +% dest - filename of the destination pdf file. The filename is assumed +% to already have the extension ".pdf". +% crop - boolean indicating whether to crop the borders off the pdf. +% Default: true. +% append - boolean indicating whether the eps should be appended to the +% end of the pdf as a new page (if the pdf exists already). +% Default: false. +% gray - boolean indicating whether the output pdf should be grayscale +% or not. Default: false. +% quality - scalar indicating the level of image bitmap quality to +% output. A larger value gives a higher quality. quality > 100 +% gives lossless output. Default: ghostscript prepress default. +% gs_options - optional ghostscript options (e.g.: '-dNoOutputFonts'). If +% multiple options are needed, enclose in call array: {'-a','-b'} + +% Copyright (C) Oliver Woodford 2009-2014, Yair Altman 2015- + +% Suggestion of appending pdf files provided by Matt C at: +% http://www.mathworks.com/matlabcentral/fileexchange/23629 + +% Thank you to Fabio Viola for pointing out compression artifacts, leading +% to the quality setting. +% Thank you to Scott for pointing out the subsampling of very small images, +% which was fixed for lossless compression settings. + +% 9/12/2011 Pass font path to ghostscript. +% 26/02/15: If temp dir is not writable, use the dest folder for temp +% destination files (Javier Paredes) +% 28/02/15: Enable users to specify optional ghostscript options (issue #36) +% 01/03/15: Upon GS error, retry without the -sFONTPATH= option (this might solve +% some /findfont errors according to James Rankin, FEX Comment 23/01/15) +% 23/06/15: Added extra debug info in case of ghostscript error; code indentation +% 04/10/15: Suggest a workaround for issue #41 (missing font path; thanks Mariia Fedotenkova) +% 22/02/16: Bug fix from latest release of this file (workaround for issue #41) + + % Intialise the options string for ghostscript + options = ['-q -dNOPAUSE -dBATCH -sDEVICE=pdfwrite -dPDFSETTINGS=/prepress -sOutputFile="' dest '"']; + % Set crop option + if nargin < 3 || crop + options = [options ' -dEPSCrop']; + end + % Set the font path + fp = font_path(); + if ~isempty(fp) + options = [options ' -sFONTPATH="' fp '"']; + end + % Set the grayscale option + if nargin > 4 && gray + options = [options ' -sColorConversionStrategy=Gray -dProcessColorModel=/DeviceGray']; + end + % Set the bitmap quality + if nargin > 5 && ~isempty(quality) + options = [options ' -dAutoFilterColorImages=false -dAutoFilterGrayImages=false']; + if quality > 100 + options = [options ' -dColorImageFilter=/FlateEncode -dGrayImageFilter=/FlateEncode -c ".setpdfwrite << /ColorImageDownsampleThreshold 10 /GrayImageDownsampleThreshold 10 >> setdistillerparams"']; + else + options = [options ' -dColorImageFilter=/DCTEncode -dGrayImageFilter=/DCTEncode']; + v = 1 + (quality < 80); + quality = 1 - quality / 100; + s = sprintf('<< /QFactor %.2f /Blend 1 /HSample [%d 1 1 %d] /VSample [%d 1 1 %d] >>', quality, v, v, v, v); + options = sprintf('%s -c ".setpdfwrite << /ColorImageDict %s /GrayImageDict %s >> setdistillerparams"', options, s, s); + end + end + % Enable users to specify optional ghostscript options (issue #36) + if nargin > 6 && ~isempty(gs_options) + if iscell(gs_options) + gs_options = sprintf(' %s',gs_options{:}); + elseif ~ischar(gs_options) + error('gs_options input argument must be a string or cell-array of strings'); + else + gs_options = [' ' gs_options]; + end + options = [options gs_options]; + end + % Check if the output file exists + if nargin > 3 && append && exist(dest, 'file') == 2 + % File exists - append current figure to the end + tmp_nam = tempname; + try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); + catch + % Temp dir is not writable, so use the dest folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = fileparts(dest); + tmp_nam = fullfile(fpath,[fname fext]); + end + % Copy the file + copyfile(dest, tmp_nam); + % Add the output file names + options = [options ' -f "' tmp_nam '" "' source '"']; + try + % Convert to pdf using ghostscript + [status, message] = ghostscript(options); + catch me + % Delete the intermediate file + delete(tmp_nam); + rethrow(me); + end + % Delete the intermediate file + delete(tmp_nam); + else + % File doesn't exist or should be over-written + % Add the output file names + options = [options ' -f "' source '"']; + % Convert to pdf using ghostscript + [status, message] = ghostscript(options); + end + % Check for error + if status + % Retry without the -sFONTPATH= option (this might solve some GS + % /findfont errors according to James Rankin, FEX Comment 23/01/15) + orig_options = options; + if ~isempty(fp) + options = regexprep(options, ' -sFONTPATH=[^ ]+ ',' '); + status = ghostscript(options); + if ~status, return; end % hurray! (no error) + end + % Report error + if isempty(message) + error('Unable to generate pdf. Check destination directory is writable.'); + elseif ~isempty(strfind(message,'/typecheck in /findfont')) + % Suggest a workaround for issue #41 (missing font path) + font_name = strtrim(regexprep(message,'.*Operand stack:\s*(.*)\s*Execution.*','$1')); + fprintf(2, 'Ghostscript error: could not find the following font(s): %s\n', font_name); + fpath = fileparts(mfilename('fullpath')); + gs_fonts_file = fullfile(fpath, '.ignore', 'gs_font_path.txt'); + fprintf(2, ' try to add the font''s folder to your %s file\n\n', gs_fonts_file); + error('export_fig error'); + else + fprintf(2, '\nGhostscript error: perhaps %s is open by another application\n', dest); + if ~isempty(gs_options) + fprintf(2, ' or maybe the%s option(s) are not accepted by your GS version\n', gs_options); + end + fprintf(2, 'Ghostscript options: %s\n\n', orig_options); + error(message); + end + end +end + +% Function to return (and create, where necessary) the font path +function fp = font_path() + fp = user_string('gs_font_path'); + if ~isempty(fp) + return + end + % Create the path + % Start with the default path + fp = getenv('GS_FONTPATH'); + % Add on the typical directories for a given OS + if ispc + if ~isempty(fp) + fp = [fp ';']; + end + fp = [fp getenv('WINDIR') filesep 'Fonts']; + else + if ~isempty(fp) + fp = [fp ':']; + end + fp = [fp '/usr/share/fonts:/usr/local/share/fonts:/usr/share/fonts/X11:/usr/local/share/fonts/X11:/usr/share/fonts/truetype:/usr/local/share/fonts/truetype']; + end + user_string('gs_font_path', fp); +end diff --git a/offline/localizer/altmany-export_fig/export_fig.m b/offline/localizer/altmany-export_fig/export_fig.m new file mode 100755 index 00000000..06b249e9 --- /dev/null +++ b/offline/localizer/altmany-export_fig/export_fig.m @@ -0,0 +1,1282 @@ +function [imageData, alpha] = export_fig(varargin) +%EXPORT_FIG Exports figures in a publication-quality format +% +% Examples: +% imageData = export_fig +% [imageData, alpha] = export_fig +% export_fig filename +% export_fig filename -format1 -format2 +% export_fig ... -nocrop +% export_fig ... -c[,,,] +% export_fig ... -transparent +% export_fig ... -native +% export_fig ... -m +% export_fig ... -r +% export_fig ... -a +% export_fig ... -q +% export_fig ... -p +% export_fig ... -d +% export_fig ... -depsc +% export_fig ... - +% export_fig ... - +% export_fig ... -append +% export_fig ... -bookmark +% export_fig ... -clipboard +% export_fig ... -update +% export_fig ... -nofontswap +% export_fig(..., handle) +% +% This function saves a figure or single axes to one or more vector and/or +% bitmap file formats, and/or outputs a rasterized version to the workspace, +% with the following properties: +% - Figure/axes reproduced as it appears on screen +% - Cropped borders (optional) +% - Embedded fonts (vector formats) +% - Improved line and grid line styles +% - Anti-aliased graphics (bitmap formats) +% - Render images at native resolution (optional for bitmap formats) +% - Transparent background supported (pdf, eps, png, tif) +% - Semi-transparent patch objects supported (png & tif only) +% - RGB, CMYK or grayscale output (CMYK only with pdf, eps, tiff) +% - Variable image compression, including lossless (pdf, eps, jpg) +% - Optionally append to file (pdf, tiff) +% - Vector formats: pdf, eps +% - Bitmap formats: png, tiff, jpg, bmp, export to workspace +% +% This function is especially suited to exporting figures for use in +% publications and presentations, because of the high quality and +% portability of media produced. +% +% Note that the background color and figure dimensions are reproduced +% (the latter approximately, and ignoring cropping & magnification) in the +% output file. For transparent background (and semi-transparent patch +% objects), use the -transparent option or set the figure 'Color' property +% to 'none'. To make axes transparent set the axes 'Color' property to +% 'none'. PDF, EPS, TIF & PNG are the only formats that support a transparent +% background; only TIF & PNG formats support transparency of patch objects. +% +% The choice of renderer (opengl, zbuffer or painters) has a large impact +% on the quality of output. The default value (opengl for bitmaps, painters +% for vector formats) generally gives good results, but if you aren't +% satisfied then try another renderer. Notes: 1) For vector formats (EPS, +% PDF), only painters generates vector graphics. 2) For bitmaps, only +% opengl can render transparent patch objects correctly. 3) For bitmaps, +% only painters will correctly scale line dash and dot lengths when +% magnifying or anti-aliasing. 4) Fonts may be substitued with Courier when +% using painters. +% +% When exporting to vector format (PDF & EPS) and bitmap format using the +% painters renderer, this function requires that ghostscript is installed +% on your system. You can download this from: +% http://www.ghostscript.com +% When exporting to eps it additionally requires pdftops, from the Xpdf +% suite of functions. You can download this from: +% http://www.foolabs.com/xpdf +% +% Inputs: +% filename - string containing the name (optionally including full or +% relative path) of the file the figure is to be saved as. If +% a path is not specified, the figure is saved in the current +% directory. If no name and no output arguments are specified, +% the default name, 'export_fig_out', is used. If neither a +% file extension nor a format are specified, a ".png" is added +% and the figure saved in that format. +% -format1, -format2, etc. - strings containing the extensions of the +% file formats the figure is to be saved as. +% Valid options are: '-pdf', '-eps', '-png', +% '-tif', '-jpg' and '-bmp'. All combinations +% of formats are valid. +% -nocrop - option indicating that the borders of the output are not to +% be cropped. +% -c[,,,] - option indicating crop amounts. Must be +% a 4-element vector of numeric values: [top,right,bottom,left] +% where NaN/Inf indicate auto-cropping, 0 means no cropping, +% and any other value mean cropping in pixel amounts. +% -transparent - option indicating that the figure background is to be +% made transparent (png, pdf, tif and eps output only). +% -m - option where val indicates the factor to magnify the +% on-screen figure pixel dimensions by when generating bitmap +% outputs (does not affect vector formats). Default: '-m1'. +% -r - option val indicates the resolution (in pixels per inch) to +% export bitmap and vector outputs at, keeping the dimensions +% of the on-screen figure. Default: '-r864' (for vector output +% only). Note that the -m option overides the -r option for +% bitmap outputs only. +% -native - option indicating that the output resolution (when outputting +% a bitmap format) should be such that the vertical resolution +% of the first suitable image found in the figure is at the +% native resolution of that image. To specify a particular +% image to use, give it the tag 'export_fig_native'. Notes: +% This overrides any value set with the -m and -r options. It +% also assumes that the image is displayed front-to-parallel +% with the screen. The output resolution is approximate and +% should not be relied upon. Anti-aliasing can have adverse +% effects on image quality (disable with the -a1 option). +% -a1, -a2, -a3, -a4 - option indicating the amount of anti-aliasing to +% use for bitmap outputs. '-a1' means no anti- +% aliasing; '-a4' is the maximum amount (default). +% - - option to force a particular renderer (painters, opengl or +% zbuffer). Default value: opengl for bitmap formats or +% figures with patches and/or transparent annotations; +% painters for vector formats without patches/transparencies. +% - - option indicating which colorspace color figures should +% be saved in: RGB (default), CMYK or gray. CMYK is only +% supported in pdf, eps and tiff output. +% -q - option to vary bitmap image quality (in pdf, eps and jpg +% files only). Larger val, in the range 0-100, gives higher +% quality/lower compression. val > 100 gives lossless +% compression. Default: '-q95' for jpg, ghostscript prepress +% default for pdf & eps. Note: lossless compression can +% sometimes give a smaller file size than the default lossy +% compression, depending on the type of images. +% -p - option to pad a border of width val to exported files, where +% val is either a relative size with respect to cropped image +% size (i.e. p=0.01 adds a 1% border). For EPS & PDF formats, +% val can also be integer in units of 1/72" points (abs(val)>1). +% val can be positive (padding) or negative (extra cropping). +% If used, the -nocrop flag will be ignored, i.e. the image will +% always be cropped and then padded. Default: 0 (i.e. no padding). +% -append - option indicating that if the file (pdfs only) already +% exists, the figure is to be appended as a new page, instead +% of being overwritten (default). +% -bookmark - option to indicate that a bookmark with the name of the +% figure is to be created in the output file (pdf only). +% -clipboard - option to save output as an image on the system clipboard. +% Note: background transparency is not preserved in clipboard +% -d - option to indicate a ghostscript setting. For example, +% -dMaxBitmap=0 or -dNoOutputFonts (Ghostscript 9.15+). +% -depsc - option to use EPS level-3 rather than the default level-2 print +% device. This solves some bugs with Matlab's default -depsc2 device +% such as discolored subplot lines on images (vector formats only). +% -update - option to download and install the latest version of export_fig +% -nofontswap - option to avoid font swapping. Font swapping is automatically +% done in vector formats (only): 11 standard Matlab fonts are +% replaced by the original figure fonts. This option prevents this. +% handle - The handle of the figure, axes or uipanels (can be an array of +% handles, but the objects must be in the same figure) to be +% saved. Default: gcf. +% +% Outputs: +% imageData - MxNxC uint8 image array of the exported image. +% alpha - MxN single array of alphamatte values in the range [0,1], +% for the case when the background is transparent. +% +% Some helpful examples and tips can be found at: +% https://github.com/altmany/export_fig +% +% See also PRINT, SAVEAS, ScreenCapture (on the Matlab File Exchange) + +%{ +% Copyright (C) Oliver Woodford 2008-2014, Yair Altman 2015- + +% The idea of using ghostscript is inspired by Peder Axensten's SAVEFIG +% (fex id: 10889) which is itself inspired by EPS2PDF (fex id: 5782). +% The idea for using pdftops came from the MATLAB newsgroup (id: 168171). +% The idea of editing the EPS file to change line styles comes from Jiro +% Doke's FIXPSLINESTYLE (fex id: 17928). +% The idea of changing dash length with line width came from comments on +% fex id: 5743, but the implementation is mine :) +% The idea of anti-aliasing bitmaps came from Anders Brun's MYAA (fex id: +% 20979). +% The idea of appending figures in pdfs came from Matt C in comments on the +% FEX (id: 23629) + +% Thanks to Roland Martin for pointing out the colour MATLAB +% bug/feature with colorbar axes and transparent backgrounds. +% Thanks also to Andrew Matthews for describing a bug to do with the figure +% size changing in -nodisplay mode. I couldn't reproduce it, but included a +% fix anyway. +% Thanks to Tammy Threadgill for reporting a bug where an axes is not +% isolated from gui objects. +%} +%{ +% 23/02/12: Ensure that axes limits don't change during printing +% 14/03/12: Fix bug in fixing the axes limits (thanks to Tobias Lamour for reporting it). +% 02/05/12: Incorporate patch of Petr Nechaev (many thanks), enabling bookmarking of figures in pdf files. +% 09/05/12: Incorporate patch of Arcelia Arrieta (many thanks), to keep tick marks fixed. +% 12/12/12: Add support for isolating uipanels. Thanks to michael for suggesting it. +% 25/09/13: Add support for changing resolution in vector formats. Thanks to Jan Jaap Meijer for suggesting it. +% 07/05/14: Add support for '~' at start of path. Thanks to Sally Warner for suggesting it. +% 24/02/15: Fix Matlab R2014b bug (issue #34): plot markers are not displayed when ZLimMode='manual' +% 25/02/15: Fix issue #4 (using HG2 on R2014a and earlier) +% 25/02/15: Fix issue #21 (bold TeX axes labels/titles in R2014b) +% 26/02/15: If temp dir is not writable, use the user-specified folder for temporary EPS/PDF files (Javier Paredes) +% 27/02/15: Modified repository URL from github.com/ojwoodford to /altmany +% Indented main function +% Added top-level try-catch block to display useful workarounds +% 28/02/15: Enable users to specify optional ghostscript options (issue #36) +% 06/03/15: Improved image padding & cropping thanks to Oscar Hartogensis +% 26/03/15: Fixed issue #49 (bug with transparent grayscale images); fixed out-of-memory issue +% 26/03/15: Fixed issue #42: non-normalized annotations on HG1 +% 26/03/15: Fixed issue #46: Ghostscript crash if figure units <> pixels +% 27/03/15: Fixed issue #39: bad export of transparent annotations/patches +% 28/03/15: Fixed issue #50: error on some Matlab versions with the fix for issue #42 +% 29/03/15: Fixed issue #33: bugs in Matlab's print() function with -cmyk +% 29/03/15: Improved processing of input args (accept space between param name & value, related to issue #51) +% 30/03/15: When exporting *.fig files, then saveas *.fig if figure is open, otherwise export the specified fig file +% 30/03/15: Fixed edge case bug introduced yesterday (commit #ae1755bd2e11dc4e99b95a7681f6e211b3fa9358) +% 09/04/15: Consolidated header comment sections; initialize output vars only if requested (nargout>0) +% 14/04/15: Workaround for issue #45: lines in image subplots are exported in invalid color +% 15/04/15: Fixed edge-case in parsing input parameters; fixed help section to show the -depsc option (issue #45) +% 21/04/15: Bug fix: Ghostscript croaks on % chars in output PDF file (reported by Sven on FEX page, 15-Jul-2014) +% 22/04/15: Bug fix: Pdftops croaks on relative paths (reported by Tintin Milou on FEX page, 19-Jan-2015) +% 04/05/15: Merged fix #63 (Kevin Mattheus Moerman): prevent tick-label changes during export +% 07/05/15: Partial fix for issue #65: PDF export used painters rather than opengl renderer (thanks Nguyenr) +% 08/05/15: Fixed issue #65: bad PDF append since commit #e9f3cdf 21/04/15 (thanks Robert Nguyen) +% 12/05/15: Fixed issue #67: exponent labels cropped in export, since fix #63 (04/05/15) +% 28/05/15: Fixed issue #69: set non-bold label font only if the string contains symbols (\beta etc.), followup to issue #21 +% 29/05/15: Added informative error message in case user requested SVG output (issue #72) +% 09/06/15: Fixed issue #58: -transparent removed anti-aliasing when exporting to PNG +% 19/06/15: Added -update option to download and install the latest version of export_fig +% 07/07/15: Added -nofontswap option to avoid font-swapping in EPS/PDF +% 16/07/15: Fixed problem with anti-aliasing on old Matlab releases +% 11/09/15: Fixed issue #103: magnification must never become negative; also fixed reported error msg in parsing input params +% 26/09/15: Alert if trying to export transparent patches/areas to non-PNG outputs (issue #108) +% 04/10/15: Do not suggest workarounds for certain errors that have already been handled previously +% 01/11/15: Fixed issue #112: use same renderer in print2eps as export_fig (thanks to Jesús Pestana Puerta) +% 10/11/15: Custom GS installation webpage for MacOS. Thanks to Andy Hueni via FEX +% 19/11/15: Fixed clipboard export in R2015b (thanks to Dan K via FEX) +% 21/02/16: Added -c option for indicating specific crop amounts (idea by Cedric Noordam on FEX) +% 08/05/16: Added message about possible error reason when groot.Units~=pixels (issue #149) +% 17/05/16: Fixed case of image YData containing more than 2 elements (issue #151) +% 08/08/16: Enabled exporting transparency to TIF, in addition to PNG/PDF (issue #168) +%} + + if nargout + [imageData, alpha] = deal([]); + end + hadError = false; + displaySuggestedWorkarounds = true; + + % Ensure the figure is rendered correctly _now_ so that properties like axes limits are up-to-date + drawnow; + pause(0.05); % this solves timing issues with Java Swing's EDT (http://undocumentedmatlab.com/blog/solving-a-matlab-hang-problem) + + % Parse the input arguments + fig = get(0, 'CurrentFigure'); + [fig, options] = parse_args(nargout, fig, varargin{:}); + + % Ensure that we have a figure handle + if isequal(fig,-1) + return; % silent bail-out + elseif isempty(fig) + error('No figure found'); + end + + % Isolate the subplot, if it is one + cls = all(ismember(get(fig, 'Type'), {'axes', 'uipanel'})); + if cls + % Given handles of one or more axes, so isolate them from the rest + fig = isolate_axes(fig); + else + % Check we have a figure + if ~isequal(get(fig, 'Type'), 'figure'); + error('Handle must be that of a figure, axes or uipanel'); + end + % Get the old InvertHardcopy mode + old_mode = get(fig, 'InvertHardcopy'); + end + + % Hack the font units where necessary (due to a font rendering bug in print?). + % This may not work perfectly in all cases. + % Also it can change the figure layout if reverted, so use a copy. + magnify = options.magnify * options.aa_factor; + if isbitmap(options) && magnify ~= 1 + fontu = findall(fig, 'FontUnits', 'normalized'); + if ~isempty(fontu) + % Some normalized font units found + if ~cls + fig = copyfig(fig); + set(fig, 'Visible', 'off'); + fontu = findall(fig, 'FontUnits', 'normalized'); + cls = true; + end + set(fontu, 'FontUnits', 'points'); + end + end + + try + % MATLAB "feature": axes limits and tick marks can change when printing + Hlims = findall(fig, 'Type', 'axes'); + if ~cls + % Record the old axes limit and tick modes + Xlims = make_cell(get(Hlims, 'XLimMode')); + Ylims = make_cell(get(Hlims, 'YLimMode')); + Zlims = make_cell(get(Hlims, 'ZLimMode')); + Xtick = make_cell(get(Hlims, 'XTickMode')); + Ytick = make_cell(get(Hlims, 'YTickMode')); + Ztick = make_cell(get(Hlims, 'ZTickMode')); + Xlabel = make_cell(get(Hlims, 'XTickLabelMode')); + Ylabel = make_cell(get(Hlims, 'YTickLabelMode')); + Zlabel = make_cell(get(Hlims, 'ZTickLabelMode')); + end + + % Set all axes limit and tick modes to manual, so the limits and ticks can't change + % Fix Matlab R2014b bug (issue #34): plot markers are not displayed when ZLimMode='manual' + set(Hlims, 'XLimMode', 'manual', 'YLimMode', 'manual'); + set_tick_mode(Hlims, 'X'); + set_tick_mode(Hlims, 'Y'); + if ~using_hg2(fig) + set(Hlims,'ZLimMode', 'manual'); + set_tick_mode(Hlims, 'Z'); + end + catch + % ignore - fix issue #4 (using HG2 on R2014a and earlier) + end + + % Fix issue #21 (bold TeX axes labels/titles in R2014b when exporting to EPS/PDF) + try + if using_hg2(fig) && isvector(options) + % Set the FontWeight of axes labels/titles to 'normal' + % Fix issue #69: set non-bold font only if the string contains symbols (\beta etc.) + texLabels = findall(fig, 'type','text', 'FontWeight','bold'); + symbolIdx = ~cellfun('isempty',strfind({texLabels.String},'\')); + set(texLabels(symbolIdx), 'FontWeight','normal'); + end + catch + % ignore + end + + % Fix issue #42: non-normalized annotations on HG1 (internal Matlab bug) + annotationHandles = []; + try + if ~using_hg2(fig) + annotationHandles = findall(fig,'Type','hggroup','-and','-property','Units','-and','-not','Units','norm'); + try % suggested by Jesús Pestana Puerta (jespestana) 30/9/2015 + originalUnits = get(annotationHandles,'Units'); + set(annotationHandles,'Units','norm'); + catch + end + end + catch + % should never happen, but ignore in any case - issue #50 + end + + % Fix issue #46: Ghostscript crash if figure units <> pixels + oldFigUnits = get(fig,'Units'); + set(fig,'Units','pixels'); + + % Set to print exactly what is there + set(fig, 'InvertHardcopy', 'off'); + % Set the renderer + switch options.renderer + case 1 + renderer = '-opengl'; + case 2 + renderer = '-zbuffer'; + case 3 + renderer = '-painters'; + otherwise + renderer = '-opengl'; % Default for bitmaps + end + + % Handle transparent patches + hasTransparency = ~isempty(findall(fig,'-property','FaceAlpha','-and','-not','FaceAlpha',1)); + hasPatches = ~isempty(findall(fig,'type','patch')); + if hasTransparency + % Alert if trying to export transparent patches/areas to non-supported outputs (issue #108) + % http://www.mathworks.com/matlabcentral/answers/265265-can-export_fig-or-else-draw-vector-graphics-with-transparent-surfaces + % TODO - use transparency when exporting to PDF by not passing via print2eps + msg = 'export_fig currently supports transparent patches/areas only in PNG output. '; + if options.pdf + warning('export_fig:transparency', '%s\nTo export transparent patches/areas to PDF, use the print command:\n print(gcf, ''-dpdf'', ''%s.pdf'');', msg, options.name); + elseif ~options.png && ~options.tif % issue #168 + warning('export_fig:transparency', '%s\nTo export the transparency correctly, try using the ScreenCapture utility on the Matlab File Exchange: http://bit.ly/1QFrBip', msg); + end + end + + try + % Do the bitmap formats first + if isbitmap(options) + if abs(options.bb_padding) > 1 + displaySuggestedWorkarounds = false; + error('For bitmap output (png,jpg,tif,bmp) the padding value (-p) must be between -1 100 + imwrite(A, [options.name '.jpg'], 'Mode', 'lossless'); + else + imwrite(A, [options.name '.jpg'], 'Quality', quality); + end + end + % Save tif images in cmyk if wanted (and possible) + if options.tif + if options.colourspace == 1 && size(A, 3) == 3 + A = double(255 - A); + K = min(A, [], 3); + K_ = 255 ./ max(255 - K, 1); + C = (A(:,:,1) - K) .* K_; + M = (A(:,:,2) - K) .* K_; + Y = (A(:,:,3) - K) .* K_; + A = uint8(cat(3, C, M, Y, K)); + clear C M Y K K_ + end + append_mode = {'overwrite', 'append'}; + imwrite(A, [options.name '.tif'], 'Resolution', options.magnify*get(0, 'ScreenPixelsPerInch'), 'WriteMode', append_mode{options.append+1}); + end + end + + % Now do the vector formats + if isvector(options) + % Set the default renderer to painters + if ~options.renderer + if hasTransparency || hasPatches + % This is *MUCH* slower, but more accurate for patches and transparent annotations (issue #39) + renderer = '-opengl'; + else + renderer = '-painters'; + end + end + options.rendererStr = renderer; % fix for issue #112 + % Generate some filenames + tmp_nam = [tempname '.eps']; + try + % Ensure that the temp dir is writable (Javier Paredes 30/1/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); + isTempDirOk = true; + catch + % Temp dir is not writable, so use the user-specified folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = fileparts(options.name); + tmp_nam = fullfile(fpath,[fname fext]); + isTempDirOk = false; + end + if isTempDirOk + pdf_nam_tmp = [tempname '.pdf']; + else + pdf_nam_tmp = fullfile(fpath,[fname '.pdf']); + end + if options.pdf + pdf_nam = [options.name '.pdf']; + try copyfile(pdf_nam, pdf_nam_tmp, 'f'); catch, end % fix for issue #65 + else + pdf_nam = pdf_nam_tmp; + end + % Generate the options for print + p2eArgs = {renderer, sprintf('-r%d', options.resolution)}; + if options.colourspace == 1 % CMYK + % Issue #33: due to internal bugs in Matlab's print() function, we can't use its -cmyk option + %p2eArgs{end+1} = '-cmyk'; + end + if ~options.crop + % Issue #56: due to internal bugs in Matlab's print() function, we can't use its internal cropping mechanism, + % therefore we always use '-loose' (in print2eps.m) and do our own cropping (in crop_borders) + %p2eArgs{end+1} = '-loose'; + end + if any(strcmpi(varargin,'-depsc')) + % Issue #45: lines in image subplots are exported in invalid color. + % The workaround is to use the -depsc parameter instead of the default -depsc2 + p2eArgs{end+1} = '-depsc'; + end + try + % Generate an eps + print2eps(tmp_nam, fig, options, p2eArgs{:}); + % Remove the background, if desired + if options.transparent && ~isequal(get(fig, 'Color'), 'none') + eps_remove_background(tmp_nam, 1 + using_hg2(fig)); + end + % Fix colorspace to CMYK, if requested (workaround for issue #33) + if options.colourspace == 1 % CMYK + % Issue #33: due to internal bugs in Matlab's print() function, we can't use its -cmyk option + change_rgb_to_cmyk(tmp_nam); + end + % Add a bookmark to the PDF if desired + if options.bookmark + fig_nam = get(fig, 'Name'); + if isempty(fig_nam) + warning('export_fig:EmptyBookmark', 'Bookmark requested for figure with no name. Bookmark will be empty.'); + end + add_bookmark(tmp_nam, fig_nam); + end + % Generate a pdf + eps2pdf(tmp_nam, pdf_nam_tmp, 1, options.append, options.colourspace==2, options.quality, options.gs_options); + % Ghostscript croaks on % chars in the output PDF file, so use tempname and then rename the file + try movefile(pdf_nam_tmp, pdf_nam, 'f'); catch, end + catch ex + % Delete the eps + delete(tmp_nam); + rethrow(ex); + end + % Delete the eps + delete(tmp_nam); + if options.eps + try + % Generate an eps from the pdf + % since pdftops can't handle relative paths (e.g., '..\'), use a temp file + eps_nam_tmp = strrep(pdf_nam_tmp,'.pdf','.eps'); + pdf2eps(pdf_nam, eps_nam_tmp); + movefile(eps_nam_tmp, [options.name '.eps'], 'f'); + catch ex + if ~options.pdf + % Delete the pdf + delete(pdf_nam); + end + try delete(eps_nam_tmp); catch, end + rethrow(ex); + end + if ~options.pdf + % Delete the pdf + delete(pdf_nam); + end + end + end + + % Revert the figure or close it (if requested) + if cls || options.closeFig + % Close the created figure + close(fig); + else + % Reset the hardcopy mode + set(fig, 'InvertHardcopy', old_mode); + % Reset the axes limit and tick modes + for a = 1:numel(Hlims) + try + set(Hlims(a), 'XLimMode', Xlims{a}, 'YLimMode', Ylims{a}, 'ZLimMode', Zlims{a},... + 'XTickMode', Xtick{a}, 'YTickMode', Ytick{a}, 'ZTickMode', Ztick{a},... + 'XTickLabelMode', Xlabel{a}, 'YTickLabelMode', Ylabel{a}, 'ZTickLabelMode', Zlabel{a}); + catch + % ignore - fix issue #4 (using HG2 on R2014a and earlier) + end + end + % Revert the tex-labels font weights + try set(texLabels, 'FontWeight','bold'); catch, end + % Revert annotation units + for handleIdx = 1 : numel(annotationHandles) + try + oldUnits = originalUnits{handleIdx}; + catch + oldUnits = originalUnits; + end + try set(annotationHandles(handleIdx),'Units',oldUnits); catch, end + end + % Revert figure units + set(fig,'Units',oldFigUnits); + end + + % Output to clipboard (if requested) + if options.clipboard + % Delete the output file if unchanged from the default name ('export_fig_out.png') + if strcmpi(options.name,'export_fig_out') + try + fileInfo = dir('export_fig_out.png'); + if ~isempty(fileInfo) + timediff = now - fileInfo.datenum; + ONE_SEC = 1/24/60/60; + if timediff < ONE_SEC + delete('export_fig_out.png'); + end + end + catch + % never mind... + end + end + + % Save the image in the system clipboard + % credit: Jiro Doke's IMCLIPBOARD: http://www.mathworks.com/matlabcentral/fileexchange/28708-imclipboard + try + error(javachk('awt', 'export_fig -clipboard output')); + catch + warning('export_fig -clipboard output failed: requires Java to work'); + return; + end + try + % Import necessary Java classes + import java.awt.Toolkit + import java.awt.image.BufferedImage + import java.awt.datatransfer.DataFlavor + + % Get System Clipboard object (java.awt.Toolkit) + cb = Toolkit.getDefaultToolkit.getSystemClipboard(); + + % Add java class (ImageSelection) to the path + if ~exist('ImageSelection', 'class') + javaaddpath(fileparts(which(mfilename)), '-end'); + end + + % Get image size + ht = size(imageData, 1); + wd = size(imageData, 2); + + % Convert to Blue-Green-Red format + try + imageData2 = imageData(:, :, [3 2 1]); + catch + % Probably gray-scaled image (2D, without the 3rd [RGB] dimension) + imageData2 = imageData(:, :, [1 1 1]); + end + + % Convert to 3xWxH format + imageData2 = permute(imageData2, [3, 2, 1]); + + % Append Alpha data (unused - transparency is not supported in clipboard copy) + alphaData2 = uint8(permute(255*alpha,[3,2,1])); %=255*ones(1,wd,ht,'uint8') + imageData2 = cat(1, imageData2, alphaData2); + + % Create image buffer + imBuffer = BufferedImage(wd, ht, BufferedImage.TYPE_INT_RGB); + imBuffer.setRGB(0, 0, wd, ht, typecast(imageData2(:), 'int32'), 0, wd); + + % Create ImageSelection object from the image buffer + imSelection = ImageSelection(imBuffer); + + % Set clipboard content to the image + cb.setContents(imSelection, []); + catch + warning('export_fig -clipboard output failed: %s', lasterr); %#ok + end + end + + % Don't output the data to console unless requested + if ~nargout + clear imageData alpha + end + catch err + % Display possible workarounds before the error message + if displaySuggestedWorkarounds && ~strcmpi(err.message,'export_fig error') + if ~hadError, fprintf(2, 'export_fig error. '); end + fprintf(2, 'Please ensure:\n'); + fprintf(2, ' that you are using the latest version of export_fig\n'); + if ismac + fprintf(2, ' and that you have Ghostscript installed\n'); + else + fprintf(2, ' and that you have Ghostscript installed\n'); + end + try + if options.eps + fprintf(2, ' and that you have pdftops installed\n'); + end + catch + % ignore - probably an error in parse_args + end + fprintf(2, ' and that you do not have multiple versions of export_fig installed by mistake\n'); + fprintf(2, ' and that you did not made a mistake in the expected input arguments\n'); + try + % Alert per issue #149 + if ~strncmpi(get(0,'Units'),'pixel',5) + fprintf(2, ' or try to set groot''s Units property back to its default value of ''pixels'' (details)\n'); + end + catch + % ignore - maybe an old MAtlab release + end + fprintf(2, '\nIf the problem persists, then please report a new issue.\n\n'); + end + rethrow(err) + end +end + +function options = default_options() + % Default options used by export_fig + options = struct(... + 'name', 'export_fig_out', ... + 'crop', true, ... + 'crop_amounts', nan(1,4), ... % auto-crop all 4 image sides + 'transparent', false, ... + 'renderer', 0, ... % 0: default, 1: OpenGL, 2: ZBuffer, 3: Painters + 'pdf', false, ... + 'eps', false, ... + 'png', false, ... + 'tif', false, ... + 'jpg', false, ... + 'bmp', false, ... + 'clipboard', false, ... + 'colourspace', 0, ... % 0: RGB/gray, 1: CMYK, 2: gray + 'append', false, ... + 'im', false, ... + 'alpha', false, ... + 'aa_factor', 0, ... + 'bb_padding', 0, ... + 'magnify', [], ... + 'resolution', [], ... + 'bookmark', false, ... + 'closeFig', false, ... + 'quality', [], ... + 'update', false, ... + 'fontswap', true, ... + 'gs_options', {{}}); +end + +function [fig, options] = parse_args(nout, fig, varargin) + % Parse the input arguments + + % Set the defaults + native = false; % Set resolution to native of an image + options = default_options(); + options.im = (nout == 1); % user requested imageData output + options.alpha = (nout == 2); % user requested alpha output + + % Go through the other arguments + skipNext = false; + for a = 1:nargin-2 + if skipNext + skipNext = false; + continue; + end + if all(ishandle(varargin{a})) + fig = varargin{a}; + elseif ischar(varargin{a}) && ~isempty(varargin{a}) + if varargin{a}(1) == '-' + switch lower(varargin{a}(2:end)) + case 'nocrop' + options.crop = false; + options.crop_amounts = [0,0,0,0]; + case {'trans', 'transparent'} + options.transparent = true; + case 'opengl' + options.renderer = 1; + case 'zbuffer' + options.renderer = 2; + case 'painters' + options.renderer = 3; + case 'pdf' + options.pdf = true; + case 'eps' + options.eps = true; + case 'png' + options.png = true; + case {'tif', 'tiff'} + options.tif = true; + case {'jpg', 'jpeg'} + options.jpg = true; + case 'bmp' + options.bmp = true; + case 'rgb' + options.colourspace = 0; + case 'cmyk' + options.colourspace = 1; + case {'gray', 'grey'} + options.colourspace = 2; + case {'a1', 'a2', 'a3', 'a4'} + options.aa_factor = str2double(varargin{a}(3)); + case 'append' + options.append = true; + case 'bookmark' + options.bookmark = true; + case 'native' + native = true; + case 'clipboard' + options.clipboard = true; + options.im = true; + options.alpha = true; + case 'svg' + msg = ['SVG output is not supported by export_fig. Use one of the following alternatives:\n' ... + ' 1. saveas(gcf,''filename.svg'')\n' ... + ' 2. plot2svg utility: http://github.com/jschwizer99/plot2svg\n' ... + ' 3. export_fig to EPS/PDF, then convert to SVG using generic (non-Matlab) tools\n']; + error(sprintf(msg)); %#ok + case 'update' + % Download the latest version of export_fig into the export_fig folder + try + zipFileName = 'https://github.com/altmany/export_fig/archive/master.zip'; + folderName = fileparts(which(mfilename('fullpath'))); + targetFileName = fullfile(folderName, datestr(now,'yyyy-mm-dd.zip')); + urlwrite(zipFileName,targetFileName); + catch + error('Could not download %s into %s\n',zipFileName,targetFileName); + end + + % Unzip the downloaded zip file in the export_fig folder + try + unzip(targetFileName,folderName); + catch + error('Could not unzip %s\n',targetFileName); + end + case 'nofontswap' + options.fontswap = false; + otherwise + try + wasError = false; + if strcmpi(varargin{a}(1:2),'-d') + varargin{a}(2) = 'd'; % ensure lowercase 'd' + options.gs_options{end+1} = varargin{a}; + elseif strcmpi(varargin{a}(1:2),'-c') + if numel(varargin{a})==2 + skipNext = true; + vals = str2num(varargin{a+1}); %#ok + else + vals = str2num(varargin{a}(3:end)); %#ok + end + if numel(vals)~=4 + wasError = true; + error('option -c cannot be parsed: must be a 4-element numeric vector'); + end + options.crop_amounts = vals; + options.crop = true; + else % scalar parameter value + val = str2double(regexp(varargin{a}, '(?<=-(m|M|r|R|q|Q|p|P))-?\d*.?\d+', 'match')); + if isempty(val) || isnan(val) + % Issue #51: improved processing of input args (accept space between param name & value) + val = str2double(varargin{a+1}); + if isscalar(val) && ~isnan(val) + skipNext = true; + end + end + if ~isscalar(val) || isnan(val) + wasError = true; + error('option %s is not recognised or cannot be parsed', varargin{a}); + end + switch lower(varargin{a}(2)) + case 'm' + % Magnification may never be negative + if val <= 0 + wasError = true; + error('Bad magnification value: %g (must be positive)', val); + end + options.magnify = val; + case 'r' + options.resolution = val; + case 'q' + options.quality = max(val, 0); + case 'p' + options.bb_padding = val; + end + end + catch err + % We might have reached here by raising an intentional error + if wasError % intentional raise + rethrow(err) + else % unintentional + error(['Unrecognized export_fig input option: ''' varargin{a} '''']); + end + end + end + else + [p, options.name, ext] = fileparts(varargin{a}); + if ~isempty(p) + options.name = [p filesep options.name]; + end + switch lower(ext) + case {'.tif', '.tiff'} + options.tif = true; + case {'.jpg', '.jpeg'} + options.jpg = true; + case '.png' + options.png = true; + case '.bmp' + options.bmp = true; + case '.eps' + options.eps = true; + case '.pdf' + options.pdf = true; + case '.fig' + % If no open figure, then load the specified .fig file and continue + if isempty(fig) + fig = openfig(varargin{a},'invisible'); + varargin{a} = fig; + options.closeFig = true; + else + % save the current figure as the specified .fig file and exit + saveas(fig(1),varargin{a}); + fig = -1; + return + end + case '.svg' + msg = ['SVG output is not supported by export_fig. Use one of the following alternatives:\n' ... + ' 1. saveas(gcf,''filename.svg'')\n' ... + ' 2. plot2svg utility: http://github.com/jschwizer99/plot2svg\n' ... + ' 3. export_fig to EPS/PDF, then convert to SVG using generic (non-Matlab) tools\n']; + error(sprintf(msg)); %#ok + otherwise + options.name = varargin{a}; + end + end + end + end + + % Quick bail-out if no figure found + if isempty(fig), return; end + + % Do border padding with repsect to a cropped image + if options.bb_padding + options.crop = true; + end + + % Set default anti-aliasing now we know the renderer + if options.aa_factor == 0 + try isAA = strcmp(get(ancestor(fig, 'figure'), 'GraphicsSmoothing'), 'on'); catch, isAA = false; end + options.aa_factor = 1 + 2 * (~(using_hg2(fig) && isAA) | (options.renderer == 3)); + end + + % Convert user dir '~' to full path + if numel(options.name) > 2 && options.name(1) == '~' && (options.name(2) == '/' || options.name(2) == '\') + options.name = fullfile(char(java.lang.System.getProperty('user.home')), options.name(2:end)); + end + + % Compute the magnification and resolution + if isempty(options.magnify) + if isempty(options.resolution) + options.magnify = 1; + options.resolution = 864; + else + options.magnify = options.resolution ./ get(0, 'ScreenPixelsPerInch'); + end + elseif isempty(options.resolution) + options.resolution = 864; + end + + % Set the default format + if ~isvector(options) && ~isbitmap(options) + options.png = true; + end + + % Check whether transparent background is wanted (old way) + if isequal(get(ancestor(fig(1), 'figure'), 'Color'), 'none') + options.transparent = true; + end + + % If requested, set the resolution to the native vertical resolution of the + % first suitable image found + if native && isbitmap(options) + % Find a suitable image + list = findall(fig, 'Type','image', 'Tag','export_fig_native'); + if isempty(list) + list = findall(fig, 'Type','image', 'Visible','on'); + end + for hIm = list(:)' + % Check height is >= 2 + height = size(get(hIm, 'CData'), 1); + if height < 2 + continue + end + % Account for the image filling only part of the axes, or vice versa + yl = get(hIm, 'YData'); + if isscalar(yl) + yl = [yl(1)-0.5 yl(1)+height+0.5]; + else + yl = [min(yl), max(yl)]; % fix issue #151 (case of yl containing more than 2 elements) + if ~diff(yl) + continue + end + yl = yl + [-0.5 0.5] * (diff(yl) / (height - 1)); + end + hAx = get(hIm, 'Parent'); + yl2 = get(hAx, 'YLim'); + % Find the pixel height of the axes + oldUnits = get(hAx, 'Units'); + set(hAx, 'Units', 'pixels'); + pos = get(hAx, 'Position'); + set(hAx, 'Units', oldUnits); + if ~pos(4) + continue + end + % Found a suitable image + % Account for stretch-to-fill being disabled + pbar = get(hAx, 'PlotBoxAspectRatio'); + pos = min(pos(4), pbar(2)*pos(3)/pbar(1)); + % Set the magnification to give native resolution + options.magnify = abs((height * diff(yl2)) / (pos * diff(yl))); % magnification must never be negative: issue #103 + break + end + end +end + +function A = downsize(A, factor) + % Downsample an image + if factor == 1 + % Nothing to do + return + end + try + % Faster, but requires image processing toolbox + A = imresize(A, 1/factor, 'bilinear'); + catch + % No image processing toolbox - resize manually + % Lowpass filter - use Gaussian as is separable, so faster + % Compute the 1d Gaussian filter + filt = (-factor-1:factor+1) / (factor * 0.6); + filt = exp(-filt .* filt); + % Normalize the filter + filt = single(filt / sum(filt)); + % Filter the image + padding = floor(numel(filt) / 2); + for a = 1:size(A, 3) + A(:,:,a) = conv2(filt, filt', single(A([ones(1, padding) 1:end repmat(end, 1, padding)],[ones(1, padding) 1:end repmat(end, 1, padding)],a)), 'valid'); + end + % Subsample + A = A(1+floor(mod(end-1, factor)/2):factor:end,1+floor(mod(end-1, factor)/2):factor:end,:); + end +end + +function A = rgb2grey(A) + A = cast(reshape(reshape(single(A), [], 3) * single([0.299; 0.587; 0.114]), size(A, 1), size(A, 2)), class(A)); %#ok +end + +function A = check_greyscale(A) + % Check if the image is greyscale + if size(A, 3) == 3 && ... + all(reshape(A(:,:,1) == A(:,:,2), [], 1)) && ... + all(reshape(A(:,:,2) == A(:,:,3), [], 1)) + A = A(:,:,1); % Save only one channel for 8-bit output + end +end + +function eps_remove_background(fname, count) + % Remove the background of an eps file + % Open the file + fh = fopen(fname, 'r+'); + if fh == -1 + error('Not able to open file %s.', fname); + end + % Read the file line by line + while count + % Get the next line + l = fgets(fh); + if isequal(l, -1) + break; % Quit, no rectangle found + end + % Check if the line contains the background rectangle + if isequal(regexp(l, ' *0 +0 +\d+ +\d+ +r[fe] *[\n\r]+', 'start'), 1) + % Set the line to whitespace and quit + l(1:regexp(l, '[\n\r]', 'start', 'once')-1) = ' '; + fseek(fh, -numel(l), 0); + fprintf(fh, l); + % Reduce the count + count = count - 1; + end + end + % Close the file + fclose(fh); +end + +function b = isvector(options) + b = options.pdf || options.eps; +end + +function b = isbitmap(options) + b = options.png || options.tif || options.jpg || options.bmp || options.im || options.alpha; +end + +% Helper function +function A = make_cell(A) + if ~iscell(A) + A = {A}; + end +end + +function add_bookmark(fname, bookmark_text) + % Adds a bookmark to the temporary EPS file after %%EndPageSetup + % Read in the file + fh = fopen(fname, 'r'); + if fh == -1 + error('File %s not found.', fname); + end + try + fstrm = fread(fh, '*char')'; + catch ex + fclose(fh); + rethrow(ex); + end + fclose(fh); + + % Include standard pdfmark prolog to maximize compatibility + fstrm = strrep(fstrm, '%%BeginProlog', sprintf('%%%%BeginProlog\n/pdfmark where {pop} {userdict /pdfmark /cleartomark load put} ifelse')); + % Add page bookmark + fstrm = strrep(fstrm, '%%EndPageSetup', sprintf('%%%%EndPageSetup\n[ /Title (%s) /OUT pdfmark',bookmark_text)); + + % Write out the updated file + fh = fopen(fname, 'w'); + if fh == -1 + error('Unable to open %s for writing.', fname); + end + try + fwrite(fh, fstrm, 'char*1'); + catch ex + fclose(fh); + rethrow(ex); + end + fclose(fh); +end + +function set_tick_mode(Hlims, ax) + % Set the tick mode of linear axes to manual + % Leave log axes alone as these are tricky + M = get(Hlims, [ax 'Scale']); + if ~iscell(M) + M = {M}; + end + M = cellfun(@(c) strcmp(c, 'linear'), M); + set(Hlims(M), [ax 'TickMode'], 'manual'); + %set(Hlims(M), [ax 'TickLabelMode'], 'manual'); % this hides exponent label in HG2! +end + +function change_rgb_to_cmyk(fname) % convert RGB => CMYK within an EPS file + % Do post-processing on the eps file + try + % Read the EPS file into memory + fstrm = read_write_entire_textfile(fname); + + % Replace all gray-scale colors + fstrm = regexprep(fstrm, '\n([\d.]+) +GC\n', '\n0 0 0 ${num2str(1-str2num($1))} CC\n'); + + % Replace all RGB colors + fstrm = regexprep(fstrm, '\n[0.]+ +[0.]+ +[0.]+ +RC\n', '\n0 0 0 1 CC\n'); % pure black + fstrm = regexprep(fstrm, '\n([\d.]+) +([\d.]+) +([\d.]+) +RC\n', '\n${sprintf(''%.4g '',[1-[str2num($1),str2num($2),str2num($3)]/max([str2num($1),str2num($2),str2num($3)]),1-max([str2num($1),str2num($2),str2num($3)])])} CC\n'); + + % Overwrite the file with the modified contents + read_write_entire_textfile(fname, fstrm); + catch + % never mind - leave as is... + end +end diff --git a/offline/localizer/altmany-export_fig/fix_lines.m b/offline/localizer/altmany-export_fig/fix_lines.m new file mode 100755 index 00000000..b160fd8d --- /dev/null +++ b/offline/localizer/altmany-export_fig/fix_lines.m @@ -0,0 +1,151 @@ +%FIX_LINES Improves the line style of eps files generated by print +% +% Examples: +% fix_lines fname +% fix_lines fname fname2 +% fstrm_out = fixlines(fstrm_in) +% +% This function improves the style of lines in eps files generated by +% MATLAB's print function, making them more similar to those seen on +% screen. Grid lines are also changed from a dashed style to a dotted +% style, for greater differentiation from dashed lines. +% +% The function also places embedded fonts after the postscript header, in +% versions of MATLAB which place the fonts first (R2006b and earlier), in +% order to allow programs such as Ghostscript to find the bounding box +% information. +% +%IN: +% fname - Name or path of source eps file. +% fname2 - Name or path of destination eps file. Default: same as fname. +% fstrm_in - File contents of a MATLAB-generated eps file. +% +%OUT: +% fstrm_out - Contents of the eps file with line styles fixed. + +% Copyright: (C) Oliver Woodford, 2008-2014 + +% The idea of editing the EPS file to change line styles comes from Jiro +% Doke's FIXPSLINESTYLE (fex id: 17928) +% The idea of changing dash length with line width came from comments on +% fex id: 5743, but the implementation is mine :) + +% Thank you to Sylvain Favrot for bringing the embedded font/bounding box +% interaction in older versions of MATLAB to my attention. +% Thank you to D Ko for bringing an error with eps files with tiff previews +% to my attention. +% Thank you to Laurence K for suggesting the check to see if the file was +% opened. + +% 01/03/15: Issue #20: warn users if using this function in HG2 (R2014b+) +% 27/03/15: Fixed out of memory issue with enormous EPS files (generated by print() with OpenGL renderer), related to issue #39 + +function fstrm = fix_lines(fstrm, fname2) + +% Issue #20: warn users if using this function in HG2 (R2014b+) +if using_hg2 + warning('export_fig:hg2','The fix_lines function should not be used in this Matlab version.'); +end + +if nargout == 0 || nargin > 1 + if nargin < 2 + % Overwrite the input file + fname2 = fstrm; + end + % Read in the file + fstrm = read_write_entire_textfile(fstrm); +end + +% Move any embedded fonts after the postscript header +if strcmp(fstrm(1:15), '%!PS-AdobeFont-') + % Find the start and end of the header + ind = regexp(fstrm, '[\n\r]%!PS-Adobe-'); + [ind2, ind2] = regexp(fstrm, '[\n\r]%%EndComments[\n\r]+'); + % Put the header first + if ~isempty(ind) && ~isempty(ind2) && ind(1) < ind2(1) + fstrm = fstrm([ind(1)+1:ind2(1) 1:ind(1) ind2(1)+1:end]); + end +end + +% Make sure all line width commands come before the line style definitions, +% so that dash lengths can be based on the correct widths +% Find all line style sections +ind = [regexp(fstrm, '[\n\r]SO[\n\r]'),... % This needs to be here even though it doesn't have dots/dashes! + regexp(fstrm, '[\n\r]DO[\n\r]'),... + regexp(fstrm, '[\n\r]DA[\n\r]'),... + regexp(fstrm, '[\n\r]DD[\n\r]')]; +ind = sort(ind); +% Find line width commands +[ind2, ind3] = regexp(fstrm, '[\n\r]\d* w[\n\r]'); +% Go through each line style section and swap with any line width commands +% near by +b = 1; +m = numel(ind); +n = numel(ind2); +for a = 1:m + % Go forwards width commands until we pass the current line style + while b <= n && ind2(b) < ind(a) + b = b + 1; + end + if b > n + % No more width commands + break; + end + % Check we haven't gone past another line style (including SO!) + if a < m && ind2(b) > ind(a+1) + continue; + end + % Are the commands close enough to be confident we can swap them? + if (ind2(b) - ind(a)) > 8 + continue; + end + % Move the line style command below the line width command + fstrm(ind(a)+1:ind3(b)) = [fstrm(ind(a)+4:ind3(b)) fstrm(ind(a)+1:ind(a)+3)]; + b = b + 1; +end + +% Find any grid line definitions and change to GR format +% Find the DO sections again as they may have moved +ind = int32(regexp(fstrm, '[\n\r]DO[\n\r]')); +if ~isempty(ind) + % Find all occurrences of what are believed to be axes and grid lines + ind2 = int32(regexp(fstrm, '[\n\r] *\d* *\d* *mt *\d* *\d* *L[\n\r]')); + if ~isempty(ind2) + % Now see which DO sections come just before axes and grid lines + ind2 = repmat(ind2', [1 numel(ind)]) - repmat(ind, [numel(ind2) 1]); + ind2 = any(ind2 > 0 & ind2 < 12); % 12 chars seems about right + ind = ind(ind2); + % Change any regions we believe to be grid lines to GR + fstrm(ind+1) = 'G'; + fstrm(ind+2) = 'R'; + end +end + +% Define the new styles, including the new GR format +% Dot and dash lengths have two parts: a constant amount plus a line width +% variable amount. The constant amount comes after dpi2point, and the +% variable amount comes after currentlinewidth. If you want to change +% dot/dash lengths for a one particular line style only, edit the numbers +% in the /DO (dotted lines), /DA (dashed lines), /DD (dot dash lines) and +% /GR (grid lines) lines for the style you want to change. +new_style = {'/dom { dpi2point 1 currentlinewidth 0.08 mul add mul mul } bdef',... % Dot length macro based on line width + '/dam { dpi2point 2 currentlinewidth 0.04 mul add mul mul } bdef',... % Dash length macro based on line width + '/SO { [] 0 setdash 0 setlinecap } bdef',... % Solid lines + '/DO { [1 dom 1.2 dom] 0 setdash 0 setlinecap } bdef',... % Dotted lines + '/DA { [4 dam 1.5 dam] 0 setdash 0 setlinecap } bdef',... % Dashed lines + '/DD { [1 dom 1.2 dom 4 dam 1.2 dom] 0 setdash 0 setlinecap } bdef',... % Dot dash lines + '/GR { [0 dpi2point mul 4 dpi2point mul] 0 setdash 1 setlinecap } bdef'}; % Grid lines - dot spacing remains constant + +% Construct the output +% This is the original (memory-intensive) code: +%first_sec = strfind(fstrm, '% line types:'); % Isolate line style definition section +%[second_sec, remaining] = strtok(fstrm(first_sec+1:end), '/'); +%[remaining, remaining] = strtok(remaining, '%'); +%fstrm = [fstrm(1:first_sec) second_sec sprintf('%s\r', new_style{:}) remaining]; +fstrm = regexprep(fstrm,'(% line types:.+?)/.+?%',['$1',sprintf('%s\r',new_style{:}),'%']); + +% Write the output file +if nargout == 0 || nargin > 1 + read_write_entire_textfile(fname2, fstrm); +end +end diff --git a/offline/localizer/altmany-export_fig/ghostscript.m b/offline/localizer/altmany-export_fig/ghostscript.m new file mode 100755 index 00000000..17608774 --- /dev/null +++ b/offline/localizer/altmany-export_fig/ghostscript.m @@ -0,0 +1,196 @@ +function varargout = ghostscript(cmd) +%GHOSTSCRIPT Calls a local GhostScript executable with the input command +% +% Example: +% [status result] = ghostscript(cmd) +% +% Attempts to locate a ghostscript executable, finally asking the user to +% specify the directory ghostcript was installed into. The resulting path +% is stored for future reference. +% +% Once found, the executable is called with the input command string. +% +% This function requires that you have Ghostscript installed on your +% system. You can download this from: http://www.ghostscript.com +% +% IN: +% cmd - Command string to be passed into ghostscript. +% +% OUT: +% status - 0 iff command ran without problem. +% result - Output from ghostscript. + +% Copyright: Oliver Woodford, 2009-2015, Yair Altman 2015- +%{ +% Thanks to Jonas Dorn for the fix for the title of the uigetdir window on Mac OS. +% Thanks to Nathan Childress for the fix to default location on 64-bit Windows systems. +% 27/04/11 - Find 64-bit Ghostscript on Windows. Thanks to Paul Durack and +% Shaun Kline for pointing out the issue +% 04/05/11 - Thanks to David Chorlian for pointing out an alternative +% location for gs on linux. +% 12/12/12 - Add extra executable name on Windows. Thanks to Ratish +% Punnoose for highlighting the issue. +% 28/06/13 - Fix error using GS 9.07 in Linux. Many thanks to Jannick +% Steinbring for proposing the fix. +% 24/10/13 - Fix error using GS 9.07 in Linux. Many thanks to Johannes +% for the fix. +% 23/01/14 - Add full path to ghostscript.txt in warning. Thanks to Koen +% Vermeer for raising the issue. +% 27/02/15 - If Ghostscript croaks, display suggested workarounds +% 30/03/15 - Improved performance by caching status of GS path check, if ok +% 14/05/15 - Clarified warning message in case GS path could not be saved +% 29/05/15 - Avoid cryptic error in case the ghostscipt path cannot be saved (issue #74) +% 10/11/15 - Custom GS installation webpage for MacOS. Thanks to Andy Hueni via FEX +%} + + try + % Call ghostscript + [varargout{1:nargout}] = system([gs_command(gs_path()) cmd]); + catch err + % Display possible workarounds for Ghostscript croaks + url1 = 'https://github.com/altmany/export_fig/issues/12#issuecomment-61467998'; % issue #12 + url2 = 'https://github.com/altmany/export_fig/issues/20#issuecomment-63826270'; % issue #20 + hg2_str = ''; if using_hg2, hg2_str = ' or Matlab R2014a'; end + fprintf(2, 'Ghostscript error. Rolling back to GS 9.10%s may possibly solve this:\n * %s ',hg2_str,url1,url1); + if using_hg2 + fprintf(2, '(GS 9.10)\n * %s (R2014a)',url2,url2); + end + fprintf('\n\n'); + if ismac || isunix + url3 = 'https://github.com/altmany/export_fig/issues/27'; % issue #27 + fprintf(2, 'Alternatively, this may possibly be due to a font path issue:\n * %s\n\n',url3,url3); + % issue #20 + fpath = which(mfilename); + if isempty(fpath), fpath = [mfilename('fullpath') '.m']; end + fprintf(2, 'Alternatively, if you are using csh, modify shell_cmd from "export..." to "setenv ..."\nat the bottom of %s\n\n',fpath,fpath); + end + rethrow(err); + end +end + +function path_ = gs_path + % Return a valid path + % Start with the currently set path + path_ = user_string('ghostscript'); + % Check the path works + if check_gs_path(path_) + return + end + % Check whether the binary is on the path + if ispc + bin = {'gswin32c.exe', 'gswin64c.exe', 'gs'}; + else + bin = {'gs'}; + end + for a = 1:numel(bin) + path_ = bin{a}; + if check_store_gs_path(path_) + return + end + end + % Search the obvious places + if ispc + default_location = 'C:\Program Files\gs\'; + dir_list = dir(default_location); + if isempty(dir_list) + default_location = 'C:\Program Files (x86)\gs\'; % Possible location on 64-bit systems + dir_list = dir(default_location); + end + executable = {'\bin\gswin32c.exe', '\bin\gswin64c.exe'}; + ver_num = 0; + % If there are multiple versions, use the newest + for a = 1:numel(dir_list) + ver_num2 = sscanf(dir_list(a).name, 'gs%g'); + if ~isempty(ver_num2) && ver_num2 > ver_num + for b = 1:numel(executable) + path2 = [default_location dir_list(a).name executable{b}]; + if exist(path2, 'file') == 2 + path_ = path2; + ver_num = ver_num2; + end + end + end + end + if check_store_gs_path(path_) + return + end + else + executable = {'/usr/bin/gs', '/usr/local/bin/gs'}; + for a = 1:numel(executable) + path_ = executable{a}; + if check_store_gs_path(path_) + return + end + end + end + % Ask the user to enter the path + while true + if strncmp(computer, 'MAC', 3) % Is a Mac + % Give separate warning as the uigetdir dialogue box doesn't have a + % title + uiwait(warndlg('Ghostscript not found. Please locate the program.')) + end + base = uigetdir('/', 'Ghostcript not found. Please locate the program.'); + if isequal(base, 0) + % User hit cancel or closed window + break; + end + base = [base filesep]; %#ok + bin_dir = {'', ['bin' filesep], ['lib' filesep]}; + for a = 1:numel(bin_dir) + for b = 1:numel(bin) + path_ = [base bin_dir{a} bin{b}]; + if exist(path_, 'file') == 2 + if check_store_gs_path(path_) + return + end + end + end + end + end + if ismac + error('Ghostscript not found. Have you installed it (http://pages.uoregon.edu/koch)?'); + else + error('Ghostscript not found. Have you installed it from www.ghostscript.com?'); + end +end + +function good = check_store_gs_path(path_) + % Check the path is valid + good = check_gs_path(path_); + if ~good + return + end + % Update the current default path to the path found + if ~user_string('ghostscript', path_) + filename = fullfile(fileparts(which('user_string.m')), '.ignore', 'ghostscript.txt'); + warning('Path to ghostscript installation could not be saved in %s (perhaps a permissions issue). You can manually create this file and set its contents to %s, to improve performance in future invocations (this warning is safe to ignore).', filename, path_); + return + end +end + +function good = check_gs_path(path_) + persistent isOk + if isempty(path_) + isOk = false; + elseif ~isequal(isOk,true) + % Check whether the path is valid + [status, message] = system([gs_command(path_) '-h']); %#ok + isOk = status == 0; + end + good = isOk; +end + +function cmd = gs_command(path_) + % Initialize any required system calls before calling ghostscript + % TODO: in Unix/Mac, find a way to determine whether to use "export" (bash) or "setenv" (csh/tcsh) + shell_cmd = ''; + if isunix + shell_cmd = 'export LD_LIBRARY_PATH=""; '; % Avoids an error on Linux with GS 9.07 + end + if ismac + shell_cmd = 'export DYLD_LIBRARY_PATH=""; '; % Avoids an error on Mac with GS 9.07 + end + % Construct the command string + cmd = sprintf('%s"%s" ', shell_cmd, path_); +end diff --git a/offline/localizer/altmany-export_fig/im2gif.m b/offline/localizer/altmany-export_fig/im2gif.m new file mode 100755 index 00000000..a6c5975d --- /dev/null +++ b/offline/localizer/altmany-export_fig/im2gif.m @@ -0,0 +1,186 @@ +%IM2GIF Convert a multiframe image to an animated GIF file +% +% Examples: +% im2gif infile +% im2gif infile outfile +% im2gif(A, outfile) +% im2gif(..., '-nocrop') +% im2gif(..., '-nodither') +% im2gif(..., '-ncolors', n) +% im2gif(..., '-loops', n) +% im2gif(..., '-delay', n) +% +% This function converts a multiframe image to an animated GIF. +% +% To create an animation from a series of figures, export to a multiframe +% TIFF file using export_fig, then convert to a GIF, as follows: +% +% for a = 2 .^ (3:6) +% peaks(a); +% export_fig test.tif -nocrop -append +% end +% im2gif('test.tif', '-delay', 0.5); +% +%IN: +% infile - string containing the name of the input image. +% outfile - string containing the name of the output image (must have the +% .gif extension). Default: infile, with .gif extension. +% A - HxWxCxN array of input images, stacked along fourth dimension, to +% be converted to gif. +% -nocrop - option indicating that the borders of the output are not to +% be cropped. +% -nodither - option indicating that dithering is not to be used when +% converting the image. +% -ncolors - option pair, the value of which indicates the maximum number +% of colors the GIF can have. This can also be a quantization +% tolerance, between 0 and 1. Default/maximum: 256. +% -loops - option pair, the value of which gives the number of times the +% animation is to be looped. Default: 65535. +% -delay - option pair, the value of which gives the time, in seconds, +% between frames. Default: 1/15. + +% Copyright (C) Oliver Woodford 2011 + +function im2gif(A, varargin) + +% Parse the input arguments +[A, options] = parse_args(A, varargin{:}); + +if options.crop ~= 0 + % Crop + A = crop_borders(A, A(ceil(end/2),1,:,1)); +end + +% Convert to indexed image +[h, w, c, n] = size(A); +A = reshape(permute(A, [1 2 4 3]), h, w*n, c); +map = unique(reshape(A, h*w*n, c), 'rows'); +if size(map, 1) > 256 + dither_str = {'dither', 'nodither'}; + dither_str = dither_str{1+(options.dither==0)}; + if options.ncolors <= 1 + [B, map] = rgb2ind(A, options.ncolors, dither_str); + if size(map, 1) > 256 + [B, map] = rgb2ind(A, 256, dither_str); + end + else + [B, map] = rgb2ind(A, min(round(options.ncolors), 256), dither_str); + end +else + if max(map(:)) > 1 + map = double(map) / 255; + A = double(A) / 255; + end + B = rgb2ind(im2double(A), map); +end +B = reshape(B, h, w, 1, n); + +% Bug fix to rgb2ind +map(B(1)+1,:) = im2double(A(1,1,:)); + +% Save as a gif +imwrite(B, map, options.outfile, 'LoopCount', round(options.loops(1)), 'DelayTime', options.delay); +end + +%% Parse the input arguments +function [A, options] = parse_args(A, varargin) +% Set the defaults +options = struct('outfile', '', ... + 'dither', true, ... + 'crop', true, ... + 'ncolors', 256, ... + 'loops', 65535, ... + 'delay', 1/15); + +% Go through the arguments +a = 0; +n = numel(varargin); +while a < n + a = a + 1; + if ischar(varargin{a}) && ~isempty(varargin{a}) + if varargin{a}(1) == '-' + opt = lower(varargin{a}(2:end)); + switch opt + case 'nocrop' + options.crop = false; + case 'nodither' + options.dither = false; + otherwise + if ~isfield(options, opt) + error('Option %s not recognized', varargin{a}); + end + a = a + 1; + if ischar(varargin{a}) && ~ischar(options.(opt)) + options.(opt) = str2double(varargin{a}); + else + options.(opt) = varargin{a}; + end + end + else + options.outfile = varargin{a}; + end + end +end + +if isempty(options.outfile) + if ~ischar(A) + error('No output filename given.'); + end + % Generate the output filename from the input filename + [path, outfile] = fileparts(A); + options.outfile = fullfile(path, [outfile '.gif']); +end + +if ischar(A) + % Read in the image + A = imread_rgb(A); +end +end + +%% Read image to uint8 rgb array +function [A, alpha] = imread_rgb(name) +% Get file info +info = imfinfo(name); +% Special case formats +switch lower(info(1).Format) + case 'gif' + [A, map] = imread(name, 'frames', 'all'); + if ~isempty(map) + map = uint8(map * 256 - 0.5); % Convert to uint8 for storage + A = reshape(map(uint32(A)+1,:), [size(A) size(map, 2)]); % Assume indexed from 0 + A = permute(A, [1 2 5 4 3]); + end + case {'tif', 'tiff'} + A = cell(numel(info), 1); + for a = 1:numel(A) + [A{a}, map] = imread(name, 'Index', a, 'Info', info); + if ~isempty(map) + map = uint8(map * 256 - 0.5); % Convert to uint8 for storage + A{a} = reshape(map(uint32(A{a})+1,:), [size(A) size(map, 2)]); % Assume indexed from 0 + end + if size(A{a}, 3) == 4 + % TIFF in CMYK colourspace - convert to RGB + if isfloat(A{a}) + A{a} = A{a} * 255; + else + A{a} = single(A{a}); + end + A{a} = 255 - A{a}; + A{a}(:,:,4) = A{a}(:,:,4) / 255; + A{a} = uint8(A(:,:,1:3) .* A{a}(:,:,[4 4 4])); + end + end + A = cat(4, A{:}); + otherwise + [A, map, alpha] = imread(name); + A = A(:,:,:,1); % Keep only first frame of multi-frame files + if ~isempty(map) + map = uint8(map * 256 - 0.5); % Convert to uint8 for storage + A = reshape(map(uint32(A)+1,:), [size(A) size(map, 2)]); % Assume indexed from 0 + elseif size(A, 3) == 4 + % Assume 4th channel is an alpha matte + alpha = A(:,:,4); + A = A(:,:,1:3); + end +end +end diff --git a/offline/localizer/altmany-export_fig/isolate_axes.m b/offline/localizer/altmany-export_fig/isolate_axes.m new file mode 100755 index 00000000..4e71e69e --- /dev/null +++ b/offline/localizer/altmany-export_fig/isolate_axes.m @@ -0,0 +1,130 @@ +function fh = isolate_axes(ah, vis) +%ISOLATE_AXES Isolate the specified axes in a figure on their own +% +% Examples: +% fh = isolate_axes(ah) +% fh = isolate_axes(ah, vis) +% +% This function will create a new figure containing the axes/uipanels +% specified, and also their associated legends and colorbars. The objects +% specified must all be in the same figure, but they will generally only be +% a subset of the objects in the figure. +% +% IN: +% ah - An array of axes and uipanel handles, which must come from the +% same figure. +% vis - A boolean indicating whether the new figure should be visible. +% Default: false. +% +% OUT: +% fh - The handle of the created figure. + +% Copyright (C) Oliver Woodford 2011-2013 + +% Thank you to Rosella Blatt for reporting a bug to do with axes in GUIs +% 16/03/12: Moved copyfig to its own function. Thanks to Bob Fratantonio +% for pointing out that the function is also used in export_fig.m +% 12/12/12: Add support for isolating uipanels. Thanks to michael for suggesting it +% 08/10/13: Bug fix to allchildren suggested by Will Grant (many thanks!) +% 05/12/13: Bug fix to axes having different units. Thanks to Remington Reid for reporting +% 21/04/15: Bug fix for exporting uipanels with legend/colorbar on HG1 (reported by Alvaro +% on FEX page as a comment on 24-Apr-2014); standardized indentation & help section +% 22/04/15: Bug fix: legends and colorbars were not exported when exporting axes handle in HG2 + + % Make sure we have an array of handles + if ~all(ishandle(ah)) + error('ah must be an array of handles'); + end + % Check that the handles are all for axes or uipanels, and are all in the same figure + fh = ancestor(ah(1), 'figure'); + nAx = numel(ah); + for a = 1:nAx + if ~ismember(get(ah(a), 'Type'), {'axes', 'uipanel'}) + error('All handles must be axes or uipanel handles.'); + end + if ~isequal(ancestor(ah(a), 'figure'), fh) + error('Axes must all come from the same figure.'); + end + end + % Tag the objects so we can find them in the copy + old_tag = get(ah, 'Tag'); + if nAx == 1 + old_tag = {old_tag}; + end + set(ah, 'Tag', 'ObjectToCopy'); + % Create a new figure exactly the same as the old one + fh = copyfig(fh); %copyobj(fh, 0); + if nargin < 2 || ~vis + set(fh, 'Visible', 'off'); + end + % Reset the object tags + for a = 1:nAx + set(ah(a), 'Tag', old_tag{a}); + end + % Find the objects to save + ah = findall(fh, 'Tag', 'ObjectToCopy'); + if numel(ah) ~= nAx + close(fh); + error('Incorrect number of objects found.'); + end + % Set the axes tags to what they should be + for a = 1:nAx + set(ah(a), 'Tag', old_tag{a}); + end + % Keep any legends and colorbars which overlap the subplots + % Note: in HG1 these are axes objects; in HG2 they are separate objects, therefore we + % don't test for the type, only the tag (hopefully nobody but Matlab uses them!) + lh = findall(fh, 'Tag', 'legend', '-or', 'Tag', 'Colorbar'); + nLeg = numel(lh); + if nLeg > 0 + set([ah(:); lh(:)], 'Units', 'normalized'); + try + ax_pos = get(ah, 'OuterPosition'); % axes and figures have the OuterPosition property + catch + ax_pos = get(ah, 'Position'); % uipanels only have Position, not OuterPosition + end + if nAx > 1 + ax_pos = cell2mat(ax_pos(:)); + end + ax_pos(:,3:4) = ax_pos(:,3:4) + ax_pos(:,1:2); + try + leg_pos = get(lh, 'OuterPosition'); + catch + leg_pos = get(lh, 'Position'); % No OuterPosition in HG2, only in HG1 + end + if nLeg > 1; + leg_pos = cell2mat(leg_pos); + end + leg_pos(:,3:4) = leg_pos(:,3:4) + leg_pos(:,1:2); + ax_pos = shiftdim(ax_pos, -1); + % Overlap test + M = bsxfun(@lt, leg_pos(:,1), ax_pos(:,:,3)) & ... + bsxfun(@lt, leg_pos(:,2), ax_pos(:,:,4)) & ... + bsxfun(@gt, leg_pos(:,3), ax_pos(:,:,1)) & ... + bsxfun(@gt, leg_pos(:,4), ax_pos(:,:,2)); + ah = [ah; lh(any(M, 2))]; + end + % Get all the objects in the figure + axs = findall(fh); + % Delete everything except for the input objects and associated items + delete(axs(~ismember(axs, [ah; allchildren(ah); allancestors(ah)]))); +end + +function ah = allchildren(ah) + ah = findall(ah); + if iscell(ah) + ah = cell2mat(ah); + end + ah = ah(:); +end + +function ph = allancestors(ah) + ph = []; + for a = 1:numel(ah) + h = get(ah(a), 'parent'); + while h ~= 0 + ph = [ph; h]; + h = get(h, 'parent'); + end + end +end diff --git a/offline/localizer/altmany-export_fig/pdf2eps.m b/offline/localizer/altmany-export_fig/pdf2eps.m new file mode 100755 index 00000000..c0766124 --- /dev/null +++ b/offline/localizer/altmany-export_fig/pdf2eps.m @@ -0,0 +1,51 @@ +%PDF2EPS Convert a pdf file to eps format using pdftops +% +% Examples: +% pdf2eps source dest +% +% This function converts a pdf file to eps format. +% +% This function requires that you have pdftops, from the Xpdf suite of +% functions, installed on your system. This can be downloaded from: +% http://www.foolabs.com/xpdf +% +%IN: +% source - filename of the source pdf file to convert. The filename is +% assumed to already have the extension ".pdf". +% dest - filename of the destination eps file. The filename is assumed to +% already have the extension ".eps". + +% Copyright (C) Oliver Woodford 2009-2010 + +% Thanks to Aldebaro Klautau for reporting a bug when saving to +% non-existant directories. + +function pdf2eps(source, dest) +% Construct the options string for pdftops +options = ['-q -paper match -eps -level2 "' source '" "' dest '"']; +% Convert to eps using pdftops +[status, message] = pdftops(options); +% Check for error +if status + % Report error + if isempty(message) + error('Unable to generate eps. Check destination directory is writable.'); + else + error(message); + end +end +% Fix the DSC error created by pdftops +fid = fopen(dest, 'r+'); +if fid == -1 + % Cannot open the file + return +end +fgetl(fid); % Get the first line +str = fgetl(fid); % Get the second line +if strcmp(str(1:min(13, end)), '% Produced by') + fseek(fid, -numel(str)-1, 'cof'); + fwrite(fid, '%'); % Turn ' ' into '%' +end +fclose(fid); +end + diff --git a/offline/localizer/altmany-export_fig/pdftops.m b/offline/localizer/altmany-export_fig/pdftops.m new file mode 100755 index 00000000..0f9aa478 --- /dev/null +++ b/offline/localizer/altmany-export_fig/pdftops.m @@ -0,0 +1,150 @@ +function varargout = pdftops(cmd) +%PDFTOPS Calls a local pdftops executable with the input command +% +% Example: +% [status result] = pdftops(cmd) +% +% Attempts to locate a pdftops executable, finally asking the user to +% specify the directory pdftops was installed into. The resulting path is +% stored for future reference. +% +% Once found, the executable is called with the input command string. +% +% This function requires that you have pdftops (from the Xpdf package) +% installed on your system. You can download this from: +% http://www.foolabs.com/xpdf +% +% IN: +% cmd - Command string to be passed into pdftops (e.g. '-help'). +% +% OUT: +% status - 0 iff command ran without problem. +% result - Output from pdftops. + +% Copyright: Oliver Woodford, 2009-2010 + +% Thanks to Jonas Dorn for the fix for the title of the uigetdir window on Mac OS. +% Thanks to Christoph Hertel for pointing out a bug in check_xpdf_path under linux. +% 23/01/2014 - Add full path to pdftops.txt in warning. +% 27/05/2015 - Fixed alert in case of missing pdftops; fixed code indentation +% 02/05/2016 - Added possible error explanation suggested by Michael Pacer (issue #137) +% 02/05/2016 - Search additional possible paths suggested by Jonas Stein (issue #147) +% 03/05/2016 - Display the specific error message if pdftops fails for some reason (issue #148) + + % Call pdftops + [varargout{1:nargout}] = system(sprintf('"%s" %s', xpdf_path, cmd)); +end + +function path_ = xpdf_path + % Return a valid path + % Start with the currently set path + path_ = user_string('pdftops'); + % Check the path works + if check_xpdf_path(path_) + return + end + % Check whether the binary is on the path + if ispc + bin = 'pdftops.exe'; + else + bin = 'pdftops'; + end + if check_store_xpdf_path(bin) + path_ = bin; + return + end + % Search the obvious places + if ispc + paths = {'C:\Program Files\xpdf\pdftops.exe', 'C:\Program Files (x86)\xpdf\pdftops.exe'}; + else + paths = {'/usr/bin/pdftops', '/usr/local/bin/pdftops'}; + end + for a = 1:numel(paths) + path_ = paths{a}; + if check_store_xpdf_path(path_) + return + end + end + + % Ask the user to enter the path + errMsg1 = 'Pdftops not found. Please locate the program, or install xpdf-tools from '; + url1 = 'http://foolabs.com/xpdf'; + fprintf(2, '%s\n', [errMsg1 '' url1 '']); + errMsg1 = [errMsg1 url1]; + %if strncmp(computer,'MAC',3) % Is a Mac + % % Give separate warning as the MacOS uigetdir dialogue box doesn't have a title + % uiwait(warndlg(errMsg1)) + %end + + % Provide an alternative possible explanation as per issue #137 + errMsg2 = 'If you have pdftops installed, perhaps Matlab is shaddowing it as described in '; + url2 = 'https://github.com/altmany/export_fig/issues/137'; + fprintf(2, '%s\n', [errMsg2 'issue #137']); + errMsg2 = [errMsg2 url1]; + + state = 0; + while 1 + if state + option1 = 'Install pdftops'; + else + option1 = 'Issue #137'; + end + answer = questdlg({errMsg1,'',errMsg2},'Pdftops error',option1,'Locate pdftops','Cancel','Cancel'); + drawnow; % prevent a Matlab hang: http://undocumentedmatlab.com/blog/solving-a-matlab-hang-problem + switch answer + case 'Install pdftops' + web('-browser',url1); + case 'Issue #137' + web('-browser',url2); + state = 1; + case 'Locate pdftops' + base = uigetdir('/', errMsg1); + if isequal(base, 0) + % User hit cancel or closed window + break + end + base = [base filesep]; %#ok + bin_dir = {'', ['bin' filesep], ['lib' filesep]}; + for a = 1:numel(bin_dir) + path_ = [base bin_dir{a} bin]; + if exist(path_, 'file') == 2 + break + end + end + if check_store_xpdf_path(path_) + return + end + + otherwise % User hit Cancel or closed window + break + end + end + error('pdftops executable not found.'); +end + +function good = check_store_xpdf_path(path_) + % Check the path is valid + good = check_xpdf_path(path_); + if ~good + return + end + % Update the current default path to the path found + if ~user_string('pdftops', path_) + warning('Path to pdftops executable could not be saved. Enter it manually in %s.', fullfile(fileparts(which('user_string.m')), '.ignore', 'pdftops.txt')); + return + end +end + +function good = check_xpdf_path(path_) + % Check the path is valid + [good, message] = system(sprintf('"%s" -h', path_)); %#ok + % system returns good = 1 even when the command runs + % Look for something distinct in the help text + good = ~isempty(strfind(message, 'PostScript')); + + % Display the error message if the pdftops executable exists but fails for some reason + if ~good && exist(path_,'file') % file exists but generates an error + fprintf('Error running %s:\n', path_); + fprintf(2,'%s\n\n',message); + end +end diff --git a/offline/localizer/altmany-export_fig/print2array.m b/offline/localizer/altmany-export_fig/print2array.m new file mode 100755 index 00000000..5f85c7c9 --- /dev/null +++ b/offline/localizer/altmany-export_fig/print2array.m @@ -0,0 +1,244 @@ +function [A, bcol] = print2array(fig, res, renderer, gs_options) +%PRINT2ARRAY Exports a figure to an image array +% +% Examples: +% A = print2array +% A = print2array(figure_handle) +% A = print2array(figure_handle, resolution) +% A = print2array(figure_handle, resolution, renderer) +% A = print2array(figure_handle, resolution, renderer, gs_options) +% [A bcol] = print2array(...) +% +% This function outputs a bitmap image of the given figure, at the desired +% resolution. +% +% If renderer is '-painters' then ghostcript needs to be installed. This +% can be downloaded from: http://www.ghostscript.com +% +% IN: +% figure_handle - The handle of the figure to be exported. Default: gcf. +% resolution - Resolution of the output, as a factor of screen +% resolution. Default: 1. +% renderer - string containing the renderer paramater to be passed to +% print. Default: '-opengl'. +% gs_options - optional ghostscript options (e.g.: '-dNoOutputFonts'). If +% multiple options are needed, enclose in call array: {'-a','-b'} +% +% OUT: +% A - MxNx3 uint8 image of the figure. +% bcol - 1x3 uint8 vector of the background color + +% Copyright (C) Oliver Woodford 2008-2014, Yair Altman 2015- +%{ +% 05/09/11: Set EraseModes to normal when using opengl or zbuffer +% renderers. Thanks to Pawel Kocieniewski for reporting the issue. +% 21/09/11: Bug fix: unit8 -> uint8! Thanks to Tobias Lamour for reporting it. +% 14/11/11: Bug fix: stop using hardcopy(), as it interfered with figure size +% and erasemode settings. Makes it a bit slower, but more reliable. +% Thanks to Phil Trinh and Meelis Lootus for reporting the issues. +% 09/12/11: Pass font path to ghostscript. +% 27/01/12: Bug fix affecting painters rendering tall figures. Thanks to +% Ken Campbell for reporting it. +% 03/04/12: Bug fix to median input. Thanks to Andy Matthews for reporting it. +% 26/10/12: Set PaperOrientation to portrait. Thanks to Michael Watts for +% reporting the issue. +% 26/02/15: If temp dir is not writable, use the current folder for temp +% EPS/TIF files (Javier Paredes) +% 27/02/15: Display suggested workarounds to internal print() error (issue #16) +% 28/02/15: Enable users to specify optional ghostscript options (issue #36) +% 10/03/15: Fixed minor warning reported by Paul Soderlind; fixed code indentation +% 28/05/15: Fixed issue #69: patches with LineWidth==0.75 appear wide (internal bug in Matlab's print() func) +% 07/07/15: Fixed issue #83: use numeric handles in HG1 +%} + + % Generate default input arguments, if needed + if nargin < 2 + res = 1; + if nargin < 1 + fig = gcf; + end + end + % Warn if output is large + old_mode = get(fig, 'Units'); + set(fig, 'Units', 'pixels'); + px = get(fig, 'Position'); + set(fig, 'Units', old_mode); + npx = prod(px(3:4)*res)/1e6; + if npx > 30 + % 30M pixels or larger! + warning('MATLAB:LargeImage', 'print2array generating a %.1fM pixel image. This could be slow and might also cause memory problems.', npx); + end + % Retrieve the background colour + bcol = get(fig, 'Color'); + % Set the resolution parameter + res_str = ['-r' num2str(ceil(get(0, 'ScreenPixelsPerInch')*res))]; + % Generate temporary file name + tmp_nam = [tempname '.tif']; + try + % Ensure that the temp dir is writable (Javier Paredes 26/2/15) + fid = fopen(tmp_nam,'w'); + fwrite(fid,1); + fclose(fid); + delete(tmp_nam); % cleanup + isTempDirOk = true; + catch + % Temp dir is not writable, so use the current folder + [dummy,fname,fext] = fileparts(tmp_nam); %#ok + fpath = pwd; + tmp_nam = fullfile(fpath,[fname fext]); + isTempDirOk = false; + end + % Enable users to specify optional ghostscript options (issue #36) + if nargin > 3 && ~isempty(gs_options) + if iscell(gs_options) + gs_options = sprintf(' %s',gs_options{:}); + elseif ~ischar(gs_options) + error('gs_options input argument must be a string or cell-array of strings'); + else + gs_options = [' ' gs_options]; + end + else + gs_options = ''; + end + if nargin > 2 && strcmp(renderer, '-painters') + % Print to eps file + if isTempDirOk + tmp_eps = [tempname '.eps']; + else + tmp_eps = fullfile(fpath,[fname '.eps']); + end + print2eps(tmp_eps, fig, 0, renderer, '-loose'); + try + % Initialize the command to export to tiff using ghostscript + cmd_str = ['-dEPSCrop -q -dNOPAUSE -dBATCH ' res_str ' -sDEVICE=tiff24nc']; + % Set the font path + fp = font_path(); + if ~isempty(fp) + cmd_str = [cmd_str ' -sFONTPATH="' fp '"']; + end + % Add the filenames + cmd_str = [cmd_str ' -sOutputFile="' tmp_nam '" "' tmp_eps '"' gs_options]; + % Execute the ghostscript command + ghostscript(cmd_str); + catch me + % Delete the intermediate file + delete(tmp_eps); + rethrow(me); + end + % Delete the intermediate file + delete(tmp_eps); + % Read in the generated bitmap + A = imread(tmp_nam); + % Delete the temporary bitmap file + delete(tmp_nam); + % Set border pixels to the correct colour + if isequal(bcol, 'none') + bcol = []; + elseif isequal(bcol, [1 1 1]) + bcol = uint8([255 255 255]); + else + for l = 1:size(A, 2) + if ~all(reshape(A(:,l,:) == 255, [], 1)) + break; + end + end + for r = size(A, 2):-1:l + if ~all(reshape(A(:,r,:) == 255, [], 1)) + break; + end + end + for t = 1:size(A, 1) + if ~all(reshape(A(t,:,:) == 255, [], 1)) + break; + end + end + for b = size(A, 1):-1:t + if ~all(reshape(A(b,:,:) == 255, [], 1)) + break; + end + end + bcol = uint8(median(single([reshape(A(:,[l r],:), [], size(A, 3)); reshape(A([t b],:,:), [], size(A, 3))]), 1)); + for c = 1:size(A, 3) + A(:,[1:l-1, r+1:end],c) = bcol(c); + A([1:t-1, b+1:end],:,c) = bcol(c); + end + end + else + if nargin < 3 + renderer = '-opengl'; + end + err = false; + % Set paper size + old_pos_mode = get(fig, 'PaperPositionMode'); + old_orientation = get(fig, 'PaperOrientation'); + set(fig, 'PaperPositionMode', 'auto', 'PaperOrientation', 'portrait'); + try + % Workaround for issue #69: patches with LineWidth==0.75 appear wide (internal bug in Matlab's print() function) + fp = []; % in case we get an error below + fp = findall(fig, 'Type','patch', 'LineWidth',0.75); + set(fp, 'LineWidth',0.5); + % Fix issue #83: use numeric handles in HG1 + if ~using_hg2(fig), fig = double(fig); end + % Print to tiff file + print(fig, renderer, res_str, '-dtiff', tmp_nam); + % Read in the printed file + A = imread(tmp_nam); + % Delete the temporary file + delete(tmp_nam); + catch ex + err = true; + end + set(fp, 'LineWidth',0.75); % restore original figure appearance + % Reset paper size + set(fig, 'PaperPositionMode', old_pos_mode, 'PaperOrientation', old_orientation); + % Throw any error that occurred + if err + % Display suggested workarounds to internal print() error (issue #16) + fprintf(2, 'An error occured with Matlab''s builtin print function.\nTry setting the figure Renderer to ''painters'' or use opengl(''software'').\n\n'); + rethrow(ex); + end + % Set the background color + if isequal(bcol, 'none') + bcol = []; + else + bcol = bcol * 255; + if isequal(bcol, round(bcol)) + bcol = uint8(bcol); + else + bcol = squeeze(A(1,1,:)); + end + end + end + % Check the output size is correct + if isequal(res, round(res)) + px = round([px([4 3])*res 3]); % round() to avoid an indexing warning below + if ~isequal(size(A), px) + % Correct the output size + A = A(1:min(end,px(1)),1:min(end,px(2)),:); + end + end +end + +% Function to return (and create, where necessary) the font path +function fp = font_path() + fp = user_string('gs_font_path'); + if ~isempty(fp) + return + end + % Create the path + % Start with the default path + fp = getenv('GS_FONTPATH'); + % Add on the typical directories for a given OS + if ispc + if ~isempty(fp) + fp = [fp ';']; + end + fp = [fp getenv('WINDIR') filesep 'Fonts']; + else + if ~isempty(fp) + fp = [fp ':']; + end + fp = [fp '/usr/share/fonts:/usr/local/share/fonts:/usr/share/fonts/X11:/usr/local/share/fonts/X11:/usr/share/fonts/truetype:/usr/local/share/fonts/truetype']; + end + user_string('gs_font_path', fp); +end diff --git a/offline/localizer/altmany-export_fig/print2eps.m b/offline/localizer/altmany-export_fig/print2eps.m new file mode 100755 index 00000000..dc7deb1d --- /dev/null +++ b/offline/localizer/altmany-export_fig/print2eps.m @@ -0,0 +1,529 @@ +function print2eps(name, fig, export_options, varargin) +%PRINT2EPS Prints figures to eps with improved line styles +% +% Examples: +% print2eps filename +% print2eps(filename, fig_handle) +% print2eps(filename, fig_handle, export_options) +% print2eps(filename, fig_handle, export_options, print_options) +% +% This function saves a figure as an eps file, with two improvements over +% MATLAB's print command. First, it improves the line style, making dashed +% lines more like those on screen and giving grid lines a dotted line style. +% Secondly, it substitutes original font names back into the eps file, +% where these have been changed by MATLAB, for up to 11 different fonts. +% +%IN: +% filename - string containing the name (optionally including full or +% relative path) of the file the figure is to be saved as. A +% ".eps" extension is added if not there already. If a path is +% not specified, the figure is saved in the current directory. +% fig_handle - The handle of the figure to be saved. Default: gcf(). +% export_options - array or struct of optional scalar values: +% bb_padding - Scalar value of amount of padding to add to border around +% the cropped image, in points (if >1) or percent (if <1). +% Can be negative as well as positive; Default: 0 +% crop - Cropping flag. Deafult: 0 +% fontswap - Whether to swap non-default fonts in figure. Default: true +% renderer - Renderer used to generate bounding-box. Default: 'opengl' +% crop_amounts - 4-element vector of crop amounts: [top,right,bottom,left] +% (available only via the struct alternative) +% print_options - Additional parameter strings to be passed to the print command + +%{ +% Copyright (C) Oliver Woodford 2008-2014, Yair Altman 2015- + +% The idea of editing the EPS file to change line styles comes from Jiro +% Doke's FIXPSLINESTYLE (fex id: 17928) +% The idea of changing dash length with line width came from comments on +% fex id: 5743, but the implementation is mine :) +%} +%{ +% 14/11/11: Fix a MATLAB bug rendering black or white text incorrectly. +% Thanks to Mathieu Morlighem for reporting the issue and +% obtaining a fix from TMW. +% 08/12/11: Added ability to correct fonts. Several people have requested +% this at one time or another, and also pointed me to printeps +% (fex id: 7501), so thank you to them. My implementation (which +% was not inspired by printeps - I'd already had the idea for my +% approach) goes slightly further in that it allows multiple +% fonts to be swapped. +% 14/12/11: Fix bug affecting font names containing spaces. Thanks to David +% Szwer for reporting the issue. +% 25/01/12: Add a font not to be swapped. Thanks to Anna Rafferty and Adam +% Jackson for reporting the issue. Also fix a bug whereby using a +% font alias can lead to another font being swapped in. +% 10/04/12: Make the font swapping case insensitive. +% 26/10/12: Set PaperOrientation to portrait. Thanks to Michael Watts for +% reporting the issue. +% 26/10/12: Fix issue to do with swapping fonts changing other fonts and +% sizes we don't want, due to listeners. Thanks to Malcolm Hudson +% for reporting the issue. +% 22/03/13: Extend font swapping to axes labels. Thanks to Rasmus Ischebeck +% for reporting the issue. +% 23/07/13: Bug fix to font swapping. Thanks to George for reporting the +% issue. +% 13/08/13: Fix MATLAB feature of not exporting white lines correctly. +% Thanks to Sebastian Hesslinger for reporting it. +% 24/02/15: Fix for Matlab R2014b bug (issue #31): LineWidths<0.75 are not +% set in the EPS (default line width is used) +% 25/02/15: Fixed issue #32: BoundingBox problem caused uncropped EPS/PDF files +% 05/03/15: Fixed issue #43: Inability to perform EPS file post-processing +% 06/03/15: Improved image padding & cropping thanks to Oscar Hartogensis +% 21/03/15: Fixed edge-case of missing handles having a 'FontName' property +% 26/03/15: Attempt to fix issue #45: white lines in subplots do not print correctly +% 27/03/15: Attempt to fix issue #44: white artifact lines appearing in patch exports +% 30/03/15: Fixed issue #52: improved performance on HG2 (R2014b+) +% 09/04/15: Comment blocks consolidation and minor code cleanup (no real code change) +% 12/04/15: Fixed issue #56: bad cropping +% 14/04/15: Workaround for issue #45: lines in image subplots are exported in invalid color +% 07/07/15: Added option to avoid font-swapping in EPS/PDF +% 07/07/15: Fixed issue #83: use numeric handles in HG1 +% 22/07/15: Fixed issue #91 (thanks to Carlos Moffat) +% 28/09/15: Fixed issue #108 (thanks to JacobD10) +% 01/11/15: Fixed issue #112: optional renderer for bounding-box computation (thanks to Jesús Pestana Puerta) +% 21/02/16: Enabled specifying non-automated crop amounts +% 22/02/16: Better support + backward compatibility for transparency (issue #108) +% 10/06/16: Fixed issue #159: text handles get cleared by Matlab in the print() command +% 12/06/16: Improved the fix for issue #159 (in the previous commit) +% 12/06/16: Fixed issue #158: transparent patch color in PDF/EPS +%} + + options = {'-loose'}; + if nargin > 3 + options = [options varargin]; + elseif nargin < 3 + export_options = 0; + if nargin < 2 + fig = gcf(); + end + end + + % Retrieve padding, crop & font-swap values + crop_amounts = nan(1,4); % auto-crop all 4 sides by default + if isstruct(export_options) + try fontswap = export_options.fontswap; catch, fontswap = true; end + try bb_crop = export_options.crop; catch, bb_crop = 0; end + try crop_amounts = export_options.crop_amounts; catch, end + try bb_padding = export_options.bb_padding; catch, bb_padding = 0; end + try renderer = export_options.rendererStr; catch, renderer = 'opengl'; end % fix for issue #110 + if renderer(1)~='-', renderer = ['-' renderer]; end + else + if numel(export_options) > 2 % font-swapping + fontswap = export_options(3); + else + fontswap = true; + end + if numel(export_options) > 1 % cropping + bb_crop = export_options(2); + else + bb_crop = 0; % scalar value, so use default bb_crop value of 0 + end + if numel(export_options) > 0 % padding + bb_padding = export_options(1); + else + bb_padding = 0; + end + renderer = '-opengl'; + end + + % Construct the filename + if numel(name) < 5 || ~strcmpi(name(end-3:end), '.eps') + name = [name '.eps']; % Add the missing extension + end + + % Set paper size + old_pos_mode = get(fig, 'PaperPositionMode'); + old_orientation = get(fig, 'PaperOrientation'); + set(fig, 'PaperPositionMode', 'auto', 'PaperOrientation', 'portrait'); + + % Find all the used fonts in the figure + font_handles = findall(fig, '-property', 'FontName'); + fonts = get(font_handles, 'FontName'); + if isempty(fonts) + fonts = {}; + elseif ~iscell(fonts) + fonts = {fonts}; + end + + % Map supported font aliases onto the correct name + fontsl = lower(fonts); + for a = 1:numel(fonts) + f = fontsl{a}; + f(f==' ') = []; + switch f + case {'times', 'timesnewroman', 'times-roman'} + fontsl{a} = 'times-roman'; + case {'arial', 'helvetica'} + fontsl{a} = 'helvetica'; + case {'newcenturyschoolbook', 'newcenturyschlbk'} + fontsl{a} = 'newcenturyschlbk'; + otherwise + end + end + fontslu = unique(fontsl); + + % Determine the font swap table + if fontswap + matlab_fonts = {'Helvetica', 'Times-Roman', 'Palatino', 'Bookman', 'Helvetica-Narrow', 'Symbol', ... + 'AvantGarde', 'NewCenturySchlbk', 'Courier', 'ZapfChancery', 'ZapfDingbats'}; + matlab_fontsl = lower(matlab_fonts); + require_swap = find(~ismember(fontslu, matlab_fontsl)); + unused_fonts = find(~ismember(matlab_fontsl, fontslu)); + font_swap = cell(3, min(numel(require_swap), numel(unused_fonts))); + fonts_new = fonts; + for a = 1:size(font_swap, 2) + font_swap{1,a} = find(strcmp(fontslu{require_swap(a)}, fontsl)); + font_swap{2,a} = matlab_fonts{unused_fonts(a)}; + font_swap{3,a} = fonts{font_swap{1,a}(1)}; + fonts_new(font_swap{1,a}) = font_swap(2,a); + end + else + font_swap = []; + end + + % Swap the fonts + if ~isempty(font_swap) + fonts_size = get(font_handles, 'FontSize'); + if iscell(fonts_size) + fonts_size = cell2mat(fonts_size); + end + M = false(size(font_handles)); + + % Loop because some changes may not stick first time, due to listeners + c = 0; + update = zeros(1000, 1); + for b = 1:10 % Limit number of loops to avoid infinite loop case + for a = 1:numel(M) + M(a) = ~isequal(get(font_handles(a), 'FontName'), fonts_new{a}) || ~isequal(get(font_handles(a), 'FontSize'), fonts_size(a)); + if M(a) + set(font_handles(a), 'FontName', fonts_new{a}, 'FontSize', fonts_size(a)); + c = c + 1; + update(c) = a; + end + end + if ~any(M) + break; + end + end + + % Compute the order to revert fonts later, without the need of a loop + [update, M] = unique(update(1:c)); + [M, M] = sort(M); + update = reshape(update(M), 1, []); + end + + % MATLAB bug fix - black and white text can come out inverted sometimes + % Find the white and black text + black_text_handles = findall(fig, 'Type', 'text', 'Color', [0 0 0]); + white_text_handles = findall(fig, 'Type', 'text', 'Color', [1 1 1]); + % Set the font colors slightly off their correct values + set(black_text_handles, 'Color', [0 0 0] + eps); + set(white_text_handles, 'Color', [1 1 1] - eps); + + % MATLAB bug fix - white lines can come out funny sometimes + % Find the white lines + white_line_handles = findall(fig, 'Type', 'line', 'Color', [1 1 1]); + % Set the line color slightly off white + set(white_line_handles, 'Color', [1 1 1] - 0.00001); + + % Workaround for issue #45: lines in image subplots are exported in invalid color + % In this case the -depsc driver solves the problem, but then all the other workarounds + % below (for all the other issues) will fail, so it's better to let the user decide by + % just issuing a warning and accepting the '-depsc' input parameter + epsLevel2 = ~any(strcmpi(options,'-depsc')); + if epsLevel2 + % Use -depsc2 (EPS color level-2) if -depsc (EPS color level-3) was not specifically requested + options{end+1} = '-depsc2'; + % Issue a warning if multiple images & lines were found in the figure, and HG1 with painters renderer is used + isPainters = any(strcmpi(options,'-painters')); + if isPainters && ~using_hg2 && numel(findall(fig,'Type','image'))>1 && ~isempty(findall(fig,'Type','line')) + warning('YMA:export_fig:issue45', ... + ['Multiple images & lines detected. In such cases, the lines might \n' ... + 'appear with an invalid color due to an internal MATLAB bug (fixed in R2014b). \n' ... + 'Possible workaround: add a ''-depsc'' or ''-opengl'' parameter to the export_fig command.']); + end + end + + % Fix issue #83: use numeric handles in HG1 + if ~using_hg2(fig), fig = double(fig); end + + % Workaround for when transparency is lost through conversion fig>EPS>PDF (issue #108) + % Replace transparent patch RGB values with an ID value (rare chance that ID color is being used already) + if using_hg2 + origAlphaColors = eps_maintainAlpha(fig); + end + + % Print to eps file + print(fig, options{:}, name); + + % Do post-processing on the eps file + try + % Read the EPS file into memory + fstrm = read_write_entire_textfile(name); + catch + fstrm = ''; + end + + % Restore colors for transparent patches/lines and apply the + % setopacityalpha setting in the EPS file (issue #108) + if using_hg2 + [~,fstrm,foundFlags] = eps_maintainAlpha(fig, fstrm, origAlphaColors); + + % If some of the transparencies were not found in the EPS file, then rerun the + % export with only the found transparencies modified (backward compatibility) + if ~isempty(fstrm) && ~all(foundFlags) + foundIdx = find(foundFlags); + for objIdx = 1 : sum(foundFlags) + colorsIdx = foundIdx(objIdx); + colorsData = origAlphaColors{colorsIdx}; + hObj = colorsData{1}; + propName = colorsData{2}; + newColor = colorsData{4}; + hObj.(propName).ColorData = newColor; + end + delete(name); + print(fig, options{:}, name); + fstrm = read_write_entire_textfile(name); + [~,fstrm] = eps_maintainAlpha(fig, fstrm, origAlphaColors(foundFlags)); + end + end + + % Fix for Matlab R2014b bug (issue #31): LineWidths<0.75 are not set in the EPS (default line width is used) + try + if ~isempty(fstrm) && using_hg2(fig) + % Convert miter joins to line joins + %fstrm = regexprep(fstrm, '\n10.0 ML\n', '\n1 LJ\n'); + % This is faster (the original regexprep could take many seconds when the axes contains many lines): + fstrm = strrep(fstrm, sprintf('\n10.0 ML\n'), sprintf('\n1 LJ\n')); + + % In HG2, grid lines and axes Ruler Axles have a default LineWidth of 0.5 => replace en-bulk (assume that 1.0 LineWidth = 1.333 LW) + % hAxes=gca; hAxes.YGridHandle.LineWidth, hAxes.YRuler.Axle.LineWidth + %fstrm = regexprep(fstrm, '(GC\n2 setlinecap\n1 LJ)\nN', '$1\n0.667 LW\nN'); + % This is faster: + fstrm = strrep(fstrm, sprintf('GC\n2 setlinecap\n1 LJ\nN'), sprintf('GC\n2 setlinecap\n1 LJ\n0.667 LW\nN')); + + % This is more accurate but *MUCH* slower (issue #52) + %{ + % Modify all thin lines in the figure to have 10x LineWidths + hLines = findall(fig,'Type','line'); + hThinLines = []; + for lineIdx = 1 : numel(hLines) + thisLine = hLines(lineIdx); + if thisLine.LineWidth < 0.75 && strcmpi(thisLine.Visible,'on') + hThinLines(end+1) = thisLine; %#ok + thisLine.LineWidth = thisLine.LineWidth * 10; + end + end + + % If any thin lines were found + if ~isempty(hThinLines) + % Prepare an EPS with large-enough line widths + print(fig, options{:}, name); + % Restore the original LineWidths in the figure + for lineIdx = 1 : numel(hThinLines) + thisLine = handle(hThinLines(lineIdx)); + thisLine.LineWidth = thisLine.LineWidth / 10; + end + + % Compare the original and the new EPS files and correct the original stream's LineWidths + fstrm_new = read_write_entire_textfile(name); + idx = 500; % skip heading with its possibly-different timestamp + markerStr = sprintf('10.0 ML\nN'); + markerLen = length(markerStr); + while ~isempty(idx) && idx < length(fstrm) + lastIdx = min(length(fstrm), length(fstrm_new)); + delta = fstrm(idx+1:lastIdx) - fstrm_new(idx+1:lastIdx); + idx = idx + find(delta,1); + if ~isempty(idx) && ... + isequal(fstrm(idx-markerLen+1:idx), markerStr) && ... + ~isempty(regexp(fstrm_new(idx-markerLen+1:idx+12),'10.0 ML\n[\d\.]+ LW\nN')) %#ok + value = str2double(regexprep(fstrm_new(idx:idx+12),' .*','')); + if isnan(value), break; end % something's wrong... - bail out + newStr = sprintf('%0.3f LW\n',value/10); + fstrm = [fstrm(1:idx-1) newStr fstrm(idx:end)]; + idx = idx + 12; + else + break; + end + end + end + %} + + % This is much faster although less accurate: fix all non-gray lines to have a LineWidth of 0.75 (=1 LW) + % Note: This will give incorrect LineWidth of 075 for lines having LineWidth<0.75, as well as for non-gray grid-lines (if present) + % However, in practice these edge-cases are very rare indeed, and the difference in LineWidth should not be noticeable + %fstrm = regexprep(fstrm, '([CR]C\n2 setlinecap\n1 LJ)\nN', '$1\n1 LW\nN'); + % This is faster (the original regexprep could take many seconds when the axes contains many lines): + fstrm = strrep(fstrm, sprintf('\n2 setlinecap\n1 LJ\nN'), sprintf('\n2 setlinecap\n1 LJ\n1 LW\nN')); + end + catch err + fprintf(2, 'Error fixing LineWidths in EPS file: %s\n at %s:%d\n', err.message, err.stack(1).file, err.stack(1).line); + end + + % Reset the font and line colors + try + set(black_text_handles, 'Color', [0 0 0]); + set(white_text_handles, 'Color', [1 1 1]); + catch + % Fix issue #159: redo findall() '*text_handles' + black_text_handles = findall(fig, 'Type', 'text', 'Color', [0 0 0]+eps); + white_text_handles = findall(fig, 'Type', 'text', 'Color', [1 1 1]-eps); + set(black_text_handles, 'Color', [0 0 0]); + set(white_text_handles, 'Color', [1 1 1]); + end + set(white_line_handles, 'Color', [1 1 1]); + + % Reset paper size + set(fig, 'PaperPositionMode', old_pos_mode, 'PaperOrientation', old_orientation); + + % Reset the font names in the figure + if ~isempty(font_swap) + for a = update + set(font_handles(a), 'FontName', fonts{a}, 'FontSize', fonts_size(a)); + end + end + + % Bail out if EPS post-processing is not possible + if isempty(fstrm) + warning('Loading EPS file failed, so unable to perform post-processing. This is usually because the figure contains a large number of patch objects. Consider exporting to a bitmap format in this case.'); + return + end + + % Replace the font names + if ~isempty(font_swap) + for a = 1:size(font_swap, 2) + %fstrm = regexprep(fstrm, [font_swap{1,a} '-?[a-zA-Z]*\>'], font_swap{3,a}(~isspace(font_swap{3,a}))); + fstrm = regexprep(fstrm, font_swap{2,a}, font_swap{3,a}(~isspace(font_swap{3,a}))); + end + end + + % Move the bounding box to the top of the file (HG2 only), or fix the line styles (HG1 only) + if using_hg2(fig) + % Move the bounding box to the top of the file (HG2 only) + [s, e] = regexp(fstrm, '%%BoundingBox: [^%]*%%'); + if numel(s) == 2 + fstrm = fstrm([1:s(1)-1 s(2):e(2)-2 e(1)-1:s(2)-1 e(2)-1:end]); + end + else + % Fix the line styles (HG1 only) + fstrm = fix_lines(fstrm); + end + + % Apply the bounding box padding & cropping, replacing Matlab's print()'s bounding box + if bb_crop + % Calculate a new bounding box based on a bitmap print using crop_border.m + % 1. Determine the Matlab BoundingBox and PageBoundingBox + [s,e] = regexp(fstrm, '%%BoundingBox: [^%]*%%'); % location BB in eps file + if numel(s)==2, s=s(2); e=e(2); end + aa = fstrm(s+15:e-3); % dimensions bb - STEP1 + bb_matlab = cell2mat(textscan(aa,'%f32%f32%f32%f32')); % dimensions bb - STEP2 + + [s,e] = regexp(fstrm, '%%PageBoundingBox: [^%]*%%'); % location bb in eps file + if numel(s)==2, s=s(2); e=e(2); end + aa = fstrm(s+19:e-3); % dimensions bb - STEP1 + pagebb_matlab = cell2mat(textscan(aa,'%f32%f32%f32%f32')); % dimensions bb - STEP2 + + % 2. Create a bitmap image and use crop_borders to create the relative + % bb with respect to the PageBoundingBox + [A, bcol] = print2array(fig, 1, renderer); + [aa, aa, aa, bb_rel] = crop_borders(A, bcol, bb_padding, crop_amounts); + + % 3. Calculate the new Bounding Box + pagew = pagebb_matlab(3)-pagebb_matlab(1); + pageh = pagebb_matlab(4)-pagebb_matlab(2); + %bb_new = [pagebb_matlab(1)+pagew*bb_rel(1) pagebb_matlab(2)+pageh*bb_rel(2) ... + % pagebb_matlab(1)+pagew*bb_rel(3) pagebb_matlab(2)+pageh*bb_rel(4)]; + bb_new = pagebb_matlab([1,2,1,2]) + [pagew,pageh,pagew,pageh].*bb_rel; % clearer + bb_offset = (bb_new-bb_matlab) + [-1,-1,1,1]; % 1px margin so that cropping is not TOO tight + + % Apply the bounding box padding + if bb_padding + if abs(bb_padding)<1 + bb_padding = round((mean([bb_new(3)-bb_new(1) bb_new(4)-bb_new(2)])*bb_padding)/0.5)*0.5; % ADJUST BB_PADDING + end + add_padding = @(n1, n2, n3, n4) sprintf(' %d', str2double({n1, n2, n3, n4}) + [-bb_padding -bb_padding bb_padding bb_padding] + bb_offset); + else + add_padding = @(n1, n2, n3, n4) sprintf(' %d', str2double({n1, n2, n3, n4}) + bb_offset); % fix small but noticeable bounding box shift + end + fstrm = regexprep(fstrm, '%%BoundingBox:[ ]+([-]?\d+)[ ]+([-]?\d+)[ ]+([-]?\d+)[ ]+([-]?\d+)', '%%BoundingBox:${add_padding($1, $2, $3, $4)}'); + end + + % Fix issue #44: white artifact lines appearing in patch exports + % Note: the problem is due to the fact that Matlab's print() function exports patches + % as a combination of filled triangles, and a white line appears where the triangles touch + % In the workaround below, we will modify such dual-triangles into a filled rectangle. + % We are careful to only modify regexps that exactly match specific patterns - it's better to not + % correct some white-line artifacts than to change the geometry of a patch, or to corrupt the EPS. + % e.g.: '0 -450 937 0 0 450 3 MP PP 937 0 0 -450 0 450 3 MP PP' => '0 -450 937 0 0 450 0 0 4 MP' + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\2 \1 \3 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\2 \3 \1 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\3 \1 \2 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + fstrm = regexprep(fstrm, '\n([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) ([-\d.]+ [-\d.]+) 3 MP\nPP\n\3 \2 \1 3 MP\nPP\n','\n$1 $2 $3 0 0 4 MP\nPP\n'); + + % Write out the fixed eps file + read_write_entire_textfile(name, fstrm); +end + +function [StoredColors, fstrm, foundFlags] = eps_maintainAlpha(fig, fstrm, StoredColors) + if nargin == 1 % in: convert transparency in Matlab figure into unique RGB colors + hObjs = findall(fig); %findobj(fig,'Type','Area'); + StoredColors = {}; + propNames = {'Face','Edge'}; + for objIdx = 1:length(hObjs) + hObj = hObjs(objIdx); + for propIdx = 1 : numel(propNames) + try + propName = propNames{propIdx}; + if strcmp(hObj.(propName).ColorType, 'truecoloralpha') + nColors = length(StoredColors); + oldColor = hObj.(propName).ColorData; + newColor = uint8([101; 102+floor(nColors/255); mod(nColors,255); 255]); + StoredColors{end+1} = {hObj, propName, oldColor, newColor}; + hObj.(propName).ColorData = newColor; + end + catch + % Never mind - ignore (either doesn't have the property or cannot change it) + end + end + end + else % restore transparency in Matlab figure by converting back from the unique RGBs + %Find the transparent patches + wasError = false; + nColors = length(StoredColors); + foundFlags = false(1,nColors); + for objIdx = 1 : nColors + colorsData = StoredColors{objIdx}; + hObj = colorsData{1}; + propName = colorsData{2}; + origColor = colorsData{3}; + newColor = colorsData{4}; + try + %Restore the EPS files patch color + colorID = num2str(round(double(newColor(1:3)') /255,3),'%.3g %.3g %.3g'); %ID for searching + origRGB = num2str(round(double(origColor(1:3)')/255,3),'%.3g %.3g %.3g'); %Replace with original color + origAlpha = num2str(round(double(origColor(end)) /255,3),'%.3g'); %Convert alpha value for EPS + + %Find and replace the RGBA values within the EPS text fstrm + if strcmpi(propName,'Face') + oldStr = sprintf(['\n' colorID ' RC\nN\n']); + newStr = sprintf(['\n' origRGB ' RC\n' origAlpha ' .setopacityalpha true\nN\n']); + else %'Edge' + oldStr = sprintf(['\n' colorID ' RC\n1 LJ\n']); + newStr = sprintf(['\n' origRGB ' RC\n' origAlpha ' .setopacityalpha true\n']); + end + foundFlags(objIdx) = ~isempty(strfind(fstrm, oldStr)); + fstrm = strrep(fstrm, oldStr, newStr); + + %Restore the figure object's original color + hObj.(propName).ColorData = origColor; + catch err + % something is wrong - cannot restore transparent color... + if ~wasError + fprintf(2, 'Error maintaining transparency in EPS file: %s\n at %s:%d\n', err.message, err.stack(1).file, err.stack(1).line); + wasError = true; + end + end + end + end +end diff --git a/offline/localizer/altmany-export_fig/read_write_entire_textfile.m b/offline/localizer/altmany-export_fig/read_write_entire_textfile.m new file mode 100755 index 00000000..9fabb22d --- /dev/null +++ b/offline/localizer/altmany-export_fig/read_write_entire_textfile.m @@ -0,0 +1,37 @@ +%READ_WRITE_ENTIRE_TEXTFILE Read or write a whole text file to/from memory +% +% Read or write an entire text file to/from memory, without leaving the +% file open if an error occurs. +% +% Reading: +% fstrm = read_write_entire_textfile(fname) +% Writing: +% read_write_entire_textfile(fname, fstrm) +% +%IN: +% fname - Pathname of text file to be read in. +% fstrm - String to be written to the file, including carriage returns. +% +%OUT: +% fstrm - String read from the file. If an fstrm input is given the +% output is the same as that input. + +function fstrm = read_write_entire_textfile(fname, fstrm) +modes = {'rt', 'wt'}; +writing = nargin > 1; +fh = fopen(fname, modes{1+writing}); +if fh == -1 + error('Unable to open file %s.', fname); +end +try + if writing + fwrite(fh, fstrm, 'char*1'); + else + fstrm = fread(fh, '*char')'; + end +catch ex + fclose(fh); + rethrow(ex); +end +fclose(fh); +end diff --git a/offline/localizer/altmany-export_fig/user_string.m b/offline/localizer/altmany-export_fig/user_string.m new file mode 100755 index 00000000..8d35b3d5 --- /dev/null +++ b/offline/localizer/altmany-export_fig/user_string.m @@ -0,0 +1,105 @@ +function string = user_string(string_name, string) +%USER_STRING Get/set a user specific string +% +% Examples: +% string = user_string(string_name) +% isSaved = user_string(string_name, new_string) +% +% Function to get and set a string in a system or user specific file. This +% enables, for example, system specific paths to binaries to be saved. +% +% The specified string will be saved in a file named .txt, +% either in a subfolder named .ignore under this file's folder, or in the +% user's prefdir folder (in case this file's folder is non-writable). +% +% IN: +% string_name - String containing the name of the string required, which +% sets the filename storing the string: .txt +% new_string - The new string to be saved in the .txt file +% +% OUT: +% string - The currently saved string. Default: '' +% isSaved - Boolean indicating whether the save was succesful + +% Copyright (C) Oliver Woodford 2011-2014, Yair Altman 2015- + +% This method of saving paths avoids changing .m files which might be in a +% version control system. Instead it saves the user dependent paths in +% separate files with a .txt extension, which need not be checked in to +% the version control system. Thank you to Jonas Dorn for suggesting this +% approach. + +% 10/01/2013 - Access files in text, not binary mode, as latter can cause +% errors. Thanks to Christian for pointing this out. +% 29/05/2015 - Save file in prefdir if current folder is non-writable (issue #74) + + if ~ischar(string_name) + error('string_name must be a string.'); + end + % Create the full filename + fname = [string_name '.txt']; + dname = fullfile(fileparts(mfilename('fullpath')), '.ignore'); + file_name = fullfile(dname, fname); + if nargin > 1 + % Set string + if ~ischar(string) + error('new_string must be a string.'); + end + % Make sure the save directory exists + %dname = fileparts(file_name); + if ~exist(dname, 'dir') + % Create the directory + try + if ~mkdir(dname) + string = false; + return + end + catch + string = false; + return + end + % Make it hidden + try + fileattrib(dname, '+h'); + catch + end + end + % Write the file + fid = fopen(file_name, 'wt'); + if fid == -1 + % file cannot be created/updated - use prefdir if file does not already exist + % (if file exists but is simply not writable, don't create a duplicate in prefdir) + if ~exist(file_name,'file') + file_name = fullfile(prefdir, fname); + fid = fopen(file_name, 'wt'); + end + if fid == -1 + string = false; + return; + end + end + try + fprintf(fid, '%s', string); + catch + fclose(fid); + string = false; + return + end + fclose(fid); + string = true; + else + % Get string + fid = fopen(file_name, 'rt'); + if fid == -1 + % file cannot be read, try to read the file in prefdir + file_name = fullfile(prefdir, fname); + fid = fopen(file_name, 'rt'); + if fid == -1 + string = ''; + return + end + end + string = fgetl(fid); + fclose(fid); + end +end diff --git a/offline/localizer/altmany-export_fig/using_hg2.m b/offline/localizer/altmany-export_fig/using_hg2.m new file mode 100755 index 00000000..ba72228b --- /dev/null +++ b/offline/localizer/altmany-export_fig/using_hg2.m @@ -0,0 +1,36 @@ +%USING_HG2 Determine if the HG2 graphics engine is used +% +% tf = using_hg2(fig) +% +%IN: +% fig - handle to the figure in question. +% +%OUT: +% tf - boolean indicating whether the HG2 graphics engine is being used +% (true) or not (false). + +% 19/06/2015 - Suppress warning in R2015b; cache result for improved performance +% 06/06/2016 - Fixed issue #156 (bad return value in R2016b) + +function tf = using_hg2(fig) + persistent tf_cached + if isempty(tf_cached) + try + if nargin < 1, fig = figure('visible','off'); end + oldWarn = warning('off','MATLAB:graphicsversion:GraphicsVersionRemoval'); + try + % This generates a [supressed] warning in R2015b: + tf = ~graphicsversion(fig, 'handlegraphics'); + catch + tf = ~verLessThan('matlab','8.4'); % =R2014b + end + warning(oldWarn); + catch + tf = false; + end + if nargin < 1, delete(fig); end + tf_cached = tf; + else + tf = tf_cached; + end +end diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py index 336f868c..cd10eccb 100644 --- a/offline/localizer/get_logs.py +++ b/offline/localizer/get_logs.py @@ -16,16 +16,19 @@ exit() messages = d['sensor_data'] -map_names = ['imu', 'imu_ang_pos', 'imu_ang_vel', +map_names = ['imu_linear_no_grav', 'imu_ang_pos', 'imu_ang_vel', 'gps', 'encoder', 'steering'] -map_names = np.asarray(map_names, dtype='object') +map_names_nd = np.asarray(map_names, dtype='object') if combined: logs = [] for msg in messages: - name = msg['topicName'] - name = name[8:] - ind = map_names.index(name) + try: + name = msg['topicName'] + name = str(name[8:]) + ind = map_names.index(name) + except: + continue if name == 'imu_linear_no_grav': logs.append([ @@ -33,7 +36,10 @@ msg['x'], msg['y'], msg['z'], 0, 0, 0, 0, 0, 0]) elif name == 'imu_ang_pos': line = [ind, msg['timestamp']] - line.extend(msg['rot']) + R = msg['rot'][0] + R.extend(msg['rot'][1]) + R.extend(msg['rot'][2]) + line.extend(R) logs.append(line) elif name == 'imu_ang_vel': logs.append([ @@ -42,8 +48,8 @@ elif name == 'gps': logs.append([ ind, msg['timestamp'], - msg['latitude'], msg['longitude']], - msg['antennaAltitude', 0, 0, 0, 0, 0, 0]) + msg['latitude'], msg['longitude'], + msg['antennaAltitude'], 0, 0, 0, 0, 0, 0]) elif name == 'encoder': logs.append([ ind, msg['timestamp'], @@ -54,7 +60,8 @@ msg['angle'], 0, 0, 0, 0, 0, 0, 0, 0]) logs = np.array(logs) - vars_map = {'map_names': map_names, 'logs': logs} + print(logs.shape) + vars_map = {'map_names': map_names_nd, 'logs': logs} scipy.io.savemat('./roll_logs_combined.mat', mdict=vars_map) else: @@ -101,7 +108,7 @@ encoder = np.array(encoder) steering = np.array(steering) - vars_map = {'map_names': map_names, 'imu': imu, + vars_map = {'map_names': map_names_nd, 'imu': imu, 'imu_ang_pos': imu_ang_pos, 'imu_ang_vel': imu_ang_vel, 'gps': gps, 'encoder': encoder, 'steering': steering} scipy.io.savemat('./roll_logs_separate.mat', mdict=vars_map) diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m index 20409451..ff52c3c3 100644 --- a/offline/localizer/localizer.m +++ b/offline/localizer/localizer.m @@ -1,31 +1,72 @@ -% localizer code, as seen in java codebase -% Bereket Abraham - -% TODO: change dt?, graph map, compare to raw latlon, fix mistakes - -addpath('./latlonutm/Codes/matlab'); -addpath('./zoharby-plot_google_map') - -% constants -dt = (1/10) * 1000; % 10 Hz in ms -wheel_base = 1.13; -utm_zone = 17; -Q = eye(7); % model noise covariance -R = eye(7); % measurement noise covariance - -% globals -first_theta = 250; -lat_long = [40.441670, -79.9416362]; -[x, y, zone] = ll2utm(lat_long(1), lat_long(2)); -first_gps = [x y]; -last_gps = [x y]; -utm_zone = zone; % fixed -last_encoder = 0; -last_encoder_time = 0; - -function llog = log_line(x) +function [trajectory] = localizer() + % localizer code, as seen in java codebase + % Bereket Abraham + + % TODO: change dt?, compare to raw latlon, granular position + + addpath('./latlonutm/Codes/matlab'); + + global wheel_base + save_data = true; + + % constants + dt = (1/10) * 1000; % 10 Hz in ms + wheel_base = 1.13; + utm_zone = 17; + first_theta = 250; + lat_long = [40.441670, -79.9416362]; + + [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); + first_gps = [x y]; + last_gps = [x y]; + utm_zone = zone; % fixed + last_encoder = 0; + last_encoder_time = 0; + + % initialize Kalman filter + P = eye(7); % covariance + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + 0; % d_Xb, body velocity + 0; % d_Yb + first_theta; % theta, deg, global + 0; % d_theta, deg/s + 0]; % heading, deg, body orientation + + trajectory = [X; lat_long'; norm(P)]; + + load('./roll_logs_combined.mat'); + + for i = 1:size(logs, 1) + llog = logs(i, :); + name = map_names{llog(1) + 1}; + time = llog(2); + + if strcmp(name, 'gps') + [C, z] = gps_meaurement(llog(3), llog(4), first_gps, last_gps, first_theta); + elseif strcmp(name,'encoder') + [C, z] = encoder_meaurement(llog(3), time, last_encoder, last_encoder_time); + elseif strcmp(name, 'steering') + [C, z] = steering_measurement(llog(3)); + else + continue; + end + + [P_pre, X_pre] = predict_step(P, X, dt); + [P, X] = update_step(C, z, P_pre, X_pre); + snapshot = summarize(P, X, utm_zone); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('localizer_v2.mat', 'trajectory', 'P'); + end +end + +function snapshot = summarize(P, x, utm_zone) [lat, lon] = utm2ll(x(1), x(2), utm_zone); - llog = [lat, lon, x(5)]'; + snapshot = [x; lat; lon; norm(P)]; end function x = scrubAngles(x) @@ -39,8 +80,9 @@ end end -function A = model(x) +function A = model(x, dt) % dynamic model + global wheel_base theta = deg2rad(x(3)); heading = deg2rad(x(7)); A = [1, 0, dt*cos(theta), -dt*sin(theta), 0, 0, 0; @@ -52,22 +94,26 @@ 0, 0, 0, 0, 0, 0, 1]; end -function [P_pre, x_pre] = predict_step(P, x) - A = model(x); +function [P_pre, x_pre] = predict_step(P, x, dt) + R = eye(7); % measurement noise covariance + A = model(x, dt); x_pre = A * x; - P_pre = eye(7) * P * eye(7)'; % !error! P_pre = A * P * A' + R; + % P_pre = eye(7) * P * eye(7)'; + P_pre = A * P * A' + R; x_pre = scrubAngles(x_pre); end function [P, x] = update_step(C, z, P_pre, x_pre) + Q = eye(7); % model noise covariance residual = z - (C * x_pre); residual = scrubAngles(residual); K = P_pre * C' * inv((C * P_pre * C') + Q); % gain x = x_pre + (K * residual); - P = (eye(7) - (K * C)); % !error! P = (eye(7) - (K * C)) * P_pre; + % P = (eye(7) - (K * C)); + P = (eye(7) - (K * C)) * P_pre; end -function [C, z] = gps_meaurement(lat, long) +function [C, z] = gps_meaurement(lat, long, first_gps, last_gps, first_theta) [x, y, ~] = ll2utm(lat, long); dx = x - last_gps(1); dy = y - last_gps(2); @@ -93,7 +139,7 @@ % TODO possible precision errors since changes are so small % maybe batch encoder updates until we get a decent margin -function [C, z] = encoder_meaurement(encoder_dist, encoder_time) +function [C, z] = encoder_meaurement(encoder_dist, encoder_time, last_encoder, last_encoder_time) dx = encoder_dist - last_encoder; dt = encoder_time - last_encoder_time; C = zeros(7, 7); @@ -116,7 +162,8 @@ z(7) = heading; end -function [C, z] = imu_measurement(R) +function [C, z] = imu_ang_pos_measurement(R) + R = reshape(R, 3,3)'; x = R(1, 1); x = R(1, 2); theta = 90 - rad2deg(atan2(y, x)); @@ -127,44 +174,3 @@ z(5) = theta; end - -% initialize Kalman filter -P = eye(7); % covariance - -X = [x; % X, m, UTM coors - y; % Y, m, UTM coors - 0; % d_Xb, body velocity - 0; % d_Yb - first_theta; % theta, deg, global - 0; % d_theta, deg/s - 0]; % heading, deg, body orientation - -trajectory = [X; norm(P)]; - -load('./roll_logs.mat'); - -for i = 1:size(logs, 2) - llog = logs(:, i); - if llog(1) == 'sensors/gps' - time = llog(2); - [C, z] = gps_meaurement(llog(3), llog(4)); - elseif llog(1) == 'sensors/encoder' - time = llog(2); - [C, z] = encoder_meaurement(llog(3), time) - elseif llog(1) == 'sensors/steering' - time = llog(2); - [C, z] = steering_measurement(llog(3)) - end - - [P_pre, X_pre] = predict_step(P, X); - [P, X] = update_step(C, z, P_pre, X_pre) - trajectory = [trajectory, [X; norm(P)]]; -end - -% graph results -lat = [48.8708 51.5188 41.9260 40.4312 52.523 37.982]; -lon = [2.4131 -0.1300 12.4951 -3.6788 13.415 23.715]; -plot(lon, lat, '.r', 'MarkerSize', 20) -plot_google_map - - diff --git a/offline/localizer/notes.txt b/offline/localizer/notes.txt new file mode 100644 index 00000000..e1a3abf5 --- /dev/null +++ b/offline/localizer/notes.txt @@ -0,0 +1,52 @@ +double[][] motionModel = { + // x y x_b y_b th th_dot heading + { 1, 0, Math.cos(th) * diff, -Math.sin(th) * diff, 0, 0, 0 }, // x + { 0, 1, Math.sin(th) * diff, Math.cos(th) * diff, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_b + { 0, 0, 0, 1, 0, 0, 0 }, // y_b + { 0, 0, 0, 0, 1, diff, 0 }, // th + { 0, 0, 180 / (Math.PI * (wheelBase / Math.sin(heading))), 0, + 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading +}; + +Measurement stuff: + +GPS: +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Oct 9, 2016 6:17:15 AM","latitude":40.441633333333336,"north":true,"longitude":-79.941644,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":1.18,"antennaAltitude":292.1,"rawGPSLat":4026.498,"rawGPSLong":7956.49864,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235951,"topicName":"sensors/gps","sequenceNumber":290}, + +Get x from latitude, y from longitude +x = x + x_dot*cos(theta)*dt - y_dot*sin(theta)*dt +y = y + y_dot*sin(theta)*dt + ydot*cos(theta)*dt + +Encoder: +{"VERSION_ID":"encoderV0.0","distance":0.4336513119533528,"velocity":NaN,"dataWord":8.0,"accel":NaN,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235950,"topicName":"sensors/encoder","sequenceNumber":14473}, + +Get x_dot from distance +x_dot = x_dot +y_dot = y_dot + +y_dot completely unaffected by measurement (if we're sliding sideways, something's going horribly wrong...) + +theta currently computed by the difference between our new GPS reading and the previous one: +if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) + + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { + th = startAngle; +} +theta_dot currently completely unaffected by measurement + +Ideally, we'd get theta and theta_dot from an IMU message. + +IMU Angular Position: + +{"VERSION_ID":"imuAngularPositionV0.0","rot":[[0.25653892010450363,0.030666396021842957,-0.01811222732067108],[0.032628461718559265,0.9959298074245453,-0.04417908191680908],[0.014278098940849304,-0.045562922954559326,0.25790490955114365]],"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235961,"topicName":"sensors/imu_ang_pos","sequenceNumber":18152}, + +IMU Angular Velocity: + +{"VERSION_ID":"imuAngularVelocityV0.0","x":0.14453125,"y":-0.361328125,"z":-0.3271484375,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235961,"topicName":"sensors/imu_ang_vel","sequenceNumber":18152}, + +Steering: + +{"VERSION_ID":"steering","angle":0.07,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235966,"topicName":"sensors/steering","sequenceNumber":14474}, + +phi is the angle field. Careful to check the topicName; we want sensors/steering, not sensors/commandedSteering diff --git a/offline/localizer/trajectory_analysis.m b/offline/localizer/trajectory_analysis.m new file mode 100644 index 00000000..ba218b3d --- /dev/null +++ b/offline/localizer/trajectory_analysis.m @@ -0,0 +1,33 @@ +% graph results + +addpath('./latlonutm/Codes/matlab'); +% addpath('./zoharby-plot_google_map'); +addpath('./altmany-export_fig'); + +load('localizer_v2.mat', 'trajectory', 'P'); +save_plot = false; + +k = 1000; +wmline(trajectory(8,1:k:end), trajectory(9,1:k:end), 'Color', 'r') + +t1 = 1476008235676; +t2 = 1476009019782; +t = linspace(0, (t2 - t1)/1000, size(trajectory,2)); +figure(); +plot(t, trajectory(10,:)) + +% plot(trajectory(8,:), trajectory(9,:), '.r', 'MarkerSize', 20) +% plot_google_map('maptype','roadmap', 'APIKey','AIzaSyBYLaDqr2QKRCCRAHhrOf21DVJL-kkkyr8') + +if save_plot + set(gcf,'renderer','zbuffer') + export_fig('out.jpg') +end + +theta = deg2rad( trajectory(5,1:k:end) ); +figure(); +hold on; +plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) +quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(theta), sin(theta)) +hold off; + From 5ed83f1f0ee82cd9916efdcc5bb71e8042a67cda Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 23 Oct 2016 20:13:26 -0400 Subject: [PATCH 005/149] new analsyis and localizer fixes --- offline/localizer/get_logs.py | 23 +++++-- offline/localizer/localizer.m | 87 ++++++++++++++++++------- offline/localizer/notes.txt | 6 ++ offline/localizer/plot_sensor.m | 33 ++++++++++ offline/localizer/trajectory_analysis.m | 9 ++- 5 files changed, 127 insertions(+), 31 deletions(-) create mode 100644 offline/localizer/plot_sensor.m diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py index cd10eccb..3924793e 100644 --- a/offline/localizer/get_logs.py +++ b/offline/localizer/get_logs.py @@ -17,8 +17,9 @@ messages = d['sensor_data'] map_names = ['imu_linear_no_grav', 'imu_ang_pos', 'imu_ang_vel', - 'gps', 'encoder', 'steering'] + 'gps', 'encoder', 'steering', 'imu_temp'] map_names_nd = np.asarray(map_names, dtype='object') +start_time = messages[0]['timestamp'] if combined: logs = [] @@ -58,10 +59,15 @@ logs.append([ ind, msg['timestamp'], msg['angle'], 0, 0, 0, 0, 0, 0, 0, 0]) + elif name == 'imu_temp': + logs.append([ + ind, msg['timestamp'], + msg['temperature'], 0, 0, 0, 0, 0, 0, 0, 0]) logs = np.array(logs) print(logs.shape) - vars_map = {'map_names': map_names_nd, 'logs': logs} + vars_map = {'map_names': map_names_nd, 'start_time': start_time, + 'logs': logs} scipy.io.savemat('./roll_logs_combined.mat', mdict=vars_map) else: @@ -71,6 +77,7 @@ gps = [] encoder = [] steering = [] + compass = [] for msg in messages: name = msg['topicName'] @@ -102,13 +109,19 @@ steering.append([ ind, msg['timestamp'], msg['angle']]) + elif name == 'imu_temp': + compass.append([ + ind, msg['timestamp'], + msg['temperature']]) imu = np.array(imu) gps = np.array(gps) encoder = np.array(encoder) steering = np.array(steering) + compass = np.array(compass) - vars_map = {'map_names': map_names_nd, 'imu': imu, - 'imu_ang_pos': imu_ang_pos, 'imu_ang_vel': imu_ang_vel, - 'gps': gps, 'encoder': encoder, 'steering': steering} + vars_map = {'map_names': map_names_nd, 'start_time': start_time, + 'imu': imu, 'imu_ang_pos': imu_ang_pos, + 'imu_ang_vel': imu_ang_vel, 'gps': gps, 'encoder': encoder, + 'steering': steering, 'compass': compass} scipy.io.savemat('./roll_logs_separate.mat', mdict=vars_map) diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m index ff52c3c3..f9c69864 100644 --- a/offline/localizer/localizer.m +++ b/offline/localizer/localizer.m @@ -2,15 +2,21 @@ % localizer code, as seen in java codebase % Bereket Abraham - % TODO: change dt?, compare to raw latlon, granular position + % TODO: compare to raw latlon, granular position truth? + % TODO: UNITS, use IMU body accel addpath('./latlonutm/Codes/matlab'); global wheel_base + global last_gps + global last_encoder + global last_encoder_time + global last_time save_data = true; % constants - dt = (1/10) * 1000; % 10 Hz in ms + ref_dt = (1/10) * 1000; % 10 Hz in ms + % dt of the controller, not the filter wheel_base = 1.13; utm_zone = 17; first_theta = 250; @@ -20,8 +26,7 @@ first_gps = [x y]; last_gps = [x y]; utm_zone = zone; % fixed - last_encoder = 0; - last_encoder_time = 0; + last_encoder = -1; % initialize Kalman filter P = eye(7); % covariance @@ -37,6 +42,9 @@ trajectory = [X; lat_long'; norm(P)]; load('./roll_logs_combined.mat'); + start_time = double(start_time); + last_encoder_time = start_time; + last_time = start_time; for i = 1:size(logs, 1) llog = logs(i, :); @@ -44,23 +52,29 @@ time = llog(2); if strcmp(name, 'gps') - [C, z] = gps_meaurement(llog(3), llog(4), first_gps, last_gps, first_theta); + [C, z] = gps_meaurement(llog(3), llog(4), first_gps, first_theta); + % [C, z] = gps_meaurement_xy(llog(3), llog(4)); elseif strcmp(name,'encoder') - [C, z] = encoder_meaurement(llog(3), time, last_encoder, last_encoder_time); + if (last_encoder == -1) + last_encoder = llog(3); + end + [C, z] = encoder_meaurement(llog(3), time); elseif strcmp(name, 'steering') [C, z] = steering_measurement(llog(3)); + % elseif strcmp(name, 'imu_temp') + % [C, z] = compass_measurement(llog(3)); else continue; end - [P_pre, X_pre] = predict_step(P, X, dt); + [P_pre, X_pre] = predict_step(P, X, time); [P, X] = update_step(C, z, P_pre, X_pre); snapshot = summarize(P, X, utm_zone); trajectory = [trajectory, snapshot]; end if save_data - save('localizer_v2.mat', 'trajectory', 'P'); + save('localizer_v3.mat', 'trajectory', 'P'); end end @@ -80,25 +94,30 @@ end end -function A = model(x, dt) - % dynamic model +% generate dynamic model +function [A] = model(x, time) global wheel_base + global last_time + + dt = (time - last_time) / 1000.0; + last_time = time; theta = deg2rad(x(3)); heading = deg2rad(x(7)); + A = [1, 0, dt*cos(theta), -dt*sin(theta), 0, 0, 0; 0, 1, dt*sin(theta), dt*cos(theta), 0, 0, 0; 0, 0, 1, 0, 0, 0, 0; 0, 0, 0, 1, 0, 0, 0; 0, 0, 0, 0, 1, dt, 0; - 0, 0, 180/(pi*(wheel_base/sin(heading))), 0, 0, 0, 0; + 0, 0, 180.0/(pi*(wheel_base/sin(heading))), 0, 0, 0, 0; 0, 0, 0, 0, 0, 0, 1]; end -function [P_pre, x_pre] = predict_step(P, x, dt) +function [P_pre, x_pre] = predict_step(P, x, time) R = eye(7); % measurement noise covariance - A = model(x, dt); + A = model(x, time); x_pre = A * x; - % P_pre = eye(7) * P * eye(7)'; + % P_pre = eye(7) * P * eye(7)'; % ERROR !! P_pre = A * P * A' + R; x_pre = scrubAngles(x_pre); end @@ -109,11 +128,13 @@ residual = scrubAngles(residual); K = P_pre * C' * inv((C * P_pre * C') + Q); % gain x = x_pre + (K * residual); - % P = (eye(7) - (K * C)); + % P = (eye(7) - (K * C)); % ERROR !! P = (eye(7) - (K * C)) * P_pre; end -function [C, z] = gps_meaurement(lat, long, first_gps, last_gps, first_theta) +function [C, z] = gps_meaurement(lat, long, first_gps, first_theta) + global last_gps + [x, y, ~] = ll2utm(lat, long); dx = x - last_gps(1); dy = y - last_gps(2); @@ -137,22 +158,31 @@ z = [x, y, 0, 0, theta, 0 , 0]'; end +function [C, z] = gps_meaurement_xy(lat, long) + [x, y, ~] = ll2utm(lat, long); + C = zeros(7, 7); + C(1, 1) = 1; + C(2, 2) = 1; + z = [x, y, 0, 0, 0, 0 , 0]'; +end + % TODO possible precision errors since changes are so small % maybe batch encoder updates until we get a decent margin -function [C, z] = encoder_meaurement(encoder_dist, encoder_time, last_encoder, last_encoder_time) +function [C, z] = encoder_meaurement(encoder_dist, encoder_time) + global last_encoder + global last_encoder_time + dx = encoder_dist - last_encoder; dt = encoder_time - last_encoder_time; C = zeros(7, 7); z = zeros(7, 1); - if (dt <= 1) - return; + if (dt > 1) + last_encoder = encoder_dist; + last_encoder_time = encoder_time; + C(3, 3) = 1; + z(3) = dx / (dt / 1000.0); end - - last_encoder = encoder_dist; - last_encoder_time = encoder_time; - C(3, 3) = 1; - z(3) = dx / (dt / 1000); end function [C, z] = steering_measurement(heading) @@ -174,3 +204,12 @@ z(5) = theta; end +% TODO add dtheta +function [C, z] = compass_measurement(c_angle) + C = zeros(7, 7); + C(5, 5) = 1; + z = zeros(7, 1); + z(5) = c_angle; +end + + diff --git a/offline/localizer/notes.txt b/offline/localizer/notes.txt index e1a3abf5..339c89e1 100644 --- a/offline/localizer/notes.txt +++ b/offline/localizer/notes.txt @@ -50,3 +50,9 @@ Steering: {"VERSION_ID":"steering","angle":0.07,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235966,"topicName":"sensors/steering","sequenceNumber":14474}, phi is the angle field. Careful to check the topicName; we want sensors/steering, not sensors/commandedSteering + +IMU Temperature: +might be the compass angle + +{"VERSION_ID":"temperatureV0.1","temperature":13.53125,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235676,"topicName":"sensors/imu_temp","sequenceNumber":18117}, + diff --git a/offline/localizer/plot_sensor.m b/offline/localizer/plot_sensor.m new file mode 100644 index 00000000..3dd8a897 --- /dev/null +++ b/offline/localizer/plot_sensor.m @@ -0,0 +1,33 @@ +clear; +close all; + +load('./roll_logs_combined.mat'); +start_time = double(start_time); +% ['imu_linear_no_grav', = 0 linear accel +% 'imu_ang_pos', = 1 angle position +% 'imu_ang_vel', = 2 angular velocity +% 'gps', = 3 +% 'encoder', = 4 encoder distance in m +% 'steering', = 5 steering wheel angle +% 'imu_temp'] = 6 compass angle, bugs + +imu_linear_acc = logs(logs(:,1) == 0, :); +time = (imu_linear_acc(:, 2) - start_time) / 1000.0; + +% figure(); +% plot(time, imu_linear_acc(:, 3), 'r'); +% figure(); +% plot(time, imu_linear_acc(:, 4), 'b'); + +imu_ang_pos = logs(logs(:,1) == 1, :); +time2 = (imu_ang_pos(:, 2) - start_time) / 1000.0; + +r21 = imu_ang_pos(:, 6); +r11 = imu_ang_pos(:, 3); +yaw = atan2(r21, r11); + +figure(); +plot(time2, yaw, 'r'); + + +% load('localizer_v3.mat', 'trajectory'); diff --git a/offline/localizer/trajectory_analysis.m b/offline/localizer/trajectory_analysis.m index ba218b3d..6f1ef1d7 100644 --- a/offline/localizer/trajectory_analysis.m +++ b/offline/localizer/trajectory_analysis.m @@ -1,10 +1,14 @@ % graph results +clear; +close all; + addpath('./latlonutm/Codes/matlab'); % addpath('./zoharby-plot_google_map'); addpath('./altmany-export_fig'); -load('localizer_v2.mat', 'trajectory', 'P'); +file = 'localizer_v3.mat'; +load(file, 'trajectory', 'P'); save_plot = false; k = 1000; @@ -15,6 +19,7 @@ t = linspace(0, (t2 - t1)/1000, size(trajectory,2)); figure(); plot(t, trajectory(10,:)) +title(['Uncertainty ', file]); % plot(trajectory(8,:), trajectory(9,:), '.r', 'MarkerSize', 20) % plot_google_map('maptype','roadmap', 'APIKey','AIzaSyBYLaDqr2QKRCCRAHhrOf21DVJL-kkkyr8') @@ -30,4 +35,4 @@ plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(theta), sin(theta)) hold off; - +title(['Heading ', file]); From d7de18cb4a9c88e8c83fd4457ea402ce0e3f08c0 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 4 Nov 2016 02:16:00 -0400 Subject: [PATCH 006/149] added skeleton files --- .../localizers/RobobuggyKFLocalizer.java | 40 +++++++++++++++++ .../planners/RobobuggyWaypointFollower.java | 44 +++++++++++++++++++ 2 files changed, 84 insertions(+) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java new file mode 100644 index 00000000..2adb3c43 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -0,0 +1,40 @@ +package com.roboclub.robobuggy.nodes.localizers; + +import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; + +/** + * Created by vivaanbahl on 11/4/16. + */ +public class RobobuggyKFLocalizer extends PeriodicNode { + private Publisher posePub; + + /** + * Create a new {@link PeriodicNode} decorator + * + * @param base {@link BuggyNode} to decorate + * @param period of the periodically executed portion of the node + * @param name + */ + protected RobobuggyKFLocalizer(BuggyNode base, int period, String name) { + super(base, period, name); + posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + } + + @Override + protected void update() { + + } + + @Override + protected boolean startDecoratorNode() { + return false; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java new file mode 100644 index 00000000..0e818332 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java @@ -0,0 +1,44 @@ +package com.roboclub.robobuggy.nodes.planners; + +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.ros.NodeChannel; + +import java.util.ArrayList; + +/** + * Created by vivaanbahl on 11/4/16. + */ +public class RobobuggyWaypointFollower extends PathPlannerNode { + private GPSPoseMessage currentPositionEsitmate; + private ArrayList waypoints; + + private double currentSteeringAngle; + private boolean currentBrakeStatus; + + /** + * Construct a new {@link PathPlannerNode} + * + * @param channel {@link NodeChannel} on which to broadcast status + * information about the node + */ + public RobobuggyWaypointFollower(NodeChannel channel) { + super(channel); + waypoints = new ArrayList<>(); + } + + @Override + protected void updatePositionEstimate(GPSPoseMessage m) { + currentPositionEsitmate = m; + } + + @Override + protected double getCommandedSteeringAngle() { + return currentSteeringAngle; + } + + @Override + protected boolean getDeployBrakeValue() { + return currentBrakeStatus; + } +} From 91735673cb14fe6d6046bc0ccbd76123dafaf646 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 6 Nov 2016 19:03:06 -0500 Subject: [PATCH 007/149] update --- offline/localizer/get_logs.py | 3 ++- offline/localizer/localizer.m | 6 +++--- offline/localizer/plot_sensor.m | 25 ++++++++++++++----------- offline/localizer/trajectory_analysis.m | 4 ++++ 4 files changed, 23 insertions(+), 15 deletions(-) diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py index 3924793e..a09a9650 100644 --- a/offline/localizer/get_logs.py +++ b/offline/localizer/get_logs.py @@ -6,8 +6,9 @@ combined = True d = None -file = '../../../rolls_logs/2016-10-09-06-17-15/sensors_2016-10-09-06-17-15.txt' # file = '../rolls_logs/sample.txt' +# file = '../../../rolls_logs/sensors_2016-10-09-06-17-15.txt' +file = '../../../rolls_logs/sensors_2016-10-15-06-26-53.txt' with open(file) as json_data: d = json.load(json_data) diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m index f9c69864..21007f14 100644 --- a/offline/localizer/localizer.m +++ b/offline/localizer/localizer.m @@ -109,12 +109,12 @@ 0, 0, 1, 0, 0, 0, 0; 0, 0, 0, 1, 0, 0, 0; 0, 0, 0, 0, 1, dt, 0; - 0, 0, 180.0/(pi*(wheel_base/sin(heading))), 0, 0, 0, 0; + 0, 0, 180.0/(pi*(wheel_base/tan(heading))), 0, 0, 0, 0; % used to be sin, ERROR !! 0, 0, 0, 0, 0, 0, 1]; end function [P_pre, x_pre] = predict_step(P, x, time) - R = eye(7); % measurement noise covariance + R = eye(7) .* (0.5); % measurement noise covariance A = model(x, time); x_pre = A * x; % P_pre = eye(7) * P * eye(7)'; % ERROR !! @@ -123,7 +123,7 @@ end function [P, x] = update_step(C, z, P_pre, x_pre) - Q = eye(7); % model noise covariance + Q = eye(7) .* (0.1); % model noise covariance residual = z - (C * x_pre); residual = scrubAngles(residual); K = P_pre * C' * inv((C * P_pre * C') + Q); % gain diff --git a/offline/localizer/plot_sensor.m b/offline/localizer/plot_sensor.m index 3dd8a897..f31718be 100644 --- a/offline/localizer/plot_sensor.m +++ b/offline/localizer/plot_sensor.m @@ -3,6 +3,7 @@ load('./roll_logs_combined.mat'); start_time = double(start_time); + % ['imu_linear_no_grav', = 0 linear accel % 'imu_ang_pos', = 1 angle position % 'imu_ang_vel', = 2 angular velocity @@ -11,23 +12,25 @@ % 'steering', = 5 steering wheel angle % 'imu_temp'] = 6 compass angle, bugs -imu_linear_acc = logs(logs(:,1) == 0, :); -time = (imu_linear_acc(:, 2) - start_time) / 1000.0; - +% imu_linear_acc = logs(logs(:,1) == 0, :); +% time = (imu_linear_acc(:, 2) - start_time) / 1000.0; % figure(); % plot(time, imu_linear_acc(:, 3), 'r'); % figure(); % plot(time, imu_linear_acc(:, 4), 'b'); -imu_ang_pos = logs(logs(:,1) == 1, :); -time2 = (imu_ang_pos(:, 2) - start_time) / 1000.0; -r21 = imu_ang_pos(:, 6); -r11 = imu_ang_pos(:, 3); -yaw = atan2(r21, r11); +% imu_ang_pos = logs(logs(:,1) == 1, :); +% time2 = (imu_ang_pos(:, 2) - start_time) / 1000.0; +% r21 = imu_ang_pos(:, 6); +% r11 = imu_ang_pos(:, 3); +% yaw = atan2(r21, r11); +% figure(); +% plot(time2, yaw, 'r'); -figure(); -plot(time2, yaw, 'r'); +% load('localizer_v3.mat', 'trajectory'); +gps = logs(logs(:,1) == 3, :); +time3 = (gps(:, 2) - start_time) / 1000.0; +wmline(gps(:, 3), gps(:, 4), 'Color', 'r') -% load('localizer_v3.mat', 'trajectory'); diff --git a/offline/localizer/trajectory_analysis.m b/offline/localizer/trajectory_analysis.m index 6f1ef1d7..0dedec1a 100644 --- a/offline/localizer/trajectory_analysis.m +++ b/offline/localizer/trajectory_analysis.m @@ -36,3 +36,7 @@ quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(theta), sin(theta)) hold off; title(['Heading ', file]); + +% plot on google maps +ss = [trajectory(8,1:k:end); trajectory(9,1:k:end)]; +fprintf(1, '%5.20f, %5.20f\n', ss); From 7bb13cd7a74daea1e3d7f8c2bafd6f43675c6e5f Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 6 Nov 2016 23:48:40 -0500 Subject: [PATCH 008/149] Fleshed out some of the details of the skeleton class --- .../localizers/RobobuggyKFLocalizer.java | 87 ++++++++++++++++++- 1 file changed, 85 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 2adb3c43..8e0b6c7a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -1,26 +1,109 @@ package com.roboclub.robobuggy.nodes.localizers; +import Jama.Matrix; +import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.messages.IMUCompassMessage; import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +import java.util.Date; /** * Created by vivaanbahl on 11/4/16. */ public class RobobuggyKFLocalizer extends PeriodicNode { + + // our transmitter for position estimate messages private Publisher posePub; + // matrices that are used in the kalman filter + private Matrix covariance; + private Matrix state; + private Matrix motionMatrix; + + // state variables + private LocTuple currentStateGPS; // current GPS location + private double currentStateEncoder; // current deadreckoning value + private double currentStateHeading; // current heading in degrees + private long currentStateEncoderTime; // current encoder time + private Date currentStateTime; // current time + + // constants we use throughout the file + private final LocTuple initialPosition; + private static final double WHEELBASE = 1.13; // in meters + private static final double[][] SEVEN_ROW_IDENTITY_MATRIX = { + {1, 0, 0, 0, 0, 0, 0}, + {0, 1, 0, 0, 0, 0, 0}, + {0, 0, 1, 0, 0, 0, 0}, + {0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 0, 1} + }; + + + /** * Create a new {@link PeriodicNode} decorator * * @param base {@link BuggyNode} to decorate * @param period of the periodically executed portion of the node - * @param name + * @param name the name of the node + * @param initialPosition the initial position of the localizer */ - protected RobobuggyKFLocalizer(BuggyNode base, int period, String name) { + protected RobobuggyKFLocalizer(BuggyNode base, int period, String name, LocTuple initialPosition) { super(base, period, name); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + this.initialPosition = initialPosition; + + // set our initial covariance + // turns out it's just the I matrix + // todo fill out descriptions + // The covariance matrix consists of 7 rows: + // x - x position in the world frame, in meters + // y - y position in the world frame, in meters + // x_b - + // y_b - + // th - + // th_dot - + // heading - + covariance = new Matrix(SEVEN_ROW_IDENTITY_MATRIX); + + // set our initial state + // - take our initial position and perform UTM conversions on it + + + // add all our subscribers for our current state update stream + // these subscribers are only meant as catchers, just stupidly simple relays + // that feed the current state variables + setupGPSSubscriber(); + setupIMUSubscriber(); + setupEncoderSubscriber(); + } + + private void setupEncoderSubscriber() { + new Subscriber("KF Localizer", NodeChannel.ENCODER.getMsgPath(), ((topicName, m) -> { + EncoderMeasurement deadreckoning = ((EncoderMeasurement) m); + currentStateEncoder = deadreckoning.getDistance(); + })); + } + + private void setupIMUSubscriber() { + new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), ((topicName, m) -> { + IMUCompassMessage compassMessage = ((IMUCompassMessage) m); + currentStateHeading = compassMessage.getCompassHeading(); + })); + } + + private void setupGPSSubscriber() { + new Subscriber("KF Localizer", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { + GpsMeasurement gpsLoc = ((GpsMeasurement) m); + currentStateGPS = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); + })); } @Override From a8bfbfd89f80eb9769acb17e0a71acd8fa456fc3 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 7 Nov 2016 23:44:56 -0500 Subject: [PATCH 009/149] added more to the skeleton localizer --- .../localizers/RobobuggyKFLocalizer.java | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 8e0b6c7a..b1b58b91 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -75,6 +75,7 @@ protected RobobuggyKFLocalizer(BuggyNode base, int period, String name, LocTuple // set our initial state // - take our initial position and perform UTM conversions on it + // todo set the initial state // add all our subscribers for our current state update stream @@ -108,9 +109,32 @@ private void setupGPSSubscriber() { @Override protected void update() { + // a kalman filter consists of two distinct steps - predict and update + + // the predict step is responsible for determining the estimate of the next state + // What we do is we + // + // todo fill this out + predictStep(); + + // the update step is responsible for updating the current state, and resolving + // discrepancies between the prediction and actual state + // What we do is we + // + // + // todo fill this out + updateStep(); + } + + private void predictStep() { + + } + + private void updateStep() { } + @Override protected boolean startDecoratorNode() { return false; From 1e3200442ce02610296e029709436fe5cb3e7392 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 12 Nov 2016 18:57:01 -0500 Subject: [PATCH 010/149] buffed out docs --- .../localizers/RobobuggyKFLocalizer.java | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index b1b58b91..6477a52c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -4,6 +4,7 @@ import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.messages.IMUCompassMessage; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -55,8 +56,8 @@ public class RobobuggyKFLocalizer extends PeriodicNode { * @param name the name of the node * @param initialPosition the initial position of the localizer */ - protected RobobuggyKFLocalizer(BuggyNode base, int period, String name, LocTuple initialPosition) { - super(base, period, name); + protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { + super(new BuggyBaseNode(NodeChannel.POSE), period, name); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); this.initialPosition = initialPosition; @@ -66,8 +67,8 @@ protected RobobuggyKFLocalizer(BuggyNode base, int period, String name, LocTuple // The covariance matrix consists of 7 rows: // x - x position in the world frame, in meters // y - y position in the world frame, in meters - // x_b - - // y_b - + // x_b - the body velocity of the buggy (x-velocity relative to the buggy frame) + // y_b - y velocity relative to the buggy frame (always 0 since we cant strafe) // th - // th_dot - // heading - @@ -110,19 +111,34 @@ private void setupGPSSubscriber() { @Override protected void update() { // a kalman filter consists of two distinct steps - predict and update + /* + + ---> {A} ---> ---> | + ^ |> {filter} ---> ---| + | z ---> | | + | | + ---------------------------------------------------------------------------------| + + */ // the predict step is responsible for determining the estimate of the next state - // What we do is we // - // todo fill this out + // What we do is we take our current state (x[k-1]) and current noise estimate + // (p[k-1]), and plug them into the motion model (A). This lets us determine our + // prediction state (xhat[k]) and our prediction noise (phat[k]) + // predictStep(); // the update step is responsible for updating the current state, and resolving // discrepancies between the prediction and actual state - // What we do is we // + // What we do is we take our measurements (z) that were captured from sensors, + // as well as the predictions from the previous step (xhat[k], phat[k]) and run + // them through the filter which todo fill this part out about innovation etc + // + // This produces your next state (x[k]) and your next noise estimation (p[k]), + // which connect in a feedback loop to become the new current state // - // todo fill this out updateStep(); } From adc9406809f5eb14518e3c27c90aab1e229ab672 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 12 Nov 2016 20:34:59 -0500 Subject: [PATCH 011/149] finished up docs for covariance and the update routine --- .../localizers/RobobuggyKFLocalizer.java | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 6477a52c..18cfd06c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -22,9 +22,12 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private Publisher posePub; // matrices that are used in the kalman filter - private Matrix covariance; - private Matrix state; - private Matrix motionMatrix; + private Matrix covariance; // a matrix that defines our levels of trust for each sensor + private Matrix currentStatePose; // our current position estimate + private Matrix currentStateNoise; // our current noise estimate + private Matrix nextStatePose; // an estimate of the next state, based on the motion model + private Matrix nextStateNoise; // an estimate of the next state's noise, based on the motion model + private Matrix motionModel; // a set of equations that model how the buggy behaves // state variables private LocTuple currentStateGPS; // current GPS location @@ -62,16 +65,14 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition this.initialPosition = initialPosition; // set our initial covariance - // turns out it's just the I matrix - // todo fill out descriptions // The covariance matrix consists of 7 rows: // x - x position in the world frame, in meters // y - y position in the world frame, in meters // x_b - the body velocity of the buggy (x-velocity relative to the buggy frame) // y_b - y velocity relative to the buggy frame (always 0 since we cant strafe) - // th - - // th_dot - - // heading - + // th - yaw angle with respect to the world frame + // th_dot - change in th over time + // heading - current steering angle covariance = new Matrix(SEVEN_ROW_IDENTITY_MATRIX); // set our initial state @@ -140,6 +141,12 @@ protected void update() { // which connect in a feedback loop to become the new current state // updateStep(); + + // publish the current findings + + // and update our understanding of time + currentStatePose = nextStatePose; + currentStateNoise = nextStateNoise; } private void predictStep() { From bd74836c70267dff79bfdea3f412ef191e15b54c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 12 Nov 2016 21:11:21 -0500 Subject: [PATCH 012/149] incremental commit --- .../localizers/RobobuggyKFLocalizer.java | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 18cfd06c..47121f49 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -48,6 +48,13 @@ public class RobobuggyKFLocalizer extends PeriodicNode { {0, 0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 0, 1} }; + private static final int X_GLOBAL_ROW = 0; + private static final int Y_GLOBAL_ROW = 1; + private static final int X_BODY_VELOCITY_ROW = 2; + private static final int Y_BODY_VELOCITY_ROW = 3; + private static final int HEADING_GLOBAL_ROW = 4; + private static final int HEADING_VELOCITY_ROW = 5; + private static final int STEERING_ANGLE_ROW = 6; @@ -64,8 +71,10 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition posePub = new Publisher(NodeChannel.POSE.getMsgPath()); this.initialPosition = initialPosition; - // set our initial covariance - // The covariance matrix consists of 7 rows: + // set our initial state + // - take our initial position and perform UTM conversions on it + // todo set the initial state + // The state matrix consists of 7 rows: // x - x position in the world frame, in meters // y - y position in the world frame, in meters // x_b - the body velocity of the buggy (x-velocity relative to the buggy frame) @@ -73,12 +82,9 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition // th - yaw angle with respect to the world frame // th_dot - change in th over time // heading - current steering angle - covariance = new Matrix(SEVEN_ROW_IDENTITY_MATRIX); - - // set our initial state - // - take our initial position and perform UTM conversions on it - // todo set the initial state + // set our initial covariance + covariance = new Matrix(SEVEN_ROW_IDENTITY_MATRIX); // add all our subscribers for our current state update stream // these subscribers are only meant as catchers, just stupidly simple relays @@ -150,7 +156,17 @@ protected void update() { } private void predictStep() { + Date currentTimeAsDate = new Date(); + + // get the time elapsed in seconds + double timeDiff = (currentTimeAsDate.getTime() - currentStateTime.getTime()) / 1000; + + // get the current state's global heading + double currentStateTH = currentStatePose.get(HEADING_GLOBAL_ROW, 0); + double currentStateSteering = currentStatePose.get(STEERING_ANGLE_ROW, 0); + double[][] motionModel = { + }; } private void updateStep() { From 92955b8b062d78893e8ed1c960ebcc9b372dad34 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 13 Nov 2016 01:18:21 -0500 Subject: [PATCH 013/149] Updated the predict step to include the motion model still need to include documentation/explanation for it --- .../localizers/RobobuggyKFLocalizer.java | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 47121f49..734838ef 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -161,12 +161,35 @@ private void predictStep() { // get the time elapsed in seconds double timeDiff = (currentTimeAsDate.getTime() - currentStateTime.getTime()) / 1000; - // get the current state's global heading + // get the current state's global heading and steering angle double currentStateTH = currentStatePose.get(HEADING_GLOBAL_ROW, 0); double currentStateSteering = currentStatePose.get(STEERING_ANGLE_ROW, 0); + // get our new model based on current heading and steering angle + motionModel = getNewMotionModel(currentStateTH, currentStateSteering, timeDiff); + + } + + private Matrix getNewMotionModel(double currentStateTH, double currentStateSteering, double timeDiff) { + + double motionModelXDotX = Math.cos(currentStateTH) * timeDiff; + double motionModelYDotX = -Math.sin(currentStateTH) * timeDiff; + double motionModelXDotY = Math.sin(currentStateTH) * timeDiff; + double motionModelYDotY = Math.cos(currentStateTH) * timeDiff; + double motionModelHeadingUpdate = 180/ (Math.PI * (WHEELBASE / Math.sin(currentStateSteering))); + double[][] motionModel = { + // x y x_b y_b th th_dot heading + { 1, 0, motionModelXDotX, motionModelYDotX, 0, 0, 0 }, // x + { 0, 1, motionModelXDotY, motionModelYDotY, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_b + { 0, 0, 0, 1, 0, 0, 0 }, // y_b + { 0, 0, 0, 0, 1, timeDiff, 0 }, // th + { 0, 0, motionModelHeadingUpdate, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading }; + + return new Matrix(motionModel); } private void updateStep() { From c60f81f0eb41041001ba91e9e896a336c928674a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 13 Nov 2016 01:35:51 -0500 Subject: [PATCH 014/149] added docs for update, and buffed out docs for predict --- .../localizers/RobobuggyKFLocalizer.java | 27 ++++++++++++++++--- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 734838ef..23c61383 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -168,10 +168,33 @@ private void predictStep() { // get our new model based on current heading and steering angle motionModel = getNewMotionModel(currentStateTH, currentStateSteering, timeDiff); + // calculate the next state prediction + nextStatePose = motionModel.times(currentStatePose); + + // get the new covariance + // todo explain + + // clamp all the angles between -180Ëš and +180Ëš + + } + + private void updateStep() { + // get the innovation matrix + // todo explain + + // clamp all angles in the innovation matrix to -180Ëš to +180Ëš + + // get the kalman gain + // todo explain + + // get the new covariance + // todo explain } private Matrix getNewMotionModel(double currentStateTH, double currentStateSteering, double timeDiff) { + // todo explain + double motionModelXDotX = Math.cos(currentStateTH) * timeDiff; double motionModelYDotX = -Math.sin(currentStateTH) * timeDiff; double motionModelXDotY = Math.sin(currentStateTH) * timeDiff; @@ -192,10 +215,6 @@ private Matrix getNewMotionModel(double currentStateTH, double currentStateSteer return new Matrix(motionModel); } - private void updateStep() { - - } - @Override protected boolean startDecoratorNode() { From 4293f048ee1aeca6ffb83cf0c8b54badc1bd954b Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 13 Nov 2016 01:40:28 -0500 Subject: [PATCH 015/149] ballmer peak ftw reformatted docs --- .../nodes/localizers/RobobuggyKFLocalizer.java | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 23c61383..53192d95 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -118,15 +118,14 @@ private void setupGPSSubscriber() { @Override protected void update() { // a kalman filter consists of two distinct steps - predict and update - /* - - ---> {A} ---> ---> | - ^ |> {filter} ---> ---| - | z ---> | | - | | - ---------------------------------------------------------------------------------| - - */ + // a state diagram is shown below + // + // ---> {A} ---> ---> | + // A |---> {filter} ---> ---| + // | z ---> | | + // | | + // ------------------------------------------------------------------------------------| + // // the predict step is responsible for determining the estimate of the next state // From 5742d56f12528d468d22c3f5eb2fb8aac8ee324a Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 13 Nov 2016 17:02:21 -0500 Subject: [PATCH 016/149] updated RoboBuggyKFLocalizer --- offline/localizer/covariance.m | 27 ++ .../nodes/localizers/KfLocalizer.java | 296 +++++++++--------- .../localizers/RobobuggyKFLocalizer.java | 265 +++++++++++----- 3 files changed, 370 insertions(+), 218 deletions(-) create mode 100644 offline/localizer/covariance.m diff --git a/offline/localizer/covariance.m b/offline/localizer/covariance.m new file mode 100644 index 00000000..830c2ef0 --- /dev/null +++ b/offline/localizer/covariance.m @@ -0,0 +1,27 @@ +% graph results + +clear; +close all; + +addpath('./latlonutm/Codes/matlab'); + +file = 'localizer_v3.mat'; +load(file, 'trajectory', 'P'); + +k = 100; +t1 = 1476008235676; +t2 = 1476009019782; +t = linspace(0, (t2 - t1)/1000, size(trajectory,2)); + +trajectory(5,:) = deg2rad( trajectory(5,:) ); + +Q = cov(trajectory(1:7,:)') + +for i = 1:size(trajectory, 1) + fprintf(1, '%d %5.5f, %5.5f\n', i, mean(trajectory(i,1:k:end)), std(trajectory(i,1:k:end))); +end + +fprintf(1, '\n'); +for i = 1:size(trajectory, 1) + fprintf(1, '%d %5.5f, %5.5f\n', i, mean(trajectory(i,1:k)), std(trajectory(i,1:k))); +end diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java index c696bba7..43f63fb4 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java @@ -83,170 +83,136 @@ public KfLocalizer(int period) { // Initialize subscriber to GPS measurements new Subscriber("htGpsLoc", NodeChannel.GPS.getMsgPath(), - new MessageListener() { - @Override - public synchronized void actionPerformed(String topicName, Message m) { - GpsMeasurement newGPSData = (GpsMeasurement) m; - LocTuple gpsLatLng = new LocTuple(newGPSData - .getLatitude(), newGPSData.getLongitude()); - UTMTuple gpsUTM = LocalizerUtil.deg2UTM(gpsLatLng); - double dx = gpsUTM.getEasting() - lastGPS.getEasting(); - double dy = gpsUTM.getNorthing() - lastGPS.getNorthing(); - double th = Math.toDegrees(Math.atan2(dy, dx)); - lastGPS = gpsUTM; - - double[][] observationModel = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; + new MessageListener() { + @Override + public synchronized void actionPerformed(String topicName, Message m) { + GpsMeasurement newGPSData = (GpsMeasurement) m; + LocTuple gpsLatLng = new LocTuple(newGPSData + .getLatitude(), newGPSData.getLongitude()); + UTMTuple gpsUTM = LocalizerUtil.deg2UTM(gpsLatLng); + double dx = gpsUTM.getEasting() - lastGPS.getEasting(); + double dy = gpsUTM.getNorthing() - lastGPS.getNorthing(); + double th = Math.toDegrees(Math.atan2(dy, dx)); + lastGPS = gpsUTM; + + double[][] observationModel = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 0 } // Heading + }; + + // don't update angle if we did not move a lot + if (Math.sqrt(dx * dx + dy * dy) < .5) {// || + // Math.abs(th- + // state.get(4, + // 0)) > 90){ + observationModel[4][4] = 0; + } - // don't update angle if we did not move a lot - if (Math.sqrt(dx * dx + dy * dy) < .5) {// || - // Math.abs(th- - // state.get(4, - // 0)) > 90){ - observationModel[4][4] = 0; - } - - if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) - + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { - th = startAngle; - } - - double[][] meassurement = { { gpsUTM.getEasting() }, - { gpsUTM.getNorthing() }, { 0 }, { 0 }, { th }, - { 0 }, { 0 } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), new Matrix( - meassurement), new Matrix(updateCovariance)); + if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) + + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { + th = startAngle; } - }); + + double[][] meassurement = { { gpsUTM.getEasting() }, + { gpsUTM.getNorthing() }, { 0 }, { 0 }, { th }, + { 0 }, { 0 } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), new Matrix( + meassurement), new Matrix(updateCovariance)); + } + }); /* new Subscriber( - "HighTrustGpsLoc", - NodeChannel.IMU_ANG_POS.getMsgPath(), - ((topicName, m) -> { - - IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); - // double y = mes.getRot()[0][1]; - // double x = mes.getRot()[0][0]; - double[][] xVar = { { 1 }, { 0 }, { 0 } }; - double[][] yVar = { { 0 }, { 1 }, { 0 } }; - Matrix xMat = new Matrix(xVar); - Matrix yMat = new Matrix(yVar); - Matrix rot = new Matrix(mes.getRot()); - double x = rot.times(xMat).get(0, 0); - double y = rot.times(yMat).get(0, 0); - double th = -(Math.toDegrees(Math.atan2(y, x)) - 90); - double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { th }, { 0 }, { 0 } }; - double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - // updateStep(new Matrix(observationModel),new - // Matrix(meassurement),new Matrix(updateCovariance)); - - })); - */ + "HighTrustGpsLoc", + NodeChannel.IMU_ANG_POS.getMsgPath(), + ((topicName, m) -> { + + IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); + // double y = mes.getRot()[0][1]; + // double x = mes.getRot()[0][0]; + double[][] xVar = { { 1 }, { 0 }, { 0 } }; + double[][] yVar = { { 0 }, { 1 }, { 0 } }; + Matrix xMat = new Matrix(xVar); + Matrix yMat = new Matrix(yVar); + Matrix rot = new Matrix(mes.getRot()); + double x = rot.times(xMat).get(0, 0); + double y = rot.times(yMat).get(0, 0); + double th = -(Math.toDegrees(Math.atan2(y, x)) - 90); + double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x + { 0, 0, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 0 } // Heading + }; + + double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, + { th }, { 0 }, { 0 } }; + double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + // updateStep(new Matrix(observationModel),new + // Matrix(meassurement),new Matrix(updateCovariance)); + + })); + */ // TODO note that we will probably run into precision errors since the // changes are so small // would be good to batch up the encoder updates until we get a margin // that we know can be represented proeprly new Subscriber("htGpsLoc", NodeChannel.ENCODER.getMsgPath(), - new MessageListener() { - @Override - public synchronized void actionPerformed(String topicName, - Message m) { - EncoderMeasurement measurement = (EncoderMeasurement) m; - - // convert the feet from the last message into a delta - // degree, and update our position - double currentEncoderMeasurement = measurement - .getDistance(); - double deltaDistance = currentEncoderMeasurement - - lastEncoderReading; - long currentTime = new Date().getTime(); - long dt = currentTime - lastEncoderReadingTime; - if (dt > 1) { // to remove numeric instability - double bodySpeed = deltaDistance / (dt / 1000.0); - lastEncoderReadingTime = currentTime; - lastEncoderReading = currentEncoderMeasurement; - - double[][] observationModel = { - { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - double[][] meassurement = { { 0 }, { 0 }, - { bodySpeed }, { 0 }, { 0 }, { 0 }, { 0 } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), - new Matrix(meassurement), new Matrix( - updateCovariance)); - } - } - }); + new MessageListener() { + @Override + public synchronized void actionPerformed(String topicName, + Message m) { + EncoderMeasurement measurement = (EncoderMeasurement) m; + + // convert the feet from the last message into a delta + // degree, and update our position + double currentEncoderMeasurement = measurement + .getDistance(); + double deltaDistance = currentEncoderMeasurement + - lastEncoderReading; + long currentTime = new Date().getTime(); + long dt = currentTime - lastEncoderReadingTime; + if (dt > 1) { // to remove numeric instability + double bodySpeed = deltaDistance / (dt / 1000.0); + lastEncoderReadingTime = currentTime; + lastEncoderReading = currentEncoderMeasurement; - new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), - new MessageListener() { - - @Override - public synchronized void actionPerformed(String topicName, - Message m) { - SteeringMeasurement steerM = (SteeringMeasurement) m; double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot_b { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b { 0, 0, 0, 0, 0, 0, 0 }, // th { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // Heading + { 0, 0, 0, 0, 0, 0, 0 } // Heading }; - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { 0 }, { 0 }, { steerM.getAngle() } }; + + double[][] meassurement = { { 0 }, { 0 }, + { bodySpeed }, { 0 }, { 0 }, { 0 }, { 0 } }; double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x { 0, 1, 0, 0, 0, 0, 0 }, // y @@ -256,10 +222,44 @@ public synchronized void actionPerformed(String topicName, { 0, 0, 0, 0, 0, 1, 0 }, // th_dot { 0, 0, 0, 0, 0, 0, 1 } // heading }; - updateStep(new Matrix(observationModel), new Matrix( - meassurement), new Matrix(updateCovariance)); + updateStep(new Matrix(observationModel), + new Matrix(meassurement), new Matrix( + updateCovariance)); } - }); + } + }); + + new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), + new MessageListener() { + + @Override + public synchronized void actionPerformed(String topicName, + Message m) { + SteeringMeasurement steerM = (SteeringMeasurement) m; + double[][] observationModel = { + { 0, 0, 0, 0, 0, 0, 0 }, // x + { 0, 0, 0, 0, 0, 0, 0 }, // y + { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b + { 0, 0, 0, 0, 0, 0, 0 }, // th + { 0, 0, 0, 0, 0, 0, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // Heading + }; + double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, + { 0 }, { 0 }, { steerM.getAngle() } }; + double[][] updateCovariance = { + { 1, 0, 0, 0, 0, 0, 0 }, // x + { 0, 1, 0, 0, 0, 0, 0 }, // y + { 0, 0, 1, 0, 0, 0, 0 }, // x_dot + { 0, 0, 0, 1, 0, 0, 0 }, // y_dot + { 0, 0, 0, 0, 1, 0, 0 }, // th + { 0, 0, 0, 0, 0, 1, 0 }, // th_dot + { 0, 0, 0, 0, 0, 0, 1 } // heading + }; + updateStep(new Matrix(observationModel), new Matrix( + meassurement), new Matrix(updateCovariance)); + } + }); resume(); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index b1b58b91..1f9eddaf 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -9,7 +9,6 @@ import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; import com.roboclub.robobuggy.ros.Subscriber; - import java.util.Date; /** @@ -20,32 +19,41 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // our transmitter for position estimate messages private Publisher posePub; - // matrices that are used in the kalman filter - private Matrix covariance; - private Matrix state; - private Matrix motionMatrix; - - // state variables - private LocTuple currentStateGPS; // current GPS location - private double currentStateEncoder; // current deadreckoning value - private double currentStateHeading; // current heading in degrees - private long currentStateEncoderTime; // current encoder time - private Date currentStateTime; // current time - // constants we use throughout the file - private final LocTuple initialPosition; - private static final double WHEELBASE = 1.13; // in meters - private static final double[][] SEVEN_ROW_IDENTITY_MATRIX = { - {1, 0, 0, 0, 0, 0, 0}, - {0, 1, 0, 0, 0, 0, 0}, - {0, 0, 1, 0, 0, 0, 0}, - {0, 0, 0, 1, 0, 0, 0}, - {0, 0, 0, 0, 1, 0, 0}, - {0, 0, 0, 0, 0, 1, 0}, - {0, 0, 0, 0, 0, 0, 1} - }; + private static final double WHEELBASE = 1.13; // meters + private static final int UTMZONE = 17; + private final LocTuple initialGPS = LocalizerUtil.deg2UTM( + new LocTuple(40.441670, -79.9416362)); + private final double initialHeading = 4.36; // rad + // The state consists of 4 elements: + // x - x position in the world frame, in meters + // y - y position in the world frame, in meters + // dy_body - forward velocity in the body frame, in meters/s + // heading - heading in the world frame, in rad + // dheading - angular velocity in the world frame, in rad/s + private Matrix R = arrayToMatrix({4, 4, 0.25, 0.01, 0.01}); // measurement noise covariance matrix + private Matrix Q = arrayToMatrix({4, 4, 0.25, 0.02, 0.02}); // model noise covariance matrix + private Matrix P = arrayToMatrix({25, 25, 0.25, 2.46, 2.46}); // covariance matrix + private Matrix x; // state + // output matrices + private Matrix C_gps = new Matrix({ + {1, 0, 0, 0, 0}, + {0, 1, 0, 0, 0}, + {0, 0, 0, 1, 0} + }); + private Matrix C_encoder = new Matrix({ + {0, 0, 1, 0, 0} + }); + private Date lastTime; + private UTMTuple lastGPS; + // private Date lastGPSTime; + private double lastEncoder; // deadreckoning value + private long lastEncoderTime; + private double currentEncoder; + private long currentEncoderTime; + private double steeringAngle = 0; /** * Create a new {@link PeriodicNode} decorator @@ -58,80 +66,177 @@ public class RobobuggyKFLocalizer extends PeriodicNode { protected RobobuggyKFLocalizer(BuggyNode base, int period, String name, LocTuple initialPosition) { super(base, period, name); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); - this.initialPosition = initialPosition; - - // set our initial covariance - // turns out it's just the I matrix - // todo fill out descriptions - // The covariance matrix consists of 7 rows: - // x - x position in the world frame, in meters - // y - y position in the world frame, in meters - // x_b - - // y_b - - // th - - // th_dot - - // heading - - covariance = new Matrix(SEVEN_ROW_IDENTITY_MATRIX); - - // set our initial state - // - take our initial position and perform UTM conversions on it - // todo set the initial state + + // set initial state + lastTime = new Date().getTime(); + lastEncoder = 0; + lastEncoderTime = lastTime; + lastGPS = initialGPS; + // lastGPSTime = lastTime; + + x = new Matrix({ + { initialGPS.getEasting() }, + { initialGPS.getNorthing() }, + { 0 }, + { initialHeading }, + { 0 } + }); // add all our subscribers for our current state update stream // these subscribers are only meant as catchers, just stupidly simple relays // that feed the current state variables setupGPSSubscriber(); - setupIMUSubscriber(); + // setupIMUSubscriber(); setupEncoderSubscriber(); + setupWheelSubscriber(); } private void setupEncoderSubscriber() { new Subscriber("KF Localizer", NodeChannel.ENCODER.getMsgPath(), ((topicName, m) -> { - EncoderMeasurement deadreckoning = ((EncoderMeasurement) m); - currentStateEncoder = deadreckoning.getDistance(); - })); - } + long currentTime = new Date().getTime(); + long dt = currentTime - lastEncoderTime; + // to remove numeric instability, limit rate to 10ms, 100Hz + if (dt < 10) { + return; + } - private void setupIMUSubscriber() { - new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), ((topicName, m) -> { - IMUCompassMessage compassMessage = ((IMUCompassMessage) m); - currentStateHeading = compassMessage.getCompassHeading(); + EncoderMeasurement odometry = (EncoderMeasurement) m; + double currentEncoder = odometry.getDistance(); + + double dx = currentEncoder - lastEncoder; + double bodySpeed = dx / (dt / 1000.0); + lastEncoderTime = currentTime; + lastEncoder = currentEncoder; + + // measurement + Matrix z = new Matrix({ + { bodySpeed } + }); + + kalmanFilter(C_encoder, z); })); } + // private void setupIMUSubscriber() { + // new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), ((topicName, m) -> { + // IMUCompassMessage compassMessage = (IMUCompassMessage) m; + // currentHeading = compassMessage.getCompassHeading(); + // })); + // } + private void setupGPSSubscriber() { new Subscriber("KF Localizer", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { - GpsMeasurement gpsLoc = ((GpsMeasurement) m); - currentStateGPS = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); + GpsMeasurement gpsLoc = (GpsMeasurement) m; + LocTuple loc = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); + UTMTuple gps = LocalizerUtil.deg2UTM(loc); + double dx = gps.getEasting() - lastGPS.getEasting(); + double dy = gps.getNorthing() - lastGPS.getNorthing(); + lastGPS = gpsUTM; + + // don't update angle if we did not move a lot + double heading = Math.atan2(dy, dx); + if ((dx * dx + dy * dy) < 0.25) { + heading = x.get(3, 0); + } + // close the loop + if (Math.abs(gps.getEasting() - initialGPS.getEasting()) + + Math.abs(gps.getNorthing() - initialGPS.getNorthing()) < 10.0) { + heading = initialHeading; + } + + // measurement + Matrix z = new Matrix({ + { gps.getEasting() }, + { gps.getNorthing() }, + { heading } + }); + + kalmanFilter(C_gps, z); + })); + } + + private void setupWheelSubscriber() { + new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), ((topicName, m) -> { + SteeringMeasurement steerM = (SteeringMeasurement) m; + steeringAngle = steerM.getAngle(); })); } @Override protected void update() { - // a kalman filter consists of two distinct steps - predict and update - - // the predict step is responsible for determining the estimate of the next state - // What we do is we - // - // todo fill this out - predictStep(); - - // the update step is responsible for updating the current state, and resolving - // discrepancies between the prediction and actual state - // What we do is we - // - // - // todo fill this out - updateStep(); + Matrix x_predict = propagate(); + + UTMTuple utm = new UTMTuple(UTMZONE, 'T', x_predict.get(0, 0), + x_predict.get(1, 0)); + LocTuple latLon = LocalizerUtil.utm2Deg(utm); + posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), + latLon.getLongitude(), x_predict.get(4, 0))); } - private void predictStep() { + // Kalman filter step 0: Generate the motion model for the buggy + private Matrix motionModel(double dt) { + return new Matrix({ + // x y dy_b heading dheading + { 1, 0, dt * Math.cos(x.get(4, 0)), 0, 0 }, // x + { 0, 1, dt * Math.sin(x.get(4, 0)), 0, 0 }, // y + { 0, 0, 1, 0, 0 }, // dy_b + { 0, 0, 0, 1, dt, 0 }, // heading + { 0, 0, Math.tan(steeringAngle) / WHEELBASE, 0, 0 }, // dheading + }); + } + + // Kalman filter step 1: Use the motion model to predict the next state + // and covariance matrix. (_pre = predicted) + // Kalman filter step 2: Update the prediction based off of measurements / + // sensor readings + private void kalmanFilter(Matrix C, Matrix z) { + // update time + Date now = new Date(); + double dt = (now.getTime() - lastTime) / 1000.0; + lastTime = now.getTime(); + + Matrix A = motionModel(dt); + /* + Predict: + x_pre = A * x + P_pre = A * P * A' + R + */ + Matrix x_pre = A.times(x); + Matrix P_pre = A.times(P).times(A.transpose()); + P_pre = P_pre.plus(R); + + x_pre.set(3, 0, scrubAngle(x_pre.get(3, 0))); + x_pre.set(4, 0, scrubAngle(x_pre.get(4, 0))); + + /* + Update: + r = z - (C * x_pre) + K = P_pre * C' * inv((C * P_pre * C') + Q) // gain + x = x_pre + (K * r) + P = (I - (K * C)) * P_pre + */ + Matrix residual = z.minus(C.times(x)); + Matrix K = C.times(P_pre).times(C.transpose()).plus(Q); + K = P_pre.times(C.transpose()).times(K.inverse()); + x = x_pre.plus(K.times(residual)); + P = Matrix.identity(5, 5).minus(K.times(C)); + P = P.times(P_pre); + + x.set(3, 0, scrubAngle(x.get(3, 0))); + x.set(4, 0, scrubAngle(x.get(4, 0))); } - private void updateStep() { + // Propagate the motion model forward in time since the last sensor reading + // not part of the Kalman algorithm + private Matrix propagate() { + Date now = new Date(); + double dt = (now.getTime() - lastTime) / 1000.0; + // x_pre = A * x + Matrix A = motionModel(dt); + return A.times(x); } @@ -144,4 +249,24 @@ protected boolean startDecoratorNode() { protected boolean shutdownDecoratorNode() { return false; } + + private static Matrix arrayToMatrix(double[] arr) { + return new Matrix({ + {arr[0], 0, 0, 0, 0}, + {0, arr[1], 0, 0, 0}, + {0, 0, arr[2], 0, 0}, + {0, 0, 0, arr[3], 0}, + {0, 0, 0, 0, arr[4]} + }); + } + + private double scrubAngle(double theta) { + while (theta < -Math.PI) { + theta = theta + (2 * Math.PI); + } + while (theta > Math.PI) { + theta = theta - (2 * Math.PI); + } + return theta; + } } From 32073331fed18adfc8a76953178c481b247ee2a2 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 13 Nov 2016 19:19:58 -0500 Subject: [PATCH 017/149] updating simulator --- offline/localizer/localizer.m | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m index 21007f14..a39429e4 100644 --- a/offline/localizer/localizer.m +++ b/offline/localizer/localizer.m @@ -12,6 +12,8 @@ global last_encoder global last_encoder_time global last_time + global R + global Q save_data = true; % constants @@ -22,6 +24,9 @@ first_theta = 250; lat_long = [40.441670, -79.9416362]; + R = eye(7) .* (0.5); % measurement noise covariance + Q = eye(7) .* (0.1); % model noise covariance + [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); first_gps = [x y]; last_gps = [x y]; @@ -29,7 +34,10 @@ last_encoder = -1; % initialize Kalman filter - P = eye(7); % covariance + % covariance matrix + P = [100 0 0 0 0; + 0 100 0 0 0; + 0 0 ]; X = [x; % X, m, UTM coors y; % Y, m, UTM coors @@ -114,7 +122,7 @@ end function [P_pre, x_pre] = predict_step(P, x, time) - R = eye(7) .* (0.5); % measurement noise covariance + global R A = model(x, time); x_pre = A * x; % P_pre = eye(7) * P * eye(7)'; % ERROR !! @@ -123,7 +131,7 @@ end function [P, x] = update_step(C, z, P_pre, x_pre) - Q = eye(7) .* (0.1); % model noise covariance + global Q residual = z - (C * x_pre); residual = scrubAngles(residual); K = P_pre * C' * inv((C * P_pre * C') + Q); % gain From 0d419498fe6782572501e68f74c47e9d846b12f9 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 13 Nov 2016 20:15:36 -0500 Subject: [PATCH 018/149] compilation issues fixed --- .../localizers/RobobuggyKFLocalizer.java | 130 ++++++++++-------- 1 file changed, 71 insertions(+), 59 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 981a744a..c6264152 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -2,14 +2,15 @@ import Jama.Matrix; import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.messages.IMUCompassMessage; +import com.roboclub.robobuggy.messages.SteeringMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; -import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; import com.roboclub.robobuggy.ros.Subscriber; + import java.util.Date; /** @@ -29,8 +30,8 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private static final double WHEELBASE = 1.13; // meters private static final int UTMZONE = 17; - private final LocTuple initialGPS = LocalizerUtil.deg2UTM( - new LocTuple(40.441670, -79.9416362)); + private final UTMTuple initialGPS = LocalizerUtil.deg2UTM( + new LocTuple(40.441670, -79.9416362)); private final double initialHeading = 4.36; // rad // The state consists of 4 elements: @@ -39,21 +40,16 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // dy_body - forward velocity in the body frame, in meters/s // heading - heading, or yaw angle, in the world frame, in rad // dheading - angular velocity in the world frame, in rad/s - private Matrix R = arrayToMatrix({4, 4, 0.25, 0.01, 0.01}); // measurement noise covariance matrix - private Matrix Q = arrayToMatrix({4, 4, 0.25, 0.02, 0.02}); // model noise covariance matrix - private Matrix P = arrayToMatrix({25, 25, 0.25, 2.46, 2.46}); // covariance matrix - private Matrix x; // state + private Matrix x; // state + private Matrix R; // measurement noise covariance matrix + private Matrix Q; // model noise covariance matrix + private Matrix P; // covariance matrix + // output matrices - private Matrix C_gps = new Matrix({ - {1, 0, 0, 0, 0}, - {0, 1, 0, 0, 0}, - {0, 0, 0, 1, 0} - }); - private Matrix C_encoder = new Matrix({ - {0, 0, 1, 0, 0} - }); - - private Date lastTime; + private Matrix C_gps; + private Matrix C_encoder; + + private long lastTime; private UTMTuple lastGPS; // private Date lastGPSTime; private double lastEncoder; // deadreckoning value @@ -65,7 +61,6 @@ public class RobobuggyKFLocalizer extends PeriodicNode { /** * Create a new {@link PeriodicNode} decorator * - * @param base {@link BuggyNode} to decorate * @param period of the periodically executed portion of the node * @param name the name of the node * @param initialPosition the initial position of the localizer @@ -81,16 +76,34 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition lastGPS = initialGPS; // lastGPSTime = lastTime; - x = new Matrix({ - { initialGPS.getEasting() }, - { initialGPS.getNorthing() }, - { 0 }, - { initialHeading }, - { 0 } - }); - - // set our initial covariance - covariance = new Matrix(SEVEN_ROW_IDENTITY_MATRIX); + double[][] x2D = { + { initialGPS.getEasting() }, + { initialGPS.getNorthing() }, + { 0 }, + { initialHeading }, + { 0 } + }; + x = new Matrix(x2D); + + double[] rArray = {4, 4, 0.25, 0.01, 0.01}; + double[] qArray = {4, 4, 0.25, 0.02, 0.02}; + double[] pArray = {25, 25, 0.25, 2.46, 2.46}; + + R = arrayToMatrix(rArray); + Q = arrayToMatrix(qArray); + P = arrayToMatrix(pArray); + + double[][] cGPS2D = { + {1, 0, 0, 0, 0}, + {0, 1, 0, 0, 0}, + {0, 0, 0, 1, 0} + }; + C_gps = new Matrix(cGPS2D); + + double[][] cEncoder2D = { + {0, 0, 1, 0, 0} + }; + C_encoder = new Matrix(cEncoder2D); // add all our subscribers for our current state update stream // these subscribers are only meant as catchers, just stupidly simple relays @@ -117,23 +130,17 @@ private void setupEncoderSubscriber() { double bodySpeed = dx / (dt / 1000.0); lastEncoderTime = currentTime; lastEncoder = currentEncoder; - + // measurement - Matrix z = new Matrix({ - { bodySpeed } - }); + double[][] z2D = { + { bodySpeed } + }; + Matrix z = new Matrix(z2D); kalmanFilter(C_encoder, z); })); } - // private void setupIMUSubscriber() { - // new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), ((topicName, m) -> { - // IMUCompassMessage compassMessage = (IMUCompassMessage) m; - // currentHeading = compassMessage.getCompassHeading(); - // })); - // } - private void setupGPSSubscriber() { new Subscriber("KF Localizer", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { GpsMeasurement gpsLoc = (GpsMeasurement) m; @@ -141,7 +148,7 @@ private void setupGPSSubscriber() { UTMTuple gps = LocalizerUtil.deg2UTM(loc); double dx = gps.getEasting() - lastGPS.getEasting(); double dy = gps.getNorthing() - lastGPS.getNorthing(); - lastGPS = gpsUTM; + lastGPS = gps; // don't update angle if we did not move a lot double heading = Math.atan2(dy, dx); @@ -155,11 +162,13 @@ private void setupGPSSubscriber() { } // measurement - Matrix z = new Matrix({ - { gps.getEasting() }, - { gps.getNorthing() }, - { heading } - }); + double[][] z2D = { + { gps.getEasting() }, + { gps.getNorthing() }, + { heading } + }; + + Matrix z = new Matrix(z2D); kalmanFilter(C_gps, z); })); @@ -184,15 +193,17 @@ protected void update() { } // Kalman filter step 0: Generate the motion model for the buggy - private Matrix motionModel(double dt) { - return new Matrix({ + private Matrix getMotionModel(double dt) { + double[][] motionModel2D = { // x y dy_b heading dheading { 1, 0, dt * Math.cos(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // x { 0, 1, dt * Math.sin(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // y { 0, 0, 1, 0, 0 }, // dy_b { 0, 0, 0, 1, dt, 0 }, // heading { 0, 0, Math.tan(steeringAngle) / WHEELBASE, 0, 0 }, // dheading - }); + }; + + return new Matrix(motionModel2D); } /* @@ -213,7 +224,7 @@ private void kalmanFilter(Matrix C, Matrix z) { double dt = (now.getTime() - lastTime) / 1000.0; lastTime = now.getTime(); - Matrix A = motionModel(dt); + Matrix A = getMotionModel(dt); /* the predict step is responsible for determining the estimate of the next state @@ -228,7 +239,7 @@ prediction state (xhat[k]) and our prediction noise (phat[k]) Matrix x_pre = A.times(x); Matrix P_pre = A.times(P).times(A.transpose()); P_pre = P_pre.plus(R); - + x_pre.set(HEADING_GLOBAL_ROW, 0, scrubAngle(x_pre.get(HEADING_GLOBAL_ROW, 0))); x_pre.set(HEADING_VEL_ROW, 0, scrubAngle(x_pre.get(HEADING_VEL_ROW, 0))); @@ -267,7 +278,7 @@ private Matrix propagate() { double dt = (now.getTime() - lastTime) / 1000.0; // x_pre = A * x - Matrix A = motionModel(dt); + Matrix A = getMotionModel(dt); return A.times(x); } @@ -283,13 +294,14 @@ protected boolean shutdownDecoratorNode() { } private static Matrix arrayToMatrix(double[] arr) { - return new Matrix({ - {arr[0], 0, 0, 0, 0}, - {0, arr[1], 0, 0, 0}, - {0, 0, arr[2], 0, 0}, - {0, 0, 0, arr[3], 0}, - {0, 0, 0, 0, arr[4]} - }); + double[][] arr2D = { + {arr[0], 0, 0, 0, 0}, + {0, arr[1], 0, 0, 0}, + {0, 0, arr[2], 0, 0}, + {0, 0, 0, arr[3], 0}, + {0, 0, 0, 0, arr[4]} + }; + return new Matrix(arr2D); } // clamp all the angles between -pi and +pi From 68d4c623251dc0e2fa6ccbbb26ad101cc740ff3b Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 13 Nov 2016 20:22:31 -0500 Subject: [PATCH 019/149] controller --- offline/controller/controller.m | 20 ++++ offline/controller/waypoints_course.txt | 134 ++++++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 offline/controller/controller.m create mode 100644 offline/controller/waypoints_course.txt diff --git a/offline/controller/controller.m b/offline/controller/controller.m new file mode 100644 index 00000000..e36831d2 --- /dev/null +++ b/offline/controller/controller.m @@ -0,0 +1,20 @@ +function [trajectory] = controller() + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + + + +end + +function [A] = model(x, u, dt) + global wheel_base + + A = [1, 0, dt*cos(x(3)), 0, 0; + 0, 1, dt*sin(x(3)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(u)/wheel_base, 0, 0]; +end + + diff --git a/offline/controller/waypoints_course.txt b/offline/controller/waypoints_course.txt new file mode 100644 index 00000000..248b92ee --- /dev/null +++ b/offline/controller/waypoints_course.txt @@ -0,0 +1,134 @@ +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44161120375703,"north":true,"longitude":-79.94159013032913,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463072} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441554046160554,"north":true,"longitude":-79.9416196346283,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463080} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441490764479234,"north":true,"longitude":-79.94164109230042,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463082} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44141931734805,"north":true,"longitude":-79.9416732788086,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463082} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44135195284055,"north":true,"longitude":-79.94171351194382,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463083} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44127234015373,"north":true,"longitude":-79.94175642728806,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463083} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44118864466336,"north":true,"longitude":-79.94180202484131,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463083} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441111073140206,"north":true,"longitude":-79.94184225797653,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44104166696468,"north":true,"longitude":-79.94187980890274,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44095592982538,"north":true,"longitude":-79.94191735982895,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440876316669566,"north":true,"longitude":-79.94195222854614,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440798744786015,"north":true,"longitude":-79.94198441505432,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44072729691936,"north":true,"longitude":-79.9420166015625,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4406436007504,"north":true,"longitude":-79.94205951690674,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440561945850995,"north":true,"longitude":-79.94209706783295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44050070461135,"north":true,"longitude":-79.94212120771408,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44043946331594,"north":true,"longitude":-79.94215339422226,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440374139205986,"north":true,"longitude":-79.94220435619354,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440312897795266,"north":true,"longitude":-79.94224458932877,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44026390462652,"north":true,"longitude":-79.94227945804596,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440227159726525,"north":true,"longitude":-79.94231969118118,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44021082865341,"north":true,"longitude":-79.94237333536148,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440198580345964,"north":true,"longitude":-79.94244039058685,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44018020788062,"north":true,"longitude":-79.94255304336548,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44016387679611,"north":true,"longitude":-79.9426656961441,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44015571125236,"north":true,"longitude":-79.9427729845047,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44014142154843,"north":true,"longitude":-79.94286686182022,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44010875935659,"north":true,"longitude":-79.94298756122589,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440061807428016,"north":true,"longitude":-79.94308948516846,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44000873129533,"north":true,"longitude":-79.9431699514389,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439937282589064,"north":true,"longitude":-79.94327992200851,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43987195799114,"north":true,"longitude":-79.94336575269699,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43978621936003,"north":true,"longitude":-79.94344890117645,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439702522019445,"north":true,"longitude":-79.94351863861084,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439629031585696,"north":true,"longitude":-79.9435830116272,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4395432926448,"north":true,"longitude":-79.94367152452469,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439463677816036,"north":true,"longitude":-79.94375199079514,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439394269939655,"north":true,"longitude":-79.94381904602051,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43932486199164,"north":true,"longitude":-79.94389146566391,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4392472883179,"north":true,"longitude":-79.9439612030983,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43918604588067,"north":true,"longitude":-79.94402289390564,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43911255488232,"north":true,"longitude":-79.94409531354904,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4390635608388,"north":true,"longitude":-79.94415432214737,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439012525338825,"north":true,"longitude":-79.94422674179077,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43896148980012,"north":true,"longitude":-79.9443045258522,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43892678561167,"north":true,"longitude":-79.94437962770462,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43889616425403,"north":true,"longitude":-79.9444654583931,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438867584307665,"north":true,"longitude":-79.94453519582748,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438849211478576,"north":true,"longitude":-79.94461566209793,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4388185900856,"north":true,"longitude":-79.94473099708557,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43879001010625,"north":true,"longitude":-79.94485437870026,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43876143011477,"north":true,"longitude":-79.94500458240509,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438736932969526,"north":true,"longitude":-79.94514405727386,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43871039438544,"north":true,"longitude":-79.94529157876968,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43868793865227,"north":true,"longitude":-79.94544982910156,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43866956577409,"north":true,"longitude":-79.94559735059738,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43867364863634,"north":true,"longitude":-79.94572341442108,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438685897221596,"north":true,"longitude":-79.94586557149887,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43871039438544,"north":true,"longitude":-79.94601845741272,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43873489154035,"north":true,"longitude":-79.94612842798233,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438769595827864,"north":true,"longitude":-79.94625717401505,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43883288007072,"north":true,"longitude":-79.94634568691254,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438918619917665,"north":true,"longitude":-79.946428835392,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439020691021426,"north":true,"longitude":-79.9465361237526,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43916359030634,"north":true,"longitude":-79.94663268327713,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43928199234091,"north":true,"longitude":-79.94669169187546,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43943509811091,"north":true,"longitude":-79.94676142930984,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439573913707655,"north":true,"longitude":-79.94682043790817,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439663735411635,"north":true,"longitude":-79.94687139987946,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43973518440866,"north":true,"longitude":-79.94691699743271,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439818881708604,"north":true,"longitude":-79.94697600603104,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43992095144552,"north":true,"longitude":-79.94706451892853,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43999648295108,"north":true,"longitude":-79.94715839624405,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44007201437179,"north":true,"longitude":-79.94724959135056,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44012713184144,"north":true,"longitude":-79.94733273983002,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44020062173069,"north":true,"longitude":-79.94741320610046,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44026798739194,"north":true,"longitude":-79.94751244783401,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440327187462756,"north":true,"longitude":-79.94759827852249,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44037618058537,"north":true,"longitude":-79.94765996932983,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44041088402565,"north":true,"longitude":-79.94772166013718,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44044967020238,"north":true,"longitude":-79.94777262210846,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440488456356746,"north":true,"longitude":-79.94782358407974,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44052520111392,"north":true,"longitude":-79.94786113500595,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44056602859833,"north":true,"longitude":-79.94789868593216,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44061706291898,"north":true,"longitude":-79.94792819023132,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4406619730891,"north":true,"longitude":-79.94794696569443,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440708924598525,"north":true,"longitude":-79.94796574115753,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44075791744295,"north":true,"longitude":-79.94798719882965,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44080486888535,"north":true,"longitude":-79.94800597429276,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440857944389435,"north":true,"longitude":-79.9480140209198,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440902854398644,"north":true,"longitude":-79.94800329208374,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4409457230158,"north":true,"longitude":-79.94798183441162,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44098042616205,"north":true,"longitude":-79.94795501232147,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44101104657035,"north":true,"longitude":-79.94791477918625,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44104370832381,"north":true,"longitude":-79.94787722826004,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44110290771151,"north":true,"longitude":-79.94780480861664,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44117639653405,"north":true,"longitude":-79.94771093130112,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44124171986446,"north":true,"longitude":-79.9476170539856,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44131112583342,"north":true,"longitude":-79.94753658771515,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44136624228716,"north":true,"longitude":-79.94746148586273,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44140094521634,"north":true,"longitude":-79.9474024772644,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44143156543307,"north":true,"longitude":-79.94734078645706,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441449937556435,"north":true,"longitude":-79.94729518890381,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44146830967477,"north":true,"longitude":-79.94722545146942,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44147443371306,"north":true,"longitude":-79.94716376066208,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441478516404985,"north":true,"longitude":-79.94710475206375,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44147239236704,"north":true,"longitude":-79.94702160358429,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441451978903146,"north":true,"longitude":-79.9468794465065,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441421358695706,"north":true,"longitude":-79.94673192501068,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441392779822834,"north":true,"longitude":-79.94658708572388,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44136011823899,"north":true,"longitude":-79.94641810655594,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44132745663924,"north":true,"longitude":-79.94626522064209,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44129275367215,"north":true,"longitude":-79.94611233472824,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44125805068712,"north":true,"longitude":-79.94595408439636,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44123559580491,"north":true,"longitude":-79.94583070278168,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441202934144684,"north":true,"longitude":-79.94568049907684,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44117231382381,"north":true,"longitude":-79.94553834199905,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44113965213282,"north":true,"longitude":-79.94537472724915,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441109031783114,"north":true,"longitude":-79.94521915912628,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44105799783596,"north":true,"longitude":-79.94498312473297,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44102125337,"north":true,"longitude":-79.94480341672897,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44098859160563,"north":true,"longitude":-79.94462370872498,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4409457230158,"north":true,"longitude":-79.94444668292999,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440908978488494,"north":true,"longitude":-79.94428038597107,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440872233941086,"north":true,"longitude":-79.94408994913101,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440839572104295,"north":true,"longitude":-79.94391292333603,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440798744786015,"north":true,"longitude":-79.94370639324188,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44076200017835,"north":true,"longitude":-79.94353741407394,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44071913144405,"north":true,"longitude":-79.94336307048798,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440684428162896,"north":true,"longitude":-79.94319140911102,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44063951800779,"north":true,"longitude":-79.94299829006195,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44059869056801,"north":true,"longitude":-79.942826628685,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440572152718836,"north":true,"longitude":-79.94267910718918,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44054153211064,"north":true,"longitude":-79.94251817464828,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} From 60e7019838ece5be4bca913c12cd626ec6dfa8d8 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 13 Nov 2016 20:38:09 -0500 Subject: [PATCH 020/149] new controller sim --- offline/controller/controller.m | 49 ++++++++++++++++++++++++ offline/controller/controller_analysis.m | 27 +++++++++++++ 2 files changed, 76 insertions(+) create mode 100644 offline/controller/controller_analysis.m diff --git a/offline/controller/controller.m b/offline/controller/controller.m index e36831d2..40b516f8 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -2,11 +2,55 @@ addpath('../localizer/latlonutm/Codes/matlab'); global wheel_base + global velocity + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.441670, -79.9416362]; + dt = 0.001; % 1000Hz + m = 20; % 20Hz + [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + 0; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints.mat'); + desired_traj = processWaypoints(waypoints); + + % time = linspace(0, 240, size(trajectory,2)); + time = 0:dt:240; + u = 0; % steering angle + trajectory = []; + + for i = 1:size(time, 1) + t = time(i); + A = model(X, u, dt); + X = A*X; + + if(mod(i, m) == 0) + u = control(desired_traj, t); + end + + trajectory = [trajectory, X]; + end + + if save_data + save('controller_v1.mat', 'trajectory'); + end end +function [trajectory] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); + trajectory = []; +end + + function [A] = model(x, u, dt) global wheel_base @@ -17,4 +61,9 @@ 0, 0, tan(u)/wheel_base, 0, 0]; end +function [u] = control(desired_traj, t) + u = pi; +end + + diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m new file mode 100644 index 00000000..524adbc9 --- /dev/null +++ b/offline/controller/controller_analysis.m @@ -0,0 +1,27 @@ +% graph results + +clear; +close all; + +addpath('./latlonutm/Codes/matlab'); +% addpath('./zoharby-plot_google_map'); +addpath('./altmany-export_fig'); + +file = 'localizer_v3.mat'; +load(file, 'trajectory', 'P'); +save_plot = false; + +k = 1000; +wmline(trajectory(8,1:k:end), trajectory(9,1:k:end), 'Color', 'r') + +theta = deg2rad( trajectory(5,1:k:end) ); +figure(); +hold on; +plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) +quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(theta), sin(theta)) +hold off; +title(['Heading ', file]); + +% plot on google maps +ss = [trajectory(8,1:k:end); trajectory(9,1:k:end)]; +fprintf(1, '%5.20f, %5.20f\n', ss); From 149ad4d6c8879b7bcf44997ee7eab24c83858229 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 15 Nov 2016 18:04:05 -0500 Subject: [PATCH 021/149] added top-level comments --- .../localizers/RobobuggyKFLocalizer.java | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index c6264152..648c5018 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -40,23 +40,20 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // dy_body - forward velocity in the body frame, in meters/s // heading - heading, or yaw angle, in the world frame, in rad // dheading - angular velocity in the world frame, in rad/s - private Matrix x; // state - private Matrix R; // measurement noise covariance matrix - private Matrix Q; // model noise covariance matrix - private Matrix P; // covariance matrix + private Matrix x; // current state + private Matrix R; // measurement noise covariance matrix + private Matrix Q; // model noise covariance matrix + private Matrix P; // covariance matrix // output matrices - private Matrix C_gps; - private Matrix C_encoder; - - private long lastTime; - private UTMTuple lastGPS; - // private Date lastGPSTime; - private double lastEncoder; // deadreckoning value - private long lastEncoderTime; - private double currentEncoder; - private long currentEncoderTime; - private double steeringAngle = 0; + private Matrix C_gps; // a description of how the GPS impacts x + private Matrix C_encoder; // how the encoder affects x + + private long lastTime; // the last time we updated the current position estimate + private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement + private double lastEncoder; // deadreckoning value + private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity + private double steeringAngle = 0; // current steering angle of the buggy /** * Create a new {@link PeriodicNode} decorator @@ -207,7 +204,7 @@ private Matrix getMotionModel(double dt) { } /* - Kalman filter step 1: Use the motion model to predict the next state + Kalman filter step 1: Use the motion model to predict the next state and covariance matrix. (_pre = predicted) Kalman filter step 2: Update the prediction based off of measurements / sensor readings From 165a85a4db77ed87db195309602006f318a0633c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 16 Nov 2016 14:32:19 -0500 Subject: [PATCH 022/149] cleanup --- .../localizers/RobobuggyKFLocalizer.java | 42 +++++++++---------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 648c5018..cce113c9 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -28,11 +28,10 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private static final int HEADING_GLOBAL_ROW = 3; private static final int HEADING_VEL_ROW = 4; - private static final double WHEELBASE = 1.13; // meters + private static final double WHEELBASE_IN_METERS = 1.13; // meters private static final int UTMZONE = 17; - private final UTMTuple initialGPS = LocalizerUtil.deg2UTM( - new LocTuple(40.441670, -79.9416362)); - private final double initialHeading = 4.36; // rad + private final UTMTuple initialLocationGPS; // new LocTuple(40.441670, -79.9416362)); + private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad // The state consists of 4 elements: // x - x position in the world frame, in meters @@ -66,18 +65,19 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition super(new BuggyBaseNode(NodeChannel.POSE), period, name); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + initialLocationGPS = LocalizerUtil.deg2UTM(initialPosition); + // set initial state lastTime = new Date().getTime(); lastEncoder = 0; lastEncoderTime = lastTime; - lastGPS = initialGPS; - // lastGPSTime = lastTime; + lastGPS = initialLocationGPS; double[][] x2D = { - { initialGPS.getEasting() }, - { initialGPS.getNorthing() }, + { initialLocationGPS.getEasting() }, + { initialLocationGPS.getNorthing() }, { 0 }, - { initialHeading }, + { INITIAL_HEADING_IN_RADS }, { 0 } }; x = new Matrix(x2D); @@ -103,10 +103,8 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition C_encoder = new Matrix(cEncoder2D); // add all our subscribers for our current state update stream - // these subscribers are only meant as catchers, just stupidly simple relays - // that feed the current state variables + // Every time we get a new sensor update, trigger the new kalman update setupGPSSubscriber(); - // setupIMUSubscriber(); setupEncoderSubscriber(); setupWheelSubscriber(); } @@ -153,9 +151,9 @@ private void setupGPSSubscriber() { heading = x.get(HEADING_GLOBAL_ROW, 0); } // close the loop - if (Math.abs(gps.getEasting() - initialGPS.getEasting()) - + Math.abs(gps.getNorthing() - initialGPS.getNorthing()) < 10.0) { - heading = initialHeading; + if (Math.abs(gps.getEasting() - initialLocationGPS.getEasting()) + + Math.abs(gps.getNorthing() - initialLocationGPS.getNorthing()) < 10.0) { + heading = INITIAL_HEADING_IN_RADS; } // measurement @@ -197,7 +195,7 @@ private Matrix getMotionModel(double dt) { { 0, 1, dt * Math.sin(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // y { 0, 0, 1, 0, 0 }, // dy_b { 0, 0, 0, 1, dt, 0 }, // heading - { 0, 0, Math.tan(steeringAngle) / WHEELBASE, 0, 0 }, // dheading + { 0, 0, Math.tan(steeringAngle) / WHEELBASE_IN_METERS, 0, 0 }, // dheading }; return new Matrix(motionModel2D); @@ -237,8 +235,8 @@ prediction state (xhat[k]) and our prediction noise (phat[k]) Matrix P_pre = A.times(P).times(A.transpose()); P_pre = P_pre.plus(R); - x_pre.set(HEADING_GLOBAL_ROW, 0, scrubAngle(x_pre.get(HEADING_GLOBAL_ROW, 0))); - x_pre.set(HEADING_VEL_ROW, 0, scrubAngle(x_pre.get(HEADING_VEL_ROW, 0))); + x_pre.set(HEADING_GLOBAL_ROW, 0, clampAngle(x_pre.get(HEADING_GLOBAL_ROW, 0))); + x_pre.set(HEADING_VEL_ROW, 0, clampAngle(x_pre.get(HEADING_VEL_ROW, 0))); /* the update step is responsible for updating the current state, and resolving @@ -246,7 +244,7 @@ prediction state (xhat[k]) and our prediction noise (phat[k]) What we do is we take our measurements (z) that were captured from sensors, as well as the predictions from the previous step (xhat[k], phat[k]) and run - them through the filter which todo fill this part out about innovation etc + them through the filter This produces your next state (x[k]) and your next noise estimation (p[k]), which connect in a feedback loop to become the new current state @@ -264,8 +262,8 @@ This produces your next state (x[k]) and your next noise estimation (p[k]), P = Matrix.identity(5, 5).minus(K.times(C)); P = P.times(P_pre); - x.set(HEADING_GLOBAL_ROW, 0, scrubAngle(x.get(HEADING_GLOBAL_ROW, 0))); - x.set(HEADING_VEL_ROW, 0, scrubAngle(x.get(HEADING_VEL_ROW, 0))); + x.set(HEADING_GLOBAL_ROW, 0, clampAngle(x.get(HEADING_GLOBAL_ROW, 0))); + x.set(HEADING_VEL_ROW, 0, clampAngle(x.get(HEADING_VEL_ROW, 0))); } // Propagate the motion model forward in time since the last sensor reading @@ -302,7 +300,7 @@ private static Matrix arrayToMatrix(double[] arr) { } // clamp all the angles between -pi and +pi - private double scrubAngle(double theta) { + private double clampAngle(double theta) { while (theta < -Math.PI) { theta = theta + (2 * Math.PI); } From 87653d4452e292cf31d6b1988509018e023f6ba7 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 16 Nov 2016 17:15:30 -0500 Subject: [PATCH 023/149] converted controller to radians --- .../java/com/roboclub/robobuggy/messages/GPSPoseMessage.java | 4 ++-- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java index e6d6ffa0..aff45b4a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java @@ -52,9 +52,9 @@ public double getLongitude() { } /** - * Returns the heading of the {@link GPSPoseMessage} (in degrees from North) + * Returns the heading of the {@link GPSPoseMessage} (in RADIANS from North) * - * @return the heading of the {@link GPSPoseMessage} (in degrees from North) + * @return the heading of the {@link GPSPoseMessage} (in RADIANS from North) */ public double getHeading() { return heading; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 4bbef890..11da3357 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -79,7 +79,7 @@ public double getCommandedSteeringAngle() { // basically we want all of our angles to be in the same range, so that we don't // have weird wraparound desiredHeading = Util.normalizeAngleDeg(desiredHeading); - double poseHeading = Util.normalizeAngleDeg(pose.getHeading()); + double poseHeading = Util.normalizeAngleDeg(Math.toDegrees(pose.getHeading())); //find the angle we need to reach that point return Util.normalizeAngleDeg(desiredHeading - poseHeading); From db8222aa536393830600d475c2045864cd587636 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 16 Nov 2016 22:34:48 -0500 Subject: [PATCH 024/149] fixed an issue where the matrices were the wrong sizes --- .../localizers/RobobuggyKFLocalizer.java | 27 ++++++++--- .../localizers/RobobuggyKFLocalizerTest.java | 46 +++++++++++++++++++ 2 files changed, 67 insertions(+), 6 deletions(-) create mode 100644 real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index cce113c9..6bcc216c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -93,12 +93,18 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, - {0, 0, 0, 1, 0} + {0, 0, 0, 0, 0}, + {0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0}, }; C_gps = new Matrix(cGPS2D); double[][] cEncoder2D = { - {0, 0, 1, 0, 0} + {0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0}, + {0, 0, 1, 0, 0}, + {0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0}, }; C_encoder = new Matrix(cEncoder2D); @@ -128,7 +134,11 @@ private void setupEncoderSubscriber() { // measurement double[][] z2D = { - { bodySpeed } + { 0 }, + { 0 }, + { bodySpeed }, + { 0 }, + { 0 }, }; Matrix z = new Matrix(z2D); @@ -160,7 +170,9 @@ private void setupGPSSubscriber() { double[][] z2D = { { gps.getEasting() }, { gps.getNorthing() }, - { heading } + { 0 }, + { heading }, + { 0 }, }; Matrix z = new Matrix(z2D); @@ -194,7 +206,7 @@ private Matrix getMotionModel(double dt) { { 1, 0, dt * Math.cos(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // x { 0, 1, dt * Math.sin(x.get(HEADING_GLOBAL_ROW, 0)), 0, 0 }, // y { 0, 0, 1, 0, 0 }, // dy_b - { 0, 0, 0, 1, dt, 0 }, // heading + { 0, 0, 0, 1, dt }, // heading { 0, 0, Math.tan(steeringAngle) / WHEELBASE_IN_METERS, 0, 0 }, // dheading }; @@ -256,7 +268,10 @@ This produces your next state (x[k]) and your next noise estimation (p[k]), P = (I - (K * C)) * P_pre */ Matrix residual = z.minus(C.times(x)); - Matrix K = C.times(P_pre).times(C.transpose()).plus(Q); + Matrix K = C; + K = K.times(P_pre); + K = K.times(C.transpose()); + K = K.plus(Q); K = P_pre.times(C.transpose()).times(K.inverse()); x = x_pre.plus(K.times(residual)); P = Matrix.identity(5, 5).minus(K.times(C)); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java new file mode 100644 index 00000000..b7fd9d74 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java @@ -0,0 +1,46 @@ +package com.roboclub.robobuggy.nodes.localizers; + +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; +import org.junit.Assert; +import org.junit.Before; +import org.junit.BeforeClass; +import org.junit.Test; + +import java.util.concurrent.LinkedBlockingQueue; + +/** + * Created by vivaanbahl on 11/16/16. + */ +public class RobobuggyKFLocalizerTest { + + private static LinkedBlockingQueue poseMessages; + + @BeforeClass + public static void oneTimeSetup() { + new Subscriber("localizerTest", NodeChannel.POSE.getMsgPath(), ((topicName, m) -> { + poseMessages.add(((GPSPoseMessage) m)); + })); + } + + @Before + public void setUp() { + poseMessages = new LinkedBlockingQueue<>(); + } + + /** + * NOTE THIS IS NOT YET A VALID TEST CASE + * @throws InterruptedException + */ + @Test + public void test_singleIteration() throws InterruptedException { + RobobuggyKFLocalizer localizer = new RobobuggyKFLocalizer(10000, "testLocalizer", new LocTuple(40.441670, -79.9416362)); + new Publisher(NodeChannel.GPS.getMsgPath()).publish(new GpsMeasurement(40.441670, -79.9412361)); + Thread.sleep(100000); + Assert.assertEquals(0.0, 1.0, 0.0); + } + +} \ No newline at end of file From 7b9ce600d1fad8772e38a1485667f7c7ecd2e7ca Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 17 Nov 2016 17:57:16 -0500 Subject: [PATCH 025/149] added map to the surface to show where pose estimates are --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 1 + .../main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java | 6 ++++++ .../Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java | 1 + 3 files changed, 8 insertions(+) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 6bcc216c..d1708a1d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -163,6 +163,7 @@ private void setupGPSSubscriber() { // close the loop if (Math.abs(gps.getEasting() - initialLocationGPS.getEasting()) + Math.abs(gps.getNorthing() - initialLocationGPS.getNorthing()) < 10.0) { + // todo should this be previous heading? heading = INITIAL_HEADING_IN_RADS; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java index 9e6b4fda..3d423bbf 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/AnalyticsPanel.java @@ -1,5 +1,7 @@ package com.roboclub.robobuggy.ui; +import java.awt.Color; + /** * {@link RobobuggyGUIContainer} used to display a {@link DataPanel} and a * {@link GraphPanel} @@ -10,6 +12,7 @@ public final class AnalyticsPanel extends RobobuggyGUIContainer { private DataPanel dataPanel; private static AnalyticsPanel instance; + private Map map; /** * @return a reference to the analytics panel @@ -28,7 +31,10 @@ public static synchronized AnalyticsPanel getInstance() { private AnalyticsPanel() { name = "analytics"; dataPanel = new DataPanel(); + map = new Map(); + this.addComponent(map, 0, 0, 1, 0.75); this.addComponent(dataPanel, 0, 0, 1, 1); + this.setBackground(Color.RED); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index e81de9dc..df514042 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -65,6 +65,7 @@ public void actionPerformed(String topicName, Message m) { zoomLevel = getMapTree().getViewer().getZoom(); getMapTree().getViewer().setDisplayPosition(new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), zoomLevel); + addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); } }); } From 76ff3dd2fa192c5a60f59e853ac07079a0de1e72 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Thu, 17 Nov 2016 18:35:46 -0500 Subject: [PATCH 026/149] localizer comfirmed in simulation --- offline/controller/controller_analysis.m | 14 +- offline/localizer/localizer.m | 177 +++++++----------- .../localizers/RobobuggyKFLocalizer.java | 47 +++-- 3 files changed, 100 insertions(+), 138 deletions(-) diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m index 524adbc9..914691ec 100644 --- a/offline/controller/controller_analysis.m +++ b/offline/controller/controller_analysis.m @@ -7,21 +7,21 @@ % addpath('./zoharby-plot_google_map'); addpath('./altmany-export_fig'); -file = 'localizer_v3.mat'; +file = 'localizer_v4.mat'; load(file, 'trajectory', 'P'); save_plot = false; -k = 1000; -wmline(trajectory(8,1:k:end), trajectory(9,1:k:end), 'Color', 'r') +k = 100; +wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r') -theta = deg2rad( trajectory(5,1:k:end) ); +heading = trajectory(4,1:k:end); figure(); hold on; plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) -quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(theta), sin(theta)) +quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading)) hold off; title(['Heading ', file]); % plot on google maps -ss = [trajectory(8,1:k:end); trajectory(9,1:k:end)]; -fprintf(1, '%5.20f, %5.20f\n', ss); +xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; +fprintf(1, '%5.20f, %5.20f\n', xy); diff --git a/offline/localizer/localizer.m b/offline/localizer/localizer.m index a39429e4..f93f2fbd 100644 --- a/offline/localizer/localizer.m +++ b/offline/localizer/localizer.m @@ -13,7 +13,9 @@ global last_encoder_time global last_time global R - global Q + global steeringAngle + global first_gps + global first_theta save_data = true; % constants @@ -21,31 +23,36 @@ % dt of the controller, not the filter wheel_base = 1.13; utm_zone = 17; - first_theta = 250; + first_theta = deg2rad(250); lat_long = [40.441670, -79.9416362]; - R = eye(7) .* (0.5); % measurement noise covariance - Q = eye(7) .* (0.1); % model noise covariance - [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); first_gps = [x y]; last_gps = [x y]; utm_zone = zone; % fixed - last_encoder = -1; - + last_encoder = 0; + steeringAngle = 0; + + % output matrices + C_encoder = [0 0 1 0 0]; + C_gps = [1 0 0 0 0; + 0 1 0 0 0; + 0 0 0 1 0]; + + % measurement noise covariance + R = diag([4, 4, 0.25, 0.01, 0.01]); + % model noise covariance + Q_gps = diag([4, 4, 0.02]); + Q_encoder = [0.25]; % initialize Kalman filter % covariance matrix - P = [100 0 0 0 0; - 0 100 0 0 0; - 0 0 ]; + P = diag([25, 25, 0.25, 2.46, 2.46]); X = [x; % X, m, UTM coors y; % Y, m, UTM coors - 0; % d_Xb, body velocity - 0; % d_Yb - first_theta; % theta, deg, global - 0; % d_theta, deg/s - 0]; % heading, deg, body orientation + 0; % d_Yb, body velocity, m/s + first_theta; % heading, deg, global + 0]; % d_heading, deg/s trajectory = [X; lat_long'; norm(P)]; @@ -60,29 +67,31 @@ time = llog(2); if strcmp(name, 'gps') - [C, z] = gps_meaurement(llog(3), llog(4), first_gps, first_theta); - % [C, z] = gps_meaurement_xy(llog(3), llog(4)); + [z] = gps_meaurement(llog(3), llog(4), X(4)); + C = C_gps; + Q = Q_gps; elseif strcmp(name,'encoder') - if (last_encoder == -1) - last_encoder = llog(3); + [z] = encoder_meaurement(llog(3), time); + C = C_encoder; + Q = Q_encoder; + if isempty(z) + continue; end - [C, z] = encoder_meaurement(llog(3), time); elseif strcmp(name, 'steering') - [C, z] = steering_measurement(llog(3)); - % elseif strcmp(name, 'imu_temp') - % [C, z] = compass_measurement(llog(3)); + steeringAngle = deg2rad(llog(3)); + continue; else continue; end [P_pre, X_pre] = predict_step(P, X, time); - [P, X] = update_step(C, z, P_pre, X_pre); + [P, X] = update_step(C, Q, z, P_pre, X_pre); snapshot = summarize(P, X, utm_zone); trajectory = [trajectory, snapshot]; end if save_data - save('localizer_v3.mat', 'trajectory', 'P'); + save('localizer_v4.mat', 'trajectory', 'P'); end end @@ -91,14 +100,12 @@ snapshot = [x; lat; lon; norm(P)]; end -function x = scrubAngles(x) - for i = 5:7 - while (x(i) < -180) - x(i) = x(i) + 360; - end - while (x(i) > 180) - x(i) = x(i) - 360; - end +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; end end @@ -106,118 +113,78 @@ function [A] = model(x, time) global wheel_base global last_time + global steeringAngle dt = (time - last_time) / 1000.0; last_time = time; - theta = deg2rad(x(3)); - heading = deg2rad(x(7)); - - A = [1, 0, dt*cos(theta), -dt*sin(theta), 0, 0, 0; - 0, 1, dt*sin(theta), dt*cos(theta), 0, 0, 0; - 0, 0, 1, 0, 0, 0, 0; - 0, 0, 0, 1, 0, 0, 0; - 0, 0, 0, 0, 1, dt, 0; - 0, 0, 180.0/(pi*(wheel_base/tan(heading))), 0, 0, 0, 0; % used to be sin, ERROR !! - 0, 0, 0, 0, 0, 0, 1]; + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steeringAngle)/wheel_base, 0, 0]; end function [P_pre, x_pre] = predict_step(P, x, time) global R A = model(x, time); x_pre = A * x; - % P_pre = eye(7) * P * eye(7)'; % ERROR !! - P_pre = A * P * A' + R; - x_pre = scrubAngles(x_pre); + P_pre = (A * P * A') + R; + + x_pre(4) = clampAngle(x_pre(4)); + x_pre(5) = clampAngle(x_pre(5)); end -function [P, x] = update_step(C, z, P_pre, x_pre) - global Q +function [P, x] = update_step(C, Q, z, P_pre, x_pre) residual = z - (C * x_pre); - residual = scrubAngles(residual); K = P_pre * C' * inv((C * P_pre * C') + Q); % gain x = x_pre + (K * residual); - % P = (eye(7) - (K * C)); % ERROR !! - P = (eye(7) - (K * C)) * P_pre; + P = (eye(5) - (K * C)) * P_pre; + + x(4) = clampAngle(x(4)); + x(5) = clampAngle(x(5)); end -function [C, z] = gps_meaurement(lat, long, first_gps, first_theta) +function [z] = gps_meaurement(lat, long, last_heading) global last_gps + global first_gps + global first_theta [x, y, ~] = ll2utm(lat, long); dx = x - last_gps(1); dy = y - last_gps(2); last_gps = [x y]; - theta = rad2deg(atan2(dy, dx)); + heading = atan2(dy, dx); - C = zeros(7, 7); - C(1, 1) = 1; - C(2, 2) = 1; - C(5, 5) = 1; % ignore small strides - if (norm([dx dy]) < 0.5) - C(5, 5) = 0; + if ((dx*dx + dy*dy) < 0.25) + heading = last_heading; end % close the loop d = abs(x - first_gps(1)) + abs(y - first_gps(2)); - if (d < 10) - theta = first_theta; + if (d < 10.0) + heading = first_theta; end - z = [x, y, 0, 0, theta, 0 , 0]'; + z = [x; y; heading]; end -function [C, z] = gps_meaurement_xy(lat, long) - [x, y, ~] = ll2utm(lat, long); - C = zeros(7, 7); - C(1, 1) = 1; - C(2, 2) = 1; - z = [x, y, 0, 0, 0, 0 , 0]'; -end - -% TODO possible precision errors since changes are so small -% maybe batch encoder updates until we get a decent margin -function [C, z] = encoder_meaurement(encoder_dist, encoder_time) +function [z] = encoder_meaurement(encoder_dist, encoder_time) global last_encoder global last_encoder_time - dx = encoder_dist - last_encoder; dt = encoder_time - last_encoder_time; - C = zeros(7, 7); - z = zeros(7, 1); - if (dt > 1) + if (dt < 10) + z = []; + else + dx = encoder_dist - last_encoder; last_encoder = encoder_dist; last_encoder_time = encoder_time; - C(3, 3) = 1; - z(3) = dx / (dt / 1000.0); + bodySpeed = dx / (dt / 1000.0); + z = [bodySpeed]; end end -function [C, z] = steering_measurement(heading) - C = zeros(7, 7); - C(7, 7) = 1; - z = zeros(7, 1); - z(7) = heading; -end - -function [C, z] = imu_ang_pos_measurement(R) - R = reshape(R, 3,3)'; - x = R(1, 1); - x = R(1, 2); - theta = 90 - rad2deg(atan2(y, x)); - - C = zeros(7, 7); - C(5, 5) = 1; - z = zeros(7, 1); - z(5) = theta; -end - -% TODO add dtheta -function [C, z] = compass_measurement(c_angle) - C = zeros(7, 7); - C(5, 5) = 1; - z = zeros(7, 1); - z(5) = c_angle; -end diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 6bcc216c..8369b913 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -41,8 +41,9 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // dheading - angular velocity in the world frame, in rad/s private Matrix x; // current state private Matrix R; // measurement noise covariance matrix - private Matrix Q; // model noise covariance matrix private Matrix P; // covariance matrix + private Matrix Q_qps; // model noise covariance matrix + private Matrix Q_encoder; // output matrices private Matrix C_gps; // a description of how the GPS impacts x @@ -83,28 +84,30 @@ protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition x = new Matrix(x2D); double[] rArray = {4, 4, 0.25, 0.01, 0.01}; - double[] qArray = {4, 4, 0.25, 0.02, 0.02}; double[] pArray = {25, 25, 0.25, 2.46, 2.46}; R = arrayToMatrix(rArray); - Q = arrayToMatrix(qArray); P = arrayToMatrix(pArray); + double[][] qGPS2D = { + {4, 0, 0}, + {0, 4, 0}, + {0, 0, 0.02}, + }; + Q_gps = new Matrix(qGPS2D); + + double[][] qEncoder2D = {{0.25}}; + Q_encoder = new Matrix(qEncoder2D); + double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, - {0, 0, 0, 0, 0}, {0, 0, 0, 1, 0}, - {0, 0, 0, 0, 0}, }; C_gps = new Matrix(cGPS2D); double[][] cEncoder2D = { - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, {0, 0, 1, 0, 0}, - {0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0}, }; C_encoder = new Matrix(cEncoder2D); @@ -133,16 +136,10 @@ private void setupEncoderSubscriber() { lastEncoder = currentEncoder; // measurement - double[][] z2D = { - { 0 }, - { 0 }, - { bodySpeed }, - { 0 }, - { 0 }, - }; + double[][] z2D = {{ bodySpeed }}; Matrix z = new Matrix(z2D); - kalmanFilter(C_encoder, z); + kalmanFilter(C_encoder, Q_encoder, z); })); } @@ -160,7 +157,7 @@ private void setupGPSSubscriber() { if ((dx * dx + dy * dy) < 0.25) { heading = x.get(HEADING_GLOBAL_ROW, 0); } - // close the loop + // close the loop, lock initial angle if (Math.abs(gps.getEasting() - initialLocationGPS.getEasting()) + Math.abs(gps.getNorthing() - initialLocationGPS.getNorthing()) < 10.0) { heading = INITIAL_HEADING_IN_RADS; @@ -170,21 +167,19 @@ private void setupGPSSubscriber() { double[][] z2D = { { gps.getEasting() }, { gps.getNorthing() }, - { 0 }, { heading }, - { 0 }, }; Matrix z = new Matrix(z2D); - kalmanFilter(C_gps, z); + kalmanFilter(C_gps, Q_gps z); })); } private void setupWheelSubscriber() { new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), ((topicName, m) -> { SteeringMeasurement steerM = (SteeringMeasurement) m; - steeringAngle = steerM.getAngle(); + steeringAngle = Math.toRadians(steerM.getAngle()); })); } @@ -225,7 +220,7 @@ private Matrix getMotionModel(double dt) { | | ------------------------------------------------------------------------------------| */ - private void kalmanFilter(Matrix C, Matrix z) { + private void kalmanFilter(Matrix C, Matrix Q, Matrix z) { // update time Date now = new Date(); double dt = (now.getTime() - lastTime) / 1000.0; @@ -267,12 +262,12 @@ This produces your next state (x[k]) and your next noise estimation (p[k]), x = x_pre + (K * r) P = (I - (K * C)) * P_pre */ - Matrix residual = z.minus(C.times(x)); - Matrix K = C; - K = K.times(P_pre); + Matrix residual = z.minus(C.times(x_pre)); + Matrix K = C.times(P_pre); K = K.times(C.transpose()); K = K.plus(Q); K = P_pre.times(C.transpose()).times(K.inverse()); + x = x_pre.plus(K.times(residual)); P = Matrix.identity(5, 5).minus(K.times(C)); P = P.times(P_pre); From 6c5a6b057cf1c437bd9c28f2e77c9a4d9831b0c7 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 17 Nov 2016 18:45:43 -0500 Subject: [PATCH 027/149] fixed naming issues --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 4 ++-- .../nodes/localizers/RobobuggyKFLocalizerTest.java | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 4241404a..c2f8e62e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -42,7 +42,7 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private Matrix x; // current state private Matrix R; // measurement noise covariance matrix private Matrix P; // covariance matrix - private Matrix Q_qps; // model noise covariance matrix + private Matrix Q_gps; // model noise covariance matrix private Matrix Q_encoder; // output matrices @@ -173,7 +173,7 @@ private void setupGPSSubscriber() { Matrix z = new Matrix(z2D); - kalmanFilter(C_gps, Q_gps z); + kalmanFilter(C_gps, Q_gps, z); })); } diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java index b7fd9d74..87a7cc8e 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java @@ -38,8 +38,10 @@ public void setUp() { @Test public void test_singleIteration() throws InterruptedException { RobobuggyKFLocalizer localizer = new RobobuggyKFLocalizer(10000, "testLocalizer", new LocTuple(40.441670, -79.9416362)); - new Publisher(NodeChannel.GPS.getMsgPath()).publish(new GpsMeasurement(40.441670, -79.9412361)); - Thread.sleep(100000); + new Publisher(NodeChannel.GPS.getMsgPath()).publish(new GpsMeasurement(40.441670, -79.9512463)); + Thread.sleep(3000); + localizer.update(); + Thread.sleep(3000); Assert.assertEquals(0.0, 1.0, 0.0); } From 17b176e473a9ad06010131258ffc33533236576f Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Fri, 18 Nov 2016 18:07:00 -0500 Subject: [PATCH 028/149] fixed loclizer trajectory analysis --- offline/localizer/trajectory_analysis.m | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/offline/localizer/trajectory_analysis.m b/offline/localizer/trajectory_analysis.m index 0dedec1a..03c81259 100644 --- a/offline/localizer/trajectory_analysis.m +++ b/offline/localizer/trajectory_analysis.m @@ -7,18 +7,18 @@ % addpath('./zoharby-plot_google_map'); addpath('./altmany-export_fig'); -file = 'localizer_v3.mat'; +file = 'localizer_v4.mat'; load(file, 'trajectory', 'P'); save_plot = false; -k = 1000; -wmline(trajectory(8,1:k:end), trajectory(9,1:k:end), 'Color', 'r') +k = 100; +wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r') t1 = 1476008235676; t2 = 1476009019782; t = linspace(0, (t2 - t1)/1000, size(trajectory,2)); figure(); -plot(t, trajectory(10,:)) +plot(t, trajectory(8,:)) title(['Uncertainty ', file]); % plot(trajectory(8,:), trajectory(9,:), '.r', 'MarkerSize', 20) @@ -29,14 +29,14 @@ export_fig('out.jpg') end -theta = deg2rad( trajectory(5,1:k:end) ); +heading = trajectory(4,1:k:end); figure(); hold on; plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) -quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(theta), sin(theta)) +quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading)) hold off; title(['Heading ', file]); % plot on google maps -ss = [trajectory(8,1:k:end); trajectory(9,1:k:end)]; +ss = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; fprintf(1, '%5.20f, %5.20f\n', ss); From 0dce024a5eb3ab57dd954d83edb750ba302beada Mon Sep 17 00:00:00 2001 From: Darshan A Patil Date: Sun, 20 Nov 2016 15:26:21 -0500 Subject: [PATCH 029/149] Matlab controller implementation added --- offline/controller/controller.m | 31 +++++++++++++++++---------- offline/controller/controller_v1.mat | Bin 0 -> 106139 bytes offline/controller/get_logs.py | 25 +++++++++++++++++++++ offline/controller/tags | 5 +++++ offline/controller/waypoints.mat | Bin 0 -> 2328 bytes 5 files changed, 50 insertions(+), 11 deletions(-) create mode 100644 offline/controller/controller_v1.mat create mode 100644 offline/controller/get_logs.py create mode 100644 offline/controller/tags create mode 100644 offline/controller/waypoints.mat diff --git a/offline/controller/controller.m b/offline/controller/controller.m index 40b516f8..40d58f05 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -12,7 +12,7 @@ dt = 0.001; % 1000Hz m = 20; % 20Hz - [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); + [x, y] = ll2utm(lat_long(1), lat_long(2)); X = [x; % X, m, UTM coors y; % Y, m, UTM coors @@ -21,33 +21,32 @@ 0]; % d_heading, rad/s load('./waypoints.mat'); - desired_traj = processWaypoints(waypoints); - + desired_traj = processWaypoints(logs); + desired_traj = desired_traj(:, 1:2); % time = linspace(0, 240, size(trajectory,2)); time = 0:dt:240; u = 0; % steering angle trajectory = []; - for i = 1:size(time, 1) + for i = 1:size(time, 2) t = time(i); A = model(X, u, dt); X = A*X; - if(mod(i, m) == 0) - u = control(desired_traj, t); + u = control(desired_traj, X) end trajectory = [trajectory, X]; end - + if save_data save('controller_v1.mat', 'trajectory'); end end function [trajectory] = processWaypoints(lat_long) - [x, y, zone] = ll2utm(lat_long(1), lat_long(2)); - trajectory = []; + [x, y, zone] = ll2utm(lat_long); + trajectory = [x,y,(ones(size(x, 1), 1)*zone)]; end @@ -61,8 +60,18 @@ 0, 0, tan(u)/wheel_base, 0, 0]; end -function [u] = control(desired_traj, t) - u = pi; +function [u] = control(desired_traj, X) + pos = X(1:2)'; + b = repmat(pos, size(desired_traj, 1), 1); + delta = 100; + possible = find(sum((desired_traj-b).^2, 2) < delta); + if isempty(possible) + u = pi; + else + target = desired_traj(possible(end), :); + deltaPath = target - pos; + u = atan2(target(2), target(1)); + end end diff --git a/offline/controller/controller_v1.mat b/offline/controller/controller_v1.mat new file mode 100644 index 0000000000000000000000000000000000000000..1f2ad8627ff49395350956f84260af17d6ebb0ad GIT binary patch literal 106139 zcmeI*ZA{f=7{_rzEuXV&xfaw(dtn+Odzls9K*hx>p3@89?O zUe`aGJuN4Da%4FA zQv7m8WNKk)B)528Bz=4&eO$)qQ5j>#M@FTkkB$7tuO64b5+04-)c*e=D-wE?f7slR z-fvOX;3X&eu3Wuj@Hf4iCsy2f_s_k@-aYt0pUumjAO6YaE&CEu4m1_4nf!Ib%43n! zndiy}ymsHxxAV76No>hG_0zKLXE(Hc`|d~c)@`b(ed_zq)-JDm@PnMzz30nbY}!(u zS${C8vhL4^Dw-|~IkMoZmT^gg-r99yc6a~$)w;0#U0uA`T)$~xR^F{yQ)+UbNm%~J z@JCY4HGJH*zJAc_y}sNrZCG{Tjdit+(cJvDs@BxxqK5s0*36%=_4K-?RTn3Xt31`+ zciZ|`gIC`F#4q>UwsC3Zo4XT_5BU7}+UA1TD;M;st3ADUY58}DCcVEcx+kOR=x=9^ zY?}4!^k~`8q&;)G`@V4S{!1@iJ@AdR9zC*0S1#^-cyVRc&cvb{@<*G#;)xIXCr8^3Ixl_x?V0b>`geUUyE?`5EUEuKYNz_omxFj@?H# z?|p8}kBwX3`E)_bE6Xd(M%12}I{TV09lq(()T2yYFoDNJCy$3gjMz3ej0O*7B}|1@=g02`WwctN!MX` z)wEyYuEY4gYA+3zhrKkod$}LRH`4Y~j=eOvSUQeN_ELMPbE#}0A6^}5gUW%9ErFbb`ikIT0 zcqv|rm*S;(DPD?~;-z?LKuTjlQSVwHV|R?c;p%hs>%&!X`2*qVbM-yL(labQ!_qS> zJ;Ty7EH9r6F5MNm7(v32Ey7czY!O^9`hbJwAzR26vW09RTgVo&g=`U=m3VEuHeMUA zjn~F&jTyYjF;l2cqv|rm*S;(DPD?~ z;-z>gUW%9ErFdyTO7#H;8g{&uLmzNlLF4KNi-fDs)#vJS^||_7eXc%NKYmc^JSK24 zf`p+DI5<3-F%%pgJr6kCJ!FeudDu(srMxy?8?TMm#%trX@!EK8yf$7NuZ`E%5ftS^ z`A|NT59LGoP(E}&ln>=Y`A|NT59LGoP~VfjCw)))p7cHGdx~jBcqv|rm*S;(DPD?~ z;-z>gUW%9ErFbb`ikIT0cqv|rm*S;(DP9_oQgfohF7B@7VNO)+qv-f)PE@c+^m6Ov zHYduQD08CBi83e3oT&Igsq>h?#Rw9HIZ?sk(VVE@@F-h^yN7HMEDw9By_DC+YvZ-? z+IVfeHeMUAjn~F&gUW%9ErFbb`ikAkYR3C6?>31y;eZX-+T_13;NVxi3eSN_Cfb{|E1J(zu z4>*2M(g%E1;i>&seZaxtQ6F${c+>|R?jEv5usnEe0j;r@+DoTL%Z3KmNPGjqX@ccp zFAdg-z0_XH5#|VUggL?-VUF-ss7AI34v#oZaCpRN;xVgZoyZo!@{lcL3)w=pkS$~j z*+RCEE%YDiKh%Gy|4{#7M^KaxZp<%%_QIMtCV+ikIT0cqv|rm*S;(DPD?~{!d=o@-M8dI(=7dV>CCvt*SLOxu{|P Zpf&SnY(2fM=~}bC7M@KQHo4#QzX4q|N!$Pc literal 0 HcmV?d00001 diff --git a/offline/controller/get_logs.py b/offline/controller/get_logs.py new file mode 100644 index 00000000..3d8280e1 --- /dev/null +++ b/offline/controller/get_logs.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python + +import json +import numpy as np +import scipy.io + +combined = True +d = None +# file = '../rolls_logs/sample.txt' +# file = '../../../rolls_logs/sensors_2016-10-09-06-17-15.txt' +file = 'waypoints_course.txt' +logs = [] +with open(file) as json_data: + for line in json_data: + d = json.loads(line) + lat = d['latitude'] + lng = d['longitude'] + logs.append([lat, lng]) + + +logs = np.array(logs) +print(logs.shape) +vars_map = {'logs': logs} +scipy.io.savemat('./waypoints.mat', mdict=vars_map) + diff --git a/offline/controller/tags b/offline/controller/tags new file mode 100644 index 00000000..5c1792d6 --- /dev/null +++ b/offline/controller/tags @@ -0,0 +1,5 @@ +!_TAG_FILE_SORTED 2 /0=unsorted, 1=sorted, 2=foldcase/ +control /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [u] = control(desired_traj, X) $/;" f language:MatLab +controller /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [trajectory] = controller()$/;" f language:MatLab +model /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [A] = model(x, u, dt)$/;" f language:MatLab +processWaypoints /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [trajectory] = processWaypoints(lat_long)$/;" f language:MatLab diff --git a/offline/controller/waypoints.mat b/offline/controller/waypoints.mat new file mode 100644 index 0000000000000000000000000000000000000000..696b17a327992741870d413d23a2c17206939723 GIT binary patch literal 2328 zcmbW%jaQ6W7y$4$D&sTDCOXF&D{b1OY#$2En^EagH8a8{O*J)>=0jx~(^PMw_0>*x zMN+7(q_Qj>SzEg=7H55(&HtbOi%UjM*$PUq)&?tO3fKKHqzgXK}d zi^RU3UgGFr`RsH}mP#C(rOZv&XKMq+27Qjk;KH;!^9HcIzedY}{T-X;6@unV-&W^?W2aR9ZUTKa-Ip1`qYn>` z%>^I+VhCCb?!Q@Crvn=-_nor9^29HA4LH$PKP&@OH5e6X;2p;fSqj*gVH2MWX1=iZ zOaialMtLNH#D*bLGfHQmjBZ&W6PTCPYD{hx9 z1LGZf^A%u?_1$wB=%hdE6Ax;O)pwVIPv2#I9}P0!&Fzl>)pnCTmVjOxtIvjjgIkq@ zi@;a!HZ5nt^poKeg20^cv6ttAF-~#80ig2b*Ajp5+(M@wAFxOa80RB}l5Jvvq^z<$l^1dcL&Q_CsSW1n{ z)`R2w_9)2ywi{}sd0=QKzliL6nn&Xf$y(7y>hLIN1xM<# zz>=ubg74qu;HdgOQooTiL>5v{qx#8mHMqNC&V5pEAKCFODo}CCv4z~jplMz+xfh0E zhfCkcc=+$5Nbt5`@?x6jMImSh%2+}%b_t`hXHb6i5*jlv1lNaS%tWAVC>Iw=WB!ET z!YCSZ6QikY2g=37U_2yvYbnlq9!q5|#$#M3N7I&3*=GqDKVCs)c7BfWABj}{SQ6@- zjQKnzm8nian^URmR27xirDJ|`2FC4b9Pia&{$3{L$7NyOUW<7@9mWK{L9rfXX%%Q5kjvm2cjN<3E&Q?63vLcWk4wNoAR; np)vmf_A7Tp=<9y$cg{h02a;8|uH#BH1m6*VM~>h7llS%?K|OWI literal 0 HcmV?d00001 From 18b16028cfe0a46c83961a4fbd3b4570362978f8 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 20 Nov 2016 19:36:16 -0500 Subject: [PATCH 030/149] updates to controller sim --- offline/controller/.gitignore | 6 ++++ offline/controller/controller.m | 36 ++++++++++++++++++----- offline/controller/controller_analysis.m | 22 ++++++++------ offline/controller/controller_v1.mat | Bin 106139 -> 0 bytes offline/controller/get_logs.py | 3 -- offline/controller/tags | 5 ---- offline/controller/waypoints.mat | Bin 2328 -> 0 bytes offline/localizer/.gitignore | 3 +- 8 files changed, 49 insertions(+), 26 deletions(-) create mode 100644 offline/controller/.gitignore delete mode 100644 offline/controller/controller_v1.mat delete mode 100644 offline/controller/tags delete mode 100644 offline/controller/waypoints.mat diff --git a/offline/controller/.gitignore b/offline/controller/.gitignore new file mode 100644 index 00000000..7b080b98 --- /dev/null +++ b/offline/controller/.gitignore @@ -0,0 +1,6 @@ +*.mat +*.jpg +*.png +.DS_Store +*.pyc + diff --git a/offline/controller/controller.m b/offline/controller/controller.m index 40d58f05..826ca9cc 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -11,32 +11,39 @@ lat_long = [40.441670, -79.9416362]; dt = 0.001; % 1000Hz m = 20; % 20Hz + velocity = 3; % m/s, 6.7mph - [x, y] = ll2utm(lat_long(1), lat_long(2)); + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); X = [x; % X, m, UTM coors y; % Y, m, UTM coors - 0; % d_Yb, body velocity + velocity; % d_Yb, body velocity first_heading; % heading, rad, world frame 0]; % d_heading, rad/s load('./waypoints.mat'); desired_traj = processWaypoints(logs); - desired_traj = desired_traj(:, 1:2); % time = linspace(0, 240, size(trajectory,2)); time = 0:dt:240; u = 0; % steering angle - trajectory = []; + trajectory = [X; lat_long'; u]; for i = 1:size(time, 2) t = time(i); A = model(X, u, dt); X = A*X; + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + if(mod(i, m) == 0) - u = control(desired_traj, X) + u = control(desired_traj, X); + u = clampAngle(u); end - trajectory = [trajectory, X]; + % trajectory = [trajectory, X]; + snapshot = summarize(X, utm_zone, u); + trajectory = [trajectory, snapshot]; end if save_data @@ -44,11 +51,24 @@ end end -function [trajectory] = processWaypoints(lat_long) +function [desired] = processWaypoints(lat_long) [x, y, zone] = ll2utm(lat_long); - trajectory = [x,y,(ones(size(x, 1), 1)*zone)]; + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; lat; lon; steeringAngle]; end +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end function [A] = model(x, u, dt) global wheel_base diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m index 914691ec..75cfd1bd 100644 --- a/offline/controller/controller_analysis.m +++ b/offline/controller/controller_analysis.m @@ -3,16 +3,18 @@ clear; close all; -addpath('./latlonutm/Codes/matlab'); -% addpath('./zoharby-plot_google_map'); -addpath('./altmany-export_fig'); +addpath('../localizer/latlonutm/Codes/matlab'); +addpath('../localizer/altmany-export_fig'); -file = 'localizer_v4.mat'; -load(file, 'trajectory', 'P'); +file = 'controller_v1.mat'; +load(file, 'trajectory'); save_plot = false; +show_maps = true; -k = 100; -wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r') +k = 1000; +if show_maps + wmline(trajectory(6,1:k:end), trajectory(7,1:k:end), 'Color', 'r') +end heading = trajectory(4,1:k:end); figure(); @@ -23,5 +25,7 @@ title(['Heading ', file]); % plot on google maps -xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; -fprintf(1, '%5.20f, %5.20f\n', xy); +if show_maps + xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; + fprintf(1, '%5.20f, %5.20f\n', xy); +end diff --git a/offline/controller/controller_v1.mat b/offline/controller/controller_v1.mat deleted file mode 100644 index 1f2ad8627ff49395350956f84260af17d6ebb0ad..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 106139 zcmeI*ZA{f=7{_rzEuXV&xfaw(dtn+Odzls9K*hx>p3@89?O zUe`aGJuN4Da%4FA zQv7m8WNKk)B)528Bz=4&eO$)qQ5j>#M@FTkkB$7tuO64b5+04-)c*e=D-wE?f7slR z-fvOX;3X&eu3Wuj@Hf4iCsy2f_s_k@-aYt0pUumjAO6YaE&CEu4m1_4nf!Ib%43n! zndiy}ymsHxxAV76No>hG_0zKLXE(Hc`|d~c)@`b(ed_zq)-JDm@PnMzz30nbY}!(u zS${C8vhL4^Dw-|~IkMoZmT^gg-r99yc6a~$)w;0#U0uA`T)$~xR^F{yQ)+UbNm%~J z@JCY4HGJH*zJAc_y}sNrZCG{Tjdit+(cJvDs@BxxqK5s0*36%=_4K-?RTn3Xt31`+ zciZ|`gIC`F#4q>UwsC3Zo4XT_5BU7}+UA1TD;M;st3ADUY58}DCcVEcx+kOR=x=9^ zY?}4!^k~`8q&;)G`@V4S{!1@iJ@AdR9zC*0S1#^-cyVRc&cvb{@<*G#;)xIXCr8^3Ixl_x?V0b>`geUUyE?`5EUEuKYNz_omxFj@?H# z?|p8}kBwX3`E)_bE6Xd(M%12}I{TV09lq(()T2yYFoDNJCy$3gjMz3ej0O*7B}|1@=g02`WwctN!MX` z)wEyYuEY4gYA+3zhrKkod$}LRH`4Y~j=eOvSUQeN_ELMPbE#}0A6^}5gUW%9ErFbb`ikIT0 zcqv|rm*S;(DPD?~;-z?LKuTjlQSVwHV|R?c;p%hs>%&!X`2*qVbM-yL(labQ!_qS> zJ;Ty7EH9r6F5MNm7(v32Ey7czY!O^9`hbJwAzR26vW09RTgVo&g=`U=m3VEuHeMUA zjn~F&jTyYjF;l2cqv|rm*S;(DPD?~ z;-z>gUW%9ErFdyTO7#H;8g{&uLmzNlLF4KNi-fDs)#vJS^||_7eXc%NKYmc^JSK24 zf`p+DI5<3-F%%pgJr6kCJ!FeudDu(srMxy?8?TMm#%trX@!EK8yf$7NuZ`E%5ftS^ z`A|NT59LGoP(E}&ln>=Y`A|NT59LGoP~VfjCw)))p7cHGdx~jBcqv|rm*S;(DPD?~ z;-z>gUW%9ErFbb`ikIT0cqv|rm*S;(DP9_oQgfohF7B@7VNO)+qv-f)PE@c+^m6Ov zHYduQD08CBi83e3oT&Igsq>h?#Rw9HIZ?sk(VVE@@F-h^yN7HMEDw9By_DC+YvZ-? z+IVfeHeMUAjn~F&gUW%9ErFbb`ikAkYR3C6?>31y;eZX-+T_13;NVxi3eSN_Cfb{|E1J(zu z4>*2M(g%E1;i>&seZaxtQ6F${c+>|R?jEv5usnEe0j;r@+DoTL%Z3KmNPGjqX@ccp zFAdg-z0_XH5#|VUggL?-VUF-ss7AI34v#oZaCpRN;xVgZoyZo!@{lcL3)w=pkS$~j z*+RCEE%YDiKh%Gy|4{#7M^KaxZp<%%_QIMtCV+ikIT0cqv|rm*S;(DPD?~{!d=o@-M8dI(=7dV>CCvt*SLOxu{|P Zpf&SnY(2fM=~}bC7M@KQHo4#QzX4q|N!$Pc diff --git a/offline/controller/get_logs.py b/offline/controller/get_logs.py index 3d8280e1..5ab1c3df 100644 --- a/offline/controller/get_logs.py +++ b/offline/controller/get_logs.py @@ -6,8 +6,6 @@ combined = True d = None -# file = '../rolls_logs/sample.txt' -# file = '../../../rolls_logs/sensors_2016-10-09-06-17-15.txt' file = 'waypoints_course.txt' logs = [] with open(file) as json_data: @@ -22,4 +20,3 @@ print(logs.shape) vars_map = {'logs': logs} scipy.io.savemat('./waypoints.mat', mdict=vars_map) - diff --git a/offline/controller/tags b/offline/controller/tags deleted file mode 100644 index 5c1792d6..00000000 --- a/offline/controller/tags +++ /dev/null @@ -1,5 +0,0 @@ -!_TAG_FILE_SORTED 2 /0=unsorted, 1=sorted, 2=foldcase/ -control /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [u] = control(desired_traj, X) $/;" f language:MatLab -controller /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [trajectory] = controller()$/;" f language:MatLab -model /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [A] = model(x, u, dt)$/;" f language:MatLab -processWaypoints /afs/andrew.cmu.edu/usr20/dapatil/RoboBuggy/offline/controller/controller.m /^function [trajectory] = processWaypoints(lat_long)$/;" f language:MatLab diff --git a/offline/controller/waypoints.mat b/offline/controller/waypoints.mat deleted file mode 100644 index 696b17a327992741870d413d23a2c17206939723..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2328 zcmbW%jaQ6W7y$4$D&sTDCOXF&D{b1OY#$2En^EagH8a8{O*J)>=0jx~(^PMw_0>*x zMN+7(q_Qj>SzEg=7H55(&HtbOi%UjM*$PUq)&?tO3fKKHqzgXK}d zi^RU3UgGFr`RsH}mP#C(rOZv&XKMq+27Qjk;KH;!^9HcIzedY}{T-X;6@unV-&W^?W2aR9ZUTKa-Ip1`qYn>` z%>^I+VhCCb?!Q@Crvn=-_nor9^29HA4LH$PKP&@OH5e6X;2p;fSqj*gVH2MWX1=iZ zOaialMtLNH#D*bLGfHQmjBZ&W6PTCPYD{hx9 z1LGZf^A%u?_1$wB=%hdE6Ax;O)pwVIPv2#I9}P0!&Fzl>)pnCTmVjOxtIvjjgIkq@ zi@;a!HZ5nt^poKeg20^cv6ttAF-~#80ig2b*Ajp5+(M@wAFxOa80RB}l5Jvvq^z<$l^1dcL&Q_CsSW1n{ z)`R2w_9)2ywi{}sd0=QKzliL6nn&Xf$y(7y>hLIN1xM<# zz>=ubg74qu;HdgOQooTiL>5v{qx#8mHMqNC&V5pEAKCFODo}CCv4z~jplMz+xfh0E zhfCkcc=+$5Nbt5`@?x6jMImSh%2+}%b_t`hXHb6i5*jlv1lNaS%tWAVC>Iw=WB!ET z!YCSZ6QikY2g=37U_2yvYbnlq9!q5|#$#M3N7I&3*=GqDKVCs)c7BfWABj}{SQ6@- zjQKnzm8nian^URmR27xirDJ|`2FC4b9Pia&{$3{L$7NyOUW<7@9mWK{L9rfXX%%Q5kjvm2cjN<3E&Q?63vLcWk4wNoAR; np)vmf_A7Tp=<9y$cg{h02a;8|uH#BH1m6*VM~>h7llS%?K|OWI diff --git a/offline/localizer/.gitignore b/offline/localizer/.gitignore index 99a185c2..f05d0f63 100644 --- a/offline/localizer/.gitignore +++ b/offline/localizer/.gitignore @@ -1,4 +1,5 @@ *.mat *.jpg *.png - +.DS_Store +*.pyc From 57dfbf9b956163c9e4c7f3b825b6fe0a23cf2e8b Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Fri, 2 Dec 2016 19:59:45 -0500 Subject: [PATCH 031/149] fixed controller sim --- offline/controller/controller.m | 21 ++++++++++++++++----- offline/controller/controller_analysis.m | 13 ++++++++++++- 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/offline/controller/controller.m b/offline/controller/controller.m index 826ca9cc..e3a3a2e1 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -10,7 +10,7 @@ first_heading = deg2rad(250); lat_long = [40.441670, -79.9416362]; dt = 0.001; % 1000Hz - m = 20; % 20Hz + m = 50; % 20Hz velocity = 3; % m/s, 6.7mph [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -70,11 +70,20 @@ end end +function a = clampSteeringAngle(a) + if(a < -deg2rad(10)) + a = -deg2rad(10); + end + if(a > deg2rad(10)) + a = deg2rad(10); + end +end + function [A] = model(x, u, dt) global wheel_base - A = [1, 0, dt*cos(x(3)), 0, 0; - 0, 1, dt*sin(x(3)), 0, 0; + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; 0, 0, 1, 0, 0; 0, 0, 0, 1, dt; 0, 0, tan(u)/wheel_base, 0, 0]; @@ -83,15 +92,17 @@ function [u] = control(desired_traj, X) pos = X(1:2)'; b = repmat(pos, size(desired_traj, 1), 1); - delta = 100; + delta = 15*15; possible = find(sum((desired_traj-b).^2, 2) < delta); if isempty(possible) - u = pi; + u = 0; else target = desired_traj(possible(end), :); deltaPath = target - pos; u = atan2(target(2), target(1)); end + + u = clampSteeringAngle(u); end diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m index 75cfd1bd..6262370d 100644 --- a/offline/controller/controller_analysis.m +++ b/offline/controller/controller_analysis.m @@ -9,7 +9,12 @@ file = 'controller_v1.mat'; load(file, 'trajectory'); save_plot = false; -show_maps = true; +show_maps = false; + +load('./waypoints.mat'); +[x, y, zone] = ll2utm(logs); +desired = [x y]; + k = 1000; if show_maps @@ -21,9 +26,15 @@ hold on; plot(trajectory(1,1:k:end), trajectory(2,1:k:end)) quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading)) +plot(desired(:,1), desired(:,2), 'g') hold off; title(['Heading ', file]); +figure(); +p = k/20; +plot(1:p:size(trajectory,2), trajectory(8,1:p:end)) +title('Control input'); + % plot on google maps if show_maps xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; From c9d6b0e7eb323a1f19c470bdc881f53c87a8390e Mon Sep 17 00:00:00 2001 From: Darshan Patil Date: Sat, 3 Dec 2016 00:13:35 -0500 Subject: [PATCH 032/149] Fixed controller simulation bugs --- offline/controller/controller.m | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/offline/controller/controller.m b/offline/controller/controller.m index e3a3a2e1..d1b71a7d 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -11,7 +11,7 @@ lat_long = [40.441670, -79.9416362]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 3; % m/s, 6.7mph + velocity = 8; % m/s, 17.9mph [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -26,7 +26,7 @@ % time = linspace(0, 240, size(trajectory,2)); time = 0:dt:240; u = 0; % steering angle - trajectory = [X; lat_long'; u]; + trajectory = [X; lat_long(1); lat_long(2); u]; for i = 1:size(time, 2) t = time(i); @@ -38,14 +38,13 @@ if(mod(i, m) == 0) u = control(desired_traj, X); - u = clampAngle(u); end % trajectory = [trajectory, X]; snapshot = summarize(X, utm_zone, u); trajectory = [trajectory, snapshot]; end - + if save_data save('controller_v1.mat', 'trajectory'); end @@ -58,7 +57,7 @@ function snapshot = summarize(x, utm_zone, steeringAngle) [lat, lon] = utm2ll(x(1), x(2), utm_zone); - snapshot = [x; lat; lon; steeringAngle]; + snapshot = [x; x(1); x(2); steeringAngle]; end function a = clampAngle(a) @@ -96,13 +95,12 @@ possible = find(sum((desired_traj-b).^2, 2) < delta); if isempty(possible) u = 0; - else + else target = desired_traj(possible(end), :); deltaPath = target - pos; - u = atan2(target(2), target(1)); + u = atan2(deltaPath(2), deltaPath(1))-X(4); end - - u = clampSteeringAngle(u); + u = clampSteeringAngle(clampAngle(u)); end From 1acf75c530a4af466cc5e76d3bb02f20350a7539 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sat, 3 Dec 2016 07:51:47 -0500 Subject: [PATCH 033/149] added steering vel --- offline/controller/controller.m | 26 +++++- offline/controller/controller_pure.m | 125 +++++++++++++++++++++++++++ 2 files changed, 147 insertions(+), 4 deletions(-) create mode 100644 offline/controller/controller_pure.m diff --git a/offline/controller/controller.m b/offline/controller/controller.m index d1b71a7d..bbcd5df5 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -3,6 +3,8 @@ addpath('../localizer/latlonutm/Codes/matlab'); global wheel_base global velocity + global steering_vel + global dt save_data = true; wheel_base = 1.13; @@ -12,6 +14,7 @@ dt = 0.001; % 1000Hz m = 50; % 20Hz velocity = 8; % m/s, 17.9mph + steering_vel = deg2rad(10); [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -25,13 +28,15 @@ desired_traj = processWaypoints(logs); % time = linspace(0, 240, size(trajectory,2)); time = 0:dt:240; - u = 0; % steering angle + u = 0; % commanded steering angle + steering = u; % steering angle trajectory = [X; lat_long(1); lat_long(2); u]; for i = 1:size(time, 2) t = time(i); - A = model(X, u, dt); + A = model(X, steering); X = A*X; + steering = updateSteering(steering, u); X(4) = clampAngle(X(4)); X(5) = clampAngle(X(5)); @@ -69,6 +74,18 @@ end end +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + function a = clampSteeringAngle(a) if(a < -deg2rad(10)) a = -deg2rad(10); @@ -78,14 +95,15 @@ end end -function [A] = model(x, u, dt) +function [A] = model(x, steering) global wheel_base + global dt A = [1, 0, dt*cos(x(4)), 0, 0; 0, 1, dt*sin(x(4)), 0, 0; 0, 0, 1, 0, 0; 0, 0, 0, 1, dt; - 0, 0, tan(u)/wheel_base, 0, 0]; + 0, 0, tan(steering)/wheel_base, 0, 0]; end function [u] = control(desired_traj, X) diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m new file mode 100644 index 00000000..1cfee80f --- /dev/null +++ b/offline/controller/controller_pure.m @@ -0,0 +1,125 @@ +function [trajectory] = controller() +% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf +% section 2.2 + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.441670, -79.9416362]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 8; % m/s, 17.9mph + steering_vel = deg2rad(10); + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints.mat'); + desired_traj = processWaypoints(logs); + % time = linspace(0, 240, size(trajectory,2)); + time = 0:dt:240; + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); u]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + % trajectory = [trajectory, X]; + snapshot = summarize(X, utm_zone, u); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_v1.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; x(1); x(2); steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + if(a < -deg2rad(10)) + a = -deg2rad(10); + end + if(a > deg2rad(10)) + a = deg2rad(10); + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + pos = X(1:2)'; + b = repmat(pos, size(desired_traj, 1), 1); + delta = 15*15; + possible = find(sum((desired_traj-b).^2, 2) < delta); + if isempty(possible) + u = 0; + else + target = desired_traj(possible(end), :); + deltaPath = target - pos; + u = atan2(deltaPath(2), deltaPath(1))-X(4); + end + u = clampSteeringAngle(clampAngle(u)); +end + From 319317b9a1e34f748a10e3ccd94225fa0f1abd7e Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sat, 3 Dec 2016 19:41:39 -0500 Subject: [PATCH 034/149] minor --- offline/controller/controller.m | 4 ++-- offline/controller/controller_pure.m | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/offline/controller/controller.m b/offline/controller/controller.m index bbcd5df5..7415d958 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -13,8 +13,8 @@ lat_long = [40.441670, -79.9416362]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 8; % m/s, 17.9mph - steering_vel = deg2rad(10); + velocity = 8; % m/s, 17.9mph, forward velocity + steering_vel = deg2rad(20); % 20deg/s, reaction speed to control cmds [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m index 1cfee80f..676f7063 100644 --- a/offline/controller/controller_pure.m +++ b/offline/controller/controller_pure.m @@ -1,4 +1,4 @@ -function [trajectory] = controller() +function [trajectory] = controller_pure() % https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf % section 2.2 @@ -15,8 +15,8 @@ lat_long = [40.441670, -79.9416362]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 8; % m/s, 17.9mph - steering_vel = deg2rad(10); + velocity = 8; % m/s, 17.9mph, forward velocity + steering_vel = deg2rad(20); % 20deg/s, reaction speed to control cmds [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); From f4073f56656bd619ec5baf615fe37209cafc21ab Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 4 Dec 2016 19:18:33 -0500 Subject: [PATCH 035/149] pure pursuit controller works very well! --- offline/controller/controller.m | 6 +++--- offline/controller/controller_analysis.m | 2 +- offline/controller/controller_pure.m | 16 +++++++++++----- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/offline/controller/controller.m b/offline/controller/controller.m index 7415d958..66e90201 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -14,7 +14,7 @@ dt = 0.001; % 1000Hz m = 50; % 20Hz velocity = 8; % m/s, 17.9mph, forward velocity - steering_vel = deg2rad(20); % 20deg/s, reaction speed to control cmds + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -30,7 +30,7 @@ time = 0:dt:240; u = 0; % commanded steering angle steering = u; % steering angle - trajectory = [X; lat_long(1); lat_long(2); u]; + trajectory = [X; lat_long(1); lat_long(2); steering]; for i = 1:size(time, 2) t = time(i); @@ -46,7 +46,7 @@ end % trajectory = [trajectory, X]; - snapshot = summarize(X, utm_zone, u); + snapshot = summarize(X, utm_zone, steering); trajectory = [trajectory, snapshot]; end diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m index 6262370d..2904e1c9 100644 --- a/offline/controller/controller_analysis.m +++ b/offline/controller/controller_analysis.m @@ -6,7 +6,7 @@ addpath('../localizer/latlonutm/Codes/matlab'); addpath('../localizer/altmany-export_fig'); -file = 'controller_v1.mat'; +file = 'controller_v2.mat'; load(file, 'trajectory'); save_plot = false; show_maps = false; diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m index 676f7063..ca510163 100644 --- a/offline/controller/controller_pure.m +++ b/offline/controller/controller_pure.m @@ -16,7 +16,8 @@ dt = 0.001; % 1000Hz m = 50; % 20Hz velocity = 8; % m/s, 17.9mph, forward velocity - steering_vel = deg2rad(20); % 20deg/s, reaction speed to control cmds + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + % full range in 0.5s [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -32,7 +33,7 @@ time = 0:dt:240; u = 0; % commanded steering angle steering = u; % steering angle - trajectory = [X; lat_long(1); lat_long(2); u]; + trajectory = [X; lat_long(1); lat_long(2); steering]; for i = 1:size(time, 2) t = time(i); @@ -48,12 +49,12 @@ end % trajectory = [trajectory, X]; - snapshot = summarize(X, utm_zone, u); + snapshot = summarize(X, utm_zone, steering); trajectory = [trajectory, snapshot]; end if save_data - save('controller_v1.mat', 'trajectory'); + save('controller_v2.mat', 'trajectory'); end end @@ -109,6 +110,8 @@ end function [u] = control(desired_traj, X) + global wheel_base + pos = X(1:2)'; b = repmat(pos, size(desired_traj, 1), 1); delta = 15*15; @@ -118,7 +121,10 @@ else target = desired_traj(possible(end), :); deltaPath = target - pos; - u = atan2(deltaPath(2), deltaPath(1))-X(4); + + k = 0.8; + a = atan2(deltaPath(2), deltaPath(1))-X(4); + u = atan2(2*wheel_base*sin(a), k*X(3)); end u = clampSteeringAngle(clampAngle(u)); end From a389370b046fda810b30a5b8bd1be5bcfc0b452c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 20 Jan 2017 18:04:50 -0500 Subject: [PATCH 036/149] ported the matlab controller to java basically the old controller was almost correct, we just corrected the body calculations --- .../roboclub/robobuggy/messages/GPSPoseMessage.java | 13 ++++++++++++- .../nodes/localizers/RobobuggyKFLocalizer.java | 8 ++++++-- .../nodes/planners/WayPointFollowerPlanner.java | 5 ++++- 3 files changed, 22 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java index aff45b4a..c0fd3a11 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java @@ -1,5 +1,6 @@ package com.roboclub.robobuggy.messages; +import Jama.Matrix; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; import com.roboclub.robobuggy.ros.Message; @@ -17,6 +18,11 @@ public class GPSPoseMessage extends BaseMessage { private final double latitude; private final double longitude; private final double heading; + private final Matrix currentState; + + public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading) { + this(timestamp, latitude, longitude, heading, null); + } /** * Constructs a new {@link GPSPoseMessage} @@ -26,11 +32,12 @@ public class GPSPoseMessage extends BaseMessage { * @param longitude of the buggy (negative is West) * @param heading of the buggy (in degrees from North) */ - public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading) { + public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, Matrix currentState) { this.latitude = latitude; this.longitude = longitude; this.heading = heading; this.timestamp = new Date(timestamp.getTime()).getTime(); + this.currentState = currentState; } /** @@ -60,6 +67,10 @@ public double getHeading() { return heading; } + public Matrix getCurrentState() { + return currentState; + } + /** * evaluates to the distance between two gps points based on an L2 metric * diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index c2f8e62e..8b6ce0a7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -28,7 +28,7 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private static final int HEADING_GLOBAL_ROW = 3; private static final int HEADING_VEL_ROW = 4; - private static final double WHEELBASE_IN_METERS = 1.13; // meters + public static final double WHEELBASE_IN_METERS = 1.13; // meters private static final int UTMZONE = 17; private final UTMTuple initialLocationGPS; // new LocTuple(40.441670, -79.9416362)); private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad @@ -55,6 +55,10 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity private double steeringAngle = 0; // current steering angle of the buggy + public Matrix getCurrentState() { + return x; + } + /** * Create a new {@link PeriodicNode} decorator * @@ -192,7 +196,7 @@ protected void update() { x_predict.get(Y_GLOBAL_ROW, 0)); LocTuple latLon = LocalizerUtil.utm2Deg(utm); posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), - latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0))); + latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x)); } // Kalman filter step 0: Generate the motion model for the buggy diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 11da3357..8d6573ce 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -4,6 +4,7 @@ import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.ros.NodeChannel; import java.util.ArrayList; @@ -82,8 +83,10 @@ public double getCommandedSteeringAngle() { double poseHeading = Util.normalizeAngleDeg(Math.toDegrees(pose.getHeading())); //find the angle we need to reach that point - return Util.normalizeAngleDeg(desiredHeading - poseHeading); + double deltaHeading = Util.normalizeAngleDeg(desiredHeading - poseHeading); + // literally magic, dont ask (yet) + return Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); } @Override From d85fa6017da408a5e0c734da42c2427bb6212556 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 21 Jan 2017 22:35:34 -0500 Subject: [PATCH 037/149] prepped buggy for auton testing --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 2 +- .../java/com/roboclub/robobuggy/robots/TransistorAuton.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 8b6ce0a7..8e1132a7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -66,7 +66,7 @@ public Matrix getCurrentState() { * @param name the name of the node * @param initialPosition the initial position of the localizer */ - protected RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { + public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { super(new BuggyBaseNode(NodeChannel.POSE), period, name); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 751b602e..8a85bf50 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -5,6 +5,8 @@ import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; @@ -59,7 +61,7 @@ private TransistorAuton() { new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); // Initialize Nodes - nodeList.add(new KfLocalizer(10)); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(0, 0))); nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, From a002bc7f628ed6183178b242fe2fd703b5d8937a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 26 Jan 2017 00:21:46 -0500 Subject: [PATCH 038/149] started migration of matlab simulator over to java --- .../robobuggy/main/RobobuggyMainFile.java | 4 +- .../robots/ControllerTesterRobot.java | 23 +++++ .../simulation/ControllerTester.java | 99 +++++++++++++++++++ 3 files changed, 124 insertions(+), 2 deletions(-) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index b2bade70..cc0b1158 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.TransistorDataCollection; +import com.roboclub.robobuggy.robots.ControllerTesterRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorDataCollection.getInstance(); + robot = ControllerTesterRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java new file mode 100644 index 00000000..fe297038 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -0,0 +1,23 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.simulation.ControllerTester; + +/** + * Created by vivaanbahl on 1/26/17. + */ +public class ControllerTesterRobot extends AbstractRobot { + private static ControllerTesterRobot instance; + + public static ControllerTesterRobot getInstance() { + if (instance == null) { + instance = new ControllerTesterRobot(); + } + return instance; + } + + private ControllerTesterRobot() { + super(); + + nodeList.add(new ControllerTester("Testing the controller")); + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java new file mode 100644 index 00000000..3a039072 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -0,0 +1,99 @@ +package com.roboclub.robobuggy.simulation; + +import Jama.Matrix; +import com.roboclub.robobuggy.messages.DriveControlMessage; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +/** + * Created by vivaanbahl on 1/25/17. + * + * adapted from @babraham's MATLAB script found in offline/controller + */ +public class ControllerTester extends PeriodicNode { + + // run every 10 ms + private static final int PERIOD = 1000; + // wheelbase in meters + private static final double WHEELBASE = 1.13; + // assume a velocity of 8 m/s + private static final double VELOCITY = 8; + // assume we start from the base of the track + private static final LocTuple INITIAL_POSITION_LL = new LocTuple(40.441670, -79.9416362); + // assume we are facing up hill 1 + private static final int INITIAL_HEADING_DEG = 250; + + private long previousTimeMillis; + private long timeDiffMillis; + + private int heading = INITIAL_HEADING_DEG; + private LocTuple firstPosition = INITIAL_POSITION_LL; + private Matrix X; + private Matrix A; + private double commandedSteeringAngle = 0; + private Publisher simulatedPosePub; + + /** + * Create a new {@link PeriodicNode} decorator + * + * @param name + */ + public ControllerTester(String name) { + super(new BuggyBaseNode(NodeChannel.AUTO), PERIOD, name); + + double[][] XAsDoubleArr = { + { LocalizerUtil.deg2UTM(firstPosition).getEasting() }, + { LocalizerUtil.deg2UTM(firstPosition).getNorthing() }, + { VELOCITY }, + { heading }, + { 0 } + }; + + X = new Matrix(XAsDoubleArr); + previousTimeMillis = System.currentTimeMillis(); + + new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { + commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); + })); + + simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); + } + + @Override + protected void update() { + timeDiffMillis = System.currentTimeMillis(); + timeDiffMillis -= previousTimeMillis; + previousTimeMillis = System.currentTimeMillis(); + + A = getNewModel(X, heading); + + + } + + private Matrix getNewModel(Matrix x, int heading) { + double[][] matrixAsDoubleArr = { + { 1, 0, timeDiffMillis * Math.cos(x.get(3, 0)), 0, 0 }, + { 0, 1, timeDiffMillis * Math.sin(x.get(3, 0)), 0, 0 }, + { 0, 0, 1, 0, 0 }, + { 0, 0, 0, 1, timeDiffMillis }, + { 0, 0, Math.tan(heading)/WHEELBASE, 0, 0 } + }; + return new Matrix(matrixAsDoubleArr); + } + + @Override + protected boolean startDecoratorNode() { + resume(); + return true; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} From da9dc2d6cf855e66b477aea8f048e827346d5326 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Thu, 26 Jan 2017 17:45:49 -0500 Subject: [PATCH 039/149] minor --- offline/controller/controller.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/offline/controller/controller.m b/offline/controller/controller.m index 66e90201..fe0594a0 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -51,7 +51,7 @@ end if save_data - save('controller_v1.mat', 'trajectory'); + save('controller_v2.mat', 'trajectory'); end end From 5083e8b4dc962d13f1ffe7a42cef821c84cb527a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 26 Jan 2017 18:47:29 -0500 Subject: [PATCH 040/149] finished first version of simulator still need to test --- .../robots/ControllerTesterRobot.java | 31 +++++++++++++++++++ .../simulation/ControllerTester.java | 28 ++++++++++++++--- 2 files changed, 54 insertions(+), 5 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index fe297038..c0ccc13f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -1,6 +1,19 @@ package com.roboclub.robobuggy.robots; +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.simulation.ControllerTester; +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; +import com.roboclub.robobuggy.ui.PathPanel; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +import java.io.IOException; +import java.util.ArrayList; /** * Created by vivaanbahl on 1/26/17. @@ -19,5 +32,23 @@ private ControllerTesterRobot() { super(); nodeList.add(new ControllerTester("Testing the controller")); + try { + ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + nodeList.add(new WayPointFollowerPlanner(wayPoints)); + } catch (IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + tabs.addTab(new MainGuiWindow(), "Home"); + // tabs.addTab(new PoseGraphsPanel(),"poses"); + // tabs.addTab(new AutonomousPanel(),"Autonomous"); + tabs.add(new PathPanel(), "Path Visualizer"); + tabs.addTab(new ConfigurationPanel(), "Configuration"); } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index 3a039072..b7fdb09f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -2,14 +2,19 @@ import Jama.Matrix; import com.roboclub.robobuggy.messages.DriveControlMessage; +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.UTMTuple; import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; import com.roboclub.robobuggy.ros.Subscriber; +import java.util.Date; + /** * Created by vivaanbahl on 1/25/17. * @@ -18,7 +23,7 @@ public class ControllerTester extends PeriodicNode { // run every 10 ms - private static final int PERIOD = 1000; + private static final int PERIOD = 10; // wheelbase in meters private static final double WHEELBASE = 1.13; // assume a velocity of 8 m/s @@ -29,7 +34,7 @@ public class ControllerTester extends PeriodicNode { private static final int INITIAL_HEADING_DEG = 250; private long previousTimeMillis; - private long timeDiffMillis; + private double timeDiffMillis; private int heading = INITIAL_HEADING_DEG; private LocTuple firstPosition = INITIAL_POSITION_LL; @@ -37,6 +42,7 @@ public class ControllerTester extends PeriodicNode { private Matrix A; private double commandedSteeringAngle = 0; private Publisher simulatedPosePub; + private Publisher gpspub; /** * Create a new {@link PeriodicNode} decorator @@ -59,9 +65,11 @@ public ControllerTester(String name) { new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); + heading += commandedSteeringAngle; })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); + gpspub = new Publisher(NodeChannel.GPS.getMsgPath()); } @Override @@ -70,18 +78,28 @@ protected void update() { timeDiffMillis -= previousTimeMillis; previousTimeMillis = System.currentTimeMillis(); - A = getNewModel(X, heading); + A = getNewModel(X); + + // update simulated position + X = A.times(X); + + UTMTuple t = new UTMTuple(17, 'T', X.get(0, 0), X.get(1, 0)); + LocTuple lt = LocalizerUtil.utm2Deg(t); + // if it's time to run the controller, update the controller's understanding of where we are + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), heading, X)); + gpspub.publish(new GpsMeasurement(lt.getLatitude(), lt.getLongitude())); } - private Matrix getNewModel(Matrix x, int heading) { + private Matrix getNewModel(Matrix x) { + timeDiffMillis = 1.0/PERIOD; double[][] matrixAsDoubleArr = { { 1, 0, timeDiffMillis * Math.cos(x.get(3, 0)), 0, 0 }, { 0, 1, timeDiffMillis * Math.sin(x.get(3, 0)), 0, 0 }, { 0, 0, 1, 0, 0 }, { 0, 0, 0, 1, timeDiffMillis }, - { 0, 0, Math.tan(heading)/WHEELBASE, 0, 0 } + { 0, 0, Math.tan(commandedSteeringAngle)/WHEELBASE, 0, 0 } }; return new Matrix(matrixAsDoubleArr); } From bf5e8b3bc53320128767d0f3a8590e3e8aecd2ce Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 00:16:47 -0500 Subject: [PATCH 041/149] simulator almost working, had to change everything to radians to cope --- .../planners/WayPointFollowerPlanner.java | 14 +++-- .../simulation/ControllerTester.java | 52 ++++++++++++------- 2 files changed, 41 insertions(+), 25 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 8d6573ce..a8e3fccb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -1,6 +1,7 @@ package com.roboclub.robobuggy.nodes.planners; -import com.roboclub.robobuggy.main.Util; +import com.roboclub.robobuggy.main.RobobuggyLogicNotification; +import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; @@ -52,6 +53,7 @@ private static int getClosestIndex(ArrayList wayPoints, GPSPoseM public double getCommandedSteeringAngle() { int closestIndex = getClosestIndex(wayPoints, pose); if (closestIndex == -1) { + new RobobuggyLogicNotification("HELP no closest index", RobobuggyMessageLevel.EXCEPTION); return 17433504; //A dummy value that we can never get } @@ -66,6 +68,7 @@ public double getCommandedSteeringAngle() { //if we are out of points then just go straight if (targetIndex >= wayPoints.size()) { + new RobobuggyLogicNotification("HELP out of points", RobobuggyMessageLevel.EXCEPTION); return 0; } @@ -75,16 +78,17 @@ public double getCommandedSteeringAngle() { //find a path from our current location to that point double dLon = targetPoint.getLongitude() - pose.getLongitude(); double dLat = targetPoint.getLatitude() - pose.getLatitude(); - double desiredHeading = Math.toDegrees(Math.atan2(LocalizerUtil.convertLatToMeters(dLat), LocalizerUtil.convertLonToMeters(dLon))); + double desiredHeading = Math.atan2(LocalizerUtil.convertLatToMeters(dLat), LocalizerUtil.convertLonToMeters(dLon)); // basically we want all of our angles to be in the same range, so that we don't // have weird wraparound - desiredHeading = Util.normalizeAngleDeg(desiredHeading); - double poseHeading = Util.normalizeAngleDeg(Math.toDegrees(pose.getHeading())); +// desiredHeading = Util.normalizeAngleDeg(desiredHeading); + double poseHeading = pose.getHeading(); //find the angle we need to reach that point - double deltaHeading = Util.normalizeAngleDeg(desiredHeading - poseHeading); + double deltaHeading = desiredHeading - poseHeading; +// return Util.normalizeAngleRad(deltaHeading); // literally magic, dont ask (yet) return Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index b7fdb09f..3a6a1060 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -1,6 +1,8 @@ package com.roboclub.robobuggy.simulation; import Jama.Matrix; +import com.roboclub.robobuggy.main.RobobuggyLogicNotification; +import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; @@ -23,26 +25,26 @@ public class ControllerTester extends PeriodicNode { // run every 10 ms - private static final int PERIOD = 10; + private static final int SIM_PERIOD = 10; + // controller runs every 10 iterations + private static final int CONTROLLER_PERIOD = 1; // wheelbase in meters private static final double WHEELBASE = 1.13; // assume a velocity of 8 m/s - private static final double VELOCITY = 8; + private static final double VELOCITY = 2; // assume we start from the base of the track private static final LocTuple INITIAL_POSITION_LL = new LocTuple(40.441670, -79.9416362); // assume we are facing up hill 1 - private static final int INITIAL_HEADING_DEG = 250; + private static final double INITIAL_HEADING_RAD = Math.toRadians(250); - private long previousTimeMillis; - private double timeDiffMillis; - - private int heading = INITIAL_HEADING_DEG; - private LocTuple firstPosition = INITIAL_POSITION_LL; + private double heading = INITIAL_HEADING_RAD; private Matrix X; private Matrix A; private double commandedSteeringAngle = 0; private Publisher simulatedPosePub; private Publisher gpspub; + private int simCounter; + private double targetHeading = INITIAL_HEADING_RAD; /** * Create a new {@link PeriodicNode} decorator @@ -50,8 +52,9 @@ public class ControllerTester extends PeriodicNode { * @param name */ public ControllerTester(String name) { - super(new BuggyBaseNode(NodeChannel.AUTO), PERIOD, name); + super(new BuggyBaseNode(NodeChannel.AUTO), SIM_PERIOD, name); + LocTuple firstPosition = INITIAL_POSITION_LL; double[][] XAsDoubleArr = { { LocalizerUtil.deg2UTM(firstPosition).getEasting() }, { LocalizerUtil.deg2UTM(firstPosition).getNorthing() }, @@ -61,11 +64,11 @@ public ControllerTester(String name) { }; X = new Matrix(XAsDoubleArr); - previousTimeMillis = System.currentTimeMillis(); new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); - heading += commandedSteeringAngle; + new RobobuggyLogicNotification("Steering angle = " + Math.toDegrees(commandedSteeringAngle), RobobuggyMessageLevel.EXCEPTION); + targetHeading = heading + commandedSteeringAngle; })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); @@ -74,31 +77,40 @@ public ControllerTester(String name) { @Override protected void update() { - timeDiffMillis = System.currentTimeMillis(); - timeDiffMillis -= previousTimeMillis; - previousTimeMillis = System.currentTimeMillis(); + simCounter++; A = getNewModel(X); // update simulated position X = A.times(X); + double steeringIncrement = Math.toRadians(0.5); + if (heading > targetHeading) { + heading -= steeringIncrement; + } + else if (heading < targetHeading) { + heading += steeringIncrement; + } + UTMTuple t = new UTMTuple(17, 'T', X.get(0, 0), X.get(1, 0)); LocTuple lt = LocalizerUtil.utm2Deg(t); // if it's time to run the controller, update the controller's understanding of where we are - simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), heading, X)); - gpspub.publish(new GpsMeasurement(lt.getLatitude(), lt.getLongitude())); + if (simCounter == CONTROLLER_PERIOD) { + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), heading, X)); + gpspub.publish(new GpsMeasurement(lt.getLatitude(), lt.getLongitude())); + simCounter = 0; + } } private Matrix getNewModel(Matrix x) { - timeDiffMillis = 1.0/PERIOD; + double dt = 1.0/SIM_PERIOD; double[][] matrixAsDoubleArr = { - { 1, 0, timeDiffMillis * Math.cos(x.get(3, 0)), 0, 0 }, - { 0, 1, timeDiffMillis * Math.sin(x.get(3, 0)), 0, 0 }, + { 1, 0, dt * Math.cos((x.get(3, 0))), 0, 0 }, + { 0, 1, dt * Math.sin((x.get(3, 0))), 0, 0 }, { 0, 0, 1, 0, 0 }, - { 0, 0, 0, 1, timeDiffMillis }, + { 0, 0, 0, 1, dt }, { 0, 0, Math.tan(commandedSteeringAngle)/WHEELBASE, 0, 0 } }; return new Matrix(matrixAsDoubleArr); From 1a01e8e7ced2375e6239264ae816485b1e90b512 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 00:24:46 -0500 Subject: [PATCH 042/149] AWW YEEEE controller simulation finished. It looks absolutely beautiful --- .../robobuggy/simulation/ControllerTester.java | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index 3a6a1060..8ff88100 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -1,8 +1,6 @@ package com.roboclub.robobuggy.simulation; import Jama.Matrix; -import com.roboclub.robobuggy.main.RobobuggyLogicNotification; -import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; @@ -27,7 +25,7 @@ public class ControllerTester extends PeriodicNode { // run every 10 ms private static final int SIM_PERIOD = 10; // controller runs every 10 iterations - private static final int CONTROLLER_PERIOD = 1; + private static final int CONTROLLER_PERIOD = 5; // wheelbase in meters private static final double WHEELBASE = 1.13; // assume a velocity of 8 m/s @@ -37,7 +35,6 @@ public class ControllerTester extends PeriodicNode { // assume we are facing up hill 1 private static final double INITIAL_HEADING_RAD = Math.toRadians(250); - private double heading = INITIAL_HEADING_RAD; private Matrix X; private Matrix A; private double commandedSteeringAngle = 0; @@ -59,7 +56,7 @@ public ControllerTester(String name) { { LocalizerUtil.deg2UTM(firstPosition).getEasting() }, { LocalizerUtil.deg2UTM(firstPosition).getNorthing() }, { VELOCITY }, - { heading }, + { INITIAL_HEADING_RAD }, { 0 } }; @@ -67,8 +64,7 @@ public ControllerTester(String name) { new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); - new RobobuggyLogicNotification("Steering angle = " + Math.toDegrees(commandedSteeringAngle), RobobuggyMessageLevel.EXCEPTION); - targetHeading = heading + commandedSteeringAngle; + targetHeading = X.get(3, 0) + commandedSteeringAngle; })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); @@ -85,11 +81,12 @@ protected void update() { X = A.times(X); double steeringIncrement = Math.toRadians(0.5); + double heading = X.get(3, 0); if (heading > targetHeading) { - heading -= steeringIncrement; + X.set(3, 0, heading - steeringIncrement); } else if (heading < targetHeading) { - heading += steeringIncrement; + X.set(3, 0, heading + steeringIncrement); } UTMTuple t = new UTMTuple(17, 'T', X.get(0, 0), X.get(1, 0)); From 4d047aeef9c627730b0df049cb11a6994f92833d Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 00:50:25 -0500 Subject: [PATCH 043/149] added skeleton of localizer tester --- .../robobuggy/simulation/LocalizerTester.java | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java new file mode 100644 index 00000000..a9376855 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -0,0 +1,66 @@ +package com.roboclub.robobuggy.simulation; + +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.ros.NodeChannel; + +import java.util.ArrayList; +import java.util.Timer; +import java.util.TimerTask; + +/** + * Created by vivaanbahl on 1/27/17. + */ +public class LocalizerTester extends BuggyDecoratorNode { + + private static final int GPS_UPDATE_PERIOD = 500; + private static final int ODOM_UPDATE_PERIOD = 50; + private static final double POSITION_UPDATE_M = 0.5; // move this many meters every tick + + private GpsMeasurement targetWaypoint; + private Timer gpsTimer; + private Timer odomTimer; + private LocTuple currentPosition; + private double heading; + private double noise; + + /** + * Creates a new decorator for the given {@link Node} + * + * @param node {@link Node} to decorate + * @param name the name we want for this node to store so that it can be referenced later + */ + public LocalizerTester(String name, ArrayList waypoints, double noise) { + super(new BuggyBaseNode(NodeChannel.POSE), name); + + gpsTimer = new Timer("GPS"); + odomTimer = new Timer("odom"); + this.noise = noise; + } + + @Override + protected boolean startDecoratorNode() { + gpsTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + //todo update sim gps + } + }, 0, GPS_UPDATE_PERIOD); + + odomTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + // todo update sim odom + } + }, 0, ODOM_UPDATE_PERIOD); + + return true; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} From 64c08717e22e62dd966f7aea7a5fcc8adee5f921 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 01:20:51 -0500 Subject: [PATCH 044/149] added robot config, still need to test --- .../robobuggy/main/RobobuggyMainFile.java | 4 +- .../robobuggy/messages/GpsMeasurement.java | 14 +++++ .../robots/LocalizerTesterRobot.java | 57 +++++++++++++++++++ .../robobuggy/simulation/LocalizerTester.java | 28 +++++++-- 4 files changed, 95 insertions(+), 8 deletions(-) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index cc0b1158..9aa36c1b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.ControllerTesterRobot; +import com.roboclub.robobuggy.robots.LocalizerTesterRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = ControllerTesterRobot.getInstance(); + robot = LocalizerTesterRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java index a7925ed0..bd9f7aab 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java @@ -1,5 +1,7 @@ package com.roboclub.robobuggy.messages; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; + import java.util.Date; /** @@ -154,4 +156,16 @@ public GPSPoseMessage toGpsPoseMessage(double heading) { return new GPSPoseMessage(gpsTimestamp, latitude, longitude, heading); } + /** + * evaluates to the distance between two gps points based on an L2 metric + * + * @param a the first gps point + * @param b the second gps point + * @return the distance in meters + */ + public static double getDistance(GpsMeasurement a, GpsMeasurement b) { + double dx = LocalizerUtil.convertLonToMeters(a.getLongitude() - b.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(a.getLatitude() - b.getLatitude()); + return Math.sqrt(dx * dx + dy * dy); + } } \ No newline at end of file diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java new file mode 100644 index 00000000..5967d330 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java @@ -0,0 +1,57 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; +import com.roboclub.robobuggy.simulation.LocalizerTester; +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; +import com.roboclub.robobuggy.ui.PathPanel; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +import java.io.FileNotFoundException; +import java.util.ArrayList; + +/** + * Created by vivaanbahl on 1/27/17. + */ +public class LocalizerTesterRobot extends AbstractRobot { + + private static LocalizerTesterRobot instance; + + protected LocalizerTesterRobot() { + super(); + + ArrayList waypoints = null; + try { + waypoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + nodeList.add(new LocalizerTester("localizer tester", waypoints, 0)); + nodeList.add(new RobobuggyKFLocalizer(10, "localizer", new LocTuple(0, 0))); + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + tabs.addTab(new MainGuiWindow(), "Home"); + // tabs.addTab(new PoseGraphsPanel(),"poses"); + // tabs.addTab(new AutonomousPanel(),"Autonomous"); + tabs.add(new PathPanel(), "Path Visualizer"); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + } + + public static LocalizerTesterRobot getInstance() { + if (instance == null) { + instance = new LocalizerTesterRobot(); + } + return instance; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index a9376855..d318d517 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -5,6 +5,7 @@ import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; import java.util.ArrayList; import java.util.Timer; @@ -19,17 +20,20 @@ public class LocalizerTester extends BuggyDecoratorNode { private static final int ODOM_UPDATE_PERIOD = 50; private static final double POSITION_UPDATE_M = 0.5; // move this many meters every tick - private GpsMeasurement targetWaypoint; + private int targetWaypointIndex = 0; + private ArrayList waypoints; private Timer gpsTimer; private Timer odomTimer; - private LocTuple currentPosition; - private double heading; - private double noise; + private LocTuple currentPosition = new LocTuple(40.441670, -79.9416362); + private double heading = Math.toRadians(90); + private double noise; // todo insert noise + + private Publisher gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); + private Publisher odomPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); /** * Creates a new decorator for the given {@link Node} * - * @param node {@link Node} to decorate * @param name the name we want for this node to store so that it can be referenced later */ public LocalizerTester(String name, ArrayList waypoints, double noise) { @@ -38,6 +42,16 @@ public LocalizerTester(String name, ArrayList waypoints, double gpsTimer = new Timer("GPS"); odomTimer = new Timer("odom"); this.noise = noise; + this.waypoints = waypoints; + } + + public GpsMeasurement getTargetWaypoint() { + GpsMeasurement currentTarget = waypoints.get(targetWaypointIndex); + double distInM = GpsMeasurement.getDistance(currentTarget, new GpsMeasurement(currentPosition.getLatitude(), currentPosition.getLongitude())); + if (distInM < 10) { + targetWaypointIndex++; + } + return waypoints.get(targetWaypointIndex); } @Override @@ -45,7 +59,9 @@ protected boolean startDecoratorNode() { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - //todo update sim gps + double newLat = currentPosition.getLatitude() + POSITION_UPDATE_M * Math.cos(heading); + double newLon = currentPosition.getLongitude() + POSITION_UPDATE_M * Math.sin(heading); + gpsPub.publish(new GpsMeasurement(newLat, newLon)); } }, 0, GPS_UPDATE_PERIOD); From adbbcafd4a0228be84c4f668991e107cd874732a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 12:14:48 -0500 Subject: [PATCH 045/149] localizer sim finished for just GPS --- .../localizers/RobobuggyKFLocalizer.java | 1 + .../robots/LocalizerTesterRobot.java | 2 +- .../robobuggy/simulation/LocalizerTester.java | 35 ++++++++++++------- 3 files changed, 25 insertions(+), 13 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 8e1132a7..f1322459 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -120,6 +120,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { setupGPSSubscriber(); setupEncoderSubscriber(); setupWheelSubscriber(); + resume(); } private void setupEncoderSubscriber() { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java index 5967d330..320f540d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java @@ -34,7 +34,7 @@ protected LocalizerTesterRobot() { } nodeList.add(new LocalizerTester("localizer tester", waypoints, 0)); - nodeList.add(new RobobuggyKFLocalizer(10, "localizer", new LocTuple(0, 0))); + nodeList.add(new RobobuggyKFLocalizer(10, "localizer", new LocTuple(40.441670, -79.9416362))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index d318d517..b268bda5 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -4,6 +4,7 @@ import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; @@ -18,7 +19,7 @@ public class LocalizerTester extends BuggyDecoratorNode { private static final int GPS_UPDATE_PERIOD = 500; private static final int ODOM_UPDATE_PERIOD = 50; - private static final double POSITION_UPDATE_M = 0.5; // move this many meters every tick + private static final double POSITION_UPDATE_M = 1; // move this many meters every tick private int targetWaypointIndex = 0; private ArrayList waypoints; @@ -32,7 +33,7 @@ public class LocalizerTester extends BuggyDecoratorNode { private Publisher odomPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); /** - * Creates a new decorator for the given {@link Node} + * Creates a new decorator for the given Node * * @param name the name we want for this node to store so that it can be referenced later */ @@ -47,8 +48,10 @@ public LocalizerTester(String name, ArrayList waypoints, double public GpsMeasurement getTargetWaypoint() { GpsMeasurement currentTarget = waypoints.get(targetWaypointIndex); - double distInM = GpsMeasurement.getDistance(currentTarget, new GpsMeasurement(currentPosition.getLatitude(), currentPosition.getLongitude())); - if (distInM < 10) { + double distInM = (GpsMeasurement.getDistance(currentTarget, new GpsMeasurement(currentPosition.getLatitude(), + currentPosition + .getLongitude()))); + if (distInM < 5) { targetWaypointIndex++; } return waypoints.get(targetWaypointIndex); @@ -59,18 +62,26 @@ protected boolean startDecoratorNode() { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - double newLat = currentPosition.getLatitude() + POSITION_UPDATE_M * Math.cos(heading); - double newLon = currentPosition.getLongitude() + POSITION_UPDATE_M * Math.sin(heading); + GpsMeasurement targetWaypoint = getTargetWaypoint(); + double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); + double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); + heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); + + double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading); + double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading); + double newLat = currentPosition.getLatitude() + updateLat; + double newLon = currentPosition.getLongitude() + updateLon; + currentPosition = new LocTuple(newLat, newLon); gpsPub.publish(new GpsMeasurement(newLat, newLon)); } }, 0, GPS_UPDATE_PERIOD); - odomTimer.scheduleAtFixedRate(new TimerTask() { - @Override - public void run() { - // todo update sim odom - } - }, 0, ODOM_UPDATE_PERIOD); +// odomTimer.scheduleAtFixedRate(new TimerTask() { +// @Override +// public void run() { +// // todo update sim odom +// } +// }, 0, ODOM_UPDATE_PERIOD); return true; } From 2b644b103ecd96707d69e2b9fcbaa8bc1dfdb6e4 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Fri, 27 Jan 2017 16:15:20 -0500 Subject: [PATCH 046/149] added steering angle clamping --- .../src/main/java/com/roboclub/robobuggy/main/Util.java | 4 ++-- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java index d19d301d..ab56f3ec 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java @@ -73,7 +73,7 @@ public static JsonObject readJSONFile(String path) throws UnsupportedEncodingExc /** - * Normalizes an an input angle in degrees to be between -180 and 180 + * Clamp an an input angle in degrees to be between -180 and 180 * * @param degrees input angle in degrees * @return normalized angle in degrees @@ -92,7 +92,7 @@ public static double normalizeAngleDeg(double degrees) { } /** - * Normalizes an an input angle in degrees to be between -pi and pi + * Clamp an an input angle in degrees to be between -pi and pi * * @param radians input angle in radians * @return normalized angle in radians diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index a8e3fccb..ee4830f1 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -89,8 +89,9 @@ public double getCommandedSteeringAngle() { double deltaHeading = desiredHeading - poseHeading; // return Util.normalizeAngleRad(deltaHeading); - // literally magic, dont ask (yet) - return Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); + // Pure Pursuit steering controller + deltaHeading = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); + return Util.normalizeAngleRad(deltaHeading); } @Override From c19df1a2ec6c1ff4701f2e7989bf62f7e496feb7 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 16:53:13 -0500 Subject: [PATCH 047/149] finished version 1 of localizer simulator --- .../robots/LocalizerTesterRobot.java | 2 +- .../robobuggy/simulation/LocalizerTester.java | 26 +++++++++++++------ .../java/com/roboclub/robobuggy/ui/Map.java | 6 +++++ 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java index 320f540d..5b28ea88 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java @@ -33,7 +33,7 @@ protected LocalizerTesterRobot() { e.printStackTrace(); } - nodeList.add(new LocalizerTester("localizer tester", waypoints, 0)); + nodeList.add(new LocalizerTester("localizer tester", waypoints, 1)); nodeList.add(new RobobuggyKFLocalizer(10, "localizer", new LocTuple(40.441670, -79.9416362))); //setup the gui diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index b268bda5..14f20ec1 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -67,8 +67,8 @@ public void run() { double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); - double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading); - double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading); + double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random() * noise); + double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random() * noise); double newLat = currentPosition.getLatitude() + updateLat; double newLon = currentPosition.getLongitude() + updateLon; currentPosition = new LocTuple(newLat, newLon); @@ -76,12 +76,22 @@ public void run() { } }, 0, GPS_UPDATE_PERIOD); -// odomTimer.scheduleAtFixedRate(new TimerTask() { -// @Override -// public void run() { -// // todo update sim odom -// } -// }, 0, ODOM_UPDATE_PERIOD); + odomTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + GpsMeasurement targetWaypoint = getTargetWaypoint(); + double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); + double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); + heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); + + double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random() * noise); + double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random() * noise); + double newLat = currentPosition.getLatitude() + updateLat; + double newLon = currentPosition.getLongitude() + updateLon; + currentPosition = new LocTuple(newLat, newLon); + gpsPub.publish(new GpsMeasurement(newLat, newLon)); + } + }, 0, ODOM_UPDATE_PERIOD); return true; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index df514042..fb9d8866 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -3,6 +3,7 @@ import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.ros.Message; import com.roboclub.robobuggy.ros.MessageListener; @@ -68,6 +69,11 @@ public void actionPerformed(String topicName, Message m) { addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); } }); + + new Subscriber("map", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { + GpsMeasurement gps = ((GpsMeasurement) m); + addPointsToMapTree(Color.BLACK, new LocTuple(gps.getLatitude(), gps.getLongitude())); + })); } From 3ff0acf7111beb3fec17f9df5aeba9ddbe4bad07 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 17:07:55 -0500 Subject: [PATCH 048/149] cleanup --- .../robobuggy/simulation/LocalizerTester.java | 40 ++++++++----------- 1 file changed, 17 insertions(+), 23 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index 14f20ec1..0a7fe58e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -27,7 +27,7 @@ public class LocalizerTester extends BuggyDecoratorNode { private Timer odomTimer; private LocTuple currentPosition = new LocTuple(40.441670, -79.9416362); private double heading = Math.toRadians(90); - private double noise; // todo insert noise + private double noise; private Publisher gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); private Publisher odomPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); @@ -57,39 +57,33 @@ public GpsMeasurement getTargetWaypoint() { return waypoints.get(targetWaypointIndex); } + private void updateSimulatedPosition() { + GpsMeasurement targetWaypoint = getTargetWaypoint(); + double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); + double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); + heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); + + double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random() * noise); + double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random() * noise); + double newLat = currentPosition.getLatitude() + updateLat; + double newLon = currentPosition.getLongitude() + updateLon; + currentPosition = new LocTuple(newLat, newLon); + gpsPub.publish(new GpsMeasurement(newLat, newLon)); + } + @Override protected boolean startDecoratorNode() { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - GpsMeasurement targetWaypoint = getTargetWaypoint(); - double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); - double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); - heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); - - double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random() * noise); - double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random() * noise); - double newLat = currentPosition.getLatitude() + updateLat; - double newLon = currentPosition.getLongitude() + updateLon; - currentPosition = new LocTuple(newLat, newLon); - gpsPub.publish(new GpsMeasurement(newLat, newLon)); + updateSimulatedPosition(); } }, 0, GPS_UPDATE_PERIOD); odomTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - GpsMeasurement targetWaypoint = getTargetWaypoint(); - double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); - double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); - heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); - - double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random() * noise); - double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random() * noise); - double newLat = currentPosition.getLatitude() + updateLat; - double newLon = currentPosition.getLongitude() + updateLon; - currentPosition = new LocTuple(newLat, newLon); - gpsPub.publish(new GpsMeasurement(newLat, newLon)); + updateSimulatedPosition(); } }, 0, ODOM_UPDATE_PERIOD); From 7c627724461581f0daea50c84bf672ce31a2d16b Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 27 Jan 2017 17:25:40 -0500 Subject: [PATCH 049/149] cleanup and fixed odom #blessed --- .../nodes/planners/WayPointFollowerPlanner.java | 1 + .../robobuggy/simulation/LocalizerTester.java | 13 +++++++------ .../main/java/com/roboclub/robobuggy/ui/Map.java | 1 + 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index ee4830f1..c3fec045 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -2,6 +2,7 @@ import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.main.Util; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index 0a7fe58e..b16ae44a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -1,5 +1,6 @@ package com.roboclub.robobuggy.simulation; +import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; @@ -18,7 +19,7 @@ public class LocalizerTester extends BuggyDecoratorNode { private static final int GPS_UPDATE_PERIOD = 500; - private static final int ODOM_UPDATE_PERIOD = 50; + private static final int ODOM_UPDATE_PERIOD = 10; private static final double POSITION_UPDATE_M = 1; // move this many meters every tick private int targetWaypointIndex = 0; @@ -27,7 +28,7 @@ public class LocalizerTester extends BuggyDecoratorNode { private Timer odomTimer; private LocTuple currentPosition = new LocTuple(40.441670, -79.9416362); private double heading = Math.toRadians(90); - private double noise; + private double noise; // todo insert noise private Publisher gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); private Publisher odomPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); @@ -57,7 +58,7 @@ public GpsMeasurement getTargetWaypoint() { return waypoints.get(targetWaypointIndex); } - private void updateSimulatedPosition() { + private GpsMeasurement updateSimulatedPosition() { GpsMeasurement targetWaypoint = getTargetWaypoint(); double dlat = currentPosition.getLatitude() - targetWaypoint.getLatitude(); double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); @@ -68,7 +69,7 @@ private void updateSimulatedPosition() { double newLat = currentPosition.getLatitude() + updateLat; double newLon = currentPosition.getLongitude() + updateLon; currentPosition = new LocTuple(newLat, newLon); - gpsPub.publish(new GpsMeasurement(newLat, newLon)); + return new GpsMeasurement(newLat, newLon); } @Override @@ -76,14 +77,14 @@ protected boolean startDecoratorNode() { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - updateSimulatedPosition(); + gpsPub.publish(updateSimulatedPosition()); } }, 0, GPS_UPDATE_PERIOD); odomTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - updateSimulatedPosition(); + odomPub.publish(new EncoderMeasurement(0.01, 8)); } }, 0, ODOM_UPDATE_PERIOD); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index fb9d8866..b1c8ae5e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -74,6 +74,7 @@ public void actionPerformed(String topicName, Message m) { GpsMeasurement gps = ((GpsMeasurement) m); addPointsToMapTree(Color.BLACK, new LocTuple(gps.getLatitude(), gps.getLongitude())); })); + } From caa2741bba6714603485bc5e94e62d0f3e18d348 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 28 Jan 2017 16:49:55 -0500 Subject: [PATCH 050/149] minor changes, added logging fix --- .../surface_src/java_src/Alice/build.gradle | 3 +- .../robobuggy/main/RobobuggyMainFile.java | 4 +- .../com/roboclub/robobuggy/main/Util.java | 6 +-- .../robobuggy/nodes/planners/SweepNode.java | 1 + .../robobuggy/nodes/sensors/LoggingNode.java | 2 +- .../robobuggy/robots/CommTestRobot.java | 47 +++++++++++++++++++ 6 files changed, 55 insertions(+), 8 deletions(-) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java diff --git a/real_time/surface_src/java_src/Alice/build.gradle b/real_time/surface_src/java_src/Alice/build.gradle index c8c7f15d..2d752f01 100644 --- a/real_time/surface_src/java_src/Alice/build.gradle +++ b/real_time/surface_src/java_src/Alice/build.gradle @@ -41,7 +41,8 @@ dependencies { compile project (':BuggyRos') //for jason object extraction from strings - compile 'com.google.code.gson:gson:2.2.+' +// compile 'com.google.code.gson:gson:2.2.+' + compile group: 'com.google.code.gson', name: 'gson', version: '2.2.+' compile 'jfreechart:jfreechart:1.0.0' compile 'gov.nist.math:jama:1.0.2' compile 'org.eclipse.jetty.aggregate:jetty-all:9.2.7.v20150116' diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 9aa36c1b..097c8459 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,9 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; +import com.roboclub.robobuggy.robots.CommTestRobot; import com.roboclub.robobuggy.robots.LocalizerTesterRobot; +import com.roboclub.robobuggy.robots.TransistorDataCollection; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +32,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = LocalizerTesterRobot.getInstance(); + robot = TransistorDataCollection.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java index ab56f3ec..1f640870 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java @@ -6,11 +6,7 @@ import com.google.gson.JsonObject; import gnu.io.CommPortIdentifier; -import java.io.File; -import java.io.FileInputStream; -import java.io.FileNotFoundException; -import java.io.InputStreamReader; -import java.io.UnsupportedEncodingException; +import java.io.*; import java.util.ArrayList; import java.util.Enumeration; import java.util.List; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java index 84cc21f3..8a5f297a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java @@ -75,6 +75,7 @@ protected void updatePositionEstimate(GPSPoseMessage m) { @Override protected double getCommandedSteeringAngle() { + System.out.println("requested steering angle"); return currentCommandedSteeringAngle; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java index b663afaf..9022f308 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/LoggingNode.java @@ -179,7 +179,7 @@ private static String formatDateIntoFile(Date d) { * @return the status of the operation - true if it succeeded, false if it didn't */ private boolean createNewLogFile() { - if (!outputDirectory.exists() || !outputDirectory.isDirectory()) { + if ((!outputDirectory.exists() && !outputDirectory.mkdirs()) || !outputDirectory.isDirectory()) { new RobobuggyLogicNotification("Output directory path isn't a folder!", RobobuggyMessageLevel.EXCEPTION); return false; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java new file mode 100644 index 00000000..f3b3d9f4 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java @@ -0,0 +1,47 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.nodes.planners.SweepNode; +import com.roboclub.robobuggy.nodes.sensors.RBSMNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ui.*; + +/** + * Created by Robot on 1/24/2017. + */ +public class CommTestRobot extends AbstractRobot { + private static CommTestRobot instance; + + public static CommTestRobot getInstance() { + if (instance == null) { + instance = new CommTestRobot(); + } + return instance; + } + + private CommTestRobot() { + super(); + try { + Thread.sleep(2000); + } catch (InterruptedException e) { + e.printStackTrace(); + } + + nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, + RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); + nodeList.add(new SweepNode(NodeChannel.DRIVE_CTRL)); + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + tabs.addTab(new MainGuiWindow(), "Home"); + // tabs.addTab(new PoseGraphsPanel(),"poses"); + // tabs.addTab(new AutonomousPanel(),"Autonomous"); + tabs.add(new PathPanel(), "Path Visualizer"); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + + } + +} From 66bc66c0062a7cef407254eaca674907ce83aad9 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 28 Jan 2017 22:23:44 -0500 Subject: [PATCH 051/149] fixed a critical bug in the system where the localizer sent the commanded angle in radians, but RBSMNode expected it to be in hundredths of degrees. To resolve, all path planners now are expected to report in radians, and the conversion takes place inside of RBSMNode --- .../roboclub/robobuggy/main/RobobuggyMainFile.java | 6 ++---- .../robobuggy/nodes/planners/PathPlannerNode.java | 2 +- .../nodes/planners/WayPointFollowerPlanner.java | 1 + .../roboclub/robobuggy/nodes/sensors/RBSMNode.java | 12 ++++++------ .../com/roboclub/robobuggy/robots/PlayBackRobot.java | 1 - .../roboclub/robobuggy/simulation/PlayBackUtil.java | 5 ++++- 6 files changed, 14 insertions(+), 13 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 097c8459..005648d0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,9 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.CommTestRobot; -import com.roboclub.robobuggy.robots.LocalizerTesterRobot; -import com.roboclub.robobuggy.robots.TransistorDataCollection; +import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -32,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorDataCollection.getInstance(); + robot = PlayBackRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java index a4adf1e8..58a9b51d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java @@ -78,7 +78,7 @@ protected boolean shutdownDecoratorNode() { * Returns the steering angle to which the {@link PathPlanner} thinks the * buggy's steering should be commanded to follow the desired path. * - * @return desired commanded steering angle + * @return desired commanded steering angle IN RADIANS */ protected abstract double getCommandedSteeringAngle(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index c3fec045..316e4b25 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -92,6 +92,7 @@ public double getCommandedSteeringAngle() { // return Util.normalizeAngleRad(deltaHeading); // Pure Pursuit steering controller deltaHeading = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); + return Util.normalizeAngleRad(deltaHeading); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java index 90fcf6a5..89e69c74 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java @@ -292,14 +292,14 @@ private final class RBSMPeriodicNode extends PeriodicNode { */ @Override protected void update() { - int outputAngle = commandedAngle;//allows for commandedAngle to be read only in this function - if (outputAngle > 1000) { - outputAngle = 1000; - } else if (outputAngle < -1000) { - outputAngle = -1000; + int outputAngleHundredthsDegrees = (int) (Math.toDegrees(commandedAngle) * 100);//allows for commandedAngle to be read only in this function + if (outputAngleHundredthsDegrees > 1000) { + outputAngleHundredthsDegrees = 1000; + } else if (outputAngleHundredthsDegrees < -1000) { + outputAngleHundredthsDegrees = -1000; } - RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngle); + RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngleHundredthsDegrees); send(msgSteer.getMessageBytes()); RBSMBrakeMessage msgBrake = new RBSMBrakeMessage(commandedBrakeEngaged); send(msgBrake.getMessageBytes()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index e25cf39f..7ff2c8cb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -38,7 +38,6 @@ private PlayBackRobot() { new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); - new KfLocalizer(10); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); Gui.getInstance().addWindow(mainWindow); RobobuggyGUITabs tabs = new RobobuggyGUITabs(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 281443ef..11eed6fb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -42,6 +42,7 @@ public final class PlayBackUtil { private Publisher steeringCommandPub; private Publisher loggingButtonPub; private Publisher logicNotificationPub; + private Publisher posePub; /** @@ -69,6 +70,7 @@ private PlayBackUtil() { steeringCommandPub = new Publisher(NodeChannel.STEERING_COMMANDED.getMsgPath()); loggingButtonPub = new Publisher(NodeChannel.GUI_LOGGING_BUTTON.getMsgPath()); logicNotificationPub = new Publisher(NodeChannel.LOGIC_NOTIFICATION.getMsgPath()); + posePub = new Publisher(NodeChannel.POSE.getMsgPath()); } /** @@ -113,7 +115,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, long sensorTime = sensorDataJson.get("timestamp").getAsLong(); long sensorDt = (sensorTime - sensorStartTime); long dt = (long) (sensorDt / playBackSpeed) - playBacktime; - if (dt > 10) { //Milliseconds + if (dt > 10) { // Milliseconds Thread.sleep(dt); } @@ -156,6 +158,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case GPSPoseMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, GPSPoseMessage.class); + getPrivateInstance().posePub.publish(transmitMessage); break; case RemoteWheelAngleRequest.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, RemoteWheelAngleRequest.class); From ed661a1438c37ca76c3179883c1da23fbe165d02 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 28 Jan 2017 22:23:44 -0500 Subject: [PATCH 052/149] fixed a critical bug in the system where the localizer sent the commanded angle in radians, but RBSMNode expected it to be in hundredths of degrees. To resolve, all path planners now are expected to report in radians, and the conversion takes place inside of RBSMNode --- .../roboclub/robobuggy/main/RobobuggyMainFile.java | 6 ++---- .../robobuggy/messages/DriveControlMessage.java | 3 +++ .../robobuggy/nodes/planners/PathPlannerNode.java | 2 +- .../nodes/planners/WayPointFollowerPlanner.java | 1 + .../roboclub/robobuggy/nodes/sensors/RBSMNode.java | 12 ++++++------ .../com/roboclub/robobuggy/robots/PlayBackRobot.java | 1 - .../roboclub/robobuggy/simulation/PlayBackUtil.java | 5 ++++- 7 files changed, 17 insertions(+), 13 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 097c8459..005648d0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,9 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.CommTestRobot; -import com.roboclub.robobuggy.robots.LocalizerTesterRobot; -import com.roboclub.robobuggy.robots.TransistorDataCollection; +import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -32,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorDataCollection.getInstance(); + robot = PlayBackRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java index 0857f10f..a3de9c50 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java @@ -9,6 +9,9 @@ public class DriveControlMessage extends BaseMessage { public static final String VERSION_ID = "drive_control_message"; + /** + * Angle is supposed to be in radians + */ private final double angle; /** diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java index a4adf1e8..58a9b51d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java @@ -78,7 +78,7 @@ protected boolean shutdownDecoratorNode() { * Returns the steering angle to which the {@link PathPlanner} thinks the * buggy's steering should be commanded to follow the desired path. * - * @return desired commanded steering angle + * @return desired commanded steering angle IN RADIANS */ protected abstract double getCommandedSteeringAngle(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index c3fec045..316e4b25 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -92,6 +92,7 @@ public double getCommandedSteeringAngle() { // return Util.normalizeAngleRad(deltaHeading); // Pure Pursuit steering controller deltaHeading = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); + return Util.normalizeAngleRad(deltaHeading); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java index 90fcf6a5..89e69c74 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java @@ -292,14 +292,14 @@ private final class RBSMPeriodicNode extends PeriodicNode { */ @Override protected void update() { - int outputAngle = commandedAngle;//allows for commandedAngle to be read only in this function - if (outputAngle > 1000) { - outputAngle = 1000; - } else if (outputAngle < -1000) { - outputAngle = -1000; + int outputAngleHundredthsDegrees = (int) (Math.toDegrees(commandedAngle) * 100);//allows for commandedAngle to be read only in this function + if (outputAngleHundredthsDegrees > 1000) { + outputAngleHundredthsDegrees = 1000; + } else if (outputAngleHundredthsDegrees < -1000) { + outputAngleHundredthsDegrees = -1000; } - RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngle); + RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngleHundredthsDegrees); send(msgSteer.getMessageBytes()); RBSMBrakeMessage msgBrake = new RBSMBrakeMessage(commandedBrakeEngaged); send(msgBrake.getMessageBytes()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index e25cf39f..7ff2c8cb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -38,7 +38,6 @@ private PlayBackRobot() { new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); - new KfLocalizer(10); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); Gui.getInstance().addWindow(mainWindow); RobobuggyGUITabs tabs = new RobobuggyGUITabs(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 281443ef..11eed6fb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -42,6 +42,7 @@ public final class PlayBackUtil { private Publisher steeringCommandPub; private Publisher loggingButtonPub; private Publisher logicNotificationPub; + private Publisher posePub; /** @@ -69,6 +70,7 @@ private PlayBackUtil() { steeringCommandPub = new Publisher(NodeChannel.STEERING_COMMANDED.getMsgPath()); loggingButtonPub = new Publisher(NodeChannel.GUI_LOGGING_BUTTON.getMsgPath()); logicNotificationPub = new Publisher(NodeChannel.LOGIC_NOTIFICATION.getMsgPath()); + posePub = new Publisher(NodeChannel.POSE.getMsgPath()); } /** @@ -113,7 +115,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, long sensorTime = sensorDataJson.get("timestamp").getAsLong(); long sensorDt = (sensorTime - sensorStartTime); long dt = (long) (sensorDt / playBackSpeed) - playBacktime; - if (dt > 10) { //Milliseconds + if (dt > 10) { // Milliseconds Thread.sleep(dt); } @@ -156,6 +158,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case GPSPoseMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, GPSPoseMessage.class); + getPrivateInstance().posePub.publish(transmitMessage); break; case RemoteWheelAngleRequest.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, RemoteWheelAngleRequest.class); From 5a05eca84a7f84ef2e802f25340b01b25d786e37 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Fri, 3 Feb 2017 19:08:28 -0500 Subject: [PATCH 053/149] edits to WayPointFollowerPlanner --- .../planners/WayPointFollowerPlanner.java | 27 ++++++++----------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 316e4b25..bf83fb79 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -52,6 +52,7 @@ private static int getClosestIndex(ArrayList wayPoints, GPSPoseM @Override public double getCommandedSteeringAngle() { + // determines the angle at which to move every 50 milliseconds int closestIndex = getClosestIndex(wayPoints, pose); if (closestIndex == -1) { new RobobuggyLogicNotification("HELP no closest index", RobobuggyMessageLevel.EXCEPTION); @@ -77,9 +78,11 @@ public double getCommandedSteeringAngle() { GpsMeasurement targetPoint = wayPoints.get(targetIndex); //find a path from our current location to that point - double dLon = targetPoint.getLongitude() - pose.getLongitude(); - double dLat = targetPoint.getLatitude() - pose.getLatitude(); - double desiredHeading = Math.atan2(LocalizerUtil.convertLatToMeters(dLat), LocalizerUtil.convertLonToMeters(dLon)); + double deltaLong = targetPoint.getLongitude() - pose.getLongitude(); + double deltaLat = targetPoint.getLatitude() - pose.getLatitude(); + double deltaLatMeters = LocalizerUtil.convertLatToMeters(deltaLat); + double deltaLongMeters = LocalizerUtil.convertLonToMeters(deltaLong); + double desiredHeading = Math.atan2(deltaLatMeters, deltaLongMeters); // basically we want all of our angles to be in the same range, so that we don't // have weird wraparound @@ -91,26 +94,18 @@ public double getCommandedSteeringAngle() { // return Util.normalizeAngleRad(deltaHeading); // Pure Pursuit steering controller - deltaHeading = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), 0.8 * pose.getCurrentState().get(2, 0)); + + double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); + double param2 = 0.8 * pose.getCurrentState().get(2, 0); + deltaHeading = Math.atan2(param1, param2); return Util.normalizeAngleRad(deltaHeading); } @Override protected boolean getDeployBrakeValue() { + // need to brake when 15 m off course return false; - /* - int closestIndex = getClosestIndex(wayPoints,pose); - - if(closestIndex == -1){ - return true; - } - - // if closest point is too far away throw breaks - - boolean shouldBrake = GPSPoseMessage.getDistance(pose, wayPoints.get(closestIndex).toGpsPoseMessage(0)) >= 5.0; - return shouldBrake; - */ } From 9c3947566cd68dc11d4e978fee8420a3dc4cf354 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Fri, 3 Feb 2017 19:16:39 -0500 Subject: [PATCH 054/149] edits to WayPointFollowerPlanner --- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index bf83fb79..c9c5e5ad 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -41,7 +41,8 @@ private static int getClosestIndex(ArrayList wayPoints, GPSPoseM int closestIndex = -1; for (int i = 0; i < wayPoints.size(); i++) { - double d = GPSPoseMessage.getDistance(currentLocation, wayPoints.get(i).toGpsPoseMessage(0)); + GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); + double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { min = d; closestIndex = i; @@ -64,7 +65,8 @@ public double getCommandedSteeringAngle() { //pick the first point that is at least delta away //pick the point to follow int targetIndex = closestIndex; - while (targetIndex < wayPoints.size() && GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)) < delta) { + double distanceFromMessage = GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)); + while (targetIndex < wayPoints.size() && distanceFromMessage < delta) { targetIndex = targetIndex + 1; } From 9fcdec0d120d4b940439928593fc007e13cca105 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 4 Feb 2017 17:22:24 -0500 Subject: [PATCH 055/149] removed the stupid angle int conversion from the messages --- .../roboclub/robobuggy/messages/DriveControlMessage.java | 9 --------- .../com/roboclub/robobuggy/nodes/sensors/RBSMNode.java | 4 ++-- .../roboclub/robobuggy/simulation/SimulatedRBSMNode.java | 6 +++--- .../main/java/com/roboclub/robobuggy/ui/PoseViewer.java | 2 +- 4 files changed, 6 insertions(+), 15 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java index a3de9c50..4b1feecd 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java @@ -34,13 +34,4 @@ public double getAngleDouble() { return angle; } - /** - * Returns the commanded angle of the steering as an int (in hundredths of degrees) - * - * @return the commanded angle of the steering as an int (in hundredths of degrees) - */ - public int getAngleInt() { - return (int) (angle * 100.0); - } - } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java index 89e69c74..8f56fba6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java @@ -272,7 +272,7 @@ public int peel(byte[] buffer, int start, int bytesAvailable) { private final class RBSMPeriodicNode extends PeriodicNode { //Stored commanded values - private int commandedAngle = 0; + private double commandedAngle = 0; private boolean commandedBrakeEngaged = true; /** @@ -315,7 +315,7 @@ protected boolean startDecoratorNode() { new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commandedAngle = -((DriveControlMessage) m).getAngleInt(); + commandedAngle = -((DriveControlMessage) m).getAngleDouble(); } }); new Subscriber("rbsm", NodeChannel.BRAKE_CTRL.getMsgPath(), diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java index f87e2752..5b0bb936 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/SimulatedRBSMNode.java @@ -29,7 +29,7 @@ public class SimulatedRBSMNode extends PeriodicNode { private Publisher messagePubControllerSteering; private Publisher messagePubBrakeState; private Publisher messagePubSteering; - private int commandedAngle = 0; + private double commandedAngle = 0; private boolean commandedBrakeEngaged = true; @@ -53,7 +53,7 @@ public SimulatedRBSMNode() { new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commandedAngle = ((DriveControlMessage) m).getAngleInt(); + commandedAngle = ((DriveControlMessage) m).getAngleDouble(); } }); @@ -96,7 +96,7 @@ protected void update() { messagePubControllerSteering.publish(new SteeringMeasurement(potAngle)); messagePubSteering.publish(new SteeringMeasurement(potAngle)); - int outputAngle = commandedAngle; + int outputAngle = (int) commandedAngle; //update the commanded angle if (outputAngle > 1000) { outputAngle = 1000; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java index 2c14ec18..4d5edc6d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/PoseViewer.java @@ -99,7 +99,7 @@ public void actionPerformed(String topicName, Message m) { new Subscriber("uiDriveAngle", NodeChannel.DRIVE_CTRL.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commtheta = ((DriveControlMessage) m).getAngleInt(); + commtheta = (int) Math.toDegrees(((DriveControlMessage) m).getAngleDouble()); } }); From 12e9f42e5425f539d7e6c306aca4822b8f888f85 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 4 Feb 2017 17:23:38 -0500 Subject: [PATCH 056/149] generified the software clamping --- .../com/roboclub/robobuggy/nodes/sensors/RBSMNode.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java index 8f56fba6..cd100437 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/RBSMNode.java @@ -275,6 +275,8 @@ private final class RBSMPeriodicNode extends PeriodicNode { private double commandedAngle = 0; private boolean commandedBrakeEngaged = true; + private static final int MAX_STEERING_ANGLE_HUNDREDTHS_DEG = 1500; + /** * Create a new {@link RBSMPeriodicNode} object * @@ -293,10 +295,10 @@ private final class RBSMPeriodicNode extends PeriodicNode { @Override protected void update() { int outputAngleHundredthsDegrees = (int) (Math.toDegrees(commandedAngle) * 100);//allows for commandedAngle to be read only in this function - if (outputAngleHundredthsDegrees > 1000) { - outputAngleHundredthsDegrees = 1000; - } else if (outputAngleHundredthsDegrees < -1000) { - outputAngleHundredthsDegrees = -1000; + if (outputAngleHundredthsDegrees > MAX_STEERING_ANGLE_HUNDREDTHS_DEG) { + outputAngleHundredthsDegrees = MAX_STEERING_ANGLE_HUNDREDTHS_DEG; + } else if (outputAngleHundredthsDegrees < -MAX_STEERING_ANGLE_HUNDREDTHS_DEG) { + outputAngleHundredthsDegrees = -MAX_STEERING_ANGLE_HUNDREDTHS_DEG; } RBSMSteeringMessage msgSteer = new RBSMSteeringMessage(outputAngleHundredthsDegrees); From a30da604f5c20003e6fa04d028f12357a12e4e72 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 4 Feb 2017 17:34:53 -0500 Subject: [PATCH 057/149] added v1 of pid stabilization the constants haven't been tuned/tested --- .../planners/WayPointFollowerPlanner.java | 40 ++++++++++++++++--- 1 file changed, 34 insertions(+), 6 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index c9c5e5ad..e478e9e2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -19,6 +19,21 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type + + // PID controller values/variables + + private double Kp = 0.4; + private double Ki = 0.1; + private double Kd = 0.05; + + private double previousHeadingError; + private double totalHeadingError; + + private long previousTimeMillis; + + // end PID controller values/variables + + /** * @param wayPoints the list of waypoints to follow */ @@ -86,22 +101,35 @@ public double getCommandedSteeringAngle() { double deltaLongMeters = LocalizerUtil.convertLonToMeters(deltaLong); double desiredHeading = Math.atan2(deltaLatMeters, deltaLongMeters); - // basically we want all of our angles to be in the same range, so that we don't - // have weird wraparound -// desiredHeading = Util.normalizeAngleDeg(desiredHeading); double poseHeading = pose.getHeading(); //find the angle we need to reach that point double deltaHeading = desiredHeading - poseHeading; -// return Util.normalizeAngleRad(deltaHeading); // Pure Pursuit steering controller - double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); double param2 = 0.8 * pose.getCurrentState().get(2, 0); deltaHeading = Math.atan2(param1, param2); - return Util.normalizeAngleRad(deltaHeading); + + // begin PID control + // calculate P + double currentHeadingError = deltaHeading; + + // calculate I + totalHeadingError += deltaHeading; + + // calculate D + long currentTimeMillis = System.currentTimeMillis(); + long dt = currentTimeMillis - previousTimeMillis; + double deltaError = (1.0 * (currentHeadingError - previousHeadingError)) / dt; + previousHeadingError = currentHeadingError; + previousTimeMillis = currentTimeMillis; + + // put it all together + double pidError = Kp * currentHeadingError + Ki * totalHeadingError + Kd * deltaError; + + return Util.normalizeAngleRad(pidError); } @Override From aaa5f2fe6292a8fd52275171e4ed2805e3d79cf5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 10 Feb 2017 16:29:11 -0500 Subject: [PATCH 058/149] toolchain fixes --- .../robobuggy/main/RobobuggyMainFile.java | 4 +- .../robobuggy/nodes/planners/SweepNode.java | 6 +- .../planners/WayPointFollowerPlanner.java | 12 +- .../robobuggy/robots/TransistorAuton.java | 7 +- .../robots/TransistorWaypointCollection.java | 124 ++++++++++++++++++ .../simulation/ControllerTester.java | 2 +- 6 files changed, 139 insertions(+), 16 deletions(-) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 005648d0..b2bade70 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.PlayBackRobot; +import com.roboclub.robobuggy.robots.TransistorDataCollection; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = PlayBackRobot.getInstance(); + robot = TransistorDataCollection.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java index 8a5f297a..14c85e2b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java @@ -14,9 +14,9 @@ public class SweepNode extends PathPlannerNode { private double currentCommandedSteeringAngle = 0; - private static final double STEERING_ANGLE_LOWER_BOUND = -20; - private static final double STEERING_ANGLE_UPPER_BOUND = 20; - private static final double STEERING_ANGLE_INCREMENT = 1; + private static final double STEERING_ANGLE_LOWER_BOUND = Math.toRadians(-20); + private static final double STEERING_ANGLE_UPPER_BOUND = Math.toRadians(20); + private static final double STEERING_ANGLE_INCREMENT = Math.toRadians(1); private Publisher steeringPublisher; private Publisher brakePublisher; private Thread t1; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index e478e9e2..78248007 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -23,7 +23,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { // PID controller values/variables private double Kp = 0.4; - private double Ki = 0.1; + private double Ki = 0.3; private double Kd = 0.05; private double previousHeadingError; @@ -79,11 +79,11 @@ public double getCommandedSteeringAngle() { double delta = 10; //meters //pick the first point that is at least delta away //pick the point to follow - int targetIndex = closestIndex; + int targetIndex = closestIndex + 1; double distanceFromMessage = GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)); - while (targetIndex < wayPoints.size() && distanceFromMessage < delta) { - targetIndex = targetIndex + 1; - } +// while (targetIndex < wayPoints.size() && distanceFromMessage < delta) { +// targetIndex = targetIndex + 1; +// } //if we are out of points then just go straight if (targetIndex >= wayPoints.size()) { @@ -121,7 +121,7 @@ public double getCommandedSteeringAngle() { // calculate D long currentTimeMillis = System.currentTimeMillis(); - long dt = currentTimeMillis - previousTimeMillis; + double dt = currentTimeMillis - previousTimeMillis; double deltaError = (1.0 * (currentHeadingError - previousHeadingError)) / dt; previousHeadingError = currentHeadingError; previousTimeMillis = currentTimeMillis; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 8a85bf50..8200e238 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -4,13 +4,11 @@ import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -69,10 +67,11 @@ private TransistorAuton() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); // nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); +// nodeList.add(new HillCrestImuNode()); try { - ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); +// ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + ArrayList wayPoints = WayPointUtil.createWaypointsFromOdomLocalizerLog(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java new file mode 100644 index 00000000..fe575b0c --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java @@ -0,0 +1,124 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.main.RobobuggyLogicNotification; +import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.sensors.CameraNode; +import com.roboclub.robobuggy.nodes.sensors.GpsNode; +import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.LoggingNode; +import com.roboclub.robobuggy.nodes.sensors.RBSMNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +import java.awt.event.WindowEvent; +import java.awt.event.WindowListener; + +/** + * A robot class for doing data collection only with the live robot, will not attempt to autonomously drive + * + * @author Trevor Decker + */ +public final class TransistorWaypointCollection extends AbstractRobot { + private static TransistorWaypointCollection instance; + private static final int ARDUINO_BOOTLOADER_TIMEOUT = 2000; + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ + public static AbstractRobot getInstance() { + if (instance == null) { + instance = new TransistorWaypointCollection(); + } + return instance; + } + + private TransistorWaypointCollection() { + super(); + try { + Thread.sleep(ARDUINO_BOOTLOADER_TIMEOUT); + } catch (InterruptedException e) { + new RobobuggyLogicNotification("Couldn't wait for bootloader, shutting down", RobobuggyMessageLevel.EXCEPTION); + shutDown(); + } + new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); + // Initialize Nodes + nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); + nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, + NodeChannel.getLoggingChannels())); + nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), + RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); + nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); + nodeList.add(new HillCrestImuNode()); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(0, 0))); + + //setup the gui + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + + mainWindow.addWindowListener(new WindowListener() { + + @Override + public void windowOpened(WindowEvent e) { + // TODO Auto-generated method stub + + } + + @Override + public void windowIconified(WindowEvent e) { + // TODO Auto-generated method stub + + } + + @Override + public void windowDeiconified(WindowEvent e) { + // TODO Auto-generated method stub + + } + + @Override + public void windowDeactivated(WindowEvent e) { + // TODO Auto-generated method stub + + } + + @Override + public void windowClosing(WindowEvent e) { + // TODO Auto-generated method stub + TransistorWaypointCollection.this.shutDown(); + } + + @Override + public void windowClosed(WindowEvent e) { + // TODO Auto-generated method stub + + } + + @Override + public void windowActivated(WindowEvent e) { + // TODO Auto-generated method stub + + } + }); + + tabs.addTab(new MainGuiWindow(), "Home"); +// tabs.addTab(new ImuVisualWindow(), "IMU"); +// tabs.addTab(new PoseGraphsPanel(),"poses"); +// tabs.addTab(new ImuPanel(),"IMU"); +// tabs.addTab(new PathPanel(),"Path Panel"); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + + } +} + diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index 8ff88100..4ea3cacb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -29,7 +29,7 @@ public class ControllerTester extends PeriodicNode { // wheelbase in meters private static final double WHEELBASE = 1.13; // assume a velocity of 8 m/s - private static final double VELOCITY = 2; + private static final double VELOCITY = 1; // assume we start from the base of the track private static final LocTuple INITIAL_POSITION_LL = new LocTuple(40.441670, -79.9416362); // assume we are facing up hill 1 From 14544002e2cc0afd535b225827e3e2af78f395f7 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sat, 11 Feb 2017 01:57:11 -0500 Subject: [PATCH 059/149] changed offline analysis to handle triangle test route --- offline/controller/controller_analysis.m | 4 +- offline/controller/controller_pure.m | 22 +- offline/controller/get_logs.py | 4 +- offline/controller/waypoints_tri.txt | 372 +++++++++++++++++++++++ offline/localizer/README.md | 18 ++ offline/localizer/get_logs.py | 3 +- 6 files changed, 409 insertions(+), 14 deletions(-) create mode 100644 offline/controller/waypoints_tri.txt create mode 100644 offline/localizer/README.md diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m index 2904e1c9..230a34e7 100644 --- a/offline/controller/controller_analysis.m +++ b/offline/controller/controller_analysis.m @@ -6,12 +6,12 @@ addpath('../localizer/latlonutm/Codes/matlab'); addpath('../localizer/altmany-export_fig'); -file = 'controller_v2.mat'; +file = 'controller_tri_v1.mat'; load(file, 'trajectory'); save_plot = false; show_maps = false; -load('./waypoints.mat'); +load('./waypoints_tri.mat'); [x, y, zone] = ll2utm(logs); desired = [x y]; diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m index ca510163..36616dde 100644 --- a/offline/controller/controller_pure.m +++ b/offline/controller/controller_pure.m @@ -12,10 +12,10 @@ wheel_base = 1.13; utm_zone = 17; first_heading = deg2rad(250); - lat_long = [40.441670, -79.9416362]; + lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 8; % m/s, 17.9mph, forward velocity + velocity = 4; % 8; % m/s, 17.9mph, forward velocity steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds % full range in 0.5s @@ -27,10 +27,10 @@ first_heading; % heading, rad, world frame 0]; % d_heading, rad/s - load('./waypoints.mat'); + load('./waypoints_tri.mat'); desired_traj = processWaypoints(logs); % time = linspace(0, 240, size(trajectory,2)); - time = 0:dt:240; + time = 0:dt:60; % 240; u = 0; % commanded steering angle steering = u; % steering angle trajectory = [X; lat_long(1); lat_long(2); steering]; @@ -54,7 +54,7 @@ end if save_data - save('controller_v2.mat', 'trajectory'); + save('controller_tri_v1.mat', 'trajectory'); end end @@ -113,9 +113,15 @@ global wheel_base pos = X(1:2)'; - b = repmat(pos, size(desired_traj, 1), 1); - delta = 15*15; - possible = find(sum((desired_traj-b).^2, 2) < delta); + pos_b = repmat(pos, size(desired_traj, 1), 1); + delta = 5; + cutoff = 100; + delta = delta * delta; + + distances = sum((desired_traj - pos_b).^2, 2); + [~, closest_idx] = min(distances); + last_idx = min([length(distances), closest_idx + cutoff]); + possible = find(distances(1:last_idx) < delta); if isempty(possible) u = 0; else diff --git a/offline/controller/get_logs.py b/offline/controller/get_logs.py index 5ab1c3df..321ef013 100644 --- a/offline/controller/get_logs.py +++ b/offline/controller/get_logs.py @@ -6,7 +6,7 @@ combined = True d = None -file = 'waypoints_course.txt' +file = 'waypoints_tri.txt' logs = [] with open(file) as json_data: for line in json_data: @@ -19,4 +19,4 @@ logs = np.array(logs) print(logs.shape) vars_map = {'logs': logs} -scipy.io.savemat('./waypoints.mat', mdict=vars_map) +scipy.io.savemat('./waypoints_tri.mat', mdict=vars_map) diff --git a/offline/controller/waypoints_tri.txt b/offline/controller/waypoints_tri.txt new file mode 100644 index 00000000..764280d0 --- /dev/null +++ b/offline/controller/waypoints_tri.txt @@ -0,0 +1,372 @@ +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286683333333,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022628} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286683333333,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022632} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022632} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022632} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4428675,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867666666665,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867666666665,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286783333333,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442868,"north":true,"longitude":-79.94273933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442868,"north":true,"longitude":-79.942739,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286833333334,"north":true,"longitude":-79.94273866666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.9427385,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286833333334,"north":true,"longitude":-79.94273833333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286833333334,"north":true,"longitude":-79.9427385,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.9427385,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.94273883333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.942739,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442868,"north":true,"longitude":-79.942739,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286783333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867666666665,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286683333333,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286533333333,"north":true,"longitude":-79.94274033333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442863,"north":true,"longitude":-79.94274166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442860333333336,"north":true,"longitude":-79.9427435,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442856666666664,"north":true,"longitude":-79.9427455,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44285266666667,"north":true,"longitude":-79.94274783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442848,"north":true,"longitude":-79.9427505,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44284283333333,"north":true,"longitude":-79.9427535,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44283733333333,"north":true,"longitude":-79.94275683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44283166666666,"north":true,"longitude":-79.94276066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44282616666667,"north":true,"longitude":-79.94276483333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4428205,"north":true,"longitude":-79.94276883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442815,"north":true,"longitude":-79.942773,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44280933333334,"north":true,"longitude":-79.942777,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44280366666667,"north":true,"longitude":-79.942781,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442797666666664,"north":true,"longitude":-79.94278533333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442791666666665,"north":true,"longitude":-79.94278966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427855,"north":true,"longitude":-79.942794,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44277316666667,"north":true,"longitude":-79.9428025,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442767,"north":true,"longitude":-79.94280666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442761,"north":true,"longitude":-79.9428105,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442755,"north":true,"longitude":-79.9428145,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427485,"north":true,"longitude":-79.94281866666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442742,"north":true,"longitude":-79.94282283333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442735666666664,"north":true,"longitude":-79.94282683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442729166666666,"north":true,"longitude":-79.94283083333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44272266666667,"north":true,"longitude":-79.942835,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442715666666665,"north":true,"longitude":-79.94283916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442709,"north":true,"longitude":-79.94284333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44270216666666,"north":true,"longitude":-79.94284716666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426955,"north":true,"longitude":-79.94285116666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44268866666667,"north":true,"longitude":-79.94285516666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44268183333333,"north":true,"longitude":-79.942859,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442675,"north":true,"longitude":-79.94286333333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442668,"north":true,"longitude":-79.94286733333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442660833333335,"north":true,"longitude":-79.94287066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44265383333333,"north":true,"longitude":-79.94287383333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426465,"north":true,"longitude":-79.94287683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44263933333333,"north":true,"longitude":-79.94287966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442632,"north":true,"longitude":-79.94288266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022642} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442625,"north":true,"longitude":-79.94288533333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022642} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261783333334,"north":true,"longitude":-79.942888,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022642} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261066666667,"north":true,"longitude":-79.94289066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44260333333333,"north":true,"longitude":-79.94289316666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44259616666667,"north":true,"longitude":-79.9428955,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258883333333,"north":true,"longitude":-79.94289783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442581833333335,"north":true,"longitude":-79.94290016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425745,"north":true,"longitude":-79.942902,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425675,"north":true,"longitude":-79.942904,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44256016666667,"north":true,"longitude":-79.94290583333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44255283333333,"north":true,"longitude":-79.9429075,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44254566666667,"north":true,"longitude":-79.942909,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44253833333333,"north":true,"longitude":-79.94291066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442531,"north":true,"longitude":-79.94291183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442524,"north":true,"longitude":-79.94291316666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442517,"north":true,"longitude":-79.94291416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425095,"north":true,"longitude":-79.94291516666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44250233333333,"north":true,"longitude":-79.9429165,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44249533333333,"north":true,"longitude":-79.94291766666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44248833333333,"north":true,"longitude":-79.942919,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424815,"north":true,"longitude":-79.94292033333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424745,"north":true,"longitude":-79.94292166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44246733333333,"north":true,"longitude":-79.94292266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44246016666666,"north":true,"longitude":-79.94292383333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442452833333334,"north":true,"longitude":-79.942925,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44244533333333,"north":true,"longitude":-79.94292583333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44243783333334,"north":true,"longitude":-79.94292683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44243016666667,"north":true,"longitude":-79.9429275,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44242233333333,"north":true,"longitude":-79.94292783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442415,"north":true,"longitude":-79.94292866666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44240683333334,"north":true,"longitude":-79.9429285,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44239983333333,"north":true,"longitude":-79.94292883333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423925,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44238516666667,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423785,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44236383333333,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44235716666667,"north":true,"longitude":-79.94292966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44235066666667,"north":true,"longitude":-79.94292966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442344166666665,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423375,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423305,"north":true,"longitude":-79.94292916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44232366666667,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44231666666667,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423095,"north":true,"longitude":-79.94292916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422945,"north":true,"longitude":-79.94293033333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442287666666665,"north":true,"longitude":-79.94293133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44228066666667,"north":true,"longitude":-79.94293266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442274,"north":true,"longitude":-79.94293416666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442267333333334,"north":true,"longitude":-79.94293616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442260833333336,"north":true,"longitude":-79.94293833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44225433333333,"north":true,"longitude":-79.942941,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224783333333,"north":true,"longitude":-79.94294333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224116666667,"north":true,"longitude":-79.94294566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442234,"north":true,"longitude":-79.942948,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442227,"north":true,"longitude":-79.94295016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442220666666664,"north":true,"longitude":-79.9429525,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44221433333333,"north":true,"longitude":-79.94295466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422085,"north":true,"longitude":-79.9429575,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422025,"north":true,"longitude":-79.94296066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442196,"north":true,"longitude":-79.94296416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218983333333,"north":true,"longitude":-79.9429685,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421835,"north":true,"longitude":-79.942973,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421775,"north":true,"longitude":-79.94297833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217233333333,"north":true,"longitude":-79.9429845,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442168333333335,"north":true,"longitude":-79.94299083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216466666666,"north":true,"longitude":-79.94299733333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216133333333,"north":true,"longitude":-79.94300416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442158166666665,"north":true,"longitude":-79.943012,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215533333333,"north":true,"longitude":-79.94302083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442153,"north":true,"longitude":-79.94302966666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442151333333335,"north":true,"longitude":-79.943039,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442150166666664,"north":true,"longitude":-79.94304816666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421485,"north":true,"longitude":-79.94305716666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214733333333,"north":true,"longitude":-79.94306616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214616666667,"north":true,"longitude":-79.94307566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214566666667,"north":true,"longitude":-79.943085,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214633333333,"north":true,"longitude":-79.94310033333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442147166666665,"north":true,"longitude":-79.94310633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214816666667,"north":true,"longitude":-79.94311216666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421495,"north":true,"longitude":-79.94311933333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215116666667,"north":true,"longitude":-79.9431275,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215283333333,"north":true,"longitude":-79.94313633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442154333333335,"north":true,"longitude":-79.9431455,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442156,"north":true,"longitude":-79.943155,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215766666667,"north":true,"longitude":-79.9431645,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421595,"north":true,"longitude":-79.94317433333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216133333333,"north":true,"longitude":-79.943184,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421635,"north":true,"longitude":-79.94319366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442165333333335,"north":true,"longitude":-79.94320283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216683333333,"north":true,"longitude":-79.94321183333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216866666667,"north":true,"longitude":-79.94322116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217066666667,"north":true,"longitude":-79.94323066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217233333333,"north":true,"longitude":-79.94323983333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421745,"north":true,"longitude":-79.94324933333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442176833333335,"north":true,"longitude":-79.94325883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217866666666,"north":true,"longitude":-79.943269,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421805,"north":true,"longitude":-79.9432785,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218216666667,"north":true,"longitude":-79.94328783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218383333333,"north":true,"longitude":-79.94329683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218583333333,"north":true,"longitude":-79.94330616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421875,"north":true,"longitude":-79.9433155,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421895,"north":true,"longitude":-79.943325,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442191666666666,"north":true,"longitude":-79.94333466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421935,"north":true,"longitude":-79.943344,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421955,"north":true,"longitude":-79.94335383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44219733333333,"north":true,"longitude":-79.94336366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442199333333335,"north":true,"longitude":-79.94337366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220166666667,"north":true,"longitude":-79.94338333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442203666666664,"north":true,"longitude":-79.94339316666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220583333333,"north":true,"longitude":-79.94340233333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220766666667,"north":true,"longitude":-79.94341183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220966666666,"north":true,"longitude":-79.94342133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44221183333333,"north":true,"longitude":-79.94343066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442214,"north":true,"longitude":-79.94344016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442216,"north":true,"longitude":-79.9434495,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442218,"north":true,"longitude":-79.94345933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44222,"north":true,"longitude":-79.94346883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442221833333335,"north":true,"longitude":-79.94347866666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44222383333334,"north":true,"longitude":-79.94348816666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442225666666666,"north":true,"longitude":-79.94349733333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442227833333334,"north":true,"longitude":-79.94350683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223,"north":true,"longitude":-79.94351583333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223216666666,"north":true,"longitude":-79.94352533333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442234166666665,"north":true,"longitude":-79.94353416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223616666667,"north":true,"longitude":-79.943543,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223816666667,"north":true,"longitude":-79.94355183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224033333333,"north":true,"longitude":-79.94356083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224233333333,"north":true,"longitude":-79.94356983333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224416666667,"north":true,"longitude":-79.94357866666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224583333333,"north":true,"longitude":-79.94358783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422475,"north":true,"longitude":-79.94359666666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442249333333336,"north":true,"longitude":-79.94360566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442251,"north":true,"longitude":-79.9436145,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442252833333335,"north":true,"longitude":-79.94362366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422545,"north":true,"longitude":-79.9436325,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422565,"north":true,"longitude":-79.94364166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442258333333335,"north":true,"longitude":-79.94365083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442260166666664,"north":true,"longitude":-79.94366,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442262166666666,"north":true,"longitude":-79.943669,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442264333333334,"north":true,"longitude":-79.94367783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442267,"north":true,"longitude":-79.9436865,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44227,"north":true,"longitude":-79.94369466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422735,"north":true,"longitude":-79.94370216666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44227766666667,"north":true,"longitude":-79.94370816666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44228233333333,"north":true,"longitude":-79.94371333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442287666666665,"north":true,"longitude":-79.94371783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442293666666664,"north":true,"longitude":-79.94372133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442307166666666,"north":true,"longitude":-79.94372616666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44231416666667,"north":true,"longitude":-79.943727,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442321166666666,"north":true,"longitude":-79.94372666666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442328,"north":true,"longitude":-79.94372466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442335166666666,"north":true,"longitude":-79.94372166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44234183333333,"north":true,"longitude":-79.9437185,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442347833333336,"north":true,"longitude":-79.94371483333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423535,"north":true,"longitude":-79.943711,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44235883333333,"north":true,"longitude":-79.94370666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442364166666664,"north":true,"longitude":-79.9437005,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442369166666666,"north":true,"longitude":-79.94369316666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442373,"north":true,"longitude":-79.94368533333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442376333333335,"north":true,"longitude":-79.9436775,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423795,"north":true,"longitude":-79.94366916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423825,"north":true,"longitude":-79.94366016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44238633333333,"north":true,"longitude":-79.94365183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44239066666667,"north":true,"longitude":-79.943644,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442395166666664,"north":true,"longitude":-79.94363566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44239966666667,"north":true,"longitude":-79.94362783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44240466666667,"north":true,"longitude":-79.94361983333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424095,"north":true,"longitude":-79.943612,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44241433333333,"north":true,"longitude":-79.94360383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442419,"north":true,"longitude":-79.94359583333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44242416666667,"north":true,"longitude":-79.943588,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442429,"north":true,"longitude":-79.94358016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442433666666666,"north":true,"longitude":-79.94357216666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424385,"north":true,"longitude":-79.94356466666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44244333333334,"north":true,"longitude":-79.94355683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44244833333333,"north":true,"longitude":-79.943549,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442453666666665,"north":true,"longitude":-79.94354116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44245866666667,"north":true,"longitude":-79.94353366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424635,"north":true,"longitude":-79.943526,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44246866666667,"north":true,"longitude":-79.94351833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442473666666665,"north":true,"longitude":-79.94351066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424785,"north":true,"longitude":-79.94350283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442483333333335,"north":true,"longitude":-79.94349466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442488166666664,"north":true,"longitude":-79.94348666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442493166666665,"north":true,"longitude":-79.94347866666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442497833333334,"north":true,"longitude":-79.94347033333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442502833333336,"north":true,"longitude":-79.94346216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442512666666666,"north":true,"longitude":-79.943446,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442517333333335,"north":true,"longitude":-79.94343783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44252216666667,"north":true,"longitude":-79.94342983333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442527,"north":true,"longitude":-79.943422,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442531833333334,"north":true,"longitude":-79.943414,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442536833333335,"north":true,"longitude":-79.94340633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44254183333334,"north":true,"longitude":-79.94339883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442547,"north":true,"longitude":-79.94339133333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442552166666665,"north":true,"longitude":-79.943384,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44255733333333,"north":true,"longitude":-79.9433765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44256216666667,"north":true,"longitude":-79.943369,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442567,"north":true,"longitude":-79.94336116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425715,"north":true,"longitude":-79.94335333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442575833333336,"north":true,"longitude":-79.9433455,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258,"north":true,"longitude":-79.94333783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258416666667,"north":true,"longitude":-79.94333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258816666667,"north":true,"longitude":-79.94332233333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442592166666664,"north":true,"longitude":-79.94331433333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442596,"north":true,"longitude":-79.943306,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426,"north":true,"longitude":-79.94329783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44260383333334,"north":true,"longitude":-79.94328966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442607333333335,"north":true,"longitude":-79.94328133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442611166666666,"north":true,"longitude":-79.94327316666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261433333333,"north":true,"longitude":-79.943265,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261783333334,"north":true,"longitude":-79.943257,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442621333333335,"north":true,"longitude":-79.94324883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44262533333333,"north":true,"longitude":-79.943241,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442629333333336,"north":true,"longitude":-79.94323266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44263316666667,"north":true,"longitude":-79.94322466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442637,"north":true,"longitude":-79.9432165,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442640833333336,"north":true,"longitude":-79.943208,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442645166666665,"north":true,"longitude":-79.94319933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44264916666667,"north":true,"longitude":-79.94319066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44265333333333,"north":true,"longitude":-79.94318233333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426575,"north":true,"longitude":-79.94317433333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426615,"north":true,"longitude":-79.94316616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44266533333333,"north":true,"longitude":-79.94315833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442669333333335,"north":true,"longitude":-79.94315016666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442673,"north":true,"longitude":-79.94314183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442677,"north":true,"longitude":-79.94313333333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442685,"north":true,"longitude":-79.94311633333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44269283333333,"north":true,"longitude":-79.94309933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442696833333336,"north":true,"longitude":-79.943091,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442700333333335,"north":true,"longitude":-79.94308233333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44270383333333,"north":true,"longitude":-79.94307383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442707666666664,"north":true,"longitude":-79.94306533333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442712,"north":true,"longitude":-79.9430575,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44272083333333,"north":true,"longitude":-79.94304116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427255,"north":true,"longitude":-79.94303283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44272983333333,"north":true,"longitude":-79.94302483333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427345,"north":true,"longitude":-79.94301633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442739,"north":true,"longitude":-79.94300833333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442743,"north":true,"longitude":-79.9429995,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274733333334,"north":true,"longitude":-79.94299133333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427515,"north":true,"longitude":-79.94298283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427545,"north":true,"longitude":-79.942977,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442756333333335,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757,"north":true,"longitude":-79.9429725,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757,"north":true,"longitude":-79.9429725,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757,"north":true,"longitude":-79.94297283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275716666667,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275733333333,"north":true,"longitude":-79.94297366666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757666666665,"north":true,"longitude":-79.94297416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757666666665,"north":true,"longitude":-79.9429745,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758,"north":true,"longitude":-79.94297483333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758166666664,"north":true,"longitude":-79.94297516666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758833333336,"north":true,"longitude":-79.9429755,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759,"north":true,"longitude":-79.942976,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759333333335,"north":true,"longitude":-79.94297633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427595,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758833333336,"north":true,"longitude":-79.94297666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275866666667,"north":true,"longitude":-79.94297683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758,"north":true,"longitude":-79.94297683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757666666665,"north":true,"longitude":-79.94297683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275716666667,"north":true,"longitude":-79.94297666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442756833333334,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442756333333335,"north":true,"longitude":-79.94297616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275583333334,"north":true,"longitude":-79.942976,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275533333333,"north":true,"longitude":-79.94297566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275483333333,"north":true,"longitude":-79.94297533333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275433333333,"north":true,"longitude":-79.942975,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275366666667,"north":true,"longitude":-79.94297466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275316666667,"north":true,"longitude":-79.94297433333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442752666666664,"north":true,"longitude":-79.942974,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275233333333,"north":true,"longitude":-79.94297383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275183333333,"north":true,"longitude":-79.94297366666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427515,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275116666667,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442750833333335,"north":true,"longitude":-79.94297316666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275066666667,"north":true,"longitude":-79.942973,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442750333333336,"north":true,"longitude":-79.94297283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275,"north":true,"longitude":-79.94297266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274983333333,"north":true,"longitude":-79.94297233333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749166666665,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274883333333,"north":true,"longitude":-79.942972,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427485,"north":true,"longitude":-79.94297166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274816666667,"north":true,"longitude":-79.9429715,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442747833333335,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274766666667,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427475,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274766666667,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427475,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274766666667,"north":true,"longitude":-79.9429715,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442748,"north":true,"longitude":-79.94297183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274816666667,"north":true,"longitude":-79.94297183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274866666667,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274933333333,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749666666664,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749666666664,"north":true,"longitude":-79.942972,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275016666667,"north":true,"longitude":-79.94297166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427505,"north":true,"longitude":-79.94297166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442750833333335,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442751,"north":true,"longitude":-79.94297116666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442751333333334,"north":true,"longitude":-79.94297083333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442751666666666,"north":true,"longitude":-79.94297066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442752166666665,"north":true,"longitude":-79.94297066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442752,"north":true,"longitude":-79.94297016666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} \ No newline at end of file diff --git a/offline/localizer/README.md b/offline/localizer/README.md new file mode 100644 index 00000000..512bbe5e --- /dev/null +++ b/offline/localizer/README.md @@ -0,0 +1,18 @@ +## All files in offline/localizer + +### 1. process json with get_logs.py +change name of log file +will write roll_logs_combined.mat -> array of all different values + +### 2. localizer.m +reads in .mat file and runs algorithm and saves resuts to localizer_v4.mat + +### 3. run trajectory_analysis.m +save plots (screenshot and save different folders for each roll) + +### 4. copy all numbers from matlab command line +go to hamstermap.com/quickmap.php +paste numbers in white box +click regenerate +click satellite +Screenshot image diff --git a/offline/localizer/get_logs.py b/offline/localizer/get_logs.py index a09a9650..7e86161d 100644 --- a/offline/localizer/get_logs.py +++ b/offline/localizer/get_logs.py @@ -6,9 +6,8 @@ combined = True d = None -# file = '../rolls_logs/sample.txt' # file = '../../../rolls_logs/sensors_2016-10-09-06-17-15.txt' -file = '../../../rolls_logs/sensors_2016-10-15-06-26-53.txt' +file = '/Users/babraham/Desktop/sensors_2017-01-28-19-19-04.txt' with open(file) as json_data: d = json.load(json_data) From a83d9c2547dd84a02af5a2d468d2e13afa70393f Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 14 Feb 2017 17:41:35 -0500 Subject: [PATCH 060/149] testing fixes and setup for Friday --- .../robobuggy/main/RobobuggyMainFile.java | 2 + .../robobuggy/robots/TransistorAuton.java | 4 +- .../robots/TransistorDataCollection.java | 4 + .../robots/TransistorWaypointCollection.java | 124 ------------------ 4 files changed, 8 insertions(+), 126 deletions(-) delete mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index b2bade70..cfc43efe 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,6 +1,8 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; +import com.roboclub.robobuggy.robots.CommTestRobot; +import com.roboclub.robobuggy.robots.TransistorAuton; import com.roboclub.robobuggy.robots.TransistorDataCollection; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 8200e238..fa3e1d53 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -70,8 +70,8 @@ private TransistorAuton() { // nodeList.add(new HillCrestImuNode()); try { -// ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); - ArrayList wayPoints = WayPointUtil.createWaypointsFromOdomLocalizerLog(RobobuggyConfigFile.getWaypointSourceLogFile()); + ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + //ArrayList wayPoints = WayPointUtil.createWaypointsFromOdomLocalizerLog(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java index 2980405d..68fc11ea 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java @@ -3,6 +3,8 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.nodes.sensors.CameraNode; import com.roboclub.robobuggy.nodes.sensors.GpsNode; import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; @@ -57,6 +59,8 @@ private TransistorDataCollection() { RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); nodeList.add(new HillCrestImuNode()); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(0, 0))); + //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java deleted file mode 100644 index fe575b0c..00000000 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorWaypointCollection.java +++ /dev/null @@ -1,124 +0,0 @@ -package com.roboclub.robobuggy.robots; - -import com.roboclub.robobuggy.main.RobobuggyConfigFile; -import com.roboclub.robobuggy.main.RobobuggyLogicNotification; -import com.roboclub.robobuggy.main.RobobuggyMessageLevel; -import com.roboclub.robobuggy.nodes.localizers.LocTuple; -import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; -import com.roboclub.robobuggy.nodes.sensors.CameraNode; -import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; -import com.roboclub.robobuggy.nodes.sensors.LoggingNode; -import com.roboclub.robobuggy.nodes.sensors.RBSMNode; -import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ui.ConfigurationPanel; -import com.roboclub.robobuggy.ui.Gui; -import com.roboclub.robobuggy.ui.MainGuiWindow; -import com.roboclub.robobuggy.ui.RobobuggyGUITabs; -import com.roboclub.robobuggy.ui.RobobuggyJFrame; - -import java.awt.event.WindowEvent; -import java.awt.event.WindowListener; - -/** - * A robot class for doing data collection only with the live robot, will not attempt to autonomously drive - * - * @author Trevor Decker - */ -public final class TransistorWaypointCollection extends AbstractRobot { - private static TransistorWaypointCollection instance; - private static final int ARDUINO_BOOTLOADER_TIMEOUT = 2000; - - /** - * Returns a reference to the one instance of the {@link Robot} object. - * If no instance exists, a new one is created. - * - * @return a reference to the one instance of the {@link Robot} object - */ - public static AbstractRobot getInstance() { - if (instance == null) { - instance = new TransistorWaypointCollection(); - } - return instance; - } - - private TransistorWaypointCollection() { - super(); - try { - Thread.sleep(ARDUINO_BOOTLOADER_TIMEOUT); - } catch (InterruptedException e) { - new RobobuggyLogicNotification("Couldn't wait for bootloader, shutting down", RobobuggyMessageLevel.EXCEPTION); - shutDown(); - } - new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); - // Initialize Nodes - nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); - nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, - NodeChannel.getLoggingChannels())); - nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), - RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); - nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); - nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(0, 0))); - - //setup the gui - RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); - Gui.getInstance().addWindow(mainWindow); - RobobuggyGUITabs tabs = new RobobuggyGUITabs(); - mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); - - mainWindow.addWindowListener(new WindowListener() { - - @Override - public void windowOpened(WindowEvent e) { - // TODO Auto-generated method stub - - } - - @Override - public void windowIconified(WindowEvent e) { - // TODO Auto-generated method stub - - } - - @Override - public void windowDeiconified(WindowEvent e) { - // TODO Auto-generated method stub - - } - - @Override - public void windowDeactivated(WindowEvent e) { - // TODO Auto-generated method stub - - } - - @Override - public void windowClosing(WindowEvent e) { - // TODO Auto-generated method stub - TransistorWaypointCollection.this.shutDown(); - } - - @Override - public void windowClosed(WindowEvent e) { - // TODO Auto-generated method stub - - } - - @Override - public void windowActivated(WindowEvent e) { - // TODO Auto-generated method stub - - } - }); - - tabs.addTab(new MainGuiWindow(), "Home"); -// tabs.addTab(new ImuVisualWindow(), "IMU"); -// tabs.addTab(new PoseGraphsPanel(),"poses"); -// tabs.addTab(new ImuPanel(),"IMU"); -// tabs.addTab(new PathPanel(),"Path Panel"); - tabs.addTab(new ConfigurationPanel(), "Configuration"); - - } -} - From 331f2cedb3efbad2b5803076e499a31d982bc202 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 17 Feb 2017 17:08:37 -0500 Subject: [PATCH 061/149] disabled PID and reenabled lookahead --- .../nodes/planners/WayPointFollowerPlanner.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 78248007..57b834a2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -81,9 +81,9 @@ public double getCommandedSteeringAngle() { //pick the point to follow int targetIndex = closestIndex + 1; double distanceFromMessage = GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)); -// while (targetIndex < wayPoints.size() && distanceFromMessage < delta) { -// targetIndex = targetIndex + 1; -// } + while (targetIndex < wayPoints.size() && distanceFromMessage < delta) { + targetIndex = targetIndex + 1; + } //if we are out of points then just go straight if (targetIndex >= wayPoints.size()) { @@ -106,6 +106,8 @@ public double getCommandedSteeringAngle() { //find the angle we need to reach that point double deltaHeading = desiredHeading - poseHeading; + /* + @vivaanbahl 2/17/17 put PID on hold in order to test the original waypoint planner // Pure Pursuit steering controller double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); double param2 = 0.8 * pose.getCurrentState().get(2, 0); @@ -128,8 +130,9 @@ public double getCommandedSteeringAngle() { // put it all together double pidError = Kp * currentHeadingError + Ki * totalHeadingError + Kd * deltaError; + */ - return Util.normalizeAngleRad(pidError); + return Util.normalizeAngleRad(deltaHeading); } @Override From fc4fbff2a8f68f3f4585be23acad77dc1879b16e Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sat, 18 Feb 2017 03:36:22 -0500 Subject: [PATCH 062/149] removed PID from steering controller, fixed lookahead distance calculation --- offline/controller/controller_pure.m | 4 +- .../planners/WayPointFollowerPlanner.java | 61 ++++--------------- 2 files changed, 15 insertions(+), 50 deletions(-) diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m index 36616dde..f68a1ce2 100644 --- a/offline/controller/controller_pure.m +++ b/offline/controller/controller_pure.m @@ -15,7 +15,7 @@ lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 4; % 8; % m/s, 17.9mph, forward velocity + velocity = 8; % m/s, 17.9mph, forward velocity steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds % full range in 0.5s @@ -114,7 +114,7 @@ pos = X(1:2)'; pos_b = repmat(pos, size(desired_traj, 1), 1); - delta = 5; + delta = 15; cutoff = 100; delta = delta * delta; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 78248007..5ad542e0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -19,21 +19,6 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type - - // PID controller values/variables - - private double Kp = 0.4; - private double Ki = 0.3; - private double Kd = 0.05; - - private double previousHeadingError; - private double totalHeadingError; - - private long previousTimeMillis; - - // end PID controller values/variables - - /** * @param wayPoints the list of waypoints to follow */ @@ -50,7 +35,7 @@ public void updatePositionEstimate(GPSPoseMessage m) { } //find the closest way point - //TODO turn into a binary search + //TODO turn into a binary kdtree search private static int getClosestIndex(ArrayList wayPoints, GPSPoseMessage currentLocation) { double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this @@ -75,15 +60,14 @@ public double getCommandedSteeringAngle() { return 17433504; //A dummy value that we can never get } - - double delta = 10; //meters - //pick the first point that is at least delta away - //pick the point to follow - int targetIndex = closestIndex + 1; - double distanceFromMessage = GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)); -// while (targetIndex < wayPoints.size() && distanceFromMessage < delta) { -// targetIndex = targetIndex + 1; -// } + double lookahead = 15; //meters + //pick the first point that is at least lookahead away, then point buggy toward it + int targetIndex = closestIndex; + double distanceFromMessage = 0; + do { + targetIndex++; + distanceFromMessage = GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)); + } while (targetIndex < wayPoints.size() && distanceFromMessage < lookahead); //if we are out of points then just go straight if (targetIndex >= wayPoints.size()) { @@ -91,7 +75,6 @@ public double getCommandedSteeringAngle() { return 0; } - GpsMeasurement targetPoint = wayPoints.get(targetIndex); //find a path from our current location to that point @@ -101,35 +84,17 @@ public double getCommandedSteeringAngle() { double deltaLongMeters = LocalizerUtil.convertLonToMeters(deltaLong); double desiredHeading = Math.atan2(deltaLatMeters, deltaLongMeters); - double poseHeading = pose.getHeading(); - //find the angle we need to reach that point + double poseHeading = pose.getHeading(); double deltaHeading = desiredHeading - poseHeading; - // Pure Pursuit steering controller + //Pure Pursuit steering controller double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); double param2 = 0.8 * pose.getCurrentState().get(2, 0); deltaHeading = Math.atan2(param1, param2); - - // begin PID control - // calculate P - double currentHeadingError = deltaHeading; - - // calculate I - totalHeadingError += deltaHeading; - - // calculate D - long currentTimeMillis = System.currentTimeMillis(); - double dt = currentTimeMillis - previousTimeMillis; - double deltaError = (1.0 * (currentHeadingError - previousHeadingError)) / dt; - previousHeadingError = currentHeadingError; - previousTimeMillis = currentTimeMillis; - - // put it all together - double pidError = Kp * currentHeadingError + Ki * totalHeadingError + Kd * deltaError; - - return Util.normalizeAngleRad(pidError); + //PD control of DC steering motor handled by low level + return Util.normalizeAngleRad(deltaHeading); } @Override From 16c843b250080444c48fe5c536061cbf312e8fc2 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 16:12:35 -0500 Subject: [PATCH 063/149] added current waypoint to the map display --- .../roboclub/robobuggy/main/RobobuggyMainFile.java | 6 ++---- .../nodes/planners/WayPointFollowerPlanner.java | 7 +++++++ .../src/main/java/com/roboclub/robobuggy/ui/Map.java | 12 ++++++++++++ 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index cfc43efe..cc0b1158 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,9 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.CommTestRobot; -import com.roboclub.robobuggy.robots.TransistorAuton; -import com.roboclub.robobuggy.robots.TransistorDataCollection; +import com.roboclub.robobuggy.robots.ControllerTesterRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -32,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorDataCollection.getInstance(); + robot = ControllerTesterRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 5ad542e0..2546c367 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -19,6 +19,12 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type + /** + * @vivaanbahl TESTING CODE ONLY + * REMOVE FOR PROD PUSH + */ + public static GpsMeasurement currentWaypoint = new GpsMeasurement(0, 0); + /** * @param wayPoints the list of waypoints to follow */ @@ -76,6 +82,7 @@ public double getCommandedSteeringAngle() { } GpsMeasurement targetPoint = wayPoints.get(targetIndex); + currentWaypoint = targetPoint; //find a path from our current location to that point double deltaLong = targetPoint.getLongitude() - pose.getLongitude(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index b1c8ae5e..8f7ce3c9 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -5,6 +5,7 @@ import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.ros.Message; import com.roboclub.robobuggy.ros.MessageListener; import com.roboclub.robobuggy.ros.NodeChannel; @@ -49,6 +50,8 @@ public class Map extends JPanel { private double mapDragY = -1; private static final int MAX_POINT_BUF_SIZE = 3000; + private MapMarkerDot currentWaypoint = new MapMarkerDot(0, 0); + /** * initializes a new Map with cache loaded */ @@ -57,6 +60,9 @@ public Map() { addCacheToTree(); this.add(getMapTree()); + currentWaypoint.setColor(Color.BLUE); + getMapTree().getViewer().addMapMarker(currentWaypoint); + //adds track buggy new Subscriber("Map", NodeChannel.POSE.getMsgPath(), new MessageListener() { //TODO make this optional @@ -67,6 +73,12 @@ public void actionPerformed(String topicName, Message m) { getMapTree().getViewer().setDisplayPosition(new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), zoomLevel); addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); + + getMapTree().getViewer().removeMapMarker(currentWaypoint); + currentWaypoint.setLat(WayPointFollowerPlanner.currentWaypoint.getLatitude()); + currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); + getMapTree().getViewer().addMapMarker(currentWaypoint); + } }); From c4e007a5c3f12961ee148179a52ad6338c360448 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 19:06:00 -0500 Subject: [PATCH 064/149] added skeleton of imu integration in kalman filter --- .../localizers/RobobuggyKFLocalizer.java | 27 ++++++++++++++++--- .../planners/WayPointFollowerPlanner.java | 2 +- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index f1322459..1e4fd49a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -4,6 +4,7 @@ import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.messages.IMUCompassMessage; import com.roboclub.robobuggy.messages.SteeringMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; @@ -44,10 +45,12 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private Matrix P; // covariance matrix private Matrix Q_gps; // model noise covariance matrix private Matrix Q_encoder; + private Matrix Q_imu; // output matrices private Matrix C_gps; // a description of how the GPS impacts x private Matrix C_encoder; // how the encoder affects x + private Matrix C_imu; private long lastTime; // the last time we updated the current position estimate private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement @@ -55,10 +58,6 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity private double steeringAngle = 0; // current steering angle of the buggy - public Matrix getCurrentState() { - return x; - } - /** * Create a new {@link PeriodicNode} decorator * @@ -103,6 +102,9 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] qEncoder2D = {{0.25}}; Q_encoder = new Matrix(qEncoder2D); + double[][] qImu2D = {{ 0 }}; + Q_imu = new Matrix(qImu2D); + double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, @@ -115,14 +117,31 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { }; C_encoder = new Matrix(cEncoder2D); + double[][] cImu2D = { + { 0, 0, 0, 1, 0 }, + }; + C_imu = new Matrix(cImu2D); + // add all our subscribers for our current state update stream // Every time we get a new sensor update, trigger the new kalman update setupGPSSubscriber(); setupEncoderSubscriber(); setupWheelSubscriber(); + setupIMUSubscriber(); resume(); } + private void setupIMUSubscriber() { + new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), (topicName, m) -> { + double heading = ((IMUCompassMessage) m).getCompassHeading(); + + double[][] z2D = {{ heading }}; + Matrix z = new Matrix(z2D); + + kalmanFilter(C_imu, Q_imu, z); + }); + } + private void setupEncoderSubscriber() { new Subscriber("KF Localizer", NodeChannel.ENCODER.getMsgPath(), ((topicName, m) -> { long currentTime = new Date().getTime(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 2546c367..a225d995 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -66,7 +66,7 @@ public double getCommandedSteeringAngle() { return 17433504; //A dummy value that we can never get } - double lookahead = 15; //meters + double lookahead = 5; //meters //pick the first point that is at least lookahead away, then point buggy toward it int targetIndex = closestIndex; double distanceFromMessage = 0; From 7ee21fd5cd6eb3366a12309a582bc609b784f50c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 19:07:24 -0500 Subject: [PATCH 065/149] forgot about the degrees to radians issue --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 1e4fd49a..fa6d9556 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -133,7 +133,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { private void setupIMUSubscriber() { new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), (topicName, m) -> { - double heading = ((IMUCompassMessage) m).getCompassHeading(); + double heading = Math.toRadians(((IMUCompassMessage) m).getCompassHeading()); double[][] z2D = {{ heading }}; Matrix z = new Matrix(z2D); From 0f9725ea364d5a4eddc46f3ed2b8df7980df868f Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Fri, 24 Feb 2017 19:37:21 -0500 Subject: [PATCH 066/149] new controller analysis files --- offline/controller/controller_pure_cont.m | 137 ++++++++++++++++++++ offline/controller/controller_stanley.m | 148 ++++++++++++++++++++++ offline/controller/waypoint_analysis.m | 39 ++++++ 3 files changed, 324 insertions(+) create mode 100644 offline/controller/controller_pure_cont.m create mode 100644 offline/controller/controller_stanley.m create mode 100644 offline/controller/waypoint_analysis.m diff --git a/offline/controller/controller_pure_cont.m b/offline/controller/controller_pure_cont.m new file mode 100644 index 00000000..f68a1ce2 --- /dev/null +++ b/offline/controller/controller_pure_cont.m @@ -0,0 +1,137 @@ +function [trajectory] = controller_pure() +% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf +% section 2.2 + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 8; % m/s, 17.9mph, forward velocity + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + % full range in 0.5s + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints_tri.mat'); + desired_traj = processWaypoints(logs); + % time = linspace(0, 240, size(trajectory,2)); + time = 0:dt:60; % 240; + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); steering]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + % trajectory = [trajectory, X]; + snapshot = summarize(X, utm_zone, steering); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_tri_v1.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; x(1); x(2); steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + if(a < -deg2rad(10)) + a = -deg2rad(10); + end + if(a > deg2rad(10)) + a = deg2rad(10); + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + global wheel_base + + pos = X(1:2)'; + pos_b = repmat(pos, size(desired_traj, 1), 1); + delta = 15; + cutoff = 100; + delta = delta * delta; + + distances = sum((desired_traj - pos_b).^2, 2); + [~, closest_idx] = min(distances); + last_idx = min([length(distances), closest_idx + cutoff]); + possible = find(distances(1:last_idx) < delta); + if isempty(possible) + u = 0; + else + target = desired_traj(possible(end), :); + deltaPath = target - pos; + + k = 0.8; + a = atan2(deltaPath(2), deltaPath(1))-X(4); + u = atan2(2*wheel_base*sin(a), k*X(3)); + end + u = clampSteeringAngle(clampAngle(u)); +end + diff --git a/offline/controller/controller_stanley.m b/offline/controller/controller_stanley.m new file mode 100644 index 00000000..af31c038 --- /dev/null +++ b/offline/controller/controller_stanley.m @@ -0,0 +1,148 @@ +function [trajectory] = controller_pure_cont() +% https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf +% section 2.2 + + addpath('../localizer/latlonutm/Codes/matlab'); + global wheel_base + global velocity + global steering_vel + global dt + + save_data = true; + wheel_base = 1.13; + utm_zone = 17; + first_heading = deg2rad(250); + lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; + dt = 0.001; % 1000Hz + m = 50; % 20Hz + velocity = 8; % m/s, 17.9mph, forward velocity + steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds + % full range in 0.5s + + [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); + + X = [x; % X, m, UTM coors + y; % Y, m, UTM coors + velocity; % d_Yb, body velocity + first_heading; % heading, rad, world frame + 0]; % d_heading, rad/s + + load('./waypoints_tri.mat'); + desired_traj = processWaypoints(logs); + % time = linspace(0, 240, size(trajectory,2)); + time = 0:dt:60; % 240; + u = 0; % commanded steering angle + steering = u; % steering angle + trajectory = [X; lat_long(1); lat_long(2); steering]; + + for i = 1:size(time, 2) + t = time(i); + A = model(X, steering); + X = A*X; + steering = updateSteering(steering, u); + + X(4) = clampAngle(X(4)); + X(5) = clampAngle(X(5)); + + if(mod(i, m) == 0) + u = control(desired_traj, X); + end + + % trajectory = [trajectory, X]; + snapshot = summarize(X, utm_zone, steering); + trajectory = [trajectory, snapshot]; + end + + if save_data + save('controller_tri_v1.mat', 'trajectory'); + end +end + +function [desired] = processWaypoints(lat_long) + [x, y, zone] = ll2utm(lat_long); + desired = [x y]; +end + +function snapshot = summarize(x, utm_zone, steeringAngle) + [lat, lon] = utm2ll(x(1), x(2), utm_zone); + snapshot = [x; x(1); x(2); steeringAngle]; +end + +function a = clampAngle(a) + while (a < -pi) + a = a + 2*pi; + end + while (a > pi) + a = a - 2*pi; + end +end + +function b = updateSteering(b, u) + global steering_vel + global dt + + if(b < u) + b = b + steering_vel*dt; + end + if(b > u) + b = b - steering_vel*dt; + end +end + +function a = clampSteeringAngle(a) + if(a < -deg2rad(10)) + a = -deg2rad(10); + end + if(a > deg2rad(10)) + a = deg2rad(10); + end +end + +function [A] = model(x, steering) + global wheel_base + global dt + + A = [1, 0, dt*cos(x(4)), 0, 0; + 0, 1, dt*sin(x(4)), 0, 0; + 0, 0, 1, 0, 0; + 0, 0, 0, 1, dt; + 0, 0, tan(steering)/wheel_base, 0, 0]; +end + +function [u] = control(desired_traj, X) + global wheel_base + + pos = X(1:2)'; + delta = 15; + + % find closest + min_idx = 0; + min_dist = 100000000; + for k=1:length(desired_traj) + distp = norm(pos, desired_traj(k,:)); + if(distp < min_dist) + min_idx = k; + min_dist = distp; + end + end + + ptA = [0 0]; + ptB = [0 0]; + + distances = sum((desired_traj - pos_b).^2, 2); + [~, closest_idx] = min(distances); + last_idx = min([length(distances), closest_idx + cutoff]); + possible = find(distances(1:last_idx) < delta); + if isempty(possible) + u = 0; + else + target = desired_traj(possible(end), :); + deltaPath = target - pos; + + k = 0.8; + a = atan2(deltaPath(2), deltaPath(1))-X(4); + u = atan2(2*wheel_base*sin(a), k*X(3)); + end + u = clampSteeringAngle(clampAngle(u)); +end + diff --git a/offline/controller/waypoint_analysis.m b/offline/controller/waypoint_analysis.m new file mode 100644 index 00000000..90e477d7 --- /dev/null +++ b/offline/controller/waypoint_analysis.m @@ -0,0 +1,39 @@ +% graph results + +clear; +close all; + +addpath('../localizer/latlonutm/Codes/matlab'); + +file = 'controller_v2.mat'; +load(file, 'trajectory'); +show_maps = false; + +load('./waypoints.mat'); +[x, y, zone] = ll2utm(logs); +desired = [x y]; + +size(desired) +size(trajectory) + +for k=1:(length(desired) - 1) + sumdist = norm(desired(k,:) - desired(k+1,:)); +end +avgdist = sumdist ./ length(desired) + +heading = trajectory(4,1:k:end); +heading = rad2deg(heading); +figure(); +hold on; +plot(1:length(heading), heading) +title(['Heading ', file]); + +figure(); +plot(1:length(trajectory), trajectory(8, 1:end)) +title('Control input'); + +% plot on google maps +if show_maps + xy = [trajectory(6,1:k:end); trajectory(7,1:k:end)]; + fprintf(1, '%5.20f, %5.20f\n', xy); +end From 6360fa9349fc1ed245ace0b43f2477ea2b71b571 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Fri, 24 Feb 2017 19:58:21 -0500 Subject: [PATCH 067/149] get heading estimation from IMU compass --- .../localizers/RobobuggyKFLocalizer.java | 28 +++---------------- 1 file changed, 4 insertions(+), 24 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index fa6d9556..a66f50bc 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -75,7 +75,6 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { lastTime = new Date().getTime(); lastEncoder = 0; lastEncoderTime = lastTime; - lastGPS = initialLocationGPS; double[][] x2D = { { initialLocationGPS.getEasting() }, @@ -93,22 +92,20 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { P = arrayToMatrix(pArray); double[][] qGPS2D = { - {4, 0, 0}, - {0, 4, 0}, - {0, 0, 0.02}, + {4, 0}, + {0, 4}, }; Q_gps = new Matrix(qGPS2D); double[][] qEncoder2D = {{0.25}}; Q_encoder = new Matrix(qEncoder2D); - double[][] qImu2D = {{ 0 }}; + double[][] qImu2D = {{0.008}}; Q_imu = new Matrix(qImu2D); double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, - {0, 0, 0, 1, 0}, }; C_gps = new Matrix(cGPS2D); @@ -118,7 +115,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { C_encoder = new Matrix(cEncoder2D); double[][] cImu2D = { - { 0, 0, 0, 1, 0 }, + {0, 0, 0, 1, 0}, }; C_imu = new Matrix(cImu2D); @@ -172,31 +169,14 @@ private void setupGPSSubscriber() { GpsMeasurement gpsLoc = (GpsMeasurement) m; LocTuple loc = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); UTMTuple gps = LocalizerUtil.deg2UTM(loc); - double dx = gps.getEasting() - lastGPS.getEasting(); - double dy = gps.getNorthing() - lastGPS.getNorthing(); - lastGPS = gps; - - // don't update angle if we did not move a lot - double heading = Math.atan2(dy, dx); - if ((dx * dx + dy * dy) < 0.25) { - heading = x.get(HEADING_GLOBAL_ROW, 0); - } - // close the loop, lock initial angle - if (Math.abs(gps.getEasting() - initialLocationGPS.getEasting()) - + Math.abs(gps.getNorthing() - initialLocationGPS.getNorthing()) < 10.0) { - // todo should this be previous heading? - heading = INITIAL_HEADING_IN_RADS; - } // measurement double[][] z2D = { { gps.getEasting() }, { gps.getNorthing() }, - { heading }, }; Matrix z = new Matrix(z2D); - kalmanFilter(C_gps, Q_gps, z); })); } From 0a2a8c98cf614b4f865c6377057ad12a6ab1d8af Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 20:19:15 -0500 Subject: [PATCH 068/149] fixes for playback robot --- .../com/roboclub/robobuggy/main/RobobuggyMainFile.java | 4 ++-- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 2 +- .../roboclub/robobuggy/nodes/planners/WayPointUtil.java | 6 +++--- .../java/com/roboclub/robobuggy/robots/PlayBackRobot.java | 3 +++ .../com/roboclub/robobuggy/simulation/PlayBackUtil.java | 7 +++++++ 5 files changed, 16 insertions(+), 6 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index cc0b1158..005648d0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.ControllerTesterRobot; +import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = ControllerTesterRobot.getInstance(); + robot = PlayBackRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index a66f50bc..6d53d170 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -130,7 +130,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { private void setupIMUSubscriber() { new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), (topicName, m) -> { - double heading = Math.toRadians(((IMUCompassMessage) m).getCompassHeading()); + double heading = Math.toRadians(((IMUCompassMessage) m).getCompassHeading()) + Math.toRadians(90); double[][] z2D = {{ heading }}; Matrix z = new Matrix(z2D); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java index 669cf175..8d5ceb59 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java @@ -108,9 +108,9 @@ public static ArrayList createWaypointsFromOdomLocalizerLog(Stri * @return waypoint list * @throws IOException we couldn't find the file or folder */ - public static ArrayList createWayPointsFromLog(String folder, String filename) throws IOException { + public static ArrayList createWayPointsFromLog(String folder, String filename) throws IOException { - ArrayList messages = new ArrayList(); + ArrayList messages = new ArrayList<>(); File outputFile = new File(folder + "/waypoints.txt"); if (!outputFile.createNewFile()) { new RobobuggyLogicNotification("couldn't create file", RobobuggyMessageLevel.EXCEPTION); @@ -139,7 +139,7 @@ public static ArrayList createWayPointsFromLog(String folder, String filename) t switch (versionID) { case GpsMeasurement.VERSION_ID: - Message transmitMessage = translator.fromJson(sensorDataJson, GpsMeasurement.class); + GpsMeasurement transmitMessage = translator.fromJson(sensorDataJson, GpsMeasurement.class); messages.add(transmitMessage); writer.write(sensorDataJson.toString() + "\n"); break; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index 7ff2c8cb..45362285 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -2,6 +2,8 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.simulation.LineByLineSensorPlayer; import com.roboclub.robobuggy.ui.AutonomousPanel; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -36,6 +38,7 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); + new RobobuggyKFLocalizer(10, "loc", new LocTuple(0, 0)); // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 11eed6fb..cfb4f250 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -12,6 +12,7 @@ import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.messages.GuiLoggingButtonMessage; import com.roboclub.robobuggy.messages.IMUAngularPositionMessage; +import com.roboclub.robobuggy.messages.IMUCompassMessage; import com.roboclub.robobuggy.messages.ImuMeasurement; import com.roboclub.robobuggy.messages.MagneticMeasurement; import com.roboclub.robobuggy.messages.RemoteWheelAngleRequest; @@ -43,6 +44,7 @@ public final class PlayBackUtil { private Publisher loggingButtonPub; private Publisher logicNotificationPub; private Publisher posePub; + private Publisher compassPub; /** @@ -71,6 +73,7 @@ private PlayBackUtil() { loggingButtonPub = new Publisher(NodeChannel.GUI_LOGGING_BUTTON.getMsgPath()); logicNotificationPub = new Publisher(NodeChannel.LOGIC_NOTIFICATION.getMsgPath()); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + compassPub = new Publisher(NodeChannel.IMU_COMPASS.getMsgPath()); } /** @@ -166,6 +169,10 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, case IMUAngularPositionMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, IMUAngularPositionMessage.class); break; + case IMUCompassMessage.VERSION_ID: + transmitMessage = translator.fromJson(sensorDataJson, IMUCompassMessage.class); + getPrivateInstance().compassPub.publish(transmitMessage); + break; case ResetMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, ResetMessage.class); break; From d688757401068ebd07066005aecf25251dd16aed Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 20:23:53 -0500 Subject: [PATCH 069/149] Revert "fixes for playback robot" This reverts commit 0a2a8c98cf614b4f865c6377057ad12a6ab1d8af. --- .../com/roboclub/robobuggy/main/RobobuggyMainFile.java | 4 ++-- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 2 +- .../roboclub/robobuggy/nodes/planners/WayPointUtil.java | 6 +++--- .../java/com/roboclub/robobuggy/robots/PlayBackRobot.java | 3 --- .../com/roboclub/robobuggy/simulation/PlayBackUtil.java | 7 ------- 5 files changed, 6 insertions(+), 16 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 005648d0..cc0b1158 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.PlayBackRobot; +import com.roboclub.robobuggy.robots.ControllerTesterRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = PlayBackRobot.getInstance(); + robot = ControllerTesterRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 6d53d170..a66f50bc 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -130,7 +130,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { private void setupIMUSubscriber() { new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), (topicName, m) -> { - double heading = Math.toRadians(((IMUCompassMessage) m).getCompassHeading()) + Math.toRadians(90); + double heading = Math.toRadians(((IMUCompassMessage) m).getCompassHeading()); double[][] z2D = {{ heading }}; Matrix z = new Matrix(z2D); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java index 8d5ceb59..669cf175 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointUtil.java @@ -108,9 +108,9 @@ public static ArrayList createWaypointsFromOdomLocalizerLog(Stri * @return waypoint list * @throws IOException we couldn't find the file or folder */ - public static ArrayList createWayPointsFromLog(String folder, String filename) throws IOException { + public static ArrayList createWayPointsFromLog(String folder, String filename) throws IOException { - ArrayList messages = new ArrayList<>(); + ArrayList messages = new ArrayList(); File outputFile = new File(folder + "/waypoints.txt"); if (!outputFile.createNewFile()) { new RobobuggyLogicNotification("couldn't create file", RobobuggyMessageLevel.EXCEPTION); @@ -139,7 +139,7 @@ public static ArrayList createWayPointsFromLog(String folder, St switch (versionID) { case GpsMeasurement.VERSION_ID: - GpsMeasurement transmitMessage = translator.fromJson(sensorDataJson, GpsMeasurement.class); + Message transmitMessage = translator.fromJson(sensorDataJson, GpsMeasurement.class); messages.add(transmitMessage); writer.write(sensorDataJson.toString() + "\n"); break; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index 45362285..7ff2c8cb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -2,8 +2,6 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; -import com.roboclub.robobuggy.nodes.localizers.LocTuple; -import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.simulation.LineByLineSensorPlayer; import com.roboclub.robobuggy.ui.AutonomousPanel; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -38,7 +36,6 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); - new RobobuggyKFLocalizer(10, "loc", new LocTuple(0, 0)); // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index cfb4f250..11eed6fb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -12,7 +12,6 @@ import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.messages.GuiLoggingButtonMessage; import com.roboclub.robobuggy.messages.IMUAngularPositionMessage; -import com.roboclub.robobuggy.messages.IMUCompassMessage; import com.roboclub.robobuggy.messages.ImuMeasurement; import com.roboclub.robobuggy.messages.MagneticMeasurement; import com.roboclub.robobuggy.messages.RemoteWheelAngleRequest; @@ -44,7 +43,6 @@ public final class PlayBackUtil { private Publisher loggingButtonPub; private Publisher logicNotificationPub; private Publisher posePub; - private Publisher compassPub; /** @@ -73,7 +71,6 @@ private PlayBackUtil() { loggingButtonPub = new Publisher(NodeChannel.GUI_LOGGING_BUTTON.getMsgPath()); logicNotificationPub = new Publisher(NodeChannel.LOGIC_NOTIFICATION.getMsgPath()); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); - compassPub = new Publisher(NodeChannel.IMU_COMPASS.getMsgPath()); } /** @@ -169,10 +166,6 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, case IMUAngularPositionMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, IMUAngularPositionMessage.class); break; - case IMUCompassMessage.VERSION_ID: - transmitMessage = translator.fromJson(sensorDataJson, IMUCompassMessage.class); - getPrivateInstance().compassPub.publish(transmitMessage); - break; case ResetMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, ResetMessage.class); break; From 316df168663f290b92b30610144e625c90d02502 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 20:24:07 -0500 Subject: [PATCH 070/149] Revert "get heading estimation from IMU compass" This reverts commit 6360fa9349fc1ed245ace0b43f2477ea2b71b571. --- .../localizers/RobobuggyKFLocalizer.java | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index a66f50bc..fa6d9556 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -75,6 +75,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { lastTime = new Date().getTime(); lastEncoder = 0; lastEncoderTime = lastTime; + lastGPS = initialLocationGPS; double[][] x2D = { { initialLocationGPS.getEasting() }, @@ -92,20 +93,22 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { P = arrayToMatrix(pArray); double[][] qGPS2D = { - {4, 0}, - {0, 4}, + {4, 0, 0}, + {0, 4, 0}, + {0, 0, 0.02}, }; Q_gps = new Matrix(qGPS2D); double[][] qEncoder2D = {{0.25}}; Q_encoder = new Matrix(qEncoder2D); - double[][] qImu2D = {{0.008}}; + double[][] qImu2D = {{ 0 }}; Q_imu = new Matrix(qImu2D); double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, + {0, 0, 0, 1, 0}, }; C_gps = new Matrix(cGPS2D); @@ -115,7 +118,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { C_encoder = new Matrix(cEncoder2D); double[][] cImu2D = { - {0, 0, 0, 1, 0}, + { 0, 0, 0, 1, 0 }, }; C_imu = new Matrix(cImu2D); @@ -169,14 +172,31 @@ private void setupGPSSubscriber() { GpsMeasurement gpsLoc = (GpsMeasurement) m; LocTuple loc = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); UTMTuple gps = LocalizerUtil.deg2UTM(loc); + double dx = gps.getEasting() - lastGPS.getEasting(); + double dy = gps.getNorthing() - lastGPS.getNorthing(); + lastGPS = gps; + + // don't update angle if we did not move a lot + double heading = Math.atan2(dy, dx); + if ((dx * dx + dy * dy) < 0.25) { + heading = x.get(HEADING_GLOBAL_ROW, 0); + } + // close the loop, lock initial angle + if (Math.abs(gps.getEasting() - initialLocationGPS.getEasting()) + + Math.abs(gps.getNorthing() - initialLocationGPS.getNorthing()) < 10.0) { + // todo should this be previous heading? + heading = INITIAL_HEADING_IN_RADS; + } // measurement double[][] z2D = { { gps.getEasting() }, { gps.getNorthing() }, + { heading }, }; Matrix z = new Matrix(z2D); + kalmanFilter(C_gps, Q_gps, z); })); } From baf5d6cb5bc9b4f79865a9bf58a6c67e372906b9 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 20:24:27 -0500 Subject: [PATCH 071/149] Revert "forgot about the degrees to radians issue" This reverts commit 7ee21fd5cd6eb3366a12309a582bc609b784f50c. --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index fa6d9556..1e4fd49a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -133,7 +133,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { private void setupIMUSubscriber() { new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), (topicName, m) -> { - double heading = Math.toRadians(((IMUCompassMessage) m).getCompassHeading()); + double heading = ((IMUCompassMessage) m).getCompassHeading(); double[][] z2D = {{ heading }}; Matrix z = new Matrix(z2D); From 619af97527f697d10b9c02732fca8fc1f55a65cf Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 24 Feb 2017 20:24:33 -0500 Subject: [PATCH 072/149] Revert "added skeleton of imu integration in kalman filter" This reverts commit c4e007a5c3f12961ee148179a52ad6338c360448. --- .../localizers/RobobuggyKFLocalizer.java | 27 +++---------------- .../planners/WayPointFollowerPlanner.java | 2 +- 2 files changed, 5 insertions(+), 24 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 1e4fd49a..f1322459 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -4,7 +4,6 @@ import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.messages.IMUCompassMessage; import com.roboclub.robobuggy.messages.SteeringMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; @@ -45,12 +44,10 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private Matrix P; // covariance matrix private Matrix Q_gps; // model noise covariance matrix private Matrix Q_encoder; - private Matrix Q_imu; // output matrices private Matrix C_gps; // a description of how the GPS impacts x private Matrix C_encoder; // how the encoder affects x - private Matrix C_imu; private long lastTime; // the last time we updated the current position estimate private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement @@ -58,6 +55,10 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity private double steeringAngle = 0; // current steering angle of the buggy + public Matrix getCurrentState() { + return x; + } + /** * Create a new {@link PeriodicNode} decorator * @@ -102,9 +103,6 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] qEncoder2D = {{0.25}}; Q_encoder = new Matrix(qEncoder2D); - double[][] qImu2D = {{ 0 }}; - Q_imu = new Matrix(qImu2D); - double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, @@ -117,31 +115,14 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { }; C_encoder = new Matrix(cEncoder2D); - double[][] cImu2D = { - { 0, 0, 0, 1, 0 }, - }; - C_imu = new Matrix(cImu2D); - // add all our subscribers for our current state update stream // Every time we get a new sensor update, trigger the new kalman update setupGPSSubscriber(); setupEncoderSubscriber(); setupWheelSubscriber(); - setupIMUSubscriber(); resume(); } - private void setupIMUSubscriber() { - new Subscriber("KF Localizer", NodeChannel.IMU_COMPASS.getMsgPath(), (topicName, m) -> { - double heading = ((IMUCompassMessage) m).getCompassHeading(); - - double[][] z2D = {{ heading }}; - Matrix z = new Matrix(z2D); - - kalmanFilter(C_imu, Q_imu, z); - }); - } - private void setupEncoderSubscriber() { new Subscriber("KF Localizer", NodeChannel.ENCODER.getMsgPath(), ((topicName, m) -> { long currentTime = new Date().getTime(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index a225d995..2546c367 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -66,7 +66,7 @@ public double getCommandedSteeringAngle() { return 17433504; //A dummy value that we can never get } - double lookahead = 5; //meters + double lookahead = 15; //meters //pick the first point that is at least lookahead away, then point buggy toward it int targetIndex = closestIndex; double distanceFromMessage = 0; From c371f0e8d3a04058c972b85f63eac87154bba3b5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 25 Feb 2017 17:42:47 -0500 Subject: [PATCH 073/149] added current commanded angle to the GUI printout --- .../nodes/planners/WayPointFollowerPlanner.java | 7 +++++-- .../src/main/java/com/roboclub/robobuggy/ui/Map.java | 12 ++++++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 2546c367..3c074f04 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -24,6 +24,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { * REMOVE FOR PROD PUSH */ public static GpsMeasurement currentWaypoint = new GpsMeasurement(0, 0); + public static double currentCommandedAngle = 0.0; /** * @param wayPoints the list of waypoints to follow @@ -100,8 +101,10 @@ public double getCommandedSteeringAngle() { double param2 = 0.8 * pose.getCurrentState().get(2, 0); deltaHeading = Math.atan2(param1, param2); - //PD control of DC steering motor handled by low level - return Util.normalizeAngleRad(deltaHeading); + //PD control of DC steering motor handled by low level + double commandedAngle = Util.normalizeAngleRad(deltaHeading); + currentCommandedAngle = commandedAngle; + return commandedAngle; } @Override diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index 8f7ce3c9..97d076d2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -51,6 +51,7 @@ public class Map extends JPanel { private static final int MAX_POINT_BUF_SIZE = 3000; private MapMarkerDot currentWaypoint = new MapMarkerDot(0, 0); + private MapPolygonImpl currentHeading = new MapPolygonImpl(); /** * initializes a new Map with cache loaded @@ -62,6 +63,7 @@ public Map() { currentWaypoint.setColor(Color.BLUE); getMapTree().getViewer().addMapMarker(currentWaypoint); + getMapTree().getViewer().addMapPolygon(currentHeading); //adds track buggy new Subscriber("Map", NodeChannel.POSE.getMsgPath(), new MessageListener() { @@ -79,6 +81,16 @@ public void actionPerformed(String topicName, Message m) { currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); getMapTree().getViewer().addMapMarker(currentWaypoint); + getMapTree().getViewer().removeMapPolygon(currentHeading); + currentHeading = new MapPolygonImpl( + new Coordinate(WayPointFollowerPlanner.currentWaypoint.getLatitude(), WayPointFollowerPlanner.currentWaypoint.getLongitude()), + new Coordinate(WayPointFollowerPlanner.currentWaypoint.getLatitude() + 0.001 * Math.sin(WayPointFollowerPlanner + .currentCommandedAngle), WayPointFollowerPlanner.currentWaypoint.getLongitude() + 0.001 * Math.cos(WayPointFollowerPlanner + .currentCommandedAngle)), + new Coordinate(WayPointFollowerPlanner.currentWaypoint.getLatitude(), WayPointFollowerPlanner.currentWaypoint.getLongitude()) + ); + getMapTree().getViewer().addMapPolygon(currentHeading); + } }); From f417a5b44d9e0b605698df9bf1e3b367a352f091 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 25 Feb 2017 17:54:56 -0500 Subject: [PATCH 074/149] fixed issues --- .../java/com/roboclub/robobuggy/ui/Map.java | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index 97d076d2..a8a172a7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -51,7 +51,7 @@ public class Map extends JPanel { private static final int MAX_POINT_BUF_SIZE = 3000; private MapMarkerDot currentWaypoint = new MapMarkerDot(0, 0); - private MapPolygonImpl currentHeading = new MapPolygonImpl(); + private MapPolygonImpl currentHeadingMapObj = new MapPolygonImpl(); /** * initializes a new Map with cache loaded @@ -63,7 +63,7 @@ public Map() { currentWaypoint.setColor(Color.BLUE); getMapTree().getViewer().addMapMarker(currentWaypoint); - getMapTree().getViewer().addMapPolygon(currentHeading); + getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); //adds track buggy new Subscriber("Map", NodeChannel.POSE.getMsgPath(), new MessageListener() { @@ -81,15 +81,16 @@ public void actionPerformed(String topicName, Message m) { currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); getMapTree().getViewer().addMapMarker(currentWaypoint); - getMapTree().getViewer().removeMapPolygon(currentHeading); - currentHeading = new MapPolygonImpl( - new Coordinate(WayPointFollowerPlanner.currentWaypoint.getLatitude(), WayPointFollowerPlanner.currentWaypoint.getLongitude()), - new Coordinate(WayPointFollowerPlanner.currentWaypoint.getLatitude() + 0.001 * Math.sin(WayPointFollowerPlanner - .currentCommandedAngle), WayPointFollowerPlanner.currentWaypoint.getLongitude() + 0.001 * Math.cos(WayPointFollowerPlanner - .currentCommandedAngle)), - new Coordinate(WayPointFollowerPlanner.currentWaypoint.getLatitude(), WayPointFollowerPlanner.currentWaypoint.getLongitude()) + double currentHeading = gpsM.getCurrentState().get(3, 0); + getMapTree().getViewer().removeMapPolygon(currentHeadingMapObj); + currentHeadingMapObj = new MapPolygonImpl( + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), + new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(WayPointFollowerPlanner + .currentCommandedAngle + currentHeading), gpsM.getLongitude() + 0.0001 * Math.cos(WayPointFollowerPlanner + .currentCommandedAngle + currentHeading)), + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) ); - getMapTree().getViewer().addMapPolygon(currentHeading); + getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); } }); From f7f0b665b1aa1a2647aa8120be52098dcdbf35f5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 25 Feb 2017 18:50:24 -0500 Subject: [PATCH 075/149] added more info to gui, disabled fancy steering calculations and just went with idealized buggy model --- .../planners/WayPointFollowerPlanner.java | 9 ++++--- .../java/com/roboclub/robobuggy/ui/Map.java | 26 ++++++++++++++++++- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 3c074f04..98a4e9a8 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -6,7 +6,6 @@ import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; -import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.ros.NodeChannel; import java.util.ArrayList; @@ -25,6 +24,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { */ public static GpsMeasurement currentWaypoint = new GpsMeasurement(0, 0); public static double currentCommandedAngle = 0.0; + public static double currentDesiredHeading = 0.0; /** * @param wayPoints the list of waypoints to follow @@ -91,15 +91,16 @@ public double getCommandedSteeringAngle() { double deltaLatMeters = LocalizerUtil.convertLatToMeters(deltaLat); double deltaLongMeters = LocalizerUtil.convertLonToMeters(deltaLong); double desiredHeading = Math.atan2(deltaLatMeters, deltaLongMeters); + currentDesiredHeading = desiredHeading; //find the angle we need to reach that point double poseHeading = pose.getHeading(); double deltaHeading = desiredHeading - poseHeading; //Pure Pursuit steering controller - double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); - double param2 = 0.8 * pose.getCurrentState().get(2, 0); - deltaHeading = Math.atan2(param1, param2); +// double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); +// double param2 = 0.8 * pose.getCurrentState().get(2, 0); +// deltaHeading = Math.atan2(param1, param2); //PD control of DC steering motor handled by low level double commandedAngle = Util.normalizeAngleRad(deltaHeading); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index a8a172a7..cb57b2d1 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -51,7 +51,9 @@ public class Map extends JPanel { private static final int MAX_POINT_BUF_SIZE = 3000; private MapMarkerDot currentWaypoint = new MapMarkerDot(0, 0); + private MapPolygonImpl currentSteeringCommandMapObj = new MapPolygonImpl(); private MapPolygonImpl currentHeadingMapObj = new MapPolygonImpl(); + private MapPolygonImpl desiredHeadingMapObj = new MapPolygonImpl(); /** * initializes a new Map with cache loaded @@ -63,7 +65,9 @@ public Map() { currentWaypoint.setColor(Color.BLUE); getMapTree().getViewer().addMapMarker(currentWaypoint); + getMapTree().getViewer().addMapPolygon(currentSteeringCommandMapObj); getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); + getMapTree().getViewer().addMapPolygon(desiredHeadingMapObj); //adds track buggy new Subscriber("Map", NodeChannel.POSE.getMsgPath(), new MessageListener() { @@ -81,16 +85,36 @@ public void actionPerformed(String topicName, Message m) { currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); getMapTree().getViewer().addMapMarker(currentWaypoint); + getMapTree().getViewer().removeMapPolygon(desiredHeadingMapObj); + desiredHeadingMapObj = new MapPolygonImpl( + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), + new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(WayPointFollowerPlanner.currentDesiredHeading), gpsM.getLongitude() + 0.0001 * + Math.cos(WayPointFollowerPlanner.currentDesiredHeading)), + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) + ); + desiredHeadingMapObj.setColor(Color.GREEN); + getMapTree().getViewer().addMapPolygon(desiredHeadingMapObj); + double currentHeading = gpsM.getCurrentState().get(3, 0); getMapTree().getViewer().removeMapPolygon(currentHeadingMapObj); currentHeadingMapObj = new MapPolygonImpl( + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), + new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(currentHeading), gpsM.getLongitude() + 0.0001 * Math.cos(currentHeading)), + new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) + ); + currentHeadingMapObj.setColor(Color.RED); + getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); + + getMapTree().getViewer().removeMapPolygon(currentSteeringCommandMapObj); + currentSteeringCommandMapObj = new MapPolygonImpl( new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(WayPointFollowerPlanner .currentCommandedAngle + currentHeading), gpsM.getLongitude() + 0.0001 * Math.cos(WayPointFollowerPlanner .currentCommandedAngle + currentHeading)), new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) ); - getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); + currentSteeringCommandMapObj.setColor(Color.BLUE); + getMapTree().getViewer().addMapPolygon(currentSteeringCommandMapObj); } }); From 87b28acbba16918cf28b858df845618ce6d61d91 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sat, 25 Feb 2017 22:08:04 -0500 Subject: [PATCH 076/149] testing new controller sim --- offline/controller/controller_pure.m | 2 +- offline/controller/controller_pure_cont.m | 103 ++- offline/controller/get_logs.py | 4 +- offline/controller/waypoints_course_v2.txt | 733 +++++++++++++++++++++ 4 files changed, 810 insertions(+), 32 deletions(-) create mode 100644 offline/controller/waypoints_course_v2.txt diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m index f68a1ce2..312f0987 100644 --- a/offline/controller/controller_pure.m +++ b/offline/controller/controller_pure.m @@ -129,7 +129,7 @@ deltaPath = target - pos; k = 0.8; - a = atan2(deltaPath(2), deltaPath(1))-X(4); + a = atan2(deltaPath(2), deltaPath(1))-X(4); % incorrect u = atan2(2*wheel_base*sin(a), k*X(3)); end u = clampSteeringAngle(clampAngle(u)); diff --git a/offline/controller/controller_pure_cont.m b/offline/controller/controller_pure_cont.m index f68a1ce2..f5fea804 100644 --- a/offline/controller/controller_pure_cont.m +++ b/offline/controller/controller_pure_cont.m @@ -7,17 +7,19 @@ global velocity global steering_vel global dt + global last_closest_idx save_data = true; wheel_base = 1.13; utm_zone = 17; first_heading = deg2rad(250); - lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; + lat_long = [40.441670, -79.9416362]; % tri [40.442867, -79.9427395]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 8; % m/s, 17.9mph, forward velocity + velocity = 3.6; % m/s, 8mph, forward velocity steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds % full range in 0.5s + last_closest_idx = 1; [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -27,10 +29,10 @@ first_heading; % heading, rad, world frame 0]; % d_heading, rad/s - load('./waypoints_tri.mat'); + load('./waypoints_course_v2.mat'); desired_traj = processWaypoints(logs); % time = linspace(0, 240, size(trajectory,2)); - time = 0:dt:60; % 240; + time = 0:dt:240; u = 0; % commanded steering angle steering = u; % steering angle trajectory = [X; lat_long(1); lat_long(2); steering]; @@ -54,7 +56,7 @@ end if save_data - save('controller_tri_v1.mat', 'trajectory'); + save('controller_v3.mat', 'trajectory'); end end @@ -90,11 +92,11 @@ end function a = clampSteeringAngle(a) - if(a < -deg2rad(10)) - a = -deg2rad(10); - end - if(a > deg2rad(10)) - a = deg2rad(10); + b = deg2rad(10); + if(a < -b) + a = -b; + elseif(a > b) + a = b; end end @@ -111,27 +113,70 @@ function [u] = control(desired_traj, X) global wheel_base + global last_closest_idx + k = 3; + vel = X(3); pos = X(1:2)'; - pos_b = repmat(pos, size(desired_traj, 1), 1); - delta = 15; - cutoff = 100; - delta = delta * delta; - - distances = sum((desired_traj - pos_b).^2, 2); - [~, closest_idx] = min(distances); - last_idx = min([length(distances), closest_idx + cutoff]); - possible = find(distances(1:last_idx) < delta); - if isempty(possible) - u = 0; - else - target = desired_traj(possible(end), :); - deltaPath = target - pos; - - k = 0.8; - a = atan2(deltaPath(2), deltaPath(1))-X(4); - u = atan2(2*wheel_base*sin(a), k*X(3)); + lookahead_bounds = [3 25]; + lookahead = k * vel; + if(lookahead < lookahead_bounds(1)) + lookahead = lookahead_bounds(1); + elseif(lookahead > lookahead_bounds(2)) + lookahead = lookahead_bounds(1); + end + + % closest_idx = last_closest_idx; + % min_dist = 100000; + % for k=last_closest_idx:(length(desired_mid) - 1) + % distp = norm(desired_mid(k,:) - X); + % if(distp < min_dist) + % min_dist = distp; + % closest_idx = k; + % end + % % cut off search somehow + % end + + % ptA = [0 0]; + % if(closest_idx == 1) + % ptA = X; + % else + % ptA = desired_traj(closest_idx, :); + % end + % ptB = desired_traj(closest_idx+1, :); + % crosstrack_error = abs(det([ptB - ptA; X - ptA])) / norm(ptB - ptA); + + closest_idx = last_closest_idx; + min_dist = 100000; + for k=last_closest_idx:length(desired_traj) + distp = norm(desired_traj(k,:) - X); + if(distp < min_dist) + min_dist = distp; + closest_idx = k; + end + % cut off search somehow + end + + lookahead_idx = 0; + for k=closest_idx:length(desired_traj) + distp = norm(desired_traj(k,:) - X); + if(distp > lookahead) + lookahead_idx = k; + break; + end end - u = clampSteeringAngle(clampAngle(u)); + + if(lookahead_idx == 0) + u = 0; + else + deltaPath = desired_traj(lookahead_idx,:) - pos; + a = atan2(deltaPath(2), deltaPath(1)) - (pi/2); + u = atan2(2 * wheel_base * sin(a), lookahead); + end + + % maybe consider deadband region + + last_closest_idx = closest_idx; + u = clampSteeringAngle(u); end diff --git a/offline/controller/get_logs.py b/offline/controller/get_logs.py index 321ef013..61fc811d 100644 --- a/offline/controller/get_logs.py +++ b/offline/controller/get_logs.py @@ -6,7 +6,7 @@ combined = True d = None -file = 'waypoints_tri.txt' +file = 'waypoints_course_v2.txt' logs = [] with open(file) as json_data: for line in json_data: @@ -19,4 +19,4 @@ logs = np.array(logs) print(logs.shape) vars_map = {'logs': logs} -scipy.io.savemat('./waypoints_tri.mat', mdict=vars_map) +scipy.io.savemat('./waypoints_course_v2.mat', mdict=vars_map) diff --git a/offline/controller/waypoints_course_v2.txt b/offline/controller/waypoints_course_v2.txt new file mode 100644 index 00000000..737889f0 --- /dev/null +++ b/offline/controller/waypoints_course_v2.txt @@ -0,0 +1,733 @@ +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:21 AM","latitude":40.44164466666667,"north":true,"longitude":-79.9415915,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49868,"rawGPSLong":7956.49549,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433081768,"topicName":"sensors/gps","sequenceNumber":40} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:22 AM","latitude":40.4416445,"north":true,"longitude":-79.94159133333334,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49867,"rawGPSLong":7956.49548,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433082284,"topicName":"sensors/gps","sequenceNumber":41} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:22 AM","latitude":40.4416445,"north":true,"longitude":-79.94159166666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49867,"rawGPSLong":7956.4955,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433082773,"topicName":"sensors/gps","sequenceNumber":42} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:23 AM","latitude":40.44164416666667,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49865,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433083267,"topicName":"sensors/gps","sequenceNumber":43} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:23 AM","latitude":40.441643666666664,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49862,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433083786,"topicName":"sensors/gps","sequenceNumber":44} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:24 AM","latitude":40.44164333333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.4986,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433084267,"topicName":"sensors/gps","sequenceNumber":45} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:24 AM","latitude":40.441643,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.3,"rawGPSLat":4026.49858,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433084782,"topicName":"sensors/gps","sequenceNumber":46} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:25 AM","latitude":40.441642333333334,"north":true,"longitude":-79.94159283333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.1,"rawGPSLat":4026.49854,"rawGPSLong":7956.49557,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433085280,"topicName":"sensors/gps","sequenceNumber":47} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:25 AM","latitude":40.441641833333335,"north":true,"longitude":-79.94159316666666,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.0,"rawGPSLat":4026.49851,"rawGPSLong":7956.49559,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433085772,"topicName":"sensors/gps","sequenceNumber":48} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:26 AM","latitude":40.4416415,"north":true,"longitude":-79.941593,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49849,"rawGPSLong":7956.49558,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433086270,"topicName":"sensors/gps","sequenceNumber":49} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:26 AM","latitude":40.44164116666666,"north":true,"longitude":-79.94159266666666,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49847,"rawGPSLong":7956.49556,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433086794,"topicName":"sensors/gps","sequenceNumber":50} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:27 AM","latitude":40.441640666666665,"north":true,"longitude":-79.9415925,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49844,"rawGPSLong":7956.49555,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433087293,"topicName":"sensors/gps","sequenceNumber":51} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:27 AM","latitude":40.4416405,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49843,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433087782,"topicName":"sensors/gps","sequenceNumber":52} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:28 AM","latitude":40.4416405,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.1,"rawGPSLat":4026.49843,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433088270,"topicName":"sensors/gps","sequenceNumber":53} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:28 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.0,"rawGPSLat":4026.49842,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433088779,"topicName":"sensors/gps","sequenceNumber":54} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:29 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.8,"rawGPSLat":4026.49842,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433089283,"topicName":"sensors/gps","sequenceNumber":55} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:29 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.7,"rawGPSLat":4026.49842,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433089780,"topicName":"sensors/gps","sequenceNumber":56} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:30 AM","latitude":40.44164033333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.7,"rawGPSLat":4026.49842,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433090275,"topicName":"sensors/gps","sequenceNumber":57} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:30 AM","latitude":40.44164033333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.6,"rawGPSLat":4026.49842,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433090779,"topicName":"sensors/gps","sequenceNumber":58} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:31 AM","latitude":40.44164033333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.5,"rawGPSLat":4026.49842,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433091283,"topicName":"sensors/gps","sequenceNumber":59} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:31 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.4,"rawGPSLat":4026.49841,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433091790,"topicName":"sensors/gps","sequenceNumber":60} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:32 AM","latitude":40.441640166666666,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.2,"rawGPSLat":4026.49841,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433092290,"topicName":"sensors/gps","sequenceNumber":61} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:32 AM","latitude":40.441640166666666,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.0,"rawGPSLat":4026.49841,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433092787,"topicName":"sensors/gps","sequenceNumber":62} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:33 AM","latitude":40.441640166666666,"north":true,"longitude":-79.94159283333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.7,"rawGPSLat":4026.49841,"rawGPSLong":7956.49557,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433093285,"topicName":"sensors/gps","sequenceNumber":63} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:33 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159316666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.5,"rawGPSLat":4026.49842,"rawGPSLong":7956.49559,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433093787,"topicName":"sensors/gps","sequenceNumber":64} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:34 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159383333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.2,"rawGPSLat":4026.49842,"rawGPSLong":7956.49563,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433094289,"topicName":"sensors/gps","sequenceNumber":65} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:34 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159383333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.0,"rawGPSLat":4026.49844,"rawGPSLong":7956.49563,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433094780,"topicName":"sensors/gps","sequenceNumber":66} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:35 AM","latitude":40.4416405,"north":true,"longitude":-79.94159433333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.8,"rawGPSLat":4026.49843,"rawGPSLong":7956.49566,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433095282,"topicName":"sensors/gps","sequenceNumber":67} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:35 AM","latitude":40.4416405,"north":true,"longitude":-79.94159466666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.7,"rawGPSLat":4026.49843,"rawGPSLong":7956.49568,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433095780,"topicName":"sensors/gps","sequenceNumber":68} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:36 AM","latitude":40.441640166666666,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.5,"rawGPSLat":4026.49841,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433096281,"topicName":"sensors/gps","sequenceNumber":69} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:36 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.3,"rawGPSLat":4026.49841,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433096790,"topicName":"sensors/gps","sequenceNumber":70} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:37 AM","latitude":40.441640166666666,"north":true,"longitude":-79.9415965,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.2,"rawGPSLat":4026.49841,"rawGPSLong":7956.49579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433097291,"topicName":"sensors/gps","sequenceNumber":71} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:37 AM","latitude":40.44164,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.1,"rawGPSLat":4026.4984,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433097795,"topicName":"sensors/gps","sequenceNumber":72} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:38 AM","latitude":40.44164,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.0,"rawGPSLat":4026.4984,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433098306,"topicName":"sensors/gps","sequenceNumber":73} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:38 AM","latitude":40.44163983333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.9,"rawGPSLat":4026.49839,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433098793,"topicName":"sensors/gps","sequenceNumber":74} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:39 AM","latitude":40.44164,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.9,"rawGPSLat":4026.4984,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433099280,"topicName":"sensors/gps","sequenceNumber":75} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:39 AM","latitude":40.441640166666666,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49841,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433099773,"topicName":"sensors/gps","sequenceNumber":76} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:40 AM","latitude":40.4416405,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49843,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433100275,"topicName":"sensors/gps","sequenceNumber":77} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:40 AM","latitude":40.441640666666665,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49844,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433100771,"topicName":"sensors/gps","sequenceNumber":78} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:41 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.7,"rawGPSLat":4026.49845,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433101290,"topicName":"sensors/gps","sequenceNumber":79} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:41 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.7,"rawGPSLat":4026.49845,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433101789,"topicName":"sensors/gps","sequenceNumber":80} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:42 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433102281,"topicName":"sensors/gps","sequenceNumber":81} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:42 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433102776,"topicName":"sensors/gps","sequenceNumber":82} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:43 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433103289,"topicName":"sensors/gps","sequenceNumber":83} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:43 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433103810,"topicName":"sensors/gps","sequenceNumber":84} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:44 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.49845,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433104289,"topicName":"sensors/gps","sequenceNumber":85} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:44 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.49844,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433104790,"topicName":"sensors/gps","sequenceNumber":86} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:45 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49844,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433105283,"topicName":"sensors/gps","sequenceNumber":87} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:45 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49844,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433105789,"topicName":"sensors/gps","sequenceNumber":88} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:46 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49845,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433106294,"topicName":"sensors/gps","sequenceNumber":89} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:46 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49844,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433106795,"topicName":"sensors/gps","sequenceNumber":90} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:47 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49844,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433107286,"topicName":"sensors/gps","sequenceNumber":91} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:47 AM","latitude":40.441640666666665,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49844,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433107780,"topicName":"sensors/gps","sequenceNumber":92} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:48 AM","latitude":40.4416405,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49843,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433108293,"topicName":"sensors/gps","sequenceNumber":93} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:48 AM","latitude":40.4416405,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49843,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433108786,"topicName":"sensors/gps","sequenceNumber":94} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:49 AM","latitude":40.4416405,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49843,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433109284,"topicName":"sensors/gps","sequenceNumber":95} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:49 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49841,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433109795,"topicName":"sensors/gps","sequenceNumber":96} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:50 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.49841,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433110282,"topicName":"sensors/gps","sequenceNumber":97} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:50 AM","latitude":40.44164,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.4984,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433110782,"topicName":"sensors/gps","sequenceNumber":98} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:51 AM","latitude":40.44163983333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49839,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433111285,"topicName":"sensors/gps","sequenceNumber":99} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:51 AM","latitude":40.44163966666667,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.7,"rawGPSLat":4026.49838,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433111791,"topicName":"sensors/gps","sequenceNumber":100} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:52 AM","latitude":40.44163966666667,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49838,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433112288,"topicName":"sensors/gps","sequenceNumber":101} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:52 AM","latitude":40.44163983333333,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.9,"rawGPSLat":4026.49839,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433112797,"topicName":"sensors/gps","sequenceNumber":102} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:53 AM","latitude":40.4416395,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.0,"rawGPSLat":4026.49837,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433113289,"topicName":"sensors/gps","sequenceNumber":103} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:53 AM","latitude":40.441639333333335,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.1,"rawGPSLat":4026.49836,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433113796,"topicName":"sensors/gps","sequenceNumber":104} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:54 AM","latitude":40.44163916666667,"north":true,"longitude":-79.94159766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.3,"rawGPSLat":4026.49835,"rawGPSLong":7956.49586,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433114290,"topicName":"sensors/gps","sequenceNumber":105} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:54 AM","latitude":40.44163916666667,"north":true,"longitude":-79.94159766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.3,"rawGPSLat":4026.49835,"rawGPSLong":7956.49586,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433114791,"topicName":"sensors/gps","sequenceNumber":106} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:55 AM","latitude":40.44163916666667,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.4,"rawGPSLat":4026.49835,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433115284,"topicName":"sensors/gps","sequenceNumber":107} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:55 AM","latitude":40.44163916666667,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.5,"rawGPSLat":4026.49835,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433115798,"topicName":"sensors/gps","sequenceNumber":108} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:56 AM","latitude":40.4416395,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.7,"rawGPSLat":4026.49837,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433116288,"topicName":"sensors/gps","sequenceNumber":109} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:56 AM","latitude":40.44163983333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.8,"rawGPSLat":4026.49839,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433116788,"topicName":"sensors/gps","sequenceNumber":110} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:57 AM","latitude":40.44164,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.9,"rawGPSLat":4026.4984,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433117288,"topicName":"sensors/gps","sequenceNumber":111} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:57 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.1,"rawGPSLat":4026.49841,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433117789,"topicName":"sensors/gps","sequenceNumber":112} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:58 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159666666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.2,"rawGPSLat":4026.49842,"rawGPSLong":7956.4958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433118281,"topicName":"sensors/gps","sequenceNumber":113} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:58 AM","latitude":40.4416405,"north":true,"longitude":-79.9415965,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.3,"rawGPSLat":4026.49843,"rawGPSLong":7956.49579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433118776,"topicName":"sensors/gps","sequenceNumber":114} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:59 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159616666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.4,"rawGPSLat":4026.49844,"rawGPSLong":7956.49577,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433119296,"topicName":"sensors/gps","sequenceNumber":115} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:59 AM","latitude":40.441641,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.5,"rawGPSLat":4026.49846,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433119789,"topicName":"sensors/gps","sequenceNumber":116} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:00 AM","latitude":40.44164116666666,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.6,"rawGPSLat":4026.49847,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433120296,"topicName":"sensors/gps","sequenceNumber":117} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:00 AM","latitude":40.44164133333334,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.6,"rawGPSLat":4026.49848,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433120788,"topicName":"sensors/gps","sequenceNumber":118} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:01 AM","latitude":40.44164166666667,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.7,"rawGPSLat":4026.4985,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433121290,"topicName":"sensors/gps","sequenceNumber":119} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:01 AM","latitude":40.441641833333335,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.7,"rawGPSLat":4026.49851,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433121820,"topicName":"sensors/gps","sequenceNumber":120} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:02 AM","latitude":40.44164216666667,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.8,"rawGPSLat":4026.49853,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433122287,"topicName":"sensors/gps","sequenceNumber":121} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:02 AM","latitude":40.441642333333334,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.9,"rawGPSLat":4026.49854,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433122787,"topicName":"sensors/gps","sequenceNumber":122} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:03 AM","latitude":40.44164266666667,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.9,"rawGPSLat":4026.49856,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433123282,"topicName":"sensors/gps","sequenceNumber":123} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:03 AM","latitude":40.441643166666665,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.0,"rawGPSLat":4026.49859,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433123788,"topicName":"sensors/gps","sequenceNumber":124} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:04 AM","latitude":40.4416435,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.0,"rawGPSLat":4026.49861,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433124290,"topicName":"sensors/gps","sequenceNumber":125} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:04 AM","latitude":40.44164383333333,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.1,"rawGPSLat":4026.49863,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433124782,"topicName":"sensors/gps","sequenceNumber":126} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:05 AM","latitude":40.441644,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.1,"rawGPSLat":4026.49864,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433125276,"topicName":"sensors/gps","sequenceNumber":127} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:05 AM","latitude":40.441644333333336,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.1,"rawGPSLat":4026.49866,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433125789,"topicName":"sensors/gps","sequenceNumber":128} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:06 AM","latitude":40.44164466666667,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.2,"rawGPSLat":4026.49868,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433126289,"topicName":"sensors/gps","sequenceNumber":129} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:06 AM","latitude":40.441645,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.2,"rawGPSLat":4026.4987,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433126790,"topicName":"sensors/gps","sequenceNumber":130} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:07 AM","latitude":40.441645333333334,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.3,"rawGPSLat":4026.49872,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433127291,"topicName":"sensors/gps","sequenceNumber":131} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:07 AM","latitude":40.44164583333333,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.3,"rawGPSLat":4026.49875,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433127788,"topicName":"sensors/gps","sequenceNumber":132} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:08 AM","latitude":40.441646166666665,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.4,"rawGPSLat":4026.49877,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433128292,"topicName":"sensors/gps","sequenceNumber":133} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:08 AM","latitude":40.441646666666664,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.4,"rawGPSLat":4026.4988,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433128789,"topicName":"sensors/gps","sequenceNumber":134} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:09 AM","latitude":40.441647,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.5,"rawGPSLat":4026.49882,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433129274,"topicName":"sensors/gps","sequenceNumber":135} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:09 AM","latitude":40.441647333333336,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.5,"rawGPSLat":4026.49884,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433129774,"topicName":"sensors/gps","sequenceNumber":136} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:10 AM","latitude":40.441647833333334,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.6,"rawGPSLat":4026.49887,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433130291,"topicName":"sensors/gps","sequenceNumber":137} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:10 AM","latitude":40.44164816666667,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.7,"rawGPSLat":4026.49889,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433130782,"topicName":"sensors/gps","sequenceNumber":138} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:11 AM","latitude":40.4416485,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.8,"rawGPSLat":4026.49891,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433131274,"topicName":"sensors/gps","sequenceNumber":139} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:11 AM","latitude":40.44164883333333,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.8,"rawGPSLat":4026.49893,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433131789,"topicName":"sensors/gps","sequenceNumber":140} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:12 AM","latitude":40.441649,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.9,"rawGPSLat":4026.49894,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433132287,"topicName":"sensors/gps","sequenceNumber":141} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:12 AM","latitude":40.44164933333333,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.9,"rawGPSLat":4026.49896,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433132789,"topicName":"sensors/gps","sequenceNumber":142} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:13 AM","latitude":40.4416495,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.0,"rawGPSLat":4026.49897,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433133282,"topicName":"sensors/gps","sequenceNumber":143} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:13 AM","latitude":40.44164983333334,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.0,"rawGPSLat":4026.49899,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433133785,"topicName":"sensors/gps","sequenceNumber":144} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:14 AM","latitude":40.44165,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.1,"rawGPSLat":4026.499,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433134289,"topicName":"sensors/gps","sequenceNumber":145} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:14 AM","latitude":40.44165016666667,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.1,"rawGPSLat":4026.49901,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433134788,"topicName":"sensors/gps","sequenceNumber":146} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:15 AM","latitude":40.441650333333335,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.2,"rawGPSLat":4026.49902,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433135286,"topicName":"sensors/gps","sequenceNumber":147} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:15 AM","latitude":40.4416505,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.2,"rawGPSLat":4026.49903,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433135784,"topicName":"sensors/gps","sequenceNumber":148} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:16 AM","latitude":40.44165066666667,"north":true,"longitude":-79.94159616666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.3,"rawGPSLat":4026.49904,"rawGPSLong":7956.49577,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433136281,"topicName":"sensors/gps","sequenceNumber":149} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:16 AM","latitude":40.44165066666667,"north":true,"longitude":-79.94159633333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.3,"rawGPSLat":4026.49904,"rawGPSLong":7956.49578,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433136785,"topicName":"sensors/gps","sequenceNumber":150} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:17 AM","latitude":40.441650833333334,"north":true,"longitude":-79.9415965,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.3,"rawGPSLat":4026.49905,"rawGPSLong":7956.49579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433137275,"topicName":"sensors/gps","sequenceNumber":151} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:17 AM","latitude":40.441651,"north":true,"longitude":-79.94159666666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.4,"rawGPSLat":4026.49906,"rawGPSLong":7956.4958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433137778,"topicName":"sensors/gps","sequenceNumber":152} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:18 AM","latitude":40.44165066666667,"north":true,"longitude":-79.94159683333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.5,"rawGPSLat":4026.49904,"rawGPSLong":7956.49581,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433138280,"topicName":"sensors/gps","sequenceNumber":153} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:18 AM","latitude":40.441646666666664,"north":true,"longitude":-79.9415985,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.5,"rawGPSLat":4026.4988,"rawGPSLong":7956.49591,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433138780,"topicName":"sensors/gps","sequenceNumber":154} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:19 AM","latitude":40.44163916666667,"north":true,"longitude":-79.94160133333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49835,"rawGPSLong":7956.49608,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433139279,"topicName":"sensors/gps","sequenceNumber":155} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:19 AM","latitude":40.44162866666667,"north":true,"longitude":-79.941605,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49772,"rawGPSLong":7956.4963,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433139797,"topicName":"sensors/gps","sequenceNumber":156} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:20 AM","latitude":40.4416165,"north":true,"longitude":-79.94161,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49699,"rawGPSLong":7956.4966,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433140289,"topicName":"sensors/gps","sequenceNumber":157} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:20 AM","latitude":40.44160316666667,"north":true,"longitude":-79.94161616666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49619,"rawGPSLong":7956.49697,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433140790,"topicName":"sensors/gps","sequenceNumber":158} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:21 AM","latitude":40.441588333333335,"north":true,"longitude":-79.941623,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.4953,"rawGPSLong":7956.49738,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433141287,"topicName":"sensors/gps","sequenceNumber":159} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:21 AM","latitude":40.44157283333333,"north":true,"longitude":-79.941629,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49437,"rawGPSLong":7956.49774,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433141792,"topicName":"sensors/gps","sequenceNumber":160} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:22 AM","latitude":40.44155583333333,"north":true,"longitude":-79.94163616666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.7,"rawGPSLat":4026.49335,"rawGPSLong":7956.49817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433142284,"topicName":"sensors/gps","sequenceNumber":161} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:22 AM","latitude":40.4415395,"north":true,"longitude":-79.94164433333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.49237,"rawGPSLong":7956.49866,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433142778,"topicName":"sensors/gps","sequenceNumber":162} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:23 AM","latitude":40.44152283333333,"north":true,"longitude":-79.94165233333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.49137,"rawGPSLong":7956.49914,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433143289,"topicName":"sensors/gps","sequenceNumber":163} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:23 AM","latitude":40.4415035,"north":true,"longitude":-79.94165816666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.49021,"rawGPSLong":7956.49949,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433143785,"topicName":"sensors/gps","sequenceNumber":164} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:24 AM","latitude":40.44148583333333,"north":true,"longitude":-79.94166333333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.48915,"rawGPSLong":7956.4998,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433144273,"topicName":"sensors/gps","sequenceNumber":165} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:24 AM","latitude":40.441469166666664,"north":true,"longitude":-79.9416695,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.0,"rawGPSLat":4026.48815,"rawGPSLong":7956.50017,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433144779,"topicName":"sensors/gps","sequenceNumber":166} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:25 AM","latitude":40.441450833333334,"north":true,"longitude":-79.9416785,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.1,"rawGPSLat":4026.48705,"rawGPSLong":7956.50071,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433145290,"topicName":"sensors/gps","sequenceNumber":167} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:25 AM","latitude":40.44143366666667,"north":true,"longitude":-79.941686,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.2,"rawGPSLat":4026.48602,"rawGPSLong":7956.50116,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433145798,"topicName":"sensors/gps","sequenceNumber":168} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:26 AM","latitude":40.441416833333335,"north":true,"longitude":-79.94169283333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.3,"rawGPSLat":4026.48501,"rawGPSLong":7956.50157,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433146293,"topicName":"sensors/gps","sequenceNumber":169} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:26 AM","latitude":40.44139833333333,"north":true,"longitude":-79.9417005,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.4,"rawGPSLat":4026.4839,"rawGPSLong":7956.50203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433146790,"topicName":"sensors/gps","sequenceNumber":170} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:27 AM","latitude":40.44138133333333,"north":true,"longitude":-79.94170766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.5,"rawGPSLat":4026.48288,"rawGPSLong":7956.50246,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433147290,"topicName":"sensors/gps","sequenceNumber":171} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:27 AM","latitude":40.441364,"north":true,"longitude":-79.941714,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.7,"rawGPSLat":4026.48184,"rawGPSLong":7956.50284,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433147794,"topicName":"sensors/gps","sequenceNumber":172} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:28 AM","latitude":40.44134533333333,"north":true,"longitude":-79.94172333333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.7,"rawGPSLat":4026.48072,"rawGPSLong":7956.5034,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433148289,"topicName":"sensors/gps","sequenceNumber":173} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:28 AM","latitude":40.441328,"north":true,"longitude":-79.94173083333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.9,"rawGPSLat":4026.47968,"rawGPSLong":7956.50385,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433148790,"topicName":"sensors/gps","sequenceNumber":174} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:29 AM","latitude":40.44131183333333,"north":true,"longitude":-79.94173766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.0,"rawGPSLat":4026.47871,"rawGPSLong":7956.50426,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433149296,"topicName":"sensors/gps","sequenceNumber":175} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:29 AM","latitude":40.44129266666667,"north":true,"longitude":-79.941745,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.2,"rawGPSLat":4026.47756,"rawGPSLong":7956.5047,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433149788,"topicName":"sensors/gps","sequenceNumber":176} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:30 AM","latitude":40.44127483333333,"north":true,"longitude":-79.9417525,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.3,"rawGPSLat":4026.47649,"rawGPSLong":7956.50515,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433150286,"topicName":"sensors/gps","sequenceNumber":177} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:30 AM","latitude":40.441259,"north":true,"longitude":-79.94176083333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.4,"rawGPSLat":4026.47554,"rawGPSLong":7956.50565,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433150787,"topicName":"sensors/gps","sequenceNumber":178} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:31 AM","latitude":40.441241833333336,"north":true,"longitude":-79.94176916666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.6,"rawGPSLat":4026.47451,"rawGPSLong":7956.50615,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433151289,"topicName":"sensors/gps","sequenceNumber":179} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:31 AM","latitude":40.4412245,"north":true,"longitude":-79.941778,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.9,"rawGPSLat":4026.47347,"rawGPSLong":7956.50668,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433151795,"topicName":"sensors/gps","sequenceNumber":180} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:32 AM","latitude":40.441208333333336,"north":true,"longitude":-79.94178583333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.0,"rawGPSLat":4026.4725,"rawGPSLong":7956.50715,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433152287,"topicName":"sensors/gps","sequenceNumber":181} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:32 AM","latitude":40.441191833333335,"north":true,"longitude":-79.94179266666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.1,"rawGPSLat":4026.47151,"rawGPSLong":7956.50756,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433152790,"topicName":"sensors/gps","sequenceNumber":182} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:33 AM","latitude":40.44117333333333,"north":true,"longitude":-79.94179933333334,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.1,"rawGPSLat":4026.4704,"rawGPSLong":7956.50796,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433153289,"topicName":"sensors/gps","sequenceNumber":183} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:33 AM","latitude":40.441156,"north":true,"longitude":-79.941806,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.4,"rawGPSLat":4026.46936,"rawGPSLong":7956.50836,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433153794,"topicName":"sensors/gps","sequenceNumber":184} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:34 AM","latitude":40.441139666666665,"north":true,"longitude":-79.94181216666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.6,"rawGPSLat":4026.46838,"rawGPSLong":7956.50873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433154289,"topicName":"sensors/gps","sequenceNumber":185} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:34 AM","latitude":40.44112116666667,"north":true,"longitude":-79.94182116666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":294.8,"rawGPSLat":4026.46727,"rawGPSLong":7956.50927,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433154788,"topicName":"sensors/gps","sequenceNumber":186} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:35 AM","latitude":40.44110383333334,"north":true,"longitude":-79.94183066666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.0,"rawGPSLat":4026.46623,"rawGPSLong":7956.50984,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433155292,"topicName":"sensors/gps","sequenceNumber":187} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:35 AM","latitude":40.44108816666667,"north":true,"longitude":-79.94183916666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.1,"rawGPSLat":4026.46529,"rawGPSLong":7956.51035,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433155790,"topicName":"sensors/gps","sequenceNumber":188} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:36 AM","latitude":40.441070333333336,"north":true,"longitude":-79.941848,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.4,"rawGPSLat":4026.46422,"rawGPSLong":7956.51088,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433156291,"topicName":"sensors/gps","sequenceNumber":189} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:36 AM","latitude":40.44105183333333,"north":true,"longitude":-79.941856,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.6,"rawGPSLat":4026.46311,"rawGPSLong":7956.51136,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433156792,"topicName":"sensors/gps","sequenceNumber":190} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:37 AM","latitude":40.441035,"north":true,"longitude":-79.94186233333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":296.0,"rawGPSLat":4026.4621,"rawGPSLong":7956.51174,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433157294,"topicName":"sensors/gps","sequenceNumber":191} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:37 AM","latitude":40.441017333333335,"north":true,"longitude":-79.94186883333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":296.1,"rawGPSLat":4026.46104,"rawGPSLong":7956.51213,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433157795,"topicName":"sensors/gps","sequenceNumber":192} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:38 AM","latitude":40.440998666666665,"north":true,"longitude":-79.941876,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":296.5,"rawGPSLat":4026.45992,"rawGPSLong":7956.51256,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433158292,"topicName":"sensors/gps","sequenceNumber":193} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:38 AM","latitude":40.4409815,"north":true,"longitude":-79.94188266666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":296.8,"rawGPSLat":4026.45889,"rawGPSLong":7956.51296,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433158788,"topicName":"sensors/gps","sequenceNumber":194} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:39 AM","latitude":40.440966,"north":true,"longitude":-79.94189016666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":296.9,"rawGPSLat":4026.45796,"rawGPSLong":7956.51341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433159290,"topicName":"sensors/gps","sequenceNumber":195} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:39 AM","latitude":40.4409485,"north":true,"longitude":-79.941899,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":297.1,"rawGPSLat":4026.45691,"rawGPSLong":7956.51394,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433159791,"topicName":"sensors/gps","sequenceNumber":196} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:40 AM","latitude":40.440932,"north":true,"longitude":-79.941907,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.2,"rawGPSLat":4026.45592,"rawGPSLong":7956.51442,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433160291,"topicName":"sensors/gps","sequenceNumber":197} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:40 AM","latitude":40.440917,"north":true,"longitude":-79.94191416666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.3,"rawGPSLat":4026.45502,"rawGPSLong":7956.51485,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433160791,"topicName":"sensors/gps","sequenceNumber":198} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:41 AM","latitude":40.440900666666664,"north":true,"longitude":-79.94192183333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.4,"rawGPSLat":4026.45404,"rawGPSLong":7956.51531,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433161290,"topicName":"sensors/gps","sequenceNumber":199} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:41 AM","latitude":40.44088333333333,"north":true,"longitude":-79.94193033333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.8,"rawGPSLat":4026.453,"rawGPSLong":7956.51582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433161790,"topicName":"sensors/gps","sequenceNumber":200} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:42 AM","latitude":40.44086733333334,"north":true,"longitude":-79.94193833333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.2,"rawGPSLat":4026.45204,"rawGPSLong":7956.5163,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433162293,"topicName":"sensors/gps","sequenceNumber":201} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:42 AM","latitude":40.44085183333333,"north":true,"longitude":-79.9419455,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.4,"rawGPSLat":4026.45111,"rawGPSLong":7956.51673,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433162793,"topicName":"sensors/gps","sequenceNumber":202} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:43 AM","latitude":40.440834,"north":true,"longitude":-79.941953,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.7,"rawGPSLat":4026.45004,"rawGPSLong":7956.51718,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433163291,"topicName":"sensors/gps","sequenceNumber":203} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:43 AM","latitude":40.44081766666667,"north":true,"longitude":-79.941959,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.8,"rawGPSLat":4026.44906,"rawGPSLong":7956.51754,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433163802,"topicName":"sensors/gps","sequenceNumber":204} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:44 AM","latitude":40.4408025,"north":true,"longitude":-79.9419645,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.1,"rawGPSLat":4026.44815,"rawGPSLong":7956.51787,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433164292,"topicName":"sensors/gps","sequenceNumber":205} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:44 AM","latitude":40.44078533333333,"north":true,"longitude":-79.94197116666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44712,"rawGPSLong":7956.51827,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433164797,"topicName":"sensors/gps","sequenceNumber":206} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:45 AM","latitude":40.440769,"north":true,"longitude":-79.94197883333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44614,"rawGPSLong":7956.51873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433165293,"topicName":"sensors/gps","sequenceNumber":207} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:45 AM","latitude":40.4407545,"north":true,"longitude":-79.94198566666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44527,"rawGPSLong":7956.51914,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433165796,"topicName":"sensors/gps","sequenceNumber":208} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:46 AM","latitude":40.44073983333333,"north":true,"longitude":-79.94199333333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.6,"rawGPSLat":4026.44439,"rawGPSLong":7956.5196,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433166293,"topicName":"sensors/gps","sequenceNumber":209} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:46 AM","latitude":40.440724833333334,"north":true,"longitude":-79.9420015,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.5,"rawGPSLat":4026.44349,"rawGPSLong":7956.52009,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433166795,"topicName":"sensors/gps","sequenceNumber":210} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:47 AM","latitude":40.44070933333333,"north":true,"longitude":-79.94200983333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44256,"rawGPSLong":7956.52059,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433167293,"topicName":"sensors/gps","sequenceNumber":211} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:47 AM","latitude":40.440695166666664,"north":true,"longitude":-79.942017,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44171,"rawGPSLong":7956.52102,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433167792,"topicName":"sensors/gps","sequenceNumber":212} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:48 AM","latitude":40.4406805,"north":true,"longitude":-79.9420235,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.44083,"rawGPSLong":7956.52141,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433168297,"topicName":"sensors/gps","sequenceNumber":213} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:48 AM","latitude":40.440665,"north":true,"longitude":-79.94203083333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.2,"rawGPSLat":4026.4399,"rawGPSLong":7956.52185,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433168801,"topicName":"sensors/gps","sequenceNumber":214} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:49 AM","latitude":40.44064866666667,"north":true,"longitude":-79.9420385,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43892,"rawGPSLong":7956.52231,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433169289,"topicName":"sensors/gps","sequenceNumber":215} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:49 AM","latitude":40.440634,"north":true,"longitude":-79.94204483333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43804,"rawGPSLong":7956.52269,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433169806,"topicName":"sensors/gps","sequenceNumber":216} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:50 AM","latitude":40.44062066666667,"north":true,"longitude":-79.94205033333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.5,"rawGPSLat":4026.43724,"rawGPSLong":7956.52302,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433170293,"topicName":"sensors/gps","sequenceNumber":217} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:50 AM","latitude":40.44060666666667,"north":true,"longitude":-79.9420565,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.4364,"rawGPSLong":7956.52339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433170802,"topicName":"sensors/gps","sequenceNumber":218} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:51 AM","latitude":40.44059133333333,"north":true,"longitude":-79.94206466666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43548,"rawGPSLong":7956.52388,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433171298,"topicName":"sensors/gps","sequenceNumber":219} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:51 AM","latitude":40.44057733333333,"north":true,"longitude":-79.94207183333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43464,"rawGPSLong":7956.52431,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433171800,"topicName":"sensors/gps","sequenceNumber":220} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:52 AM","latitude":40.4405635,"north":true,"longitude":-79.9420795,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43381,"rawGPSLong":7956.52477,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433172292,"topicName":"sensors/gps","sequenceNumber":221} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:52 AM","latitude":40.440546166666664,"north":true,"longitude":-79.942088,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43277,"rawGPSLong":7956.52528,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433172798,"topicName":"sensors/gps","sequenceNumber":222} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:53 AM","latitude":40.440529166666664,"north":true,"longitude":-79.94209666666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43175,"rawGPSLong":7956.5258,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433173297,"topicName":"sensors/gps","sequenceNumber":223} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:53 AM","latitude":40.440513833333334,"north":true,"longitude":-79.94210516666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43083,"rawGPSLong":7956.52631,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433173798,"topicName":"sensors/gps","sequenceNumber":224} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:54 AM","latitude":40.4405,"north":true,"longitude":-79.942114,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43,"rawGPSLong":7956.52684,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433174298,"topicName":"sensors/gps","sequenceNumber":225} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:54 AM","latitude":40.44048516666667,"north":true,"longitude":-79.94212583333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.42911,"rawGPSLong":7956.52755,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433174785,"topicName":"sensors/gps","sequenceNumber":226} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:55 AM","latitude":40.440468833333334,"north":true,"longitude":-79.9421365,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.1,"rawGPSLat":4026.42813,"rawGPSLong":7956.52819,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433175294,"topicName":"sensors/gps","sequenceNumber":227} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:55 AM","latitude":40.440453166666664,"north":true,"longitude":-79.94214683333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.2,"rawGPSLat":4026.42719,"rawGPSLong":7956.52881,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433175789,"topicName":"sensors/gps","sequenceNumber":228} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:56 AM","latitude":40.440438666666665,"north":true,"longitude":-79.942156,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.1,"rawGPSLat":4026.42632,"rawGPSLong":7956.52936,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433176300,"topicName":"sensors/gps","sequenceNumber":229} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:56 AM","latitude":40.44042366666667,"north":true,"longitude":-79.94216566666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.9,"rawGPSLat":4026.42542,"rawGPSLong":7956.52994,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433176789,"topicName":"sensors/gps","sequenceNumber":230} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:57 AM","latitude":40.44040716666667,"north":true,"longitude":-79.94217833333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.5,"rawGPSLat":4026.42443,"rawGPSLong":7956.5307,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433177297,"topicName":"sensors/gps","sequenceNumber":231} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:57 AM","latitude":40.44039083333333,"north":true,"longitude":-79.9421905,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42345,"rawGPSLong":7956.53143,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433177783,"topicName":"sensors/gps","sequenceNumber":232} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:58 AM","latitude":40.4403755,"north":true,"longitude":-79.94220233333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42253,"rawGPSLong":7956.53214,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433178301,"topicName":"sensors/gps","sequenceNumber":233} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:58 AM","latitude":40.44036033333333,"north":true,"longitude":-79.94221516666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42162,"rawGPSLong":7956.53291,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433178802,"topicName":"sensors/gps","sequenceNumber":234} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:59 AM","latitude":40.440344333333336,"north":true,"longitude":-79.94223283333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42066,"rawGPSLong":7956.53397,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433179297,"topicName":"sensors/gps","sequenceNumber":235} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:59 AM","latitude":40.44032766666667,"north":true,"longitude":-79.94224916666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.41966,"rawGPSLong":7956.53495,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433179795,"topicName":"sensors/gps","sequenceNumber":236} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:00 AM","latitude":40.440310833333335,"north":true,"longitude":-79.94226416666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.41865,"rawGPSLong":7956.53585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433180295,"topicName":"sensors/gps","sequenceNumber":237} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:00 AM","latitude":40.440294333333334,"north":true,"longitude":-79.94227916666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.2,"rawGPSLat":4026.41766,"rawGPSLong":7956.53675,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433180795,"topicName":"sensors/gps","sequenceNumber":238} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:01 AM","latitude":40.44027666666667,"north":true,"longitude":-79.94229833333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.1,"rawGPSLat":4026.4166,"rawGPSLong":7956.5379,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433181309,"topicName":"sensors/gps","sequenceNumber":239} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:01 AM","latitude":40.44025966666667,"north":true,"longitude":-79.942322,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.9,"rawGPSLat":4026.41558,"rawGPSLong":7956.53932,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433181810,"topicName":"sensors/gps","sequenceNumber":240} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:02 AM","latitude":40.440244666666665,"north":true,"longitude":-79.94234933333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.9,"rawGPSLat":4026.41468,"rawGPSLong":7956.54096,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433182339,"topicName":"sensors/gps","sequenceNumber":241} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:02 AM","latitude":40.440232,"north":true,"longitude":-79.942379,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.8,"rawGPSLat":4026.41392,"rawGPSLong":7956.54274,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433182823,"topicName":"sensors/gps","sequenceNumber":242} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:03 AM","latitude":40.440220833333335,"north":true,"longitude":-79.94241033333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.7,"rawGPSLat":4026.41325,"rawGPSLong":7956.54462,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433183308,"topicName":"sensors/gps","sequenceNumber":243} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:03 AM","latitude":40.44021266666667,"north":true,"longitude":-79.94244366666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.6,"rawGPSLat":4026.41276,"rawGPSLong":7956.54662,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433183808,"topicName":"sensors/gps","sequenceNumber":244} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:04 AM","latitude":40.44020583333333,"north":true,"longitude":-79.94247883333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.3,"rawGPSLat":4026.41235,"rawGPSLong":7956.54873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433184312,"topicName":"sensors/gps","sequenceNumber":245} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:04 AM","latitude":40.4402,"north":true,"longitude":-79.94251483333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.2,"rawGPSLat":4026.412,"rawGPSLong":7956.55089,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433184824,"topicName":"sensors/gps","sequenceNumber":246} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:05 AM","latitude":40.44019216666667,"north":true,"longitude":-79.94255066666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.1,"rawGPSLat":4026.41153,"rawGPSLong":7956.55304,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433185324,"topicName":"sensors/gps","sequenceNumber":247} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:05 AM","latitude":40.4401825,"north":true,"longitude":-79.94258716666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.9,"rawGPSLat":4026.41095,"rawGPSLong":7956.55523,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433185826,"topicName":"sensors/gps","sequenceNumber":248} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:06 AM","latitude":40.440174166666665,"north":true,"longitude":-79.94262533333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.8,"rawGPSLat":4026.41045,"rawGPSLong":7956.55752,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433186311,"topicName":"sensors/gps","sequenceNumber":249} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:06 AM","latitude":40.44016766666667,"north":true,"longitude":-79.94266466666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.4,"rawGPSLat":4026.41006,"rawGPSLong":7956.55988,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433186815,"topicName":"sensors/gps","sequenceNumber":250} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:07 AM","latitude":40.44016166666667,"north":true,"longitude":-79.94270616666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.2,"rawGPSLat":4026.4097,"rawGPSLong":7956.56237,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433187316,"topicName":"sensors/gps","sequenceNumber":251} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:07 AM","latitude":40.44015366666667,"north":true,"longitude":-79.9427485,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.0,"rawGPSLat":4026.40922,"rawGPSLong":7956.56491,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433187839,"topicName":"sensors/gps","sequenceNumber":252} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:08 AM","latitude":40.44014383333333,"north":true,"longitude":-79.942791,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.40863,"rawGPSLong":7956.56746,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433188329,"topicName":"sensors/gps","sequenceNumber":253} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:08 AM","latitude":40.440135166666664,"north":true,"longitude":-79.94283483333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.5,"rawGPSLat":4026.40811,"rawGPSLong":7956.57009,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433188820,"topicName":"sensors/gps","sequenceNumber":254} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:09 AM","latitude":40.440125333333334,"north":true,"longitude":-79.94287833333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.1,"rawGPSLat":4026.40752,"rawGPSLong":7956.5727,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433189327,"topicName":"sensors/gps","sequenceNumber":255} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:09 AM","latitude":40.440113,"north":true,"longitude":-79.94292216666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.7,"rawGPSLat":4026.40678,"rawGPSLong":7956.57533,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433189820,"topicName":"sensors/gps","sequenceNumber":256} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:10 AM","latitude":40.440096,"north":true,"longitude":-79.94296516666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.5,"rawGPSLat":4026.40576,"rawGPSLong":7956.57791,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433190333,"topicName":"sensors/gps","sequenceNumber":257} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:10 AM","latitude":40.44007783333333,"north":true,"longitude":-79.94300816666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.3,"rawGPSLat":4026.40467,"rawGPSLong":7956.58049,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433190825,"topicName":"sensors/gps","sequenceNumber":258} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:11 AM","latitude":40.44006016666667,"north":true,"longitude":-79.943053,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.0,"rawGPSLat":4026.40361,"rawGPSLong":7956.58318,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433191329,"topicName":"sensors/gps","sequenceNumber":259} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:11 AM","latitude":40.44004,"north":true,"longitude":-79.943097,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.8,"rawGPSLat":4026.4024,"rawGPSLong":7956.58582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433191825,"topicName":"sensors/gps","sequenceNumber":260} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:12 AM","latitude":40.440016,"north":true,"longitude":-79.9431385,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.5,"rawGPSLat":4026.40096,"rawGPSLong":7956.58831,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433192327,"topicName":"sensors/gps","sequenceNumber":261} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:12 AM","latitude":40.43999,"north":true,"longitude":-79.94317916666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.0,"rawGPSLat":4026.3994,"rawGPSLong":7956.59075,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433192827,"topicName":"sensors/gps","sequenceNumber":262} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:13 AM","latitude":40.439963,"north":true,"longitude":-79.94322033333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.7,"rawGPSLat":4026.39778,"rawGPSLong":7956.59322,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433193325,"topicName":"sensors/gps","sequenceNumber":263} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:13 AM","latitude":40.439936833333334,"north":true,"longitude":-79.94326316666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.5,"rawGPSLat":4026.39621,"rawGPSLong":7956.59579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433193838,"topicName":"sensors/gps","sequenceNumber":264} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:14 AM","latitude":40.43991116666667,"north":true,"longitude":-79.94330766666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.1,"rawGPSLat":4026.39467,"rawGPSLong":7956.59846,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433194329,"topicName":"sensors/gps","sequenceNumber":265} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:14 AM","latitude":40.4398795,"north":true,"longitude":-79.9433445,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.4,"rawGPSLat":4026.39277,"rawGPSLong":7956.60067,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433194828,"topicName":"sensors/gps","sequenceNumber":266} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:15 AM","latitude":40.4398465,"north":true,"longitude":-79.94337916666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.0,"rawGPSLat":4026.39079,"rawGPSLong":7956.60275,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433195326,"topicName":"sensors/gps","sequenceNumber":267} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:15 AM","latitude":40.4398105,"north":true,"longitude":-79.94340883333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.6,"rawGPSLat":4026.38863,"rawGPSLong":7956.60453,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433195816,"topicName":"sensors/gps","sequenceNumber":268} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:16 AM","latitude":40.439777166666666,"north":true,"longitude":-79.943441,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.38663,"rawGPSLong":7956.60646,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433196325,"topicName":"sensors/gps","sequenceNumber":269} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:16 AM","latitude":40.439743,"north":true,"longitude":-79.94347516666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.38458,"rawGPSLong":7956.60851,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433196835,"topicName":"sensors/gps","sequenceNumber":270} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:17 AM","latitude":40.43970816666667,"north":true,"longitude":-79.94350783333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.38249,"rawGPSLong":7956.61047,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433197323,"topicName":"sensors/gps","sequenceNumber":271} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:17 AM","latitude":40.439676666666664,"north":true,"longitude":-79.9435455,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.3,"rawGPSLat":4026.3806,"rawGPSLong":7956.61273,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433197812,"topicName":"sensors/gps","sequenceNumber":272} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:18 AM","latitude":40.439645166666665,"north":true,"longitude":-79.94358316666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.1,"rawGPSLat":4026.37871,"rawGPSLong":7956.61499,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433198322,"topicName":"sensors/gps","sequenceNumber":273} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:18 AM","latitude":40.43961066666667,"north":true,"longitude":-79.94361533333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.3,"rawGPSLat":4026.37664,"rawGPSLong":7956.61692,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433198823,"topicName":"sensors/gps","sequenceNumber":274} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:19 AM","latitude":40.43957416666667,"north":true,"longitude":-79.943644,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.37445,"rawGPSLong":7956.61864,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433199309,"topicName":"sensors/gps","sequenceNumber":275} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:19 AM","latitude":40.439538166666665,"north":true,"longitude":-79.94367216666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.37229,"rawGPSLong":7956.62033,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433199831,"topicName":"sensors/gps","sequenceNumber":276} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:20 AM","latitude":40.43950316666667,"north":true,"longitude":-79.94370033333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.37019,"rawGPSLong":7956.62202,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433200328,"topicName":"sensors/gps","sequenceNumber":277} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:20 AM","latitude":40.439469333333335,"north":true,"longitude":-79.94373166666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.36816,"rawGPSLong":7956.6239,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433200827,"topicName":"sensors/gps","sequenceNumber":278} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:21 AM","latitude":40.439438333333335,"north":true,"longitude":-79.9437665,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.3663,"rawGPSLong":7956.62599,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433201329,"topicName":"sensors/gps","sequenceNumber":279} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:21 AM","latitude":40.439407333333335,"north":true,"longitude":-79.9438,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.36444,"rawGPSLong":7956.628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433201830,"topicName":"sensors/gps","sequenceNumber":280} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:22 AM","latitude":40.439378,"north":true,"longitude":-79.94383533333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.36268,"rawGPSLong":7956.63012,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433202326,"topicName":"sensors/gps","sequenceNumber":281} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:22 AM","latitude":40.439348833333334,"north":true,"longitude":-79.94387033333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.36093,"rawGPSLong":7956.63222,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433202829,"topicName":"sensors/gps","sequenceNumber":282} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:23 AM","latitude":40.439318,"north":true,"longitude":-79.94390133333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.35908,"rawGPSLong":7956.63408,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433203327,"topicName":"sensors/gps","sequenceNumber":283} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:23 AM","latitude":40.439287,"north":true,"longitude":-79.94393183333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.35722,"rawGPSLong":7956.63591,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433203816,"topicName":"sensors/gps","sequenceNumber":284} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:24 AM","latitude":40.43925766666667,"north":true,"longitude":-79.94396416666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.35546,"rawGPSLong":7956.63785,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433204321,"topicName":"sensors/gps","sequenceNumber":285} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:24 AM","latitude":40.43922966666667,"north":true,"longitude":-79.94399766666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":1.08,"antennaAltitude":289.5,"rawGPSLat":4026.35378,"rawGPSLong":7956.63986,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433204832,"topicName":"sensors/gps","sequenceNumber":286} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:25 AM","latitude":40.43919966666667,"north":true,"longitude":-79.94402866666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.35198,"rawGPSLong":7956.64172,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433205328,"topicName":"sensors/gps","sequenceNumber":287} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:25 AM","latitude":40.439172,"north":true,"longitude":-79.944061,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.2,"rawGPSLat":4026.35032,"rawGPSLong":7956.64366,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433205814,"topicName":"sensors/gps","sequenceNumber":288} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:26 AM","latitude":40.439146,"north":true,"longitude":-79.9440955,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.34876,"rawGPSLong":7956.64573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433206329,"topicName":"sensors/gps","sequenceNumber":289} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:26 AM","latitude":40.439120833333334,"north":true,"longitude":-79.94413033333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.34725,"rawGPSLong":7956.64782,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433206837,"topicName":"sensors/gps","sequenceNumber":290} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:27 AM","latitude":40.43909433333334,"north":true,"longitude":-79.94416316666667,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.34566,"rawGPSLong":7956.64979,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433207332,"topicName":"sensors/gps","sequenceNumber":291} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:27 AM","latitude":40.439068166666665,"north":true,"longitude":-79.9441955,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.8,"rawGPSLat":4026.34409,"rawGPSLong":7956.65173,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433207817,"topicName":"sensors/gps","sequenceNumber":292} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:28 AM","latitude":40.4390435,"north":true,"longitude":-79.94423,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.7,"rawGPSLat":4026.34261,"rawGPSLong":7956.6538,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433208315,"topicName":"sensors/gps","sequenceNumber":293} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:28 AM","latitude":40.439020666666664,"north":true,"longitude":-79.944266,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.6,"rawGPSLat":4026.34124,"rawGPSLong":7956.65596,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433208833,"topicName":"sensors/gps","sequenceNumber":294} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:29 AM","latitude":40.43900083333333,"north":true,"longitude":-79.94430416666667,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.34005,"rawGPSLong":7956.65825,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433209333,"topicName":"sensors/gps","sequenceNumber":295} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:29 AM","latitude":40.4389825,"north":true,"longitude":-79.94434366666667,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.4,"rawGPSLat":4026.33895,"rawGPSLong":7956.66062,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433209826,"topicName":"sensors/gps","sequenceNumber":296} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:30 AM","latitude":40.4389655,"north":true,"longitude":-79.94438333333333,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.2,"rawGPSLat":4026.33793,"rawGPSLong":7956.663,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433210342,"topicName":"sensors/gps","sequenceNumber":297} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:30 AM","latitude":40.43895033333333,"north":true,"longitude":-79.94442416666666,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.33702,"rawGPSLong":7956.66545,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433210949,"topicName":"sensors/gps","sequenceNumber":298} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:31 AM","latitude":40.43893633333333,"north":true,"longitude":-79.94446566666667,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.33618,"rawGPSLong":7956.66794,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433211330,"topicName":"sensors/gps","sequenceNumber":299} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:32 AM","latitude":40.438923,"north":true,"longitude":-79.94450633333334,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.0,"rawGPSLat":4026.33538,"rawGPSLong":7956.67038,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433212132,"topicName":"sensors/gps","sequenceNumber":300} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:32 AM","latitude":40.438908166666664,"north":true,"longitude":-79.94454633333334,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.9,"rawGPSLat":4026.33449,"rawGPSLong":7956.67278,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433212339,"topicName":"sensors/gps","sequenceNumber":301} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:32 AM","latitude":40.438893,"north":true,"longitude":-79.9445855,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.33358,"rawGPSLong":7956.67513,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433212838,"topicName":"sensors/gps","sequenceNumber":302} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:33 AM","latitude":40.43887933333333,"north":true,"longitude":-79.94462466666667,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.33276,"rawGPSLong":7956.67748,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433213326,"topicName":"sensors/gps","sequenceNumber":303} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:33 AM","latitude":40.4388675,"north":true,"longitude":-79.9446655,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.7,"rawGPSLat":4026.33205,"rawGPSLong":7956.67993,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433213822,"topicName":"sensors/gps","sequenceNumber":304} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:34 AM","latitude":40.43885683333333,"north":true,"longitude":-79.94470616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.5,"rawGPSLat":4026.33141,"rawGPSLong":7956.68237,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433214331,"topicName":"sensors/gps","sequenceNumber":305} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:34 AM","latitude":40.438846833333336,"north":true,"longitude":-79.94474333333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.1,"rawGPSLat":4026.33081,"rawGPSLong":7956.6846,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433214813,"topicName":"sensors/gps","sequenceNumber":306} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:35 AM","latitude":40.43883916666667,"north":true,"longitude":-79.94478483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.8,"rawGPSLat":4026.33035,"rawGPSLong":7956.68709,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433215326,"topicName":"sensors/gps","sequenceNumber":307} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:35 AM","latitude":40.43883133333333,"north":true,"longitude":-79.94482433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.5,"rawGPSLat":4026.32988,"rawGPSLong":7956.68946,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433215823,"topicName":"sensors/gps","sequenceNumber":308} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:36 AM","latitude":40.43882283333333,"north":true,"longitude":-79.944864,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.3,"rawGPSLat":4026.32937,"rawGPSLong":7956.69184,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433216320,"topicName":"sensors/gps","sequenceNumber":309} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:36 AM","latitude":40.438814,"north":true,"longitude":-79.94490366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.3,"rawGPSLat":4026.32884,"rawGPSLong":7956.69422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433216811,"topicName":"sensors/gps","sequenceNumber":310} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:37 AM","latitude":40.438806,"north":true,"longitude":-79.94494333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.1,"rawGPSLat":4026.32836,"rawGPSLong":7956.6966,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433217324,"topicName":"sensors/gps","sequenceNumber":311} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:37 AM","latitude":40.43879916666667,"north":true,"longitude":-79.944983,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.8,"rawGPSLat":4026.32795,"rawGPSLong":7956.69898,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433217833,"topicName":"sensors/gps","sequenceNumber":312} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:38 AM","latitude":40.438790833333336,"north":true,"longitude":-79.94502216666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.9,"rawGPSLat":4026.32745,"rawGPSLong":7956.70133,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433218329,"topicName":"sensors/gps","sequenceNumber":313} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:38 AM","latitude":40.4387815,"north":true,"longitude":-79.945061,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.9,"rawGPSLat":4026.32689,"rawGPSLong":7956.70366,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433218825,"topicName":"sensors/gps","sequenceNumber":314} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:39 AM","latitude":40.438772666666665,"north":true,"longitude":-79.94509916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.7,"rawGPSLat":4026.32636,"rawGPSLong":7956.70595,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433219328,"topicName":"sensors/gps","sequenceNumber":315} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:39 AM","latitude":40.438763333333334,"north":true,"longitude":-79.94513733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.7,"rawGPSLat":4026.3258,"rawGPSLong":7956.70824,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433219815,"topicName":"sensors/gps","sequenceNumber":316} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:40 AM","latitude":40.43875516666667,"north":true,"longitude":-79.94517583333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.8,"rawGPSLat":4026.32531,"rawGPSLong":7956.71055,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433220320,"topicName":"sensors/gps","sequenceNumber":317} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:40 AM","latitude":40.43874816666667,"north":true,"longitude":-79.94521483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.8,"rawGPSLat":4026.32489,"rawGPSLong":7956.71289,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433220803,"topicName":"sensors/gps","sequenceNumber":318} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:41 AM","latitude":40.4387435,"north":true,"longitude":-79.945253,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.4,"rawGPSLat":4026.32461,"rawGPSLong":7956.71518,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433221324,"topicName":"sensors/gps","sequenceNumber":319} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:41 AM","latitude":40.4387375,"north":true,"longitude":-79.94529033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.0,"rawGPSLat":4026.32425,"rawGPSLong":7956.71742,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433221809,"topicName":"sensors/gps","sequenceNumber":320} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:42 AM","latitude":40.438729333333335,"north":true,"longitude":-79.945328,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.9,"rawGPSLat":4026.32376,"rawGPSLong":7956.71968,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433222328,"topicName":"sensors/gps","sequenceNumber":321} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:42 AM","latitude":40.4387245,"north":true,"longitude":-79.9453655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.5,"rawGPSLat":4026.32347,"rawGPSLong":7956.72193,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433222817,"topicName":"sensors/gps","sequenceNumber":322} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:43 AM","latitude":40.43872,"north":true,"longitude":-79.94540366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.3,"rawGPSLat":4026.3232,"rawGPSLong":7956.72422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433223326,"topicName":"sensors/gps","sequenceNumber":323} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:43 AM","latitude":40.4387165,"north":true,"longitude":-79.94544083333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.9,"rawGPSLat":4026.32299,"rawGPSLong":7956.72645,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433223829,"topicName":"sensors/gps","sequenceNumber":324} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:44 AM","latitude":40.43871266666667,"north":true,"longitude":-79.9454785,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.9,"rawGPSLat":4026.32276,"rawGPSLong":7956.72871,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433224324,"topicName":"sensors/gps","sequenceNumber":325} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:44 AM","latitude":40.43870916666667,"north":true,"longitude":-79.945516,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.9,"rawGPSLat":4026.32255,"rawGPSLong":7956.73096,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433224812,"topicName":"sensors/gps","sequenceNumber":326} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:45 AM","latitude":40.438706333333336,"north":true,"longitude":-79.94555316666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.6,"rawGPSLat":4026.32238,"rawGPSLong":7956.73319,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433225323,"topicName":"sensors/gps","sequenceNumber":327} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:45 AM","latitude":40.438703333333336,"north":true,"longitude":-79.94559016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.6,"rawGPSLat":4026.3222,"rawGPSLong":7956.73541,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433225802,"topicName":"sensors/gps","sequenceNumber":328} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:46 AM","latitude":40.43870183333333,"north":true,"longitude":-79.945627,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.3,"rawGPSLat":4026.32211,"rawGPSLong":7956.73762,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433226328,"topicName":"sensors/gps","sequenceNumber":329} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:46 AM","latitude":40.43870066666667,"north":true,"longitude":-79.94566333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.8,"rawGPSLat":4026.32204,"rawGPSLong":7956.7398,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433226816,"topicName":"sensors/gps","sequenceNumber":330} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:47 AM","latitude":40.4386995,"north":true,"longitude":-79.94569933333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.6,"rawGPSLat":4026.32197,"rawGPSLong":7956.74196,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433227315,"topicName":"sensors/gps","sequenceNumber":331} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:47 AM","latitude":40.438701,"north":true,"longitude":-79.94573566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.3,"rawGPSLat":4026.32206,"rawGPSLong":7956.74414,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433227838,"topicName":"sensors/gps","sequenceNumber":332} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:48 AM","latitude":40.438704333333334,"north":true,"longitude":-79.9457715,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.32226,"rawGPSLong":7956.74629,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433228318,"topicName":"sensors/gps","sequenceNumber":333} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:48 AM","latitude":40.438709333333335,"north":true,"longitude":-79.94580666666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32256,"rawGPSLong":7956.7484,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433228816,"topicName":"sensors/gps","sequenceNumber":334} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:49 AM","latitude":40.438715333333334,"north":true,"longitude":-79.94584166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32292,"rawGPSLong":7956.7505,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433229316,"topicName":"sensors/gps","sequenceNumber":335} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:49 AM","latitude":40.43872316666667,"north":true,"longitude":-79.9458755,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32339,"rawGPSLong":7956.75253,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433229813,"topicName":"sensors/gps","sequenceNumber":336} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:50 AM","latitude":40.4387315,"north":true,"longitude":-79.94590883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32389,"rawGPSLong":7956.75453,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433230304,"topicName":"sensors/gps","sequenceNumber":337} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:50 AM","latitude":40.43874016666667,"north":true,"longitude":-79.945942,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.6,"rawGPSLat":4026.32441,"rawGPSLong":7956.75652,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433230800,"topicName":"sensors/gps","sequenceNumber":338} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:51 AM","latitude":40.43874733333333,"north":true,"longitude":-79.9459755,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32484,"rawGPSLong":7956.75853,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433231319,"topicName":"sensors/gps","sequenceNumber":339} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:51 AM","latitude":40.43875383333334,"north":true,"longitude":-79.9460085,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32523,"rawGPSLong":7956.76051,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433231818,"topicName":"sensors/gps","sequenceNumber":340} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:52 AM","latitude":40.438760333333335,"north":true,"longitude":-79.946041,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32562,"rawGPSLong":7956.76246,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433232317,"topicName":"sensors/gps","sequenceNumber":341} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:52 AM","latitude":40.43876783333333,"north":true,"longitude":-79.94607216666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32607,"rawGPSLong":7956.76433,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433232819,"topicName":"sensors/gps","sequenceNumber":342} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:53 AM","latitude":40.438777,"north":true,"longitude":-79.94610166666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.6,"rawGPSLat":4026.32662,"rawGPSLong":7956.7661,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433233313,"topicName":"sensors/gps","sequenceNumber":343} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:53 AM","latitude":40.43878683333333,"north":true,"longitude":-79.94613083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32721,"rawGPSLong":7956.76785,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433233823,"topicName":"sensors/gps","sequenceNumber":344} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:54 AM","latitude":40.438796833333335,"north":true,"longitude":-79.94615916666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.9,"rawGPSLat":4026.32781,"rawGPSLong":7956.76955,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433234314,"topicName":"sensors/gps","sequenceNumber":345} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:54 AM","latitude":40.4388075,"north":true,"longitude":-79.94618583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.8,"rawGPSLat":4026.32845,"rawGPSLong":7956.77115,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433234810,"topicName":"sensors/gps","sequenceNumber":346} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:55 AM","latitude":40.438818833333336,"north":true,"longitude":-79.94621166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.9,"rawGPSLat":4026.32913,"rawGPSLong":7956.7727,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433235305,"topicName":"sensors/gps","sequenceNumber":347} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:55 AM","latitude":40.438831,"north":true,"longitude":-79.94623583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.32986,"rawGPSLong":7956.77415,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433235815,"topicName":"sensors/gps","sequenceNumber":348} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:56 AM","latitude":40.4388445,"north":true,"longitude":-79.94625816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33067,"rawGPSLong":7956.77549,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433236299,"topicName":"sensors/gps","sequenceNumber":349} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:56 AM","latitude":40.438858333333336,"north":true,"longitude":-79.94627933333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.3315,"rawGPSLong":7956.77676,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433236809,"topicName":"sensors/gps","sequenceNumber":350} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:57 AM","latitude":40.43887216666667,"north":true,"longitude":-79.94630016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33233,"rawGPSLong":7956.77801,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433237307,"topicName":"sensors/gps","sequenceNumber":351} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:57 AM","latitude":40.43888583333333,"north":true,"longitude":-79.94632116666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33315,"rawGPSLong":7956.77927,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433237809,"topicName":"sensors/gps","sequenceNumber":352} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:58 AM","latitude":40.438899666666664,"north":true,"longitude":-79.94634116666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33398,"rawGPSLong":7956.78047,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433238295,"topicName":"sensors/gps","sequenceNumber":353} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:58 AM","latitude":40.4389145,"north":true,"longitude":-79.94636016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33487,"rawGPSLong":7956.78161,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433238810,"topicName":"sensors/gps","sequenceNumber":354} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:59 AM","latitude":40.43892916666667,"north":true,"longitude":-79.94637883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33575,"rawGPSLong":7956.78273,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433239307,"topicName":"sensors/gps","sequenceNumber":355} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:59 AM","latitude":40.4389435,"north":true,"longitude":-79.94639783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.9,"rawGPSLat":4026.33661,"rawGPSLong":7956.78387,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433239810,"topicName":"sensors/gps","sequenceNumber":356} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:00 AM","latitude":40.438958166666666,"north":true,"longitude":-79.94641616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.33749,"rawGPSLong":7956.78497,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433240308,"topicName":"sensors/gps","sequenceNumber":357} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:00 AM","latitude":40.438974333333334,"north":true,"longitude":-79.9464325,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.33846,"rawGPSLong":7956.78595,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433240797,"topicName":"sensors/gps","sequenceNumber":358} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:01 AM","latitude":40.438991,"north":true,"longitude":-79.94644833333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.33946,"rawGPSLong":7956.7869,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433241319,"topicName":"sensors/gps","sequenceNumber":359} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:01 AM","latitude":40.43900816666667,"north":true,"longitude":-79.94646333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.6,"rawGPSLat":4026.34049,"rawGPSLong":7956.7878,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433241811,"topicName":"sensors/gps","sequenceNumber":360} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:02 AM","latitude":40.439025,"north":true,"longitude":-79.94647883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.3415,"rawGPSLong":7956.78873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433242309,"topicName":"sensors/gps","sequenceNumber":361} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:02 AM","latitude":40.43904216666667,"north":true,"longitude":-79.946494,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.34253,"rawGPSLong":7956.78964,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433242810,"topicName":"sensors/gps","sequenceNumber":362} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:03 AM","latitude":40.43905933333333,"north":true,"longitude":-79.94650916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.34356,"rawGPSLong":7956.79055,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433243306,"topicName":"sensors/gps","sequenceNumber":363} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:03 AM","latitude":40.43907683333333,"north":true,"longitude":-79.9465235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.34461,"rawGPSLong":7956.79141,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433243798,"topicName":"sensors/gps","sequenceNumber":364} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:04 AM","latitude":40.439095333333334,"north":true,"longitude":-79.94653566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.4,"rawGPSLat":4026.34572,"rawGPSLong":7956.79214,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433244307,"topicName":"sensors/gps","sequenceNumber":365} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:04 AM","latitude":40.439113166666665,"north":true,"longitude":-79.94654866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.4,"rawGPSLat":4026.34679,"rawGPSLong":7956.79292,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433244795,"topicName":"sensors/gps","sequenceNumber":366} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:05 AM","latitude":40.439131333333336,"north":true,"longitude":-79.9465615,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.34788,"rawGPSLong":7956.79369,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433245309,"topicName":"sensors/gps","sequenceNumber":367} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:05 AM","latitude":40.439149,"north":true,"longitude":-79.94657516666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.34894,"rawGPSLong":7956.79451,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433245792,"topicName":"sensors/gps","sequenceNumber":368} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:06 AM","latitude":40.4391675,"north":true,"longitude":-79.9465865,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35005,"rawGPSLong":7956.79519,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433246297,"topicName":"sensors/gps","sequenceNumber":369} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:06 AM","latitude":40.439186,"north":true,"longitude":-79.94659733333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35116,"rawGPSLong":7956.79584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433246793,"topicName":"sensors/gps","sequenceNumber":370} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:07 AM","latitude":40.439204333333336,"north":true,"longitude":-79.94660833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35226,"rawGPSLong":7956.7965,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433247307,"topicName":"sensors/gps","sequenceNumber":371} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:07 AM","latitude":40.4392225,"north":true,"longitude":-79.94661916666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35335,"rawGPSLong":7956.79715,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433247824,"topicName":"sensors/gps","sequenceNumber":372} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:08 AM","latitude":40.43924066666667,"north":true,"longitude":-79.94663,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.35444,"rawGPSLong":7956.7978,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433248307,"topicName":"sensors/gps","sequenceNumber":373} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:08 AM","latitude":40.439259166666666,"north":true,"longitude":-79.94663983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.35555,"rawGPSLong":7956.79839,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433248811,"topicName":"sensors/gps","sequenceNumber":374} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:09 AM","latitude":40.4392775,"north":true,"longitude":-79.94664983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.35665,"rawGPSLong":7956.79899,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433249301,"topicName":"sensors/gps","sequenceNumber":375} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:09 AM","latitude":40.439295666666666,"north":true,"longitude":-79.94665966666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.1,"rawGPSLat":4026.35774,"rawGPSLong":7956.79958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433249808,"topicName":"sensors/gps","sequenceNumber":376} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:10 AM","latitude":40.43931383333334,"north":true,"longitude":-79.94666983333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35883,"rawGPSLong":7956.80019,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433250310,"topicName":"sensors/gps","sequenceNumber":377} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:10 AM","latitude":40.439331833333334,"north":true,"longitude":-79.94668016666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35991,"rawGPSLong":7956.80081,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433250810,"topicName":"sensors/gps","sequenceNumber":378} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:11 AM","latitude":40.439349666666665,"north":true,"longitude":-79.94669066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.36098,"rawGPSLong":7956.80144,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433251313,"topicName":"sensors/gps","sequenceNumber":379} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:11 AM","latitude":40.43936766666667,"north":true,"longitude":-79.9467005,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.36206,"rawGPSLong":7956.80203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433251803,"topicName":"sensors/gps","sequenceNumber":380} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:12 AM","latitude":40.43938583333333,"north":true,"longitude":-79.94671,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.36315,"rawGPSLong":7956.8026,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433252303,"topicName":"sensors/gps","sequenceNumber":381} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:12 AM","latitude":40.43940416666667,"north":true,"longitude":-79.946719,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.36425,"rawGPSLong":7956.80314,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433252807,"topicName":"sensors/gps","sequenceNumber":382} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:13 AM","latitude":40.43942283333333,"north":true,"longitude":-79.9467275,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.36537,"rawGPSLong":7956.80365,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433253297,"topicName":"sensors/gps","sequenceNumber":383} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:13 AM","latitude":40.43944166666667,"north":true,"longitude":-79.94673566666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.3665,"rawGPSLong":7956.80414,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433253812,"topicName":"sensors/gps","sequenceNumber":384} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:14 AM","latitude":40.43946016666666,"north":true,"longitude":-79.9467435,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.41,"antennaAltitude":281.0,"rawGPSLat":4026.36761,"rawGPSLong":7956.80461,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433254300,"topicName":"sensors/gps","sequenceNumber":385} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:14 AM","latitude":40.439479,"north":true,"longitude":-79.94675183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.0,"rawGPSLat":4026.36874,"rawGPSLong":7956.80511,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433254807,"topicName":"sensors/gps","sequenceNumber":386} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:15 AM","latitude":40.439497833333334,"north":true,"longitude":-79.946761,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.9,"rawGPSLat":4026.36987,"rawGPSLong":7956.80566,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433255301,"topicName":"sensors/gps","sequenceNumber":387} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:15 AM","latitude":40.439516166666664,"north":true,"longitude":-79.94677133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.8,"rawGPSLat":4026.37097,"rawGPSLong":7956.80628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433255784,"topicName":"sensors/gps","sequenceNumber":388} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:16 AM","latitude":40.439534333333334,"north":true,"longitude":-79.94678316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.8,"rawGPSLat":4026.37206,"rawGPSLong":7956.80699,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433256307,"topicName":"sensors/gps","sequenceNumber":389} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:16 AM","latitude":40.439552166666665,"north":true,"longitude":-79.9467965,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.7,"rawGPSLat":4026.37313,"rawGPSLong":7956.80779,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433256800,"topicName":"sensors/gps","sequenceNumber":390} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:17 AM","latitude":40.4395705,"north":true,"longitude":-79.94680983333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.5,"rawGPSLat":4026.37423,"rawGPSLong":7956.80859,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433257317,"topicName":"sensors/gps","sequenceNumber":391} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:17 AM","latitude":40.439589166666664,"north":true,"longitude":-79.9468225,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.5,"rawGPSLat":4026.37535,"rawGPSLong":7956.80935,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433257808,"topicName":"sensors/gps","sequenceNumber":392} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:18 AM","latitude":40.439608,"north":true,"longitude":-79.94683616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.4,"rawGPSLat":4026.37648,"rawGPSLong":7956.81017,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433258318,"topicName":"sensors/gps","sequenceNumber":393} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:18 AM","latitude":40.43962766666667,"north":true,"longitude":-79.94684883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.3,"rawGPSLat":4026.37766,"rawGPSLong":7956.81093,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433258802,"topicName":"sensors/gps","sequenceNumber":394} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:19 AM","latitude":40.439648,"north":true,"longitude":-79.94686133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.3,"rawGPSLat":4026.37888,"rawGPSLong":7956.81168,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433259316,"topicName":"sensors/gps","sequenceNumber":395} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:19 AM","latitude":40.43966833333333,"north":true,"longitude":-79.9468745,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.1,"rawGPSLat":4026.3801,"rawGPSLong":7956.81247,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433259814,"topicName":"sensors/gps","sequenceNumber":396} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:20 AM","latitude":40.4396885,"north":true,"longitude":-79.946889,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.0,"rawGPSLat":4026.38131,"rawGPSLong":7956.81334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433260315,"topicName":"sensors/gps","sequenceNumber":397} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:20 AM","latitude":40.439708833333334,"north":true,"longitude":-79.946905,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.9,"rawGPSLat":4026.38253,"rawGPSLong":7956.8143,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433260812,"topicName":"sensors/gps","sequenceNumber":398} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:21 AM","latitude":40.439729,"north":true,"longitude":-79.94692233333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.8,"rawGPSLat":4026.38374,"rawGPSLong":7956.81534,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433261327,"topicName":"sensors/gps","sequenceNumber":399} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:21 AM","latitude":40.4397495,"north":true,"longitude":-79.94694083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.6,"rawGPSLat":4026.38497,"rawGPSLong":7956.81645,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433261825,"topicName":"sensors/gps","sequenceNumber":400} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:22 AM","latitude":40.4397705,"north":true,"longitude":-79.94695916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.5,"rawGPSLat":4026.38623,"rawGPSLong":7956.81755,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433262318,"topicName":"sensors/gps","sequenceNumber":401} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:22 AM","latitude":40.439791666666665,"north":true,"longitude":-79.9469785,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.3,"rawGPSLat":4026.3875,"rawGPSLong":7956.81871,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433262825,"topicName":"sensors/gps","sequenceNumber":402} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:23 AM","latitude":40.439812833333335,"north":true,"longitude":-79.94699866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.2,"rawGPSLat":4026.38877,"rawGPSLong":7956.81992,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433263329,"topicName":"sensors/gps","sequenceNumber":403} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:23 AM","latitude":40.43983433333333,"north":true,"longitude":-79.94701933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.0,"rawGPSLat":4026.39006,"rawGPSLong":7956.82116,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433263831,"topicName":"sensors/gps","sequenceNumber":404} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:24 AM","latitude":40.43985633333333,"north":true,"longitude":-79.9470395,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.39138,"rawGPSLong":7956.82237,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433264326,"topicName":"sensors/gps","sequenceNumber":405} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:24 AM","latitude":40.43987883333333,"north":true,"longitude":-79.9470595,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.8,"rawGPSLat":4026.39273,"rawGPSLong":7956.82357,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433264820,"topicName":"sensors/gps","sequenceNumber":406} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:25 AM","latitude":40.439901166666665,"north":true,"longitude":-79.9470805,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.7,"rawGPSLat":4026.39407,"rawGPSLong":7956.82483,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433265322,"topicName":"sensors/gps","sequenceNumber":407} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:25 AM","latitude":40.439922833333334,"north":true,"longitude":-79.94710283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.6,"rawGPSLat":4026.39537,"rawGPSLong":7956.82617,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433265834,"topicName":"sensors/gps","sequenceNumber":408} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:26 AM","latitude":40.43994433333334,"north":true,"longitude":-79.94712616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.39666,"rawGPSLong":7956.82757,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433266325,"topicName":"sensors/gps","sequenceNumber":409} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:26 AM","latitude":40.43996516666667,"north":true,"longitude":-79.947151,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.39791,"rawGPSLong":7956.82906,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433266818,"topicName":"sensors/gps","sequenceNumber":410} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:27 AM","latitude":40.439987333333335,"north":true,"longitude":-79.94717416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.4,"rawGPSLat":4026.39924,"rawGPSLong":7956.83045,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433267321,"topicName":"sensors/gps","sequenceNumber":411} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:27 AM","latitude":40.44001033333333,"north":true,"longitude":-79.94719683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.40062,"rawGPSLong":7956.83181,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433267819,"topicName":"sensors/gps","sequenceNumber":412} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:28 AM","latitude":40.440032333333335,"north":true,"longitude":-79.94722083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.2,"rawGPSLat":4026.40194,"rawGPSLong":7956.83325,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433268320,"topicName":"sensors/gps","sequenceNumber":413} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:28 AM","latitude":40.4400535,"north":true,"longitude":-79.94724683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.1,"rawGPSLat":4026.40321,"rawGPSLong":7956.83481,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433268816,"topicName":"sensors/gps","sequenceNumber":414} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:29 AM","latitude":40.4400745,"north":true,"longitude":-79.9472735,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.0,"rawGPSLat":4026.40447,"rawGPSLong":7956.83641,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433269313,"topicName":"sensors/gps","sequenceNumber":415} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:29 AM","latitude":40.44009633333334,"north":true,"longitude":-79.94729883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.0,"rawGPSLat":4026.40578,"rawGPSLong":7956.83793,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433269814,"topicName":"sensors/gps","sequenceNumber":416} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:30 AM","latitude":40.44011916666667,"north":true,"longitude":-79.94732383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.9,"rawGPSLat":4026.40715,"rawGPSLong":7956.83943,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433270316,"topicName":"sensors/gps","sequenceNumber":417} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:30 AM","latitude":40.44014166666667,"north":true,"longitude":-79.94734933333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.7,"rawGPSLat":4026.4085,"rawGPSLong":7956.84096,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433270806,"topicName":"sensors/gps","sequenceNumber":418} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:31 AM","latitude":40.440164833333334,"north":true,"longitude":-79.9473735,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.7,"rawGPSLat":4026.40989,"rawGPSLong":7956.84241,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433271321,"topicName":"sensors/gps","sequenceNumber":419} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:31 AM","latitude":40.44018833333333,"north":true,"longitude":-79.94739766666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.6,"rawGPSLat":4026.4113,"rawGPSLong":7956.84386,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433271826,"topicName":"sensors/gps","sequenceNumber":420} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:32 AM","latitude":40.44021166666667,"north":true,"longitude":-79.94742266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.6,"rawGPSLat":4026.4127,"rawGPSLong":7956.84536,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433272322,"topicName":"sensors/gps","sequenceNumber":421} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:32 AM","latitude":40.4402355,"north":true,"longitude":-79.94744666666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.5,"rawGPSLat":4026.41413,"rawGPSLong":7956.8468,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433272827,"topicName":"sensors/gps","sequenceNumber":422} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:33 AM","latitude":40.4402595,"north":true,"longitude":-79.9474705,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.4,"rawGPSLat":4026.41557,"rawGPSLong":7956.84823,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433273322,"topicName":"sensors/gps","sequenceNumber":423} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:33 AM","latitude":40.440283666666666,"north":true,"longitude":-79.94749466666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.4,"rawGPSLat":4026.41702,"rawGPSLong":7956.84968,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433273809,"topicName":"sensors/gps","sequenceNumber":424} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:34 AM","latitude":40.440306666666665,"north":true,"longitude":-79.94752033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.4,"rawGPSLat":4026.4184,"rawGPSLong":7956.85122,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433274315,"topicName":"sensors/gps","sequenceNumber":425} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:34 AM","latitude":40.44033016666667,"north":true,"longitude":-79.9475455,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.3,"rawGPSLat":4026.41981,"rawGPSLong":7956.85273,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433274817,"topicName":"sensors/gps","sequenceNumber":426} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:35 AM","latitude":40.44035383333333,"north":true,"longitude":-79.94757083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.2,"rawGPSLat":4026.42123,"rawGPSLong":7956.85425,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433275324,"topicName":"sensors/gps","sequenceNumber":427} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:35 AM","latitude":40.440376666666666,"north":true,"longitude":-79.94759716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.1,"rawGPSLat":4026.4226,"rawGPSLong":7956.85583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433275829,"topicName":"sensors/gps","sequenceNumber":428} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:36 AM","latitude":40.4404,"north":true,"longitude":-79.9476235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.1,"rawGPSLat":4026.424,"rawGPSLong":7956.85741,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433276318,"topicName":"sensors/gps","sequenceNumber":429} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:36 AM","latitude":40.4404245,"north":true,"longitude":-79.94764783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.0,"rawGPSLat":4026.42547,"rawGPSLong":7956.85887,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433276811,"topicName":"sensors/gps","sequenceNumber":430} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:37 AM","latitude":40.44045,"north":true,"longitude":-79.94767133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.8,"rawGPSLat":4026.427,"rawGPSLong":7956.86028,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433277316,"topicName":"sensors/gps","sequenceNumber":431} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:37 AM","latitude":40.440475166666666,"north":true,"longitude":-79.94769566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.7,"rawGPSLat":4026.42851,"rawGPSLong":7956.86174,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433277839,"topicName":"sensors/gps","sequenceNumber":432} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:38 AM","latitude":40.44050083333333,"north":true,"longitude":-79.9477195,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.6,"rawGPSLat":4026.43005,"rawGPSLong":7956.86317,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433278321,"topicName":"sensors/gps","sequenceNumber":433} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:38 AM","latitude":40.440528,"north":true,"longitude":-79.94774116666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.4,"rawGPSLat":4026.43168,"rawGPSLong":7956.86447,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433278821,"topicName":"sensors/gps","sequenceNumber":434} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:39 AM","latitude":40.44055516666667,"north":true,"longitude":-79.94776216666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.3,"rawGPSLat":4026.43331,"rawGPSLong":7956.86573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433279323,"topicName":"sensors/gps","sequenceNumber":435} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:39 AM","latitude":40.440581333333334,"north":true,"longitude":-79.94778583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.2,"rawGPSLat":4026.43488,"rawGPSLong":7956.86715,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433279828,"topicName":"sensors/gps","sequenceNumber":436} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:40 AM","latitude":40.440606333333335,"north":true,"longitude":-79.94781183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.0,"rawGPSLat":4026.43638,"rawGPSLong":7956.86871,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433280323,"topicName":"sensors/gps","sequenceNumber":437} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:40 AM","latitude":40.440632,"north":true,"longitude":-79.9478365,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.9,"rawGPSLat":4026.43792,"rawGPSLong":7956.87019,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433280810,"topicName":"sensors/gps","sequenceNumber":438} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:41 AM","latitude":40.440659333333336,"north":true,"longitude":-79.9478585,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.7,"rawGPSLat":4026.43956,"rawGPSLong":7956.87151,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433281317,"topicName":"sensors/gps","sequenceNumber":439} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:41 AM","latitude":40.44068816666667,"north":true,"longitude":-79.9478785,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.6,"rawGPSLat":4026.44129,"rawGPSLong":7956.87271,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433281818,"topicName":"sensors/gps","sequenceNumber":440} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:42 AM","latitude":40.440718333333336,"north":true,"longitude":-79.94789633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.4,"rawGPSLat":4026.4431,"rawGPSLong":7956.87378,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433282315,"topicName":"sensors/gps","sequenceNumber":441} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:42 AM","latitude":40.4407485,"north":true,"longitude":-79.94791483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.3,"rawGPSLat":4026.44491,"rawGPSLong":7956.87489,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433282818,"topicName":"sensors/gps","sequenceNumber":442} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:43 AM","latitude":40.4407795,"north":true,"longitude":-79.9479315,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.2,"rawGPSLat":4026.44677,"rawGPSLong":7956.87589,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433283323,"topicName":"sensors/gps","sequenceNumber":443} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:43 AM","latitude":40.440810666666664,"north":true,"longitude":-79.94794616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.1,"rawGPSLat":4026.44864,"rawGPSLong":7956.87677,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433283835,"topicName":"sensors/gps","sequenceNumber":444} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:44 AM","latitude":40.44084333333333,"north":true,"longitude":-79.94795433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.0,"rawGPSLat":4026.4506,"rawGPSLong":7956.87726,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433284317,"topicName":"sensors/gps","sequenceNumber":445} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:44 AM","latitude":40.440876,"north":true,"longitude":-79.947957,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.0,"rawGPSLat":4026.45256,"rawGPSLong":7956.87742,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433284823,"topicName":"sensors/gps","sequenceNumber":446} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:45 AM","latitude":40.44090833333333,"north":true,"longitude":-79.94795483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":274.9,"rawGPSLat":4026.4545,"rawGPSLong":7956.87729,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433285307,"topicName":"sensors/gps","sequenceNumber":447} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:45 AM","latitude":40.44093933333333,"north":true,"longitude":-79.94794733333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":274.9,"rawGPSLat":4026.45636,"rawGPSLong":7956.87684,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433285794,"topicName":"sensors/gps","sequenceNumber":448} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:46 AM","latitude":40.44096783333333,"north":true,"longitude":-79.94793366666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.0,"rawGPSLat":4026.45807,"rawGPSLong":7956.87602,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433286307,"topicName":"sensors/gps","sequenceNumber":449} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:46 AM","latitude":40.44099333333333,"north":true,"longitude":-79.94791483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.1,"rawGPSLat":4026.4596,"rawGPSLong":7956.87489,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433286795,"topicName":"sensors/gps","sequenceNumber":450} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:47 AM","latitude":40.44101666666667,"north":true,"longitude":-79.947894,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.2,"rawGPSLat":4026.461,"rawGPSLong":7956.87364,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433287302,"topicName":"sensors/gps","sequenceNumber":451} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:47 AM","latitude":40.441039,"north":true,"longitude":-79.94787333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.2,"rawGPSLat":4026.46234,"rawGPSLong":7956.8724,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433287795,"topicName":"sensors/gps","sequenceNumber":452} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:48 AM","latitude":40.44105966666667,"north":true,"longitude":-79.94785283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.1,"rawGPSLat":4026.46358,"rawGPSLong":7956.87117,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433288310,"topicName":"sensors/gps","sequenceNumber":453} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:48 AM","latitude":40.441078,"north":true,"longitude":-79.94783016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.2,"rawGPSLat":4026.46468,"rawGPSLong":7956.86981,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433288807,"topicName":"sensors/gps","sequenceNumber":454} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:49 AM","latitude":40.44109533333334,"north":true,"longitude":-79.94780783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.3,"rawGPSLat":4026.46572,"rawGPSLong":7956.86847,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433289310,"topicName":"sensors/gps","sequenceNumber":455} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:49 AM","latitude":40.4411115,"north":true,"longitude":-79.94778716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.5,"rawGPSLat":4026.46669,"rawGPSLong":7956.86723,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433289807,"topicName":"sensors/gps","sequenceNumber":456} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:50 AM","latitude":40.441126833333335,"north":true,"longitude":-79.94776683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.6,"rawGPSLat":4026.46761,"rawGPSLong":7956.86601,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433290296,"topicName":"sensors/gps","sequenceNumber":457} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:50 AM","latitude":40.44114166666667,"north":true,"longitude":-79.94774783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.7,"rawGPSLat":4026.4685,"rawGPSLong":7956.86487,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433290805,"topicName":"sensors/gps","sequenceNumber":458} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:51 AM","latitude":40.441156166666666,"north":true,"longitude":-79.94773,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.9,"rawGPSLat":4026.46937,"rawGPSLong":7956.8638,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433291309,"topicName":"sensors/gps","sequenceNumber":459} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:51 AM","latitude":40.441169333333335,"north":true,"longitude":-79.94771316666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":276.2,"rawGPSLat":4026.47016,"rawGPSLong":7956.86279,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433291800,"topicName":"sensors/gps","sequenceNumber":460} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:52 AM","latitude":40.44118133333333,"north":true,"longitude":-79.947696,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.4,"rawGPSLat":4026.47088,"rawGPSLong":7956.86176,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433292308,"topicName":"sensors/gps","sequenceNumber":461} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:52 AM","latitude":40.44119283333333,"north":true,"longitude":-79.9476795,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.6,"rawGPSLat":4026.47157,"rawGPSLong":7956.86077,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433292807,"topicName":"sensors/gps","sequenceNumber":462} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:53 AM","latitude":40.441204,"north":true,"longitude":-79.94766483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.8,"rawGPSLat":4026.47224,"rawGPSLong":7956.85989,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433293303,"topicName":"sensors/gps","sequenceNumber":463} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:53 AM","latitude":40.441214333333335,"north":true,"longitude":-79.947651,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.0,"rawGPSLat":4026.47286,"rawGPSLong":7956.85906,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433293810,"topicName":"sensors/gps","sequenceNumber":464} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:54 AM","latitude":40.44122383333333,"north":true,"longitude":-79.9476375,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.1,"rawGPSLat":4026.47343,"rawGPSLong":7956.85825,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433294296,"topicName":"sensors/gps","sequenceNumber":465} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:54 AM","latitude":40.44123333333334,"north":true,"longitude":-79.94762533333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.3,"rawGPSLat":4026.474,"rawGPSLong":7956.85752,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433294809,"topicName":"sensors/gps","sequenceNumber":466} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:55 AM","latitude":40.441241833333336,"north":true,"longitude":-79.947614,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.5,"rawGPSLat":4026.47451,"rawGPSLong":7956.85684,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433295308,"topicName":"sensors/gps","sequenceNumber":467} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:55 AM","latitude":40.44125016666667,"north":true,"longitude":-79.94760316666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.8,"rawGPSLat":4026.47501,"rawGPSLong":7956.85619,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433295809,"topicName":"sensors/gps","sequenceNumber":468} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:56 AM","latitude":40.4412595,"north":true,"longitude":-79.94759183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.0,"rawGPSLat":4026.47557,"rawGPSLong":7956.85551,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433296297,"topicName":"sensors/gps","sequenceNumber":469} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:56 AM","latitude":40.44126883333333,"north":true,"longitude":-79.94757983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.1,"rawGPSLat":4026.47613,"rawGPSLong":7956.85479,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433296814,"topicName":"sensors/gps","sequenceNumber":470} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:57 AM","latitude":40.44127916666667,"north":true,"longitude":-79.9475655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.2,"rawGPSLat":4026.47675,"rawGPSLong":7956.85393,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433297300,"topicName":"sensors/gps","sequenceNumber":471} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:57 AM","latitude":40.44129066666667,"north":true,"longitude":-79.94754816666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.47744,"rawGPSLong":7956.85289,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433297808,"topicName":"sensors/gps","sequenceNumber":472} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:58 AM","latitude":40.441303,"north":true,"longitude":-79.94752783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.47818,"rawGPSLong":7956.85167,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433298308,"topicName":"sensors/gps","sequenceNumber":473} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:58 AM","latitude":40.44131733333333,"north":true,"longitude":-79.947508,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.4,"rawGPSLat":4026.47904,"rawGPSLong":7956.85048,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433298789,"topicName":"sensors/gps","sequenceNumber":474} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:59 AM","latitude":40.44133133333333,"north":true,"longitude":-79.94748383333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.6,"rawGPSLat":4026.47988,"rawGPSLong":7956.84903,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433299302,"topicName":"sensors/gps","sequenceNumber":475} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:59 AM","latitude":40.4413445,"north":true,"longitude":-79.94745766666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.7,"rawGPSLat":4026.48067,"rawGPSLong":7956.84746,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433299806,"topicName":"sensors/gps","sequenceNumber":476} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:00 AM","latitude":40.4413575,"north":true,"longitude":-79.94742983333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48145,"rawGPSLong":7956.84579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433300306,"topicName":"sensors/gps","sequenceNumber":477} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:00 AM","latitude":40.44137216666667,"north":true,"longitude":-79.94740383333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48233,"rawGPSLong":7956.84423,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433300812,"topicName":"sensors/gps","sequenceNumber":478} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:01 AM","latitude":40.44138816666667,"north":true,"longitude":-79.9473805,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48329,"rawGPSLong":7956.84283,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433301300,"topicName":"sensors/gps","sequenceNumber":479} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:01 AM","latitude":40.441402833333335,"north":true,"longitude":-79.94735833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.8,"rawGPSLat":4026.48417,"rawGPSLong":7956.8415,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433301804,"topicName":"sensors/gps","sequenceNumber":480} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:02 AM","latitude":40.441414333333334,"north":true,"longitude":-79.9473345,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.48486,"rawGPSLong":7956.84007,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433302289,"topicName":"sensors/gps","sequenceNumber":481} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:02 AM","latitude":40.44142516666667,"north":true,"longitude":-79.94730883333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.48551,"rawGPSLong":7956.83853,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433302805,"topicName":"sensors/gps","sequenceNumber":482} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:03 AM","latitude":40.44143533333333,"north":true,"longitude":-79.947281,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.48612,"rawGPSLong":7956.83686,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433303305,"topicName":"sensors/gps","sequenceNumber":483} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:03 AM","latitude":40.4414435,"north":true,"longitude":-79.947252,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.4,"rawGPSLat":4026.48661,"rawGPSLong":7956.83512,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433303801,"topicName":"sensors/gps","sequenceNumber":484} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:04 AM","latitude":40.44145016666667,"north":true,"longitude":-79.947221,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.7,"rawGPSLat":4026.48701,"rawGPSLong":7956.83326,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433304288,"topicName":"sensors/gps","sequenceNumber":485} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:04 AM","latitude":40.441456333333335,"north":true,"longitude":-79.94719066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48738,"rawGPSLong":7956.83144,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433304793,"topicName":"sensors/gps","sequenceNumber":486} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:05 AM","latitude":40.44146116666667,"north":true,"longitude":-79.94716033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.3,"rawGPSLat":4026.48767,"rawGPSLong":7956.82962,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433305284,"topicName":"sensors/gps","sequenceNumber":487} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:05 AM","latitude":40.441464,"north":true,"longitude":-79.94713033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.6,"rawGPSLat":4026.48784,"rawGPSLong":7956.82782,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433305807,"topicName":"sensors/gps","sequenceNumber":488} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:06 AM","latitude":40.4414635,"north":true,"longitude":-79.94710083333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.9,"rawGPSLat":4026.48781,"rawGPSLong":7956.82605,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433306305,"topicName":"sensors/gps","sequenceNumber":489} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:06 AM","latitude":40.441460666666664,"north":true,"longitude":-79.94707166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.3,"rawGPSLat":4026.48764,"rawGPSLong":7956.8243,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433306808,"topicName":"sensors/gps","sequenceNumber":490} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:07 AM","latitude":40.44145816666666,"north":true,"longitude":-79.94704366666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.4,"rawGPSLat":4026.48749,"rawGPSLong":7956.82262,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433307293,"topicName":"sensors/gps","sequenceNumber":491} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:07 AM","latitude":40.441455833333336,"north":true,"longitude":-79.94701566666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.7,"rawGPSLat":4026.48735,"rawGPSLong":7956.82094,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433307810,"topicName":"sensors/gps","sequenceNumber":492} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:08 AM","latitude":40.44145416666667,"north":true,"longitude":-79.94698816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.7,"rawGPSLat":4026.48725,"rawGPSLong":7956.81929,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433308293,"topicName":"sensors/gps","sequenceNumber":493} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:08 AM","latitude":40.441452,"north":true,"longitude":-79.94696066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.9,"rawGPSLat":4026.48712,"rawGPSLong":7956.81764,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433308801,"topicName":"sensors/gps","sequenceNumber":494} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:09 AM","latitude":40.44144883333333,"north":true,"longitude":-79.94693333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.0,"rawGPSLat":4026.48693,"rawGPSLong":7956.816,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433309290,"topicName":"sensors/gps","sequenceNumber":495} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:09 AM","latitude":40.4414445,"north":true,"longitude":-79.94690616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.1,"rawGPSLat":4026.48667,"rawGPSLong":7956.81437,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433309801,"topicName":"sensors/gps","sequenceNumber":496} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:10 AM","latitude":40.4414395,"north":true,"longitude":-79.94687866666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.3,"rawGPSLat":4026.48637,"rawGPSLong":7956.81272,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433310312,"topicName":"sensors/gps","sequenceNumber":497} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:10 AM","latitude":40.441435166666665,"north":true,"longitude":-79.9468515,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.5,"rawGPSLat":4026.48611,"rawGPSLong":7956.81109,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433310795,"topicName":"sensors/gps","sequenceNumber":498} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:11 AM","latitude":40.441431666666666,"north":true,"longitude":-79.94682366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.6,"rawGPSLat":4026.4859,"rawGPSLong":7956.80942,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433311308,"topicName":"sensors/gps","sequenceNumber":499} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:11 AM","latitude":40.4414295,"north":true,"longitude":-79.946796,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.7,"rawGPSLat":4026.48577,"rawGPSLong":7956.80776,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433311802,"topicName":"sensors/gps","sequenceNumber":500} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:12 AM","latitude":40.44142633333333,"north":true,"longitude":-79.946769,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.9,"rawGPSLat":4026.48558,"rawGPSLong":7956.80614,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433312296,"topicName":"sensors/gps","sequenceNumber":501} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:12 AM","latitude":40.44142133333333,"north":true,"longitude":-79.9467425,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.1,"rawGPSLat":4026.48528,"rawGPSLong":7956.80455,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433312802,"topicName":"sensors/gps","sequenceNumber":502} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:13 AM","latitude":40.44141583333333,"north":true,"longitude":-79.94671683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.3,"rawGPSLat":4026.48495,"rawGPSLong":7956.80301,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433313311,"topicName":"sensors/gps","sequenceNumber":503} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:13 AM","latitude":40.441410166666664,"north":true,"longitude":-79.9466915,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.5,"rawGPSLat":4026.48461,"rawGPSLong":7956.80149,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433313817,"topicName":"sensors/gps","sequenceNumber":504} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:14 AM","latitude":40.44140516666667,"north":true,"longitude":-79.9466655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.6,"rawGPSLat":4026.48431,"rawGPSLong":7956.79993,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433314309,"topicName":"sensors/gps","sequenceNumber":505} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:14 AM","latitude":40.441401,"north":true,"longitude":-79.94663966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.8,"rawGPSLat":4026.48406,"rawGPSLong":7956.79838,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433314789,"topicName":"sensors/gps","sequenceNumber":506} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:15 AM","latitude":40.4413975,"north":true,"longitude":-79.94661366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.0,"rawGPSLat":4026.48385,"rawGPSLong":7956.79682,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433315300,"topicName":"sensors/gps","sequenceNumber":507} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:15 AM","latitude":40.441394,"north":true,"longitude":-79.94658783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.1,"rawGPSLat":4026.48364,"rawGPSLong":7956.79527,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433315785,"topicName":"sensors/gps","sequenceNumber":508} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:16 AM","latitude":40.4413905,"north":true,"longitude":-79.94656283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.2,"rawGPSLat":4026.48343,"rawGPSLong":7956.79377,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433316301,"topicName":"sensors/gps","sequenceNumber":509} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:16 AM","latitude":40.441386333333334,"north":true,"longitude":-79.946538,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.3,"rawGPSLat":4026.48318,"rawGPSLong":7956.79228,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433316801,"topicName":"sensors/gps","sequenceNumber":510} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:17 AM","latitude":40.4413825,"north":true,"longitude":-79.94651316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.4,"rawGPSLat":4026.48295,"rawGPSLong":7956.79079,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433317291,"topicName":"sensors/gps","sequenceNumber":511} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:17 AM","latitude":40.441379,"north":true,"longitude":-79.946488,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.6,"rawGPSLat":4026.48274,"rawGPSLong":7956.78928,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433317801,"topicName":"sensors/gps","sequenceNumber":512} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:18 AM","latitude":40.441374833333334,"north":true,"longitude":-79.94646266666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.7,"rawGPSLat":4026.48249,"rawGPSLong":7956.78776,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433318309,"topicName":"sensors/gps","sequenceNumber":513} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:18 AM","latitude":40.44136966666667,"north":true,"longitude":-79.94643816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.8,"rawGPSLat":4026.48218,"rawGPSLong":7956.78629,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433318787,"topicName":"sensors/gps","sequenceNumber":514} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:19 AM","latitude":40.441364,"north":true,"longitude":-79.94641333333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.0,"rawGPSLat":4026.48184,"rawGPSLong":7956.7848,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433319302,"topicName":"sensors/gps","sequenceNumber":515} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:19 AM","latitude":40.44135883333333,"north":true,"longitude":-79.94638883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.1,"rawGPSLat":4026.48153,"rawGPSLong":7956.78333,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433319808,"topicName":"sensors/gps","sequenceNumber":516} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:20 AM","latitude":40.441354,"north":true,"longitude":-79.94636416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.2,"rawGPSLat":4026.48124,"rawGPSLong":7956.78185,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433320312,"topicName":"sensors/gps","sequenceNumber":517} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:20 AM","latitude":40.4413505,"north":true,"longitude":-79.9463395,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.4,"rawGPSLat":4026.48103,"rawGPSLong":7956.78037,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433320796,"topicName":"sensors/gps","sequenceNumber":518} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:21 AM","latitude":40.44134733333333,"north":true,"longitude":-79.9463145,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.5,"rawGPSLat":4026.48084,"rawGPSLong":7956.77887,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433321304,"topicName":"sensors/gps","sequenceNumber":519} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:21 AM","latitude":40.44134433333333,"north":true,"longitude":-79.94629,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.6,"rawGPSLat":4026.48066,"rawGPSLong":7956.7774,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433321801,"topicName":"sensors/gps","sequenceNumber":520} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:22 AM","latitude":40.441340833333335,"north":true,"longitude":-79.94626583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.7,"rawGPSLat":4026.48045,"rawGPSLong":7956.77595,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433322310,"topicName":"sensors/gps","sequenceNumber":521} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:22 AM","latitude":40.441336166666666,"north":true,"longitude":-79.9462415,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.8,"rawGPSLat":4026.48017,"rawGPSLong":7956.77449,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433322804,"topicName":"sensors/gps","sequenceNumber":522} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:23 AM","latitude":40.4413305,"north":true,"longitude":-79.9462175,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.0,"rawGPSLat":4026.47983,"rawGPSLong":7956.77305,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433323301,"topicName":"sensors/gps","sequenceNumber":523} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:23 AM","latitude":40.44132466666667,"north":true,"longitude":-79.94619483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.2,"rawGPSLat":4026.47948,"rawGPSLong":7956.77169,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433323800,"topicName":"sensors/gps","sequenceNumber":524} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:24 AM","latitude":40.441319,"north":true,"longitude":-79.94617233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.4,"rawGPSLat":4026.47914,"rawGPSLong":7956.77034,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433324293,"topicName":"sensors/gps","sequenceNumber":525} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:24 AM","latitude":40.44131483333334,"north":true,"longitude":-79.94614916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.5,"rawGPSLat":4026.47889,"rawGPSLong":7956.76895,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433324800,"topicName":"sensors/gps","sequenceNumber":526} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:25 AM","latitude":40.44131083333333,"north":true,"longitude":-79.946127,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.7,"rawGPSLat":4026.47865,"rawGPSLong":7956.76762,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433325288,"topicName":"sensors/gps","sequenceNumber":527} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:25 AM","latitude":40.4413075,"north":true,"longitude":-79.94610266666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.9,"rawGPSLat":4026.47845,"rawGPSLong":7956.76616,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433325812,"topicName":"sensors/gps","sequenceNumber":528} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:26 AM","latitude":40.4413025,"north":true,"longitude":-79.94607733333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.1,"rawGPSLat":4026.47815,"rawGPSLong":7956.76464,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433326288,"topicName":"sensors/gps","sequenceNumber":529} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:26 AM","latitude":40.441297166666665,"north":true,"longitude":-79.94605416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.3,"rawGPSLat":4026.47783,"rawGPSLong":7956.76325,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433326799,"topicName":"sensors/gps","sequenceNumber":530} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:27 AM","latitude":40.4412935,"north":true,"longitude":-79.94603383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.4,"rawGPSLat":4026.47761,"rawGPSLong":7956.76203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433327301,"topicName":"sensors/gps","sequenceNumber":531} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:27 AM","latitude":40.44128966666667,"north":true,"longitude":-79.946015,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.6,"rawGPSLat":4026.47738,"rawGPSLong":7956.7609,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433327810,"topicName":"sensors/gps","sequenceNumber":532} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:28 AM","latitude":40.44128633333333,"north":true,"longitude":-79.9459965,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.8,"rawGPSLat":4026.47718,"rawGPSLong":7956.75979,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433328306,"topicName":"sensors/gps","sequenceNumber":533} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:28 AM","latitude":40.44128216666667,"north":true,"longitude":-79.9459765,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.3,"rawGPSLat":4026.47693,"rawGPSLong":7956.75859,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433328808,"topicName":"sensors/gps","sequenceNumber":534} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:29 AM","latitude":40.441278,"north":true,"longitude":-79.94595483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.5,"rawGPSLat":4026.47668,"rawGPSLong":7956.75729,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433329305,"topicName":"sensors/gps","sequenceNumber":535} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:29 AM","latitude":40.441274166666666,"north":true,"longitude":-79.9459345,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.7,"rawGPSLat":4026.47645,"rawGPSLong":7956.75607,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433329806,"topicName":"sensors/gps","sequenceNumber":536} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:30 AM","latitude":40.441270833333334,"north":true,"longitude":-79.94591616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.47625,"rawGPSLong":7956.75497,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433330301,"topicName":"sensors/gps","sequenceNumber":537} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:30 AM","latitude":40.44126633333333,"north":true,"longitude":-79.94589316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.9,"rawGPSLat":4026.47598,"rawGPSLong":7956.75359,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433330806,"topicName":"sensors/gps","sequenceNumber":538} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:31 AM","latitude":40.44126033333333,"north":true,"longitude":-79.94586883333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.2,"rawGPSLat":4026.47562,"rawGPSLong":7956.75213,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433331302,"topicName":"sensors/gps","sequenceNumber":539} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:31 AM","latitude":40.441254666666666,"north":true,"longitude":-79.945847,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.3,"rawGPSLat":4026.47528,"rawGPSLong":7956.75082,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433331808,"topicName":"sensors/gps","sequenceNumber":540} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:32 AM","latitude":40.44124933333333,"north":true,"longitude":-79.94582816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.6,"rawGPSLat":4026.47496,"rawGPSLong":7956.74969,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433332304,"topicName":"sensors/gps","sequenceNumber":541} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:32 AM","latitude":40.44124583333333,"north":true,"longitude":-79.94581033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.7,"rawGPSLat":4026.47475,"rawGPSLong":7956.74862,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433332805,"topicName":"sensors/gps","sequenceNumber":542} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:33 AM","latitude":40.44124083333333,"north":true,"longitude":-79.94579033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.9,"rawGPSLat":4026.47445,"rawGPSLong":7956.74742,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433333301,"topicName":"sensors/gps","sequenceNumber":543} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:33 AM","latitude":40.441236,"north":true,"longitude":-79.9457655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.47416,"rawGPSLong":7956.74593,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433333802,"topicName":"sensors/gps","sequenceNumber":544} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:34 AM","latitude":40.441230166666664,"north":true,"longitude":-79.94574283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.3,"rawGPSLat":4026.47381,"rawGPSLong":7956.74457,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433334309,"topicName":"sensors/gps","sequenceNumber":545} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:34 AM","latitude":40.441225333333335,"north":true,"longitude":-79.94572183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47352,"rawGPSLong":7956.74331,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433334797,"topicName":"sensors/gps","sequenceNumber":546} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:35 AM","latitude":40.44122216666667,"north":true,"longitude":-79.945702,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.47333,"rawGPSLong":7956.74212,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433335308,"topicName":"sensors/gps","sequenceNumber":547} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:35 AM","latitude":40.44121966666667,"north":true,"longitude":-79.94568366666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.47318,"rawGPSLong":7956.74102,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433335797,"topicName":"sensors/gps","sequenceNumber":548} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:36 AM","latitude":40.44121616666666,"north":true,"longitude":-79.94566333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.47297,"rawGPSLong":7956.7398,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433336304,"topicName":"sensors/gps","sequenceNumber":549} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:36 AM","latitude":40.441212166666666,"north":true,"longitude":-79.94563933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47273,"rawGPSLong":7956.73836,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433336802,"topicName":"sensors/gps","sequenceNumber":550} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:37 AM","latitude":40.44120816666667,"north":true,"longitude":-79.94561583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47249,"rawGPSLong":7956.73695,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433337307,"topicName":"sensors/gps","sequenceNumber":551} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:37 AM","latitude":40.441205,"north":true,"longitude":-79.9455935,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.4723,"rawGPSLong":7956.73561,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433337803,"topicName":"sensors/gps","sequenceNumber":552} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:38 AM","latitude":40.44120216666666,"north":true,"longitude":-79.94557383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47213,"rawGPSLong":7956.73443,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433338310,"topicName":"sensors/gps","sequenceNumber":553} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:38 AM","latitude":40.44119966666667,"north":true,"longitude":-79.945556,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.47198,"rawGPSLong":7956.73336,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433338808,"topicName":"sensors/gps","sequenceNumber":554} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:39 AM","latitude":40.441196833333336,"north":true,"longitude":-79.94553833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.47181,"rawGPSLong":7956.7323,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433339308,"topicName":"sensors/gps","sequenceNumber":555} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:39 AM","latitude":40.4411935,"north":true,"longitude":-79.945517,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.47161,"rawGPSLong":7956.73102,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433339808,"topicName":"sensors/gps","sequenceNumber":556} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:40 AM","latitude":40.441190166666665,"north":true,"longitude":-79.94549383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.47141,"rawGPSLong":7956.72963,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433340308,"topicName":"sensors/gps","sequenceNumber":557} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:40 AM","latitude":40.441187166666666,"north":true,"longitude":-79.9454715,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.47123,"rawGPSLong":7956.72829,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433340804,"topicName":"sensors/gps","sequenceNumber":558} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:41 AM","latitude":40.44118316666667,"north":true,"longitude":-79.94545233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.47099,"rawGPSLong":7956.72714,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433341303,"topicName":"sensors/gps","sequenceNumber":559} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:41 AM","latitude":40.441179,"north":true,"longitude":-79.945435,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.7,"rawGPSLat":4026.47074,"rawGPSLong":7956.7261,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433341788,"topicName":"sensors/gps","sequenceNumber":560} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:42 AM","latitude":40.4411755,"north":true,"longitude":-79.94541866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.47053,"rawGPSLong":7956.72512,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433342301,"topicName":"sensors/gps","sequenceNumber":561} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:42 AM","latitude":40.441171,"north":true,"longitude":-79.9453975,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.47026,"rawGPSLong":7956.72385,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433342808,"topicName":"sensors/gps","sequenceNumber":562} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:43 AM","latitude":40.441166333333335,"north":true,"longitude":-79.945373,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.46998,"rawGPSLong":7956.72238,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433343298,"topicName":"sensors/gps","sequenceNumber":563} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:43 AM","latitude":40.4411625,"north":true,"longitude":-79.94534966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.0,"rawGPSLat":4026.46975,"rawGPSLong":7956.72098,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433343821,"topicName":"sensors/gps","sequenceNumber":564} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:44 AM","latitude":40.441159166666665,"north":true,"longitude":-79.94532783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.0,"rawGPSLat":4026.46955,"rawGPSLong":7956.71967,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433344300,"topicName":"sensors/gps","sequenceNumber":565} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:44 AM","latitude":40.441156666666664,"north":true,"longitude":-79.94530783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.0,"rawGPSLat":4026.4694,"rawGPSLong":7956.71847,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433344800,"topicName":"sensors/gps","sequenceNumber":566} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:45 AM","latitude":40.44115383333333,"north":true,"longitude":-79.94528783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.1,"rawGPSLat":4026.46923,"rawGPSLong":7956.71727,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433345301,"topicName":"sensors/gps","sequenceNumber":567} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:45 AM","latitude":40.441149,"north":true,"longitude":-79.945264,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.1,"rawGPSLat":4026.46894,"rawGPSLong":7956.71584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433345791,"topicName":"sensors/gps","sequenceNumber":568} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:46 AM","latitude":40.44114366666667,"north":true,"longitude":-79.94523866666667,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.14,"antennaAltitude":290.0,"rawGPSLat":4026.46862,"rawGPSLong":7956.71432,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433346294,"topicName":"sensors/gps","sequenceNumber":569} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:46 AM","latitude":40.441139166666666,"north":true,"longitude":-79.94521483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.46835,"rawGPSLong":7956.71289,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433346798,"topicName":"sensors/gps","sequenceNumber":570} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:47 AM","latitude":40.441135,"north":true,"longitude":-79.9451925,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.4681,"rawGPSLong":7956.71155,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433347300,"topicName":"sensors/gps","sequenceNumber":571} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:47 AM","latitude":40.441130333333334,"north":true,"longitude":-79.94517183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.46782,"rawGPSLong":7956.71031,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433347803,"topicName":"sensors/gps","sequenceNumber":572} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:48 AM","latitude":40.44112583333333,"north":true,"longitude":-79.94515266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.46755,"rawGPSLong":7956.70916,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433348314,"topicName":"sensors/gps","sequenceNumber":573} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:48 AM","latitude":40.44112116666667,"north":true,"longitude":-79.94513333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.0,"rawGPSLat":4026.46727,"rawGPSLong":7956.708,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433348792,"topicName":"sensors/gps","sequenceNumber":574} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:49 AM","latitude":40.4411165,"north":true,"longitude":-79.945109,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.46699,"rawGPSLong":7956.70654,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433349307,"topicName":"sensors/gps","sequenceNumber":575} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:49 AM","latitude":40.44111183333333,"north":true,"longitude":-79.94508483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.46671,"rawGPSLong":7956.70509,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433349809,"topicName":"sensors/gps","sequenceNumber":576} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:50 AM","latitude":40.44110766666667,"north":true,"longitude":-79.94506133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.2,"rawGPSLat":4026.46646,"rawGPSLong":7956.70368,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433350301,"topicName":"sensors/gps","sequenceNumber":577} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:50 AM","latitude":40.4411025,"north":true,"longitude":-79.94504033333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.2,"rawGPSLat":4026.46615,"rawGPSLong":7956.70242,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433350803,"topicName":"sensors/gps","sequenceNumber":578} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:51 AM","latitude":40.44109783333333,"north":true,"longitude":-79.94502033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.3,"rawGPSLat":4026.46587,"rawGPSLong":7956.70122,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433351307,"topicName":"sensors/gps","sequenceNumber":579} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:51 AM","latitude":40.44109366666667,"north":true,"longitude":-79.945,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.3,"rawGPSLat":4026.46562,"rawGPSLong":7956.7,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433351803,"topicName":"sensors/gps","sequenceNumber":580} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:52 AM","latitude":40.44108933333333,"north":true,"longitude":-79.94497566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.3,"rawGPSLat":4026.46536,"rawGPSLong":7956.69854,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433352303,"topicName":"sensors/gps","sequenceNumber":581} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:52 AM","latitude":40.4410845,"north":true,"longitude":-79.94494966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.4,"rawGPSLat":4026.46507,"rawGPSLong":7956.69698,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433352792,"topicName":"sensors/gps","sequenceNumber":582} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:53 AM","latitude":40.44107916666667,"north":true,"longitude":-79.94492566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.5,"rawGPSLat":4026.46475,"rawGPSLong":7956.69554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433353298,"topicName":"sensors/gps","sequenceNumber":583} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:53 AM","latitude":40.44107316666667,"north":true,"longitude":-79.944903,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.7,"rawGPSLat":4026.46439,"rawGPSLong":7956.69418,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433353803,"topicName":"sensors/gps","sequenceNumber":584} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:54 AM","latitude":40.44106766666667,"north":true,"longitude":-79.94488183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.8,"rawGPSLat":4026.46406,"rawGPSLong":7956.69291,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433354288,"topicName":"sensors/gps","sequenceNumber":585} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:54 AM","latitude":40.441063166666666,"north":true,"longitude":-79.94486166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.8,"rawGPSLat":4026.46379,"rawGPSLong":7956.6917,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433354801,"topicName":"sensors/gps","sequenceNumber":586} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:55 AM","latitude":40.44105916666667,"north":true,"longitude":-79.94484066666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.7,"rawGPSLat":4026.46355,"rawGPSLong":7956.69044,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433355302,"topicName":"sensors/gps","sequenceNumber":587} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:55 AM","latitude":40.441055166666665,"north":true,"longitude":-79.94481516666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.46331,"rawGPSLong":7956.68891,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433355814,"topicName":"sensors/gps","sequenceNumber":588} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:56 AM","latitude":40.4410515,"north":true,"longitude":-79.94478883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.46309,"rawGPSLong":7956.68733,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433356296,"topicName":"sensors/gps","sequenceNumber":589} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:56 AM","latitude":40.441047833333336,"north":true,"longitude":-79.9447635,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.46287,"rawGPSLong":7956.68581,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433356809,"topicName":"sensors/gps","sequenceNumber":590} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:57 AM","latitude":40.44104316666667,"north":true,"longitude":-79.94473966666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.8,"rawGPSLat":4026.46259,"rawGPSLong":7956.68438,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433357297,"topicName":"sensors/gps","sequenceNumber":591} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:57 AM","latitude":40.4410385,"north":true,"longitude":-79.94471683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.8,"rawGPSLat":4026.46231,"rawGPSLong":7956.68301,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433357812,"topicName":"sensors/gps","sequenceNumber":592} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:58 AM","latitude":40.44103416666667,"north":true,"longitude":-79.944695,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.9,"rawGPSLat":4026.46205,"rawGPSLong":7956.6817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433358298,"topicName":"sensors/gps","sequenceNumber":593} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:58 AM","latitude":40.441030166666664,"north":true,"longitude":-79.94467183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.0,"rawGPSLat":4026.46181,"rawGPSLong":7956.68031,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433358813,"topicName":"sensors/gps","sequenceNumber":594} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:59 AM","latitude":40.4410255,"north":true,"longitude":-79.944647,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.1,"rawGPSLat":4026.46153,"rawGPSLong":7956.67882,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433359305,"topicName":"sensors/gps","sequenceNumber":595} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:59 AM","latitude":40.441019,"north":true,"longitude":-79.9446215,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.1,"rawGPSLat":4026.46114,"rawGPSLong":7956.67729,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433359811,"topicName":"sensors/gps","sequenceNumber":596} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:00 AM","latitude":40.441011833333334,"north":true,"longitude":-79.94459583333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.2,"rawGPSLat":4026.46071,"rawGPSLong":7956.67575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433360295,"topicName":"sensors/gps","sequenceNumber":597} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:00 AM","latitude":40.44100566666667,"north":true,"longitude":-79.94457033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.3,"rawGPSLat":4026.46034,"rawGPSLong":7956.67422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433360807,"topicName":"sensors/gps","sequenceNumber":598} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:01 AM","latitude":40.44100016666667,"north":true,"longitude":-79.94454616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.4,"rawGPSLat":4026.46001,"rawGPSLong":7956.67277,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433361322,"topicName":"sensors/gps","sequenceNumber":599} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:01 AM","latitude":40.440995666666666,"north":true,"longitude":-79.94452233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.6,"rawGPSLat":4026.45974,"rawGPSLong":7956.67134,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433361799,"topicName":"sensors/gps","sequenceNumber":600} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:02 AM","latitude":40.440991833333335,"north":true,"longitude":-79.94449966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.6,"rawGPSLat":4026.45951,"rawGPSLong":7956.66998,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433362296,"topicName":"sensors/gps","sequenceNumber":601} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:02 AM","latitude":40.44098833333334,"north":true,"longitude":-79.94447883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.5,"rawGPSLat":4026.4593,"rawGPSLong":7956.66873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433362808,"topicName":"sensors/gps","sequenceNumber":602} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:03 AM","latitude":40.440984666666665,"north":true,"longitude":-79.94445616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.4,"rawGPSLat":4026.45908,"rawGPSLong":7956.66737,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433363309,"topicName":"sensors/gps","sequenceNumber":603} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:03 AM","latitude":40.440979166666665,"north":true,"longitude":-79.944428,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.5,"rawGPSLat":4026.45875,"rawGPSLong":7956.66568,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433363807,"topicName":"sensors/gps","sequenceNumber":604} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:04 AM","latitude":40.44097466666667,"north":true,"longitude":-79.944401,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.45848,"rawGPSLong":7956.66406,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433364313,"topicName":"sensors/gps","sequenceNumber":605} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:04 AM","latitude":40.44097,"north":true,"longitude":-79.94437433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.4582,"rawGPSLong":7956.66246,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433364784,"topicName":"sensors/gps","sequenceNumber":606} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:05 AM","latitude":40.440964,"north":true,"longitude":-79.94434866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.2,"rawGPSLat":4026.45784,"rawGPSLong":7956.66092,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433365308,"topicName":"sensors/gps","sequenceNumber":607} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:05 AM","latitude":40.4409585,"north":true,"longitude":-79.94432166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.45751,"rawGPSLong":7956.6593,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433365801,"topicName":"sensors/gps","sequenceNumber":608} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:06 AM","latitude":40.440953666666665,"north":true,"longitude":-79.944289,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.45722,"rawGPSLong":7956.65734,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433366323,"topicName":"sensors/gps","sequenceNumber":609} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:06 AM","latitude":40.440949333333336,"north":true,"longitude":-79.944256,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.4,"rawGPSLat":4026.45696,"rawGPSLong":7956.65536,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433366808,"topicName":"sensors/gps","sequenceNumber":610} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:07 AM","latitude":40.4409405,"north":true,"longitude":-79.944226,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.5,"rawGPSLat":4026.45643,"rawGPSLong":7956.65356,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433367305,"topicName":"sensors/gps","sequenceNumber":611} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:07 AM","latitude":40.44092916666666,"north":true,"longitude":-79.94419883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.7,"rawGPSLat":4026.45575,"rawGPSLong":7956.65193,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433367832,"topicName":"sensors/gps","sequenceNumber":612} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:08 AM","latitude":40.44091916666667,"north":true,"longitude":-79.94417233333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.45515,"rawGPSLong":7956.65034,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433368305,"topicName":"sensors/gps","sequenceNumber":613} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:08 AM","latitude":40.440911666666665,"north":true,"longitude":-79.94414483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.4547,"rawGPSLong":7956.64869,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433368792,"topicName":"sensors/gps","sequenceNumber":614} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:09 AM","latitude":40.44090516666667,"north":true,"longitude":-79.94411733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.45431,"rawGPSLong":7956.64704,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433369300,"topicName":"sensors/gps","sequenceNumber":615} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:09 AM","latitude":40.44089916666667,"north":true,"longitude":-79.94408866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.1,"rawGPSLat":4026.45395,"rawGPSLong":7956.64532,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433369800,"topicName":"sensors/gps","sequenceNumber":616} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:10 AM","latitude":40.44089016666667,"north":true,"longitude":-79.94405933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.45341,"rawGPSLong":7956.64356,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433370318,"topicName":"sensors/gps","sequenceNumber":617} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:10 AM","latitude":40.440880166666666,"north":true,"longitude":-79.94402933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.45281,"rawGPSLong":7956.64176,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433370816,"topicName":"sensors/gps","sequenceNumber":618} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:11 AM","latitude":40.440870833333335,"north":true,"longitude":-79.94400083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.6,"rawGPSLat":4026.45225,"rawGPSLong":7956.64005,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433371315,"topicName":"sensors/gps","sequenceNumber":619} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:11 AM","latitude":40.44086333333333,"north":true,"longitude":-79.94397166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.4518,"rawGPSLong":7956.6383,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433371816,"topicName":"sensors/gps","sequenceNumber":620} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:12 AM","latitude":40.44085583333333,"north":true,"longitude":-79.943942,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.45135,"rawGPSLong":7956.63652,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433372316,"topicName":"sensors/gps","sequenceNumber":621} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:12 AM","latitude":40.440848333333335,"north":true,"longitude":-79.9439125,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.4509,"rawGPSLong":7956.63475,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433372805,"topicName":"sensors/gps","sequenceNumber":622} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:13 AM","latitude":40.440839,"north":true,"longitude":-79.9438845,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.2,"rawGPSLat":4026.45034,"rawGPSLong":7956.63307,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433373307,"topicName":"sensors/gps","sequenceNumber":623} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:13 AM","latitude":40.440833833333336,"north":true,"longitude":-79.94385833333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.1,"rawGPSLat":4026.45003,"rawGPSLong":7956.6315,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433373821,"topicName":"sensors/gps","sequenceNumber":624} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:14 AM","latitude":40.4408295,"north":true,"longitude":-79.94383216666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.44977,"rawGPSLong":7956.62993,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433374295,"topicName":"sensors/gps","sequenceNumber":625} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:14 AM","latitude":40.44082216666666,"north":true,"longitude":-79.94380283333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.44933,"rawGPSLong":7956.62817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433374823,"topicName":"sensors/gps","sequenceNumber":626} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:15 AM","latitude":40.440814,"north":true,"longitude":-79.94377433333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.6,"rawGPSLat":4026.44884,"rawGPSLong":7956.62646,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433375316,"topicName":"sensors/gps","sequenceNumber":627} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:15 AM","latitude":40.440804166666666,"north":true,"longitude":-79.94374866666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.6,"rawGPSLat":4026.44825,"rawGPSLong":7956.62492,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433375807,"topicName":"sensors/gps","sequenceNumber":628} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:16 AM","latitude":40.4407955,"north":true,"longitude":-79.94371916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.44773,"rawGPSLong":7956.62315,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433376305,"topicName":"sensors/gps","sequenceNumber":629} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:16 AM","latitude":40.440788833333336,"north":true,"longitude":-79.94369266666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.44733,"rawGPSLong":7956.62156,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433376804,"topicName":"sensors/gps","sequenceNumber":630} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:17 AM","latitude":40.440782166666665,"north":true,"longitude":-79.943666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.44693,"rawGPSLong":7956.61996,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433377300,"topicName":"sensors/gps","sequenceNumber":631} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:17 AM","latitude":40.44077816666667,"north":true,"longitude":-79.94363683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.4,"rawGPSLat":4026.44669,"rawGPSLong":7956.61821,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433377796,"topicName":"sensors/gps","sequenceNumber":632} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:18 AM","latitude":40.440773666666665,"north":true,"longitude":-79.94360633333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.7,"rawGPSLat":4026.44642,"rawGPSLong":7956.61638,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433378315,"topicName":"sensors/gps","sequenceNumber":633} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:18 AM","latitude":40.44076866666666,"north":true,"longitude":-79.94357816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.44612,"rawGPSLong":7956.61469,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433378800,"topicName":"sensors/gps","sequenceNumber":634} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:19 AM","latitude":40.440764333333334,"north":true,"longitude":-79.94354966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.44586,"rawGPSLong":7956.61298,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433379302,"topicName":"sensors/gps","sequenceNumber":635} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:19 AM","latitude":40.440760166666664,"north":true,"longitude":-79.94352183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.44561,"rawGPSLong":7956.61131,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433379787,"topicName":"sensors/gps","sequenceNumber":636} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:20 AM","latitude":40.440755,"north":true,"longitude":-79.943493,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.4453,"rawGPSLong":7956.60958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433380306,"topicName":"sensors/gps","sequenceNumber":637} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:20 AM","latitude":40.4407505,"north":true,"longitude":-79.9434615,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.44503,"rawGPSLong":7956.60769,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433380819,"topicName":"sensors/gps","sequenceNumber":638} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:21 AM","latitude":40.440745666666665,"north":true,"longitude":-79.94343183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.7,"rawGPSLat":4026.44474,"rawGPSLong":7956.60591,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433381323,"topicName":"sensors/gps","sequenceNumber":639} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:21 AM","latitude":40.44074033333333,"north":true,"longitude":-79.94340283333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.6,"rawGPSLat":4026.44442,"rawGPSLong":7956.60417,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433381791,"topicName":"sensors/gps","sequenceNumber":640} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:22 AM","latitude":40.440734,"north":true,"longitude":-79.94337416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.44404,"rawGPSLong":7956.60245,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433382304,"topicName":"sensors/gps","sequenceNumber":641} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:22 AM","latitude":40.440728166666666,"north":true,"longitude":-79.94334733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.44369,"rawGPSLong":7956.60084,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433382809,"topicName":"sensors/gps","sequenceNumber":642} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:23 AM","latitude":40.440721833333335,"north":true,"longitude":-79.94332183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.3,"rawGPSLat":4026.44331,"rawGPSLong":7956.59931,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433383293,"topicName":"sensors/gps","sequenceNumber":643} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:23 AM","latitude":40.440717166666666,"north":true,"longitude":-79.94329516666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.44303,"rawGPSLong":7956.59771,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433383796,"topicName":"sensors/gps","sequenceNumber":644} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:24 AM","latitude":40.440712,"north":true,"longitude":-79.94326483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.7,"rawGPSLat":4026.44272,"rawGPSLong":7956.59589,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433384311,"topicName":"sensors/gps","sequenceNumber":645} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:24 AM","latitude":40.44070516666667,"north":true,"longitude":-79.94323633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.44231,"rawGPSLong":7956.59418,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433384808,"topicName":"sensors/gps","sequenceNumber":646} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:25 AM","latitude":40.44069833333333,"north":true,"longitude":-79.94320933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.9,"rawGPSLat":4026.4419,"rawGPSLong":7956.59256,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433385309,"topicName":"sensors/gps","sequenceNumber":647} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:25 AM","latitude":40.44069283333333,"north":true,"longitude":-79.94318233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.3,"rawGPSLat":4026.44157,"rawGPSLong":7956.59094,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433385799,"topicName":"sensors/gps","sequenceNumber":648} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:26 AM","latitude":40.4406865,"north":true,"longitude":-79.94315783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.44119,"rawGPSLong":7956.58947,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433386312,"topicName":"sensors/gps","sequenceNumber":649} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:26 AM","latitude":40.44068033333333,"north":true,"longitude":-79.943134,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.44082,"rawGPSLong":7956.58804,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433386801,"topicName":"sensors/gps","sequenceNumber":650} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:27 AM","latitude":40.44067166666667,"north":true,"longitude":-79.943112,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.3,"rawGPSLat":4026.4403,"rawGPSLong":7956.58672,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433387307,"topicName":"sensors/gps","sequenceNumber":651} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:27 AM","latitude":40.44066183333333,"north":true,"longitude":-79.9430875,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.2,"rawGPSLat":4026.43971,"rawGPSLong":7956.58525,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433387801,"topicName":"sensors/gps","sequenceNumber":652} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:28 AM","latitude":40.4406565,"north":true,"longitude":-79.94305983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.0,"rawGPSLat":4026.43939,"rawGPSLong":7956.58359,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433388304,"topicName":"sensors/gps","sequenceNumber":653} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:28 AM","latitude":40.44065233333333,"north":true,"longitude":-79.943031,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.9,"rawGPSLat":4026.43914,"rawGPSLong":7956.58186,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433388797,"topicName":"sensors/gps","sequenceNumber":654} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:29 AM","latitude":40.44064783333334,"north":true,"longitude":-79.943005,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.8,"rawGPSLat":4026.43887,"rawGPSLong":7956.5803,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433389301,"topicName":"sensors/gps","sequenceNumber":655} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:29 AM","latitude":40.440644,"north":true,"longitude":-79.94297866666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.6,"rawGPSLat":4026.43864,"rawGPSLong":7956.57872,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433389786,"topicName":"sensors/gps","sequenceNumber":656} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:30 AM","latitude":40.44063933333333,"north":true,"longitude":-79.94295033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.5,"rawGPSLat":4026.43836,"rawGPSLong":7956.57702,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433390300,"topicName":"sensors/gps","sequenceNumber":657} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:30 AM","latitude":40.44063583333333,"north":true,"longitude":-79.94292133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43815,"rawGPSLong":7956.57528,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433390791,"topicName":"sensors/gps","sequenceNumber":658} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:31 AM","latitude":40.44063,"north":true,"longitude":-79.94289383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.9,"rawGPSLat":4026.4378,"rawGPSLong":7956.57363,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433391300,"topicName":"sensors/gps","sequenceNumber":659} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:31 AM","latitude":40.4406235,"north":true,"longitude":-79.94286716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43741,"rawGPSLong":7956.57203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433391803,"topicName":"sensors/gps","sequenceNumber":660} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:32 AM","latitude":40.4406195,"north":true,"longitude":-79.94284066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.4,"rawGPSLat":4026.43717,"rawGPSLong":7956.57044,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433392308,"topicName":"sensors/gps","sequenceNumber":661} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:32 AM","latitude":40.440615,"north":true,"longitude":-79.94281716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.1,"rawGPSLat":4026.4369,"rawGPSLong":7956.56903,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433392799,"topicName":"sensors/gps","sequenceNumber":662} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:33 AM","latitude":40.440608833333336,"north":true,"longitude":-79.94279633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.9,"rawGPSLat":4026.43653,"rawGPSLong":7956.56778,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433393303,"topicName":"sensors/gps","sequenceNumber":663} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:33 AM","latitude":40.4406045,"north":true,"longitude":-79.94277133333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.4,"rawGPSLat":4026.43627,"rawGPSLong":7956.56628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433393796,"topicName":"sensors/gps","sequenceNumber":664} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:34 AM","latitude":40.4406005,"north":true,"longitude":-79.94274683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.5,"rawGPSLat":4026.43603,"rawGPSLong":7956.56481,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433394304,"topicName":"sensors/gps","sequenceNumber":665} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:34 AM","latitude":40.440596,"north":true,"longitude":-79.94272133333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.9,"rawGPSLat":4026.43576,"rawGPSLong":7956.56328,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433394803,"topicName":"sensors/gps","sequenceNumber":666} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:35 AM","latitude":40.440591166666664,"north":true,"longitude":-79.94269683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43547,"rawGPSLong":7956.56181,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433395302,"topicName":"sensors/gps","sequenceNumber":667} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:35 AM","latitude":40.44058533333333,"north":true,"longitude":-79.942672,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.6,"rawGPSLat":4026.43512,"rawGPSLong":7956.56032,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433395809,"topicName":"sensors/gps","sequenceNumber":668} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:36 AM","latitude":40.44058066666667,"north":true,"longitude":-79.942646,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.8,"rawGPSLat":4026.43484,"rawGPSLong":7956.55876,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433396299,"topicName":"sensors/gps","sequenceNumber":669} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:36 AM","latitude":40.44057766666667,"north":true,"longitude":-79.942618,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.2,"rawGPSLat":4026.43466,"rawGPSLong":7956.55708,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433396799,"topicName":"sensors/gps","sequenceNumber":670} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:37 AM","latitude":40.44057433333333,"north":true,"longitude":-79.942592,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.8,"rawGPSLat":4026.43446,"rawGPSLong":7956.55552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433397299,"topicName":"sensors/gps","sequenceNumber":671} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:37 AM","latitude":40.44057083333333,"north":true,"longitude":-79.94256833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.9,"rawGPSLat":4026.43425,"rawGPSLong":7956.5541,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433397817,"topicName":"sensors/gps","sequenceNumber":672} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:38 AM","latitude":40.4405675,"north":true,"longitude":-79.942548,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.3,"rawGPSLat":4026.43405,"rawGPSLong":7956.55288,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433398285,"topicName":"sensors/gps","sequenceNumber":673} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:38 AM","latitude":40.440565166666666,"north":true,"longitude":-79.942532,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.2,"rawGPSLat":4026.43391,"rawGPSLong":7956.55192,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433398810,"topicName":"sensors/gps","sequenceNumber":674} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:39 AM","latitude":40.44056416666667,"north":true,"longitude":-79.94251983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.2,"rawGPSLat":4026.43385,"rawGPSLong":7956.55119,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433399284,"topicName":"sensors/gps","sequenceNumber":675} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:39 AM","latitude":40.440563833333336,"north":true,"longitude":-79.94251166666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.3,"rawGPSLat":4026.43383,"rawGPSLong":7956.5507,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433399793,"topicName":"sensors/gps","sequenceNumber":676} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:40 AM","latitude":40.440564,"north":true,"longitude":-79.9425045,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.4,"rawGPSLat":4026.43384,"rawGPSLong":7956.55027,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433400299,"topicName":"sensors/gps","sequenceNumber":677} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:40 AM","latitude":40.440563,"north":true,"longitude":-79.94249766666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.6,"rawGPSLat":4026.43378,"rawGPSLong":7956.54986,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433400801,"topicName":"sensors/gps","sequenceNumber":678} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:41 AM","latitude":40.44056283333333,"north":true,"longitude":-79.94249016666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43377,"rawGPSLong":7956.54941,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433401301,"topicName":"sensors/gps","sequenceNumber":679} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:41 AM","latitude":40.44055983333333,"north":true,"longitude":-79.94248366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.43359,"rawGPSLong":7956.54902,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433401807,"topicName":"sensors/gps","sequenceNumber":680} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:42 AM","latitude":40.440558833333334,"north":true,"longitude":-79.94247633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.0,"rawGPSLat":4026.43353,"rawGPSLong":7956.54858,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433402302,"topicName":"sensors/gps","sequenceNumber":681} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:42 AM","latitude":40.440557166666665,"north":true,"longitude":-79.9424695,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.2,"rawGPSLat":4026.43343,"rawGPSLong":7956.54817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433402791,"topicName":"sensors/gps","sequenceNumber":682} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:43 AM","latitude":40.44055566666667,"north":true,"longitude":-79.94246033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.2,"rawGPSLat":4026.43334,"rawGPSLong":7956.54762,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433403309,"topicName":"sensors/gps","sequenceNumber":683} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:43 AM","latitude":40.4405545,"north":true,"longitude":-79.942452,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.43327,"rawGPSLong":7956.54712,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433403805,"topicName":"sensors/gps","sequenceNumber":684} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:44 AM","latitude":40.44055316666667,"north":true,"longitude":-79.9424435,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.43319,"rawGPSLong":7956.54661,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433404307,"topicName":"sensors/gps","sequenceNumber":685} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:44 AM","latitude":40.440551666666664,"north":true,"longitude":-79.94243533333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.4331,"rawGPSLong":7956.54612,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433404796,"topicName":"sensors/gps","sequenceNumber":686} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:45 AM","latitude":40.440550333333334,"north":true,"longitude":-79.94242683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.43302,"rawGPSLong":7956.54561,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433405304,"topicName":"sensors/gps","sequenceNumber":687} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:45 AM","latitude":40.440549833333336,"north":true,"longitude":-79.94241883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.43299,"rawGPSLong":7956.54513,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433405805,"topicName":"sensors/gps","sequenceNumber":688} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:46 AM","latitude":40.44054833333333,"north":true,"longitude":-79.942411,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.3,"rawGPSLat":4026.4329,"rawGPSLong":7956.54466,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433406309,"topicName":"sensors/gps","sequenceNumber":689} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:46 AM","latitude":40.44054716666667,"north":true,"longitude":-79.942403,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":290.5,"rawGPSLat":4026.43283,"rawGPSLong":7956.54418,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433406808,"topicName":"sensors/gps","sequenceNumber":690} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:47 AM","latitude":40.44054633333333,"north":true,"longitude":-79.94239633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":290.7,"rawGPSLat":4026.43278,"rawGPSLong":7956.54378,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433407311,"topicName":"sensors/gps","sequenceNumber":691} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:47 AM","latitude":40.440546,"north":true,"longitude":-79.942391,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":290.7,"rawGPSLat":4026.43276,"rawGPSLong":7956.54346,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433407800,"topicName":"sensors/gps","sequenceNumber":692} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:48 AM","latitude":40.440546,"north":true,"longitude":-79.9423865,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.0,"rawGPSLat":4026.43276,"rawGPSLong":7956.54319,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433408314,"topicName":"sensors/gps","sequenceNumber":693} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:48 AM","latitude":40.440546166666664,"north":true,"longitude":-79.94238383333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.1,"rawGPSLat":4026.43277,"rawGPSLong":7956.54303,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433408823,"topicName":"sensors/gps","sequenceNumber":694} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:49 AM","latitude":40.440546166666664,"north":true,"longitude":-79.942382,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.2,"rawGPSLat":4026.43277,"rawGPSLong":7956.54292,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433409313,"topicName":"sensors/gps","sequenceNumber":695} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:49 AM","latitude":40.440546166666664,"north":true,"longitude":-79.94238016666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.43277,"rawGPSLong":7956.54281,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433409801,"topicName":"sensors/gps","sequenceNumber":696} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:50 AM","latitude":40.440545666666665,"north":true,"longitude":-79.94237866666667,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.7,"rawGPSLat":4026.43274,"rawGPSLong":7956.54272,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433410303,"topicName":"sensors/gps","sequenceNumber":697} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:50 AM","latitude":40.440545166666666,"north":true,"longitude":-79.94237766666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":291.9,"rawGPSLat":4026.43271,"rawGPSLong":7956.54266,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433410808,"topicName":"sensors/gps","sequenceNumber":698} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:51 AM","latitude":40.440545,"north":true,"longitude":-79.9423755,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.1,"rawGPSLat":4026.4327,"rawGPSLong":7956.54253,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433411310,"topicName":"sensors/gps","sequenceNumber":699} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:51 AM","latitude":40.44054466666667,"north":true,"longitude":-79.94237066666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.3,"rawGPSLat":4026.43268,"rawGPSLong":7956.54224,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433411803,"topicName":"sensors/gps","sequenceNumber":700} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:52 AM","latitude":40.440544,"north":true,"longitude":-79.94236516666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.4,"rawGPSLat":4026.43264,"rawGPSLong":7956.54191,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433412303,"topicName":"sensors/gps","sequenceNumber":701} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:52 AM","latitude":40.44054333333333,"north":true,"longitude":-79.94235833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.5,"rawGPSLat":4026.4326,"rawGPSLong":7956.5415,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433412796,"topicName":"sensors/gps","sequenceNumber":702} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:53 AM","latitude":40.440542666666666,"north":true,"longitude":-79.94235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.6,"rawGPSLat":4026.43256,"rawGPSLong":7956.541,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433413303,"topicName":"sensors/gps","sequenceNumber":703} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:53 AM","latitude":40.44054216666667,"north":true,"longitude":-79.942341,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.7,"rawGPSLat":4026.43253,"rawGPSLong":7956.54046,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433413803,"topicName":"sensors/gps","sequenceNumber":704} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:54 AM","latitude":40.440541833333334,"north":true,"longitude":-79.942332,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.8,"rawGPSLat":4026.43251,"rawGPSLong":7956.53992,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433414309,"topicName":"sensors/gps","sequenceNumber":705} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:54 AM","latitude":40.44054166666667,"north":true,"longitude":-79.9423225,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.9,"rawGPSLat":4026.4325,"rawGPSLong":7956.53935,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433414815,"topicName":"sensors/gps","sequenceNumber":706} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:55 AM","latitude":40.44054166666667,"north":true,"longitude":-79.94231266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.0,"rawGPSLat":4026.4325,"rawGPSLong":7956.53876,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433415307,"topicName":"sensors/gps","sequenceNumber":707} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:55 AM","latitude":40.440542,"north":true,"longitude":-79.942303,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.0,"rawGPSLat":4026.43252,"rawGPSLong":7956.53818,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433415805,"topicName":"sensors/gps","sequenceNumber":708} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:56 AM","latitude":40.44054216666667,"north":true,"longitude":-79.94229466666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.1,"rawGPSLat":4026.43253,"rawGPSLong":7956.53768,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433416307,"topicName":"sensors/gps","sequenceNumber":709} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:56 AM","latitude":40.4405425,"north":true,"longitude":-79.94228683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.1,"rawGPSLat":4026.43255,"rawGPSLong":7956.53721,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433416811,"topicName":"sensors/gps","sequenceNumber":710} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:57 AM","latitude":40.44054283333333,"north":true,"longitude":-79.94227916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.2,"rawGPSLat":4026.43257,"rawGPSLong":7956.53675,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433417293,"topicName":"sensors/gps","sequenceNumber":711} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:57 AM","latitude":40.440543166666664,"north":true,"longitude":-79.94227133333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.3,"rawGPSLat":4026.43259,"rawGPSLong":7956.53628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433417808,"topicName":"sensors/gps","sequenceNumber":712} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:58 AM","latitude":40.4405435,"north":true,"longitude":-79.9422645,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.4,"rawGPSLat":4026.43261,"rawGPSLong":7956.53587,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433418298,"topicName":"sensors/gps","sequenceNumber":713} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:58 AM","latitude":40.440544,"north":true,"longitude":-79.94225816666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.5,"rawGPSLat":4026.43264,"rawGPSLong":7956.53549,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433418812,"topicName":"sensors/gps","sequenceNumber":714} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:59 AM","latitude":40.4405445,"north":true,"longitude":-79.94225166666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.6,"rawGPSLat":4026.43267,"rawGPSLong":7956.5351,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433419299,"topicName":"sensors/gps","sequenceNumber":715} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:59 AM","latitude":40.440545,"north":true,"longitude":-79.94224616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.8,"rawGPSLat":4026.4327,"rawGPSLong":7956.53477,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433419797,"topicName":"sensors/gps","sequenceNumber":716} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:00 AM","latitude":40.44054533333333,"north":true,"longitude":-79.942242,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.9,"rawGPSLat":4026.43272,"rawGPSLong":7956.53452,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433420305,"topicName":"sensors/gps","sequenceNumber":717} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:00 AM","latitude":40.4405455,"north":true,"longitude":-79.9422395,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.0,"rawGPSLat":4026.43273,"rawGPSLong":7956.53437,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433420815,"topicName":"sensors/gps","sequenceNumber":718} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:01 AM","latitude":40.4405455,"north":true,"longitude":-79.94223833333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.2,"rawGPSLat":4026.43273,"rawGPSLong":7956.5343,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433421321,"topicName":"sensors/gps","sequenceNumber":719} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:01 AM","latitude":40.4405455,"north":true,"longitude":-79.94223733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.3,"rawGPSLat":4026.43273,"rawGPSLong":7956.53424,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433421816,"topicName":"sensors/gps","sequenceNumber":720} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:02 AM","latitude":40.44054533333333,"north":true,"longitude":-79.942237,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.5,"rawGPSLat":4026.43272,"rawGPSLong":7956.53422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433422309,"topicName":"sensors/gps","sequenceNumber":721} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:02 AM","latitude":40.44054533333333,"north":true,"longitude":-79.942237,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.5,"rawGPSLat":4026.43272,"rawGPSLong":7956.53422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433422809,"topicName":"sensors/gps","sequenceNumber":722} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:03 AM","latitude":40.4405455,"north":true,"longitude":-79.94223733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.6,"rawGPSLat":4026.43273,"rawGPSLong":7956.53424,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433423303,"topicName":"sensors/gps","sequenceNumber":723} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:03 AM","latitude":40.44054633333333,"north":true,"longitude":-79.94223733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.6,"rawGPSLat":4026.43278,"rawGPSLong":7956.53424,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433423805,"topicName":"sensors/gps","sequenceNumber":724} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:04 AM","latitude":40.440549,"north":true,"longitude":-79.9422355,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":294.6,"rawGPSLat":4026.43294,"rawGPSLong":7956.53413,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433424293,"topicName":"sensors/gps","sequenceNumber":725} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:04 AM","latitude":40.44055216666667,"north":true,"longitude":-79.94223383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.7,"rawGPSLat":4026.43313,"rawGPSLong":7956.53403,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433424815,"topicName":"sensors/gps","sequenceNumber":726} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:05 AM","latitude":40.440556,"north":true,"longitude":-79.94223266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.7,"rawGPSLat":4026.43336,"rawGPSLong":7956.53396,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433425295,"topicName":"sensors/gps","sequenceNumber":727} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:05 AM","latitude":40.44055933333333,"north":true,"longitude":-79.9422305,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.8,"rawGPSLat":4026.43356,"rawGPSLong":7956.53383,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433425810,"topicName":"sensors/gps","sequenceNumber":728} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:06 AM","latitude":40.4405625,"north":true,"longitude":-79.94222866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.9,"rawGPSLat":4026.43375,"rawGPSLong":7956.53372,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433426295,"topicName":"sensors/gps","sequenceNumber":729} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:06 AM","latitude":40.4405645,"north":true,"longitude":-79.94222683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.0,"rawGPSLat":4026.43387,"rawGPSLong":7956.53361,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433426808,"topicName":"sensors/gps","sequenceNumber":730} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:07 AM","latitude":40.4405655,"north":true,"longitude":-79.94222566666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.1,"rawGPSLat":4026.43393,"rawGPSLong":7956.53354,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433427315,"topicName":"sensors/gps","sequenceNumber":731} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:07 AM","latitude":40.440566,"north":true,"longitude":-79.94222466666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.3,"rawGPSLat":4026.43396,"rawGPSLong":7956.53348,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433427826,"topicName":"sensors/gps","sequenceNumber":732} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:08 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.4,"rawGPSLat":4026.43395,"rawGPSLong":7956.53346,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433428310,"topicName":"sensors/gps","sequenceNumber":733} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:08 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942224,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.5,"rawGPSLat":4026.43395,"rawGPSLong":7956.53344,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433428794,"topicName":"sensors/gps","sequenceNumber":734} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:09 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.6,"rawGPSLat":4026.43395,"rawGPSLong":7956.53343,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433429312,"topicName":"sensors/gps","sequenceNumber":735} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:09 AM","latitude":40.44056583333333,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.7,"rawGPSLat":4026.43395,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433429810,"topicName":"sensors/gps","sequenceNumber":736} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:10 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.7,"rawGPSLat":4026.43394,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433430306,"topicName":"sensors/gps","sequenceNumber":737} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:10 AM","latitude":40.440565666666664,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43394,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433430810,"topicName":"sensors/gps","sequenceNumber":738} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:11 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43395,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433431319,"topicName":"sensors/gps","sequenceNumber":739} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:11 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43395,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433431801,"topicName":"sensors/gps","sequenceNumber":740} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:12 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433432313,"topicName":"sensors/gps","sequenceNumber":741} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:12 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43394,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433432809,"topicName":"sensors/gps","sequenceNumber":742} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:13 AM","latitude":40.440565666666664,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.9,"rawGPSLat":4026.43394,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433433314,"topicName":"sensors/gps","sequenceNumber":743} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:13 AM","latitude":40.440565666666664,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.9,"rawGPSLat":4026.43394,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433433827,"topicName":"sensors/gps","sequenceNumber":744} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:14 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.0,"rawGPSLat":4026.43394,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433434311,"topicName":"sensors/gps","sequenceNumber":745} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:14 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.0,"rawGPSLat":4026.43394,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433434806,"topicName":"sensors/gps","sequenceNumber":746} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:15 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.1,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433435309,"topicName":"sensors/gps","sequenceNumber":747} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:15 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.1,"rawGPSLat":4026.43395,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433435800,"topicName":"sensors/gps","sequenceNumber":748} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:16 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43394,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433436309,"topicName":"sensors/gps","sequenceNumber":749} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:16 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433436800,"topicName":"sensors/gps","sequenceNumber":750} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:17 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433437305,"topicName":"sensors/gps","sequenceNumber":751} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:17 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433437783,"topicName":"sensors/gps","sequenceNumber":752} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:18 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433438294,"topicName":"sensors/gps","sequenceNumber":753} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:18 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433438802,"topicName":"sensors/gps","sequenceNumber":754} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:19 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433439295,"topicName":"sensors/gps","sequenceNumber":755} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:19 AM","latitude":40.440566,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.3,"rawGPSLat":4026.43396,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433439810,"topicName":"sensors/gps","sequenceNumber":756} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:20 AM","latitude":40.440566,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.7,"antennaAltitude":296.3,"rawGPSLat":4026.43396,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433440299,"topicName":"sensors/gps","sequenceNumber":757} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:20 AM","latitude":40.44056633333334,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.7,"antennaAltitude":296.3,"rawGPSLat":4026.43398,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433440789,"topicName":"sensors/gps","sequenceNumber":758} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:21 AM","latitude":40.4405665,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.4,"rawGPSLat":4026.43399,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433441295,"topicName":"sensors/gps","sequenceNumber":759} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:21 AM","latitude":40.44056666666667,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.5,"rawGPSLat":4026.434,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433441798,"topicName":"sensors/gps","sequenceNumber":760} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:22 AM","latitude":40.440566833333335,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.25,"antennaAltitude":296.6,"rawGPSLat":4026.43401,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433442292,"topicName":"sensors/gps","sequenceNumber":761} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:22 AM","latitude":40.44056716666667,"north":true,"longitude":-79.94222283333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.7,"rawGPSLat":4026.43403,"rawGPSLong":7956.53337,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433442793,"topicName":"sensors/gps","sequenceNumber":762} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:23 AM","latitude":40.440567333333334,"north":true,"longitude":-79.94222266666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.7,"rawGPSLat":4026.43404,"rawGPSLong":7956.53336,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433443283,"topicName":"sensors/gps","sequenceNumber":763} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:23 AM","latitude":40.440567333333334,"north":true,"longitude":-79.9422225,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.8,"rawGPSLat":4026.43404,"rawGPSLong":7956.53335,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433443793,"topicName":"sensors/gps","sequenceNumber":764} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:24 AM","latitude":40.4405675,"north":true,"longitude":-79.94222216666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.9,"rawGPSLat":4026.43405,"rawGPSLong":7956.53333,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433444293,"topicName":"sensors/gps","sequenceNumber":765} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:24 AM","latitude":40.44056766666667,"north":true,"longitude":-79.94222166666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.0,"rawGPSLat":4026.43406,"rawGPSLong":7956.5333,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433444802,"topicName":"sensors/gps","sequenceNumber":766} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:25 AM","latitude":40.44056766666667,"north":true,"longitude":-79.94222116666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.1,"rawGPSLat":4026.43406,"rawGPSLong":7956.53327,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433445292,"topicName":"sensors/gps","sequenceNumber":767} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:25 AM","latitude":40.44056783333333,"north":true,"longitude":-79.94222083333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.1,"rawGPSLat":4026.43407,"rawGPSLong":7956.53325,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433445801,"topicName":"sensors/gps","sequenceNumber":768} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:26 AM","latitude":40.440568,"north":true,"longitude":-79.94222033333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.2,"rawGPSLat":4026.43408,"rawGPSLong":7956.53322,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433446288,"topicName":"sensors/gps","sequenceNumber":769} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:26 AM","latitude":40.440568166666665,"north":true,"longitude":-79.94222033333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.2,"rawGPSLat":4026.43409,"rawGPSLong":7956.53322,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433446795,"topicName":"sensors/gps","sequenceNumber":770} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:27 AM","latitude":40.440568166666665,"north":true,"longitude":-79.94222,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.3,"rawGPSLat":4026.43409,"rawGPSLong":7956.5332,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433447297,"topicName":"sensors/gps","sequenceNumber":771} +{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:27 AM","latitude":40.440568,"north":true,"longitude":-79.94221966666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.25,"antennaAltitude":297.3,"rawGPSLat":4026.43408,"rawGPSLong":7956.53318,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433447788,"topicName":"sensors/gps","sequenceNumber":772} From 7e7d6868f27005ef71474e65beb3e7c1ce7f07a3 Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Sun, 26 Feb 2017 04:49:09 -0500 Subject: [PATCH 077/149] fixed steering controller, tested new version --- offline/controller/controller_analysis.m | 14 ++- offline/controller/controller_pure.m | 2 +- offline/controller/controller_pure_cont.m | 49 +++----- offline/controller/controller_stanley.m | 90 +++++++------- offline/controller/waypoint_analysis.m | 22 +--- .../planners/WayPointFollowerPlanner.java | 110 ++++++++++++------ 6 files changed, 161 insertions(+), 126 deletions(-) diff --git a/offline/controller/controller_analysis.m b/offline/controller/controller_analysis.m index 230a34e7..ec6b89ef 100644 --- a/offline/controller/controller_analysis.m +++ b/offline/controller/controller_analysis.m @@ -6,15 +6,15 @@ addpath('../localizer/latlonutm/Codes/matlab'); addpath('../localizer/altmany-export_fig'); -file = 'controller_tri_v1.mat'; +file = 'controller_v2.mat'; load(file, 'trajectory'); save_plot = false; -show_maps = false; +show_maps = true; -load('./waypoints_tri.mat'); +load('./waypoints_course_v2.mat'); [x, y, zone] = ll2utm(logs); desired = [x y]; - +desired = desired(112:(end-50), :); k = 1000; if show_maps @@ -28,6 +28,12 @@ quiver(trajectory(1,1:k:end), trajectory(2,1:k:end), cos(heading), sin(heading)) plot(desired(:,1), desired(:,2), 'g') hold off; +title(['Map ', file]); + +headingd = rad2deg(heading); +figure(); +hold on; +plot(1:length(headingd), headingd) title(['Heading ', file]); figure(); diff --git a/offline/controller/controller_pure.m b/offline/controller/controller_pure.m index 312f0987..f68a1ce2 100644 --- a/offline/controller/controller_pure.m +++ b/offline/controller/controller_pure.m @@ -129,7 +129,7 @@ deltaPath = target - pos; k = 0.8; - a = atan2(deltaPath(2), deltaPath(1))-X(4); % incorrect + a = atan2(deltaPath(2), deltaPath(1))-X(4); u = atan2(2*wheel_base*sin(a), k*X(3)); end u = clampSteeringAngle(clampAngle(u)); diff --git a/offline/controller/controller_pure_cont.m b/offline/controller/controller_pure_cont.m index f5fea804..8f89175f 100644 --- a/offline/controller/controller_pure_cont.m +++ b/offline/controller/controller_pure_cont.m @@ -1,4 +1,4 @@ -function [trajectory] = controller_pure() +function [trajectory] = controller_pure_cont() % https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf % section 2.2 @@ -8,6 +8,7 @@ global steering_vel global dt global last_closest_idx + global last_u save_data = true; wheel_base = 1.13; @@ -20,6 +21,8 @@ steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds % full range in 0.5s last_closest_idx = 1; + last_u = 0; + total_time = 6; % min [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -31,8 +34,7 @@ load('./waypoints_course_v2.mat'); desired_traj = processWaypoints(logs); - % time = linspace(0, 240, size(trajectory,2)); - time = 0:dt:240; + time = 0:dt:(total_time*60); u = 0; % commanded steering angle steering = u; % steering angle trajectory = [X; lat_long(1); lat_long(2); steering]; @@ -50,7 +52,6 @@ u = control(desired_traj, X); end - % trajectory = [trajectory, X]; snapshot = summarize(X, utm_zone, steering); trajectory = [trajectory, snapshot]; end @@ -67,7 +68,7 @@ function snapshot = summarize(x, utm_zone, steeringAngle) [lat, lon] = utm2ll(x(1), x(2), utm_zone); - snapshot = [x; x(1); x(2); steeringAngle]; + snapshot = [x; lat; lon; steeringAngle]; end function a = clampAngle(a) @@ -114,42 +115,23 @@ function [u] = control(desired_traj, X) global wheel_base global last_closest_idx + global last_u - k = 3; + K = 4; vel = X(3); pos = X(1:2)'; lookahead_bounds = [3 25]; - lookahead = k * vel; + lookahead = K * vel; if(lookahead < lookahead_bounds(1)) lookahead = lookahead_bounds(1); elseif(lookahead > lookahead_bounds(2)) lookahead = lookahead_bounds(1); end - % closest_idx = last_closest_idx; - % min_dist = 100000; - % for k=last_closest_idx:(length(desired_mid) - 1) - % distp = norm(desired_mid(k,:) - X); - % if(distp < min_dist) - % min_dist = distp; - % closest_idx = k; - % end - % % cut off search somehow - % end - - % ptA = [0 0]; - % if(closest_idx == 1) - % ptA = X; - % else - % ptA = desired_traj(closest_idx, :); - % end - % ptB = desired_traj(closest_idx+1, :); - % crosstrack_error = abs(det([ptB - ptA; X - ptA])) / norm(ptB - ptA); - closest_idx = last_closest_idx; min_dist = 100000; for k=last_closest_idx:length(desired_traj) - distp = norm(desired_traj(k,:) - X); + distp = norm(desired_traj(k,:) - pos); if(distp < min_dist) min_dist = distp; closest_idx = k; @@ -159,7 +141,7 @@ lookahead_idx = 0; for k=closest_idx:length(desired_traj) - distp = norm(desired_traj(k,:) - X); + distp = norm(desired_traj(k,:) - pos); if(distp > lookahead) lookahead_idx = k; break; @@ -170,13 +152,16 @@ u = 0; else deltaPath = desired_traj(lookahead_idx,:) - pos; - a = atan2(deltaPath(2), deltaPath(1)) - (pi/2); + a = atan2(deltaPath(2), deltaPath(1)) - X(4); u = atan2(2 * wheel_base * sin(a), lookahead); end - % maybe consider deadband region + % try moving average + % temp_u = u; + % u = (last_u + u) / 2; + % last_u = temp_u; - last_closest_idx = closest_idx; u = clampSteeringAngle(u); + last_closest_idx = closest_idx; end diff --git a/offline/controller/controller_stanley.m b/offline/controller/controller_stanley.m index af31c038..8db24bd9 100644 --- a/offline/controller/controller_stanley.m +++ b/offline/controller/controller_stanley.m @@ -1,23 +1,28 @@ -function [trajectory] = controller_pure_cont() +function [trajectory] = controller_stanley() % https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf -% section 2.2 +% section 2.3 addpath('../localizer/latlonutm/Codes/matlab'); global wheel_base global velocity global steering_vel global dt + global last_closest_idx + global last_u save_data = true; wheel_base = 1.13; utm_zone = 17; first_heading = deg2rad(250); - lat_long = [40.442867, -79.9427395]; % [40.441670, -79.9416362]; + lat_long = [40.441670, -79.9416362]; % tri [40.442867, -79.9427395]; dt = 0.001; % 1000Hz m = 50; % 20Hz - velocity = 8; % m/s, 17.9mph, forward velocity + velocity = 3.6; % m/s, 8mph, forward velocity steering_vel = deg2rad(40); % 40deg/s, reaction speed to control cmds % full range in 0.5s + last_closest_idx = 1; + last_u = 0; + total_time = 6; % min [x, y, ~] = ll2utm(lat_long(1), lat_long(2)); @@ -27,10 +32,11 @@ first_heading; % heading, rad, world frame 0]; % d_heading, rad/s - load('./waypoints_tri.mat'); + load('./waypoints_course_v2.mat'); desired_traj = processWaypoints(logs); - % time = linspace(0, 240, size(trajectory,2)); - time = 0:dt:60; % 240; + desired_traj = desired_traj(112:(end-50), :); + + time = 0:dt:(total_time*60); u = 0; % commanded steering angle steering = u; % steering angle trajectory = [X; lat_long(1); lat_long(2); steering]; @@ -48,13 +54,12 @@ u = control(desired_traj, X); end - % trajectory = [trajectory, X]; snapshot = summarize(X, utm_zone, steering); trajectory = [trajectory, snapshot]; end if save_data - save('controller_tri_v1.mat', 'trajectory'); + save('controller_v2.mat', 'trajectory'); end end @@ -65,7 +70,7 @@ function snapshot = summarize(x, utm_zone, steeringAngle) [lat, lon] = utm2ll(x(1), x(2), utm_zone); - snapshot = [x; x(1); x(2); steeringAngle]; + snapshot = [x; lat; lon; steeringAngle]; end function a = clampAngle(a) @@ -90,11 +95,11 @@ end function a = clampSteeringAngle(a) - if(a < -deg2rad(10)) - a = -deg2rad(10); - end - if(a > deg2rad(10)) - a = deg2rad(10); + b = deg2rad(10); + if(a < -b) + a = -b; + elseif(a > b) + a = b; end end @@ -111,38 +116,45 @@ function [u] = control(desired_traj, X) global wheel_base + global last_closest_idx + global last_u + K = 0.1; + K2 = 0.8; pos = X(1:2)'; - delta = 15; - % find closest - min_idx = 0; - min_dist = 100000000; - for k=1:length(desired_traj) - distp = norm(pos, desired_traj(k,:)); + closest_idx = last_closest_idx; + min_dist = 100000; + for k=last_closest_idx:length(desired_traj) + distp = norm(desired_traj(k,:) - pos); if(distp < min_dist) - min_idx = k; min_dist = distp; + closest_idx = k; end + % cut off search somehow end - ptA = [0 0]; - ptB = [0 0]; - - distances = sum((desired_traj - pos_b).^2, 2); - [~, closest_idx] = min(distances); - last_idx = min([length(distances), closest_idx + cutoff]); - possible = find(distances(1:last_idx) < delta); - if isempty(possible) - u = 0; - else - target = desired_traj(possible(end), :); - deltaPath = target - pos; - - k = 0.8; - a = atan2(deltaPath(2), deltaPath(1))-X(4); - u = atan2(2*wheel_base*sin(a), k*X(3)); + if(closest_idx == length(desired_traj)) + u = 0; + return; end - u = clampSteeringAngle(clampAngle(u)); + + ptA = desired_traj(closest_idx, :); + ptB = desired_traj(closest_idx+1, :); + + p = ptB - ptA; % path + path_heading = atan2(p(2), p(1)); + heading_error = clampAngle(path_heading) - clampAngle(X(4)); + crosstrack_error = - det([p; pos - ptA]) / norm(p); + + u = K2*heading_error + atan2(K * crosstrack_error, X(3)); + + % try moving average + % temp_u = u; + % u = (last_u + u) / 2; + % last_u = temp_u; + + u = clampSteeringAngle(u); + last_closest_idx = closest_idx; end diff --git a/offline/controller/waypoint_analysis.m b/offline/controller/waypoint_analysis.m index 90e477d7..a977b37d 100644 --- a/offline/controller/waypoint_analysis.m +++ b/offline/controller/waypoint_analysis.m @@ -4,33 +4,23 @@ close all; addpath('../localizer/latlonutm/Codes/matlab'); - -file = 'controller_v2.mat'; -load(file, 'trajectory'); show_maps = false; -load('./waypoints.mat'); +load('./waypoints_course.mat'); [x, y, zone] = ll2utm(logs); desired = [x y]; size(desired) -size(trajectory) +sumdist = 0; for k=1:(length(desired) - 1) - sumdist = norm(desired(k,:) - desired(k+1,:)); + sumdist = sumdist + norm(desired(k,:) - desired(k+1,:)); end avgdist = sumdist ./ length(desired) -heading = trajectory(4,1:k:end); -heading = rad2deg(heading); -figure(); -hold on; -plot(1:length(heading), heading) -title(['Heading ', file]); - -figure(); -plot(1:length(trajectory), trajectory(8, 1:end)) -title('Control input'); +% figure(); +% plot(1:length(trajectory), trajectory(8, 1:end)) +% title('Control input'); % plot on google maps if show_maps diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 98a4e9a8..40249af2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -17,6 +17,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type + private int lastClosestIndex = 0; /** * @vivaanbahl TESTING CODE ONLY @@ -42,69 +43,110 @@ public void updatePositionEstimate(GPSPoseMessage m) { } //find the closest way point - //TODO turn into a binary kdtree search - private static int getClosestIndex(ArrayList wayPoints, GPSPoseMessage currentLocation) { + private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage currentLocation) { double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this - int closestIndex = -1; - for (int i = 0; i < wayPoints.size(); i++) { + int closestIndex = lastClosestIndex; + for (int i = lastClosestIndex; i < wayPoints.size(); i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { min = d; closestIndex = i; } + // eventually cut off search somehow } + lastClosestIndex = closestIndex; return closestIndex; } @Override public double getCommandedSteeringAngle() { // determines the angle at which to move every 50 milliseconds + //PD control of DC steering motor handled by low level + + double commandedAngle = purePursuitController(); + // double commandedAngle = stanleyMethodController(); + + currentCommandedAngle = commandedAngle; + currentDesiredHeading = commandedAngle; // purpose? + return commandedAngle; + } + + private double purePursuitController() { + // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf + // section 2.2 + int closestIndex = getClosestIndex(wayPoints, pose); - if (closestIndex == -1) { - new RobobuggyLogicNotification("HELP no closest index", RobobuggyMessageLevel.EXCEPTION); - return 17433504; //A dummy value that we can never get + + double K = 4.0; + double velocity = pose.getCurrentState().get(2, 0); + double lookaheadLowerBound = 3.0; + double lookaheadUpperBound = 25.0; + double lookahead = K * velocity; + if(lookahead < lookaheadLowerBound) { + lookahead = lookaheadLowerBound; + } + else if(lookahead > lookaheadUpperBound) { + lookahead = lookaheadUpperBound; } - double lookahead = 15; //meters //pick the first point that is at least lookahead away, then point buggy toward it - int targetIndex = closestIndex; - double distanceFromMessage = 0; - do { - targetIndex++; - distanceFromMessage = GPSPoseMessage.getDistance(pose, wayPoints.get(targetIndex).toGpsPoseMessage(0)); - } while (targetIndex < wayPoints.size() && distanceFromMessage < lookahead); + int lookaheadIndex = 0; + for(lookaheadIndex = closestIndex; lookaheadIndex < wayPoints.size(); lookaheadIndex++) { + if(GPSPoseMessage.getDistance(pose, wayPoints.get(lookaheadIndex).toGpsPoseMessage(0)) > lookahead) { + break; + } + } //if we are out of points then just go straight - if (targetIndex >= wayPoints.size()) { + if (lookaheadIndex >= wayPoints.size()) { new RobobuggyLogicNotification("HELP out of points", RobobuggyMessageLevel.EXCEPTION); return 0; } - GpsMeasurement targetPoint = wayPoints.get(targetIndex); - currentWaypoint = targetPoint; - //find a path from our current location to that point - double deltaLong = targetPoint.getLongitude() - pose.getLongitude(); - double deltaLat = targetPoint.getLatitude() - pose.getLatitude(); - double deltaLatMeters = LocalizerUtil.convertLatToMeters(deltaLat); - double deltaLongMeters = LocalizerUtil.convertLonToMeters(deltaLong); - double desiredHeading = Math.atan2(deltaLatMeters, deltaLongMeters); - currentDesiredHeading = desiredHeading; - - //find the angle we need to reach that point - double poseHeading = pose.getHeading(); - double deltaHeading = desiredHeading - poseHeading; + GpsMeasurement target = wayPoints.get(lookaheadIndex); + double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); + double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); //Pure Pursuit steering controller -// double param1 = 2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading); -// double param2 = 0.8 * pose.getCurrentState().get(2, 0); -// deltaHeading = Math.atan2(param1, param2); + double commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), lookahead); + commandedAngle = Util.normalizeAngleRad(commandedAngle); + return commandedAngle; + } - //PD control of DC steering motor handled by low level - double commandedAngle = Util.normalizeAngleRad(deltaHeading); - currentCommandedAngle = commandedAngle; + private double stanleyMethodController() { + // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf + // section 2.3 + + int closestIndex = getClosestIndex(wayPoints, pose); + + double K = 0.1; + double velocity = pose.getCurrentState().get(2, 0); + + //if we are out of points then just go straight + if (closestIndex >= (wayPoints.size() - 1)) { + new RobobuggyLogicNotification("HELP out of points", RobobuggyMessageLevel.EXCEPTION); + return 0; + } + + GpsMeasurement ptA = wayPoints.get(closestIndex); + GpsMeasurement ptB = wayPoints.get(closestIndex + 1); + double pathx = LocalizerUtil.convertLonToMeters(ptB.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); + double pathy = LocalizerUtil.convertLatToMeters(ptB.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); + double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); + + double pathHeading = Math.atan2(pathy, pathx); + double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); + double determinant = (pathx * dy) - (pathy - dx); + double crosstrackError = - determinant / Math.sqrt(pathx*pathx + pathy*pathy); + + //Stanley steering controller + double commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); + commandedAngle = Util.normalizeAngleRad(commandedAngle); return commandedAngle; } From 63b50da371976c39bb57ddb0f90c42aff9cf949a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 28 Feb 2017 17:52:10 -0500 Subject: [PATCH 078/149] added a limit on how many waypoints we consider for picking --- .../nodes/planners/WayPointFollowerPlanner.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 40249af2..ae7caf1d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -6,6 +6,7 @@ import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.ros.NodeChannel; import java.util.ArrayList; @@ -19,6 +20,9 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private GPSPoseMessage pose; //TODO change this to a reasonable type private int lastClosestIndex = 0; + // we only want to look at the next 10 waypoints as possible candidates + private final static int WAYPOINT_LOOKAHEAD_MAX = 10; + /** * @vivaanbahl TESTING CODE ONLY * REMOVE FOR PROD PUSH @@ -47,7 +51,7 @@ private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this int closestIndex = lastClosestIndex; - for (int i = lastClosestIndex; i < wayPoints.size(); i++) { + for (int i = lastClosestIndex; i < lastClosestIndex + WAYPOINT_LOOKAHEAD_MAX; i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { @@ -66,7 +70,7 @@ public double getCommandedSteeringAngle() { //PD control of DC steering motor handled by low level double commandedAngle = purePursuitController(); - // double commandedAngle = stanleyMethodController(); +// double commandedAngle = stanleyMethodController(); currentCommandedAngle = commandedAngle; currentDesiredHeading = commandedAngle; // purpose? @@ -75,7 +79,7 @@ public double getCommandedSteeringAngle() { private double purePursuitController() { // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf - // section 2.2 + // section 2.sssasdf int closestIndex = getClosestIndex(wayPoints, pose); @@ -107,6 +111,7 @@ else if(lookahead > lookaheadUpperBound) { //find a path from our current location to that point GpsMeasurement target = wayPoints.get(lookaheadIndex); + currentWaypoint = target; double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); From 66c558db6d826d98fd09db8ad3209e58addfec19 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 3 Mar 2017 16:56:08 -0500 Subject: [PATCH 079/149] reworked the controller tester to start at the first waypoint instead of a hardcoded latlng, fixed an issue with running out of waypoints --- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 2 +- .../roboclub/robobuggy/robots/ControllerTesterRobot.java | 6 ++++-- .../com/roboclub/robobuggy/simulation/ControllerTester.java | 6 ++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index ae7caf1d..ed8a5367 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -51,7 +51,7 @@ private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this int closestIndex = lastClosestIndex; - for (int i = lastClosestIndex; i < lastClosestIndex + WAYPOINT_LOOKAHEAD_MAX; i++) { + for (int i = lastClosestIndex; i < lastClosestIndex + WAYPOINT_LOOKAHEAD_MAX && i < wayPoints.size(); i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index c0ccc13f..91674c86 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -2,6 +2,7 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.simulation.ControllerTester; @@ -31,14 +32,15 @@ public static ControllerTesterRobot getInstance() { private ControllerTesterRobot() { super(); - nodeList.add(new ControllerTester("Testing the controller")); + ArrayList wayPoints = null; try { - ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); + wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block e.printStackTrace(); } + nodeList.add(new ControllerTester("Testing the controller", new LocTuple(wayPoints.get(0).getLatitude(), wayPoints.get(0).getLongitude()))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index 4ea3cacb..d0d7717a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -30,8 +30,6 @@ public class ControllerTester extends PeriodicNode { private static final double WHEELBASE = 1.13; // assume a velocity of 8 m/s private static final double VELOCITY = 1; - // assume we start from the base of the track - private static final LocTuple INITIAL_POSITION_LL = new LocTuple(40.441670, -79.9416362); // assume we are facing up hill 1 private static final double INITIAL_HEADING_RAD = Math.toRadians(250); @@ -48,10 +46,10 @@ public class ControllerTester extends PeriodicNode { * * @param name */ - public ControllerTester(String name) { + public ControllerTester(String name, LocTuple initialPosition) { super(new BuggyBaseNode(NodeChannel.AUTO), SIM_PERIOD, name); - LocTuple firstPosition = INITIAL_POSITION_LL; + LocTuple firstPosition = initialPosition; double[][] XAsDoubleArr = { { LocalizerUtil.deg2UTM(firstPosition).getEasting() }, { LocalizerUtil.deg2UTM(firstPosition).getNorthing() }, From f1466ae73e760e4fef12a6f8dc3330db9adb4fce Mon Sep 17 00:00:00 2001 From: Bereket Abraham Date: Tue, 7 Mar 2017 00:38:07 -0500 Subject: [PATCH 080/149] style --- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index ed8a5367..9aab85a2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -51,7 +51,7 @@ private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this int closestIndex = lastClosestIndex; - for (int i = lastClosestIndex; i < lastClosestIndex + WAYPOINT_LOOKAHEAD_MAX && i < wayPoints.size(); i++) { + for (int i = lastClosestIndex; i < (lastClosestIndex + WAYPOINT_LOOKAHEAD_MAX) && i < wayPoints.size(); i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { @@ -79,7 +79,7 @@ public double getCommandedSteeringAngle() { private double purePursuitController() { // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf - // section 2.sssasdf + // section 2.2 int closestIndex = getClosestIndex(wayPoints, pose); From 984956d37d46d5c6796a71661d22ef70dafd2606 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 29 Mar 2017 15:53:05 -0400 Subject: [PATCH 081/149] @agirish wrote a fix to stop the localizer from transmitting until we get a GPS update --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index f1322459..c2e4ed1d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -20,6 +20,7 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // our transmitter for position estimate messages private Publisher posePub; + private boolean gpsMessagesReceived; // constants we use throughout the file private static final int X_GLOBAL_ROW = 0; @@ -150,6 +151,7 @@ private void setupEncoderSubscriber() { private void setupGPSSubscriber() { new Subscriber("KF Localizer", NodeChannel.GPS.getMsgPath(), ((topicName, m) -> { + gpsMessagesReceived = true; GpsMeasurement gpsLoc = (GpsMeasurement) m; LocTuple loc = new LocTuple(gpsLoc.getLatitude(), gpsLoc.getLongitude()); UTMTuple gps = LocalizerUtil.deg2UTM(loc); @@ -196,8 +198,10 @@ protected void update() { UTMTuple utm = new UTMTuple(UTMZONE, 'T', x_predict.get(X_GLOBAL_ROW, 0), x_predict.get(Y_GLOBAL_ROW, 0)); LocTuple latLon = LocalizerUtil.utm2Deg(utm); - posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), - latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x)); + if (gpsMessagesReceived) { + posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), + latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x)); + } } // Kalman filter step 0: Generate the motion model for the buggy From 4a7dafcf8b9fc3ffe361cfcb5ea1f148874a9c8e Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 31 Mar 2017 15:45:25 -0400 Subject: [PATCH 082/149] added steering feedback into the localizer --- .../localizers/RobobuggyKFLocalizer.java | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index c2e4ed1d..fcca14da 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -101,7 +101,10 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { }; Q_gps = new Matrix(qGPS2D); - double[][] qEncoder2D = {{0.25}}; + double[][] qEncoder2D = { + {0.25, 0}, + {0, 1}, + }; Q_encoder = new Matrix(qEncoder2D); double[][] cGPS2D = { @@ -113,6 +116,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] cEncoder2D = { {0, 0, 1, 0, 0}, + {0, 0, 0, 1, 0}, }; C_encoder = new Matrix(cEncoder2D); @@ -120,7 +124,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { // Every time we get a new sensor update, trigger the new kalman update setupGPSSubscriber(); setupEncoderSubscriber(); - setupWheelSubscriber(); + setupSteeringSubscriber(); resume(); } @@ -141,8 +145,15 @@ private void setupEncoderSubscriber() { lastEncoderTime = currentTime; lastEncoder = currentEncoder; + double arcRadius = WHEELBASE_IN_METERS / Math.sin(steeringAngle); + double arcLength = dx; + double headingChange = arcLength / arcRadius; + // measurement - double[][] z2D = {{ bodySpeed }}; + double[][] z2D = { + { bodySpeed }, + { headingChange }, + }; Matrix z = new Matrix(z2D); kalmanFilter(C_encoder, Q_encoder, z); @@ -184,7 +195,7 @@ private void setupGPSSubscriber() { })); } - private void setupWheelSubscriber() { + private void setupSteeringSubscriber() { new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), ((topicName, m) -> { SteeringMeasurement steerM = (SteeringMeasurement) m; steeringAngle = Math.toRadians(steerM.getAngle()); From 1200264b08d7387d0e3544cb8895c7086352acb2 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 2 Apr 2017 23:55:55 -0400 Subject: [PATCH 083/149] minor fixes and added skeleton of new simulation toolbox --- .../robobuggy/main/RobobuggyMainFile.java | 4 +-- .../localizers/RobobuggyKFLocalizer.java | 7 ++-- .../robobuggy/simulation/FullSimRunner.java | 35 +++++++++++++++++++ 3 files changed, 40 insertions(+), 6 deletions(-) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index cc0b1158..f65b3269 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.ControllerTesterRobot; +import com.roboclub.robobuggy.robots.TransistorAuton; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = ControllerTesterRobot.getInstance(); + robot = TransistorAuton.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index fcca14da..b01e4827 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -103,7 +103,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] qEncoder2D = { {0.25, 0}, - {0, 1}, + {0, 0.1}, }; Q_encoder = new Matrix(qEncoder2D); @@ -116,7 +116,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] cEncoder2D = { {0, 0, 1, 0, 0}, - {0, 0, 0, 1, 0}, + {0, 0, 0, 0, 1}, }; C_encoder = new Matrix(cEncoder2D); @@ -146,8 +146,7 @@ private void setupEncoderSubscriber() { lastEncoder = currentEncoder; double arcRadius = WHEELBASE_IN_METERS / Math.sin(steeringAngle); - double arcLength = dx; - double headingChange = arcLength / arcRadius; + double headingChange = dx / arcRadius; // measurement double[][] z2D = { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java new file mode 100644 index 00000000..1b27dd4d --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -0,0 +1,35 @@ +package com.roboclub.robobuggy.simulation; + +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.ros.NodeChannel; + +/** + * Created by vivaanbahl on 4/2/17. + */ +public class FullSimRunner extends PeriodicNode { + /** + * Create a new {@link PeriodicNode} decorator + * + * @param period of the periodically executed portion of the node + * @param name + */ + protected FullSimRunner(int period, String name) { + super(new BuggyBaseNode(NodeChannel.SIMULATION), period, name); + } + + @Override + protected void update() { + + } + + @Override + protected boolean startDecoratorNode() { + return false; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} From 53febf2d72564fdcb0ddd60eb784ba55a9e15298 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 3 Apr 2017 00:13:43 -0400 Subject: [PATCH 084/149] filled in init routine for new sim toolbox --- .../robobuggy/simulation/FullSimRunner.java | 94 ++++++++++++++++++- 1 file changed, 93 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index 1b27dd4d..86fc0099 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -1,21 +1,113 @@ package com.roboclub.robobuggy.simulation; +import Jama.Matrix; +import com.roboclub.robobuggy.main.RobobuggyConfigFile; +import com.roboclub.robobuggy.messages.SteeringMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.localizers.UTMTuple; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; +import com.roboclub.robobuggy.ros.Subscriber; + +import java.io.FileNotFoundException; +import java.util.Timer; +import java.util.TimerTask; /** * Created by vivaanbahl on 4/2/17. */ public class FullSimRunner extends PeriodicNode { + + private static final double WHEELBASE_IN_METERS = 1.13; // meters + private static final int UTMZONE = 17; + private static final int VELOCITY = 3; + private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad + private static final int LOCALIZER_UPDATE_PERIOD = 10; + private static final int CONTROLLER_UPDATE_PERIOD = 10; + private static final int GPS_UPDATE_PERIOD = 500; + private static final int ENC_UPDATE_PERIOD = 10; + + private Matrix motionModel; + private Matrix state; + + private Publisher gpsPub; + private Publisher encPub; + private Subscriber steerSub; + + private RobobuggyKFLocalizer localizer; + private WayPointFollowerPlanner controller; + + private Timer gpsTimer; + private Timer encTimer; + /** * Create a new {@link PeriodicNode} decorator * * @param period of the periodically executed portion of the node * @param name */ - protected FullSimRunner(int period, String name) { + protected FullSimRunner(int period, String name, LocTuple initialPos) { super(new BuggyBaseNode(NodeChannel.SIMULATION), period, name); + + // initialized to the identity matrix (goes nowhere) + double[][] motionModelArr = { + {1, 0, 0, 0, 0}, + {0, 1, 0, 0, 0}, + {0, 0, 1, 0, 0}, + {0, 0, 0, 1, 0}, + {0, 0, 0, 0, 1}, + }; + motionModel = new Matrix(motionModelArr); + + UTMTuple t = LocalizerUtil.deg2UTM(initialPos); + double[][] stateArr = { + { t.getEasting() }, + { t.getNorthing() }, + { VELOCITY }, + { INITIAL_HEADING_IN_RADS }, + { 0 }, + }; + state = new Matrix(stateArr); + + gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); + encPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); + steerSub = new Subscriber("Full Sim Toolbox", NodeChannel.STEERING.getMsgPath(), (topicName, m) -> { + updateMotionModelSteering(Math.toRadians(((SteeringMeasurement) m).getAngle())); + }); + + try { + localizer = new RobobuggyKFLocalizer(LOCALIZER_UPDATE_PERIOD, "Simulated Localization", initialPos); + controller = new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile())); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + + gpsTimer = new Timer(); + gpsTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + // TODO: 4/3/17 fill this out once we get the main loop running + } + }, 0, GPS_UPDATE_PERIOD); + + encTimer = new Timer(); + encTimer.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + // TODO: 4/3/17 fill this out once we get the main loop running + } + }, 0, ENC_UPDATE_PERIOD); + + } + + private void updateMotionModelSteering(double v) { + // TODO: 4/3/17 fill this out once we get the main loop running } @Override From a345948eebfdb487bc73ce91c56050cb4c2d5232 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 3 Apr 2017 18:28:08 -0400 Subject: [PATCH 085/149] added better sim toolbox --- .../robobuggy/main/RobobuggyMainFile.java | 4 +- .../planners/WayPointFollowerPlanner.java | 2 +- .../roboclub/robobuggy/robots/SimRobot.java | 36 ++----------- .../robobuggy/simulation/FullSimRunner.java | 51 ++++++++++++++----- 4 files changed, 44 insertions(+), 49 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index f65b3269..97a5a13f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.TransistorAuton; +import com.roboclub.robobuggy.robots.SimRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorAuton.getInstance(); + robot = SimRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 9aab85a2..023bdfe7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -73,7 +73,7 @@ public double getCommandedSteeringAngle() { // double commandedAngle = stanleyMethodController(); currentCommandedAngle = commandedAngle; - currentDesiredHeading = commandedAngle; // purpose? + currentDesiredHeading = pose.getHeading() + commandedAngle; return commandedAngle; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java index 14577f56..d5431e07 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java @@ -3,11 +3,13 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.HighTrustGPSLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.simulation.FullSimRunner; import com.roboclub.robobuggy.simulation.SimulatedBuggy; import com.roboclub.robobuggy.simulation.SimulatedGPSNode; import com.roboclub.robobuggy.simulation.SimulatedImuNode; @@ -49,32 +51,7 @@ public static AbstractRobot getInstance() { private SimRobot() { super(); - nodeList.add(new HighTrustGPSLocalizer()); - // nodeList.add(new KfLocalizer(100)); - nodeList.add(new SimulatedImuNode(100)); - nodeList.add(new SimulatedGPSNode(500)); - nodeList.add(new SimulatedRBSMNode()); - - try { - ArrayList wayPoints = - WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); - nodeList.add(new WayPointFollowerPlanner(wayPoints)); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - - SimulatedBuggy simBuggy = SimulatedBuggy.getInstance(); - simBuggy.setY(LocalizerUtil.convertLatToMeters(40.441705)); - simBuggy.setX(LocalizerUtil.convertLonToMeters(-79.941585)); - simBuggy.setTh(-110); - simBuggy.setDx(10); - - - nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, - NodeChannel.getLoggingChannels())); - //simBuggy.setDth(1); - // simBuggy.setDth(0.10); - + nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(40.441670, -79.9416362))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); @@ -82,13 +59,8 @@ private SimRobot() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new ImuPanel(),"IMU"); - tabs.addTab(new AutonomousPanel(), "Autonomous"); - tabs.addTab(new SimulationPanel(), "Simulation"); - tabs.addTab(new PathPanel(), "Path Panel"); + tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); - } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index 86fc0099..4bf43c8f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -2,7 +2,9 @@ import Jama.Matrix; import com.roboclub.robobuggy.main.RobobuggyConfigFile; -import com.roboclub.robobuggy.messages.SteeringMeasurement; +import com.roboclub.robobuggy.messages.DriveControlMessage; +import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.nodes.localizers.LocTuple; @@ -16,6 +18,7 @@ import com.roboclub.robobuggy.ros.Subscriber; import java.io.FileNotFoundException; +import java.util.Date; import java.util.Timer; import java.util.TimerTask; @@ -29,12 +32,13 @@ public class FullSimRunner extends PeriodicNode { private static final int VELOCITY = 3; private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad private static final int LOCALIZER_UPDATE_PERIOD = 10; - private static final int CONTROLLER_UPDATE_PERIOD = 10; private static final int GPS_UPDATE_PERIOD = 500; - private static final int ENC_UPDATE_PERIOD = 10; + private static final int ENC_UPDATE_PERIOD = 100; + private static final int SIM_UPDATE_PERIOD = 100; private Matrix motionModel; private Matrix state; + private double totalDistance = 0; private Publisher gpsPub; private Publisher encPub; @@ -49,19 +53,18 @@ public class FullSimRunner extends PeriodicNode { /** * Create a new {@link PeriodicNode} decorator * - * @param period of the periodically executed portion of the node * @param name */ - protected FullSimRunner(int period, String name, LocTuple initialPos) { - super(new BuggyBaseNode(NodeChannel.SIMULATION), period, name); + public FullSimRunner(String name, LocTuple initialPos) { + super(new BuggyBaseNode(NodeChannel.SIMULATION), SIM_UPDATE_PERIOD, name); - // initialized to the identity matrix (goes nowhere) + // initialized to the identity matrix except for dHeading (goes nowhere) double[][] motionModelArr = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, {0, 0, 1, 0, 0}, {0, 0, 0, 1, 0}, - {0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0}, }; motionModel = new Matrix(motionModelArr); @@ -77,13 +80,16 @@ protected FullSimRunner(int period, String name, LocTuple initialPos) { gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); encPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); - steerSub = new Subscriber("Full Sim Toolbox", NodeChannel.STEERING.getMsgPath(), (topicName, m) -> { - updateMotionModelSteering(Math.toRadians(((SteeringMeasurement) m).getAngle())); + steerSub = new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { + updateMotionModel(((DriveControlMessage) m).getAngleDouble()); }); + updateMotionModel(0); try { localizer = new RobobuggyKFLocalizer(LOCALIZER_UPDATE_PERIOD, "Simulated Localization", initialPos); + localizer.startNode(); controller = new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile())); + controller.startNode(); } catch (FileNotFoundException e) { e.printStackTrace(); } @@ -92,7 +98,12 @@ protected FullSimRunner(int period, String name, LocTuple initialPos) { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - // TODO: 4/3/17 fill this out once we get the main loop running + Matrix nextStep = generateNextStep(); + UTMTuple nextLocUTM = new UTMTuple(UTMZONE, 'T', state.get(0, 0), state.get(1, 0)); + LocTuple nextLocLL = LocalizerUtil.utm2Deg(nextLocUTM); + gpsPub.publish(new GpsMeasurement(new Date(), nextLocLL.getLatitude(), true, nextLocLL.getLongitude(), true, 0, 0, 0, 0, 0, 0)); + state = nextStep; + } }, 0, GPS_UPDATE_PERIOD); @@ -100,14 +111,26 @@ public void run() { encTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - // TODO: 4/3/17 fill this out once we get the main loop running + state = generateNextStep(); + double deltaDist = ENC_UPDATE_PERIOD / 1000.0 * state.get(2, 0); + totalDistance += deltaDist; + encPub.publish(new EncoderMeasurement(totalDistance, state.get(2, 0))); } }, 0, ENC_UPDATE_PERIOD); } - private void updateMotionModelSteering(double v) { - // TODO: 4/3/17 fill this out once we get the main loop running + private void updateMotionModel(double v) { + double dt = ENC_UPDATE_PERIOD / 1000.0; + motionModel.set(0, 2, dt*Math.cos(state.get(3, 0))); + motionModel.set(1, 2, dt*Math.sin(state.get(3, 0))); + motionModel.set(3, 4, dt); + motionModel.set(4, 2, Math.tan(v) / WHEELBASE_IN_METERS); + + } + + private Matrix generateNextStep() { + return motionModel.times(state); } @Override From a9ffef8526d1d9a39ca57d2015fb0c81acbebb26 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 3 Apr 2017 23:36:25 -0400 Subject: [PATCH 086/149] added noise and disabled periodicity of simulator --- .../localizers/RobobuggyKFLocalizer.java | 11 +++---- .../planners/WayPointFollowerPlanner.java | 2 +- .../robobuggy/simulation/FullSimRunner.java | 32 ++++++++----------- 3 files changed, 20 insertions(+), 25 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index b01e4827..32d02991 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -95,15 +95,15 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { P = arrayToMatrix(pArray); double[][] qGPS2D = { - {4, 0, 0}, - {0, 4, 0}, - {0, 0, 0.02}, + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 0}, }; Q_gps = new Matrix(qGPS2D); double[][] qEncoder2D = { {0.25, 0}, - {0, 0.1}, + {0, 1}, }; Q_encoder = new Matrix(qEncoder2D); @@ -145,8 +145,7 @@ private void setupEncoderSubscriber() { lastEncoderTime = currentTime; lastEncoder = currentEncoder; - double arcRadius = WHEELBASE_IN_METERS / Math.sin(steeringAngle); - double headingChange = dx / arcRadius; + double headingChange = Math.tan(steeringAngle) / WHEELBASE_IN_METERS; // measurement double[][] z2D = { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 023bdfe7..b3319f58 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -83,7 +83,7 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 4.0; + double K = 2.0; double velocity = pose.getCurrentState().get(2, 0); double lookaheadLowerBound = 3.0; double lookaheadUpperBound = 25.0; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index 4bf43c8f..752432ab 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -5,6 +5,7 @@ import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.messages.SteeringMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.nodes.localizers.LocTuple; @@ -25,14 +26,14 @@ /** * Created by vivaanbahl on 4/2/17. */ -public class FullSimRunner extends PeriodicNode { +public class FullSimRunner extends BuggyBaseNode { private static final double WHEELBASE_IN_METERS = 1.13; // meters private static final int UTMZONE = 17; - private static final int VELOCITY = 3; + private static final int VELOCITY = 4; private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad private static final int LOCALIZER_UPDATE_PERIOD = 10; - private static final int GPS_UPDATE_PERIOD = 500; + private static final int GPS_UPDATE_PERIOD = 2000; private static final int ENC_UPDATE_PERIOD = 100; private static final int SIM_UPDATE_PERIOD = 100; @@ -42,6 +43,7 @@ public class FullSimRunner extends PeriodicNode { private Publisher gpsPub; private Publisher encPub; + private Publisher steerPub; private Subscriber steerSub; private RobobuggyKFLocalizer localizer; @@ -56,7 +58,7 @@ public class FullSimRunner extends PeriodicNode { * @param name */ public FullSimRunner(String name, LocTuple initialPos) { - super(new BuggyBaseNode(NodeChannel.SIMULATION), SIM_UPDATE_PERIOD, name); + super(NodeChannel.SIMULATION); // initialized to the identity matrix except for dHeading (goes nowhere) double[][] motionModelArr = { @@ -80,8 +82,10 @@ public FullSimRunner(String name, LocTuple initialPos) { gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); encPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); + steerPub = new Publisher(NodeChannel.STEERING.getMsgPath()); steerSub = new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { updateMotionModel(((DriveControlMessage) m).getAngleDouble()); + steerPub.publish(new SteeringMeasurement(Math.toDegrees(((DriveControlMessage) m).getAngleDouble()))); }); updateMotionModel(0); @@ -98,8 +102,14 @@ public FullSimRunner(String name, LocTuple initialPos) { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { + // 50-50 shot that we get noise on the left or right +// double noise = Math.random() > 0.5? 0.5 : -0.5; + double noise = 0; + Matrix nextStep = generateNextStep(); UTMTuple nextLocUTM = new UTMTuple(UTMZONE, 'T', state.get(0, 0), state.get(1, 0)); + nextLocUTM.setEasting(nextLocUTM.getEasting() + Math.random() * noise); + nextLocUTM.setNorthing(nextLocUTM.getNorthing() + Math.random() * noise); LocTuple nextLocLL = LocalizerUtil.utm2Deg(nextLocUTM); gpsPub.publish(new GpsMeasurement(new Date(), nextLocLL.getLatitude(), true, nextLocLL.getLongitude(), true, 0, 0, 0, 0, 0, 0)); state = nextStep; @@ -133,18 +143,4 @@ private Matrix generateNextStep() { return motionModel.times(state); } - @Override - protected void update() { - - } - - @Override - protected boolean startDecoratorNode() { - return false; - } - - @Override - protected boolean shutdownDecoratorNode() { - return false; - } } From 82bd186cae280b5589f85e5b9d98332e1d6f2bf7 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 4 Apr 2017 16:35:14 -0400 Subject: [PATCH 087/149] changed noise covariance matrices to fit better with what we actually observe --- .../nodes/localizers/RobobuggyKFLocalizer.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 32d02991..61217361 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -46,6 +46,14 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private Matrix Q_gps; // model noise covariance matrix private Matrix Q_encoder; + // Q is variance of new measurements + // P is variance of old measurements + // C is observation matrix + // Rk = covariance of measurements + // S-1 models confidence of new measurements + // if Rk is 0, that means kalman gain is high, means you weight measurement more than model + // if Rk is large, kalman gain low, weight model more than measurements + // output matrices private Matrix C_gps; // a description of how the GPS impacts x private Matrix C_encoder; // how the encoder affects x @@ -95,15 +103,15 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { P = arrayToMatrix(pArray); double[][] qGPS2D = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 0}, + {4, 0, 0}, + {0, 4, 0}, + {0, 0, 0.1}, }; Q_gps = new Matrix(qGPS2D); double[][] qEncoder2D = { {0.25, 0}, - {0, 1}, + {0, 0.1}, }; Q_encoder = new Matrix(qEncoder2D); From df87f20c20ea086dd4479f2ef32313390d7eb7c5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 4 Apr 2017 22:39:50 -0400 Subject: [PATCH 088/149] added skeleton of polynomial path planner --- .../nodes/planners/PolynomialPlanner.java | 41 +++++++++++++++++++ .../robobuggy/simulation/FullSimRunner.java | 2 +- 2 files changed, 42 insertions(+), 1 deletion(-) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java new file mode 100644 index 00000000..af98db97 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java @@ -0,0 +1,41 @@ +package com.roboclub.robobuggy.nodes.planners; + +import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.ros.NodeChannel; + +import java.util.ArrayList; + +/** + * Created by vivaanbahl on 4/4/17. + */ +public class PolynomialPlanner extends PathPlannerNode { + + private ArrayList waypoints; + + /** + * Construct a new {@link PathPlannerNode} + * + * @param channel {@link NodeChannel} on which to broadcast status + * information about the node + */ + public PolynomialPlanner(ArrayList waypoints) { + super(NodeChannel.PATH_PLANNER); + this.waypoints = waypoints; + } + + @Override + protected void updatePositionEstimate(GPSPoseMessage m) { + + } + + @Override + protected double getCommandedSteeringAngle() { + return 0; + } + + @Override + protected boolean getDeployBrakeValue() { + return false; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index 752432ab..0c7f1671 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -33,7 +33,7 @@ public class FullSimRunner extends BuggyBaseNode { private static final int VELOCITY = 4; private static final double INITIAL_HEADING_IN_RADS = 4.36; // rad private static final int LOCALIZER_UPDATE_PERIOD = 10; - private static final int GPS_UPDATE_PERIOD = 2000; + private static final int GPS_UPDATE_PERIOD = 500; private static final int ENC_UPDATE_PERIOD = 100; private static final int SIM_UPDATE_PERIOD = 100; From e8486ee290636253f8f2d2d86e99c4b4340fb631 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 4 Apr 2017 22:42:42 -0400 Subject: [PATCH 089/149] added current waypoint into logs --- .../roboclub/robobuggy/messages/DriveControlMessage.java | 7 +++++++ .../roboclub/robobuggy/nodes/planners/PathPlannerNode.java | 3 ++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java index 4b1feecd..78e302fd 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java @@ -13,6 +13,7 @@ public class DriveControlMessage extends BaseMessage { * Angle is supposed to be in radians */ private final double angle; + private GpsMeasurement currentWaypoint; /** * Construct a new DriveControlMessage @@ -25,6 +26,12 @@ public DriveControlMessage(Date timestamp, double angle) { this.timestamp = new Date(timestamp.getTime()).getTime(); } + public DriveControlMessage(Date timestamp, double angle, GpsMeasurement currentWaypoint) { + this.angle = angle; + this.timestamp = new Date(timestamp.getTime()).getTime(); + this.currentWaypoint = currentWaypoint; + } + /** * Returns the commanded angle of the steering as a double (in degrees) * diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java index 58a9b51d..147e6ba2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java @@ -12,6 +12,7 @@ import com.roboclub.robobuggy.ros.Publisher; import com.roboclub.robobuggy.ros.Subscriber; +import java.time.temporal.WeekFields; import java.util.Date; /** @@ -48,7 +49,7 @@ protected final boolean startDecoratorNode() { public void actionPerformed(String topicName, Message m) { updatePositionEstimate((GPSPoseMessage) m); steeringCommandPub.publish(new DriveControlMessage(new Date(), - getCommandedSteeringAngle())); + getCommandedSteeringAngle(), WayPointFollowerPlanner.currentWaypoint)); brakingCommandPub.publish(new BrakeControlMessage(new Date(), getDeployBrakeValue())); } From 396a4493d13140833a20a9e4aad789b6f57d6f4e Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 5 Apr 2017 17:02:49 -0400 Subject: [PATCH 090/149] testing fixes --- .../com/roboclub/robobuggy/main/RobobuggyMainFile.java | 4 +++- .../robobuggy/messages/DriveControlMessage.java | 3 +++ .../nodes/localizers/RobobuggyKFLocalizer.java | 5 +---- .../nodes/planners/WayPointFollowerPlanner.java | 4 ++-- .../com/roboclub/robobuggy/robots/TransistorAuton.java | 2 +- .../roboclub/robobuggy/simulation/PlayBackUtil.java | 5 ++++- .../src/main/java/com/roboclub/robobuggy/ui/Map.java | 10 ++++++++-- 7 files changed, 22 insertions(+), 11 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 97a5a13f..381fc195 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,9 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; +import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.robots.SimRobot; +import com.roboclub.robobuggy.robots.TransistorAuton; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +32,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = SimRobot.getInstance(); + robot = PlayBackRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java index 78e302fd..ecce2953 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java @@ -41,4 +41,7 @@ public double getAngleDouble() { return angle; } + public GpsMeasurement getWaypoint() { + return currentWaypoint; + } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 61217361..6d8b50a4 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -110,8 +110,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { Q_gps = new Matrix(qGPS2D); double[][] qEncoder2D = { - {0.25, 0}, - {0, 0.1}, + {0.25}, }; Q_encoder = new Matrix(qEncoder2D); @@ -124,7 +123,6 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] cEncoder2D = { {0, 0, 1, 0, 0}, - {0, 0, 0, 0, 1}, }; C_encoder = new Matrix(cEncoder2D); @@ -158,7 +156,6 @@ private void setupEncoderSubscriber() { // measurement double[][] z2D = { { bodySpeed }, - { headingChange }, }; Matrix z = new Matrix(z2D); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index b3319f58..03b4a733 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -21,7 +21,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private int lastClosestIndex = 0; // we only want to look at the next 10 waypoints as possible candidates - private final static int WAYPOINT_LOOKAHEAD_MAX = 10; + private final static int WAYPOINT_LOOKAHEAD_MAX = 50; /** * @vivaanbahl TESTING CODE ONLY @@ -83,7 +83,7 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 2.0; + double K = 3.0; double velocity = pose.getCurrentState().get(2, 0); double lookaheadLowerBound = 3.0; double lookaheadUpperBound = 25.0; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index fa3e1d53..45c8de37 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -59,7 +59,7 @@ private TransistorAuton() { new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); // Initialize Nodes - nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(0, 0))); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 11eed6fb..6a61a325 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -23,6 +23,7 @@ import com.roboclub.robobuggy.ros.Message; import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; +import com.sun.org.apache.xml.internal.resolver.helpers.PublicId; /** * utilities for playback @@ -43,7 +44,7 @@ public final class PlayBackUtil { private Publisher loggingButtonPub; private Publisher logicNotificationPub; private Publisher posePub; - + private Publisher driveCtrlPub; /** * Gets the PlaybackUtil instance @@ -71,6 +72,7 @@ private PlayBackUtil() { loggingButtonPub = new Publisher(NodeChannel.GUI_LOGGING_BUTTON.getMsgPath()); logicNotificationPub = new Publisher(NodeChannel.LOGIC_NOTIFICATION.getMsgPath()); posePub = new Publisher(NodeChannel.POSE.getMsgPath()); + driveCtrlPub = new Publisher(NodeChannel.DRIVE_CTRL.getMsgPath()); } /** @@ -136,6 +138,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); + getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index cb57b2d1..6704a749 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -2,6 +2,7 @@ import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; @@ -81,8 +82,8 @@ public void actionPerformed(String topicName, Message m) { addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); getMapTree().getViewer().removeMapMarker(currentWaypoint); - currentWaypoint.setLat(WayPointFollowerPlanner.currentWaypoint.getLatitude()); - currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); +// currentWaypoint.setLat(WayPointFollowerPlanner.currentWaypoint.getLatitude()); +// currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); getMapTree().getViewer().addMapMarker(currentWaypoint); getMapTree().getViewer().removeMapPolygon(desiredHeadingMapObj); @@ -123,6 +124,11 @@ public void actionPerformed(String topicName, Message m) { GpsMeasurement gps = ((GpsMeasurement) m); addPointsToMapTree(Color.BLACK, new LocTuple(gps.getLatitude(), gps.getLongitude())); })); + new Subscriber("map", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { + DriveControlMessage dcm = ((DriveControlMessage) m); + currentWaypoint.setLat(dcm.getWaypoint().getLatitude()); + currentWaypoint.setLon(dcm.getWaypoint().getLongitude()); + }); } From 85a512841bc6cb06107ec6b34e3780f708916122 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 5 Apr 2017 22:33:29 -0400 Subject: [PATCH 091/149] swapped out Pure Pursuit for Stanley controller Stanley controller is better for keeping the heading consistent with the path, which is what we need --- .../roboclub/robobuggy/nodes/planners/PolynomialPlanner.java | 2 -- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java index af98db97..14d850a7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java @@ -16,8 +16,6 @@ public class PolynomialPlanner extends PathPlannerNode { /** * Construct a new {@link PathPlannerNode} * - * @param channel {@link NodeChannel} on which to broadcast status - * information about the node */ public PolynomialPlanner(ArrayList waypoints) { super(NodeChannel.PATH_PLANNER); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 03b4a733..fa26847a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -69,8 +69,8 @@ public double getCommandedSteeringAngle() { // determines the angle at which to move every 50 milliseconds //PD control of DC steering motor handled by low level - double commandedAngle = purePursuitController(); -// double commandedAngle = stanleyMethodController(); +// double commandedAngle = purePursuitController(); + double commandedAngle = stanleyMethodController(); currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; From 34ddd6b1669fe476c65568c7766fc763a56ea04f Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 5 Apr 2017 22:43:20 -0400 Subject: [PATCH 092/149] added in extremely crude dynamic GPS covariance Basically whenever the number of satellites drops, we weight the GPS by 0.75 less (divide the entire Q matrix by 0.75) Whenever the number of satellites increases, we weight the GPS by 0.75 more (multiply the entire Q matrix by 0.75) --- .../robobuggy/messages/GpsMeasurement.java | 4 +++ .../localizers/RobobuggyKFLocalizer.java | 29 ++++++++++++++++--- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java index bd9f7aab..a18dd0f9 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java @@ -168,4 +168,8 @@ public static double getDistance(GpsMeasurement a, GpsMeasurement b) { double dy = LocalizerUtil.convertLatToMeters(a.getLatitude() - b.getLatitude()); return Math.sqrt(dx * dx + dy * dy); } + + public int getNumSatellites() { + return numSatellites; + } } \ No newline at end of file diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 6d8b50a4..98fbb391 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -63,10 +63,7 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private double lastEncoder; // deadreckoning value private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity private double steeringAngle = 0; // current steering angle of the buggy - - public Matrix getCurrentState() { - return x; - } + private int prevNumSatellites = 0; /** * Create a new {@link PeriodicNode} decorator @@ -194,6 +191,30 @@ private void setupGPSSubscriber() { Matrix z = new Matrix(z2D); + + // very crude dynamic covariance + if (prevNumSatellites == 0) { + prevNumSatellites = gpsLoc.getNumSatellites(); + } + else { + if (prevNumSatellites < gpsLoc.getNumSatellites()) { + // want to trust the GPS more, since we have better lock + double pos_covariance = Q_gps.get(0, 0); + double orient_covariance = Q_gps.get(2, 2); + Q_gps.set(0, 0, pos_covariance * 0.75); + Q_gps.set(1, 1, pos_covariance * 0.75); + Q_gps.set(2, 2, orient_covariance * 0.75); + } + else if (prevNumSatellites > gpsLoc.getNumSatellites()) { + // want to trust the GPS less, since we have worse lock + double pos_covariance = Q_gps.get(0, 0); + double orient_covariance = Q_gps.get(2, 2); + Q_gps.set(0, 0, pos_covariance * 1/0.75); + Q_gps.set(1, 1, pos_covariance * 1/0.75); + Q_gps.set(2, 2, orient_covariance * 1/0.75); + } + } + kalmanFilter(C_gps, Q_gps, z); })); } From 275d5f0314645616849eed8d08cca709b7e61048 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 5 Apr 2017 23:48:08 -0400 Subject: [PATCH 093/149] this is a terrible commit and i should feel bad about it basically after consulting with trevor we established that we should switch between static versions of the covariance matrices instead of dymanically computing them This is meant as a last-ditch effort, hopefully we don't have to use it --- .../localizers/RobobuggyKFLocalizer.java | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 98fbb391..c95d9eb8 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -197,21 +197,23 @@ private void setupGPSSubscriber() { prevNumSatellites = gpsLoc.getNumSatellites(); } else { - if (prevNumSatellites < gpsLoc.getNumSatellites()) { + if (prevNumSatellites <= 11) { // want to trust the GPS more, since we have better lock - double pos_covariance = Q_gps.get(0, 0); - double orient_covariance = Q_gps.get(2, 2); - Q_gps.set(0, 0, pos_covariance * 0.75); - Q_gps.set(1, 1, pos_covariance * 0.75); - Q_gps.set(2, 2, orient_covariance * 0.75); + double[][] qGPS2D = { + {4, 0, 0}, + {0, 4, 0}, + {0, 0, 0.1}, + }; + Q_gps = new Matrix(qGPS2D); } - else if (prevNumSatellites > gpsLoc.getNumSatellites()) { + else if (prevNumSatellites > 11) { // want to trust the GPS less, since we have worse lock - double pos_covariance = Q_gps.get(0, 0); - double orient_covariance = Q_gps.get(2, 2); - Q_gps.set(0, 0, pos_covariance * 1/0.75); - Q_gps.set(1, 1, pos_covariance * 1/0.75); - Q_gps.set(2, 2, orient_covariance * 1/0.75); + double[][] qGPS2D = { + {5, 0, 0}, + {0, 5, 0}, + {0, 0, 0.5}, + }; + Q_gps = new Matrix(qGPS2D); } } From 63b7c9bc2f94dd9093d1ddd4f2bcb054bc73dacb Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 5 Apr 2017 23:50:56 -0400 Subject: [PATCH 094/149] continuing the disgusting code quality --- .../nodes/localizers/RobobuggyKFLocalizer.java | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index c95d9eb8..51cf673f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -63,7 +63,6 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private double lastEncoder; // deadreckoning value private long lastEncoderTime; // the most recent time of the encoder reading, used for getting buggy velocity private double steeringAngle = 0; // current steering angle of the buggy - private int prevNumSatellites = 0; /** * Create a new {@link PeriodicNode} decorator @@ -193,11 +192,7 @@ private void setupGPSSubscriber() { // very crude dynamic covariance - if (prevNumSatellites == 0) { - prevNumSatellites = gpsLoc.getNumSatellites(); - } - else { - if (prevNumSatellites <= 11) { + if (gpsLoc.getNumSatellites() > 11) { // want to trust the GPS more, since we have better lock double[][] qGPS2D = { {4, 0, 0}, @@ -206,7 +201,7 @@ private void setupGPSSubscriber() { }; Q_gps = new Matrix(qGPS2D); } - else if (prevNumSatellites > 11) { + else if (gpsLoc.getNumSatellites() <= 11) { // want to trust the GPS less, since we have worse lock double[][] qGPS2D = { {5, 0, 0}, @@ -215,7 +210,6 @@ else if (prevNumSatellites > 11) { }; Q_gps = new Matrix(qGPS2D); } - } kalmanFilter(C_gps, Q_gps, z); })); From 48086f4705ff446c273ac396f8c5dc7eaf033ae1 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 6 Apr 2017 00:43:10 -0400 Subject: [PATCH 095/149] added in the controller to avoid trusting sudden changes in the orientation angle. This comes in handy when we are on the free roll section and the localizer can't get a proper lock on orientation --- .../localizers/RobobuggyKFLocalizer.java | 36 +++++++++---------- .../planners/WayPointFollowerPlanner.java | 21 ++++++++--- 2 files changed, 34 insertions(+), 23 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 51cf673f..d92bfbca 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -192,24 +192,24 @@ private void setupGPSSubscriber() { // very crude dynamic covariance - if (gpsLoc.getNumSatellites() > 11) { - // want to trust the GPS more, since we have better lock - double[][] qGPS2D = { - {4, 0, 0}, - {0, 4, 0}, - {0, 0, 0.1}, - }; - Q_gps = new Matrix(qGPS2D); - } - else if (gpsLoc.getNumSatellites() <= 11) { - // want to trust the GPS less, since we have worse lock - double[][] qGPS2D = { - {5, 0, 0}, - {0, 5, 0}, - {0, 0, 0.5}, - }; - Q_gps = new Matrix(qGPS2D); - } + if (gpsLoc.getNumSatellites() > 11) { + // want to trust the GPS more, since we have better lock + double[][] qGPS2D = { + {4, 0, 0}, + {0, 4, 0}, + {0, 0, 0.1}, + }; + Q_gps = new Matrix(qGPS2D); + } + else if (gpsLoc.getNumSatellites() <= 11) { + // want to trust the GPS less, since we have worse lock + double[][] qGPS2D = { + {5, 0, 0}, + {0, 5, 0}, + {0, 0, 0.5}, + }; + Q_gps = new Matrix(qGPS2D); + } kalmanFilter(C_gps, Q_gps, z); })); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index fa26847a..fe0d70ec 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -19,9 +19,11 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type private int lastClosestIndex = 0; + private double previousSteeringAngle = 0; // we only want to look at the next 10 waypoints as possible candidates private final static int WAYPOINT_LOOKAHEAD_MAX = 50; + private final static double MAX_ORIENTATION_ERROR = Math.toRadians(45); /** * @vivaanbahl TESTING CODE ONLY @@ -74,6 +76,7 @@ public double getCommandedSteeringAngle() { currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; + previousSteeringAngle = commandedAngle; return commandedAngle; } @@ -146,12 +149,20 @@ private double stanleyMethodController() { double pathHeading = Math.atan2(pathy, pathx); double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); - double determinant = (pathx * dy) - (pathy - dx); - double crosstrackError = - determinant / Math.sqrt(pathx*pathx + pathy*pathy); + double commandedAngle; - //Stanley steering controller - double commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); - commandedAngle = Util.normalizeAngleRad(commandedAngle); + if (Math.abs(headingError) > MAX_ORIENTATION_ERROR) { + commandedAngle = previousSteeringAngle; + } + else { + + double determinant = (pathx * dy) - (pathy - dx); + double crosstrackError = -determinant / Math.sqrt(pathx * pathx + pathy * pathy); + + //Stanley steering controller + commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); + commandedAngle = Util.normalizeAngleRad(commandedAngle); + } return commandedAngle; } From bbccdb25b64aac0deddd29420f4dce2ca75d8b65 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 6 Apr 2017 01:53:15 -0400 Subject: [PATCH 096/149] added mroe to the polynomial planner --- .../nodes/planners/PolynomialPlanner.java | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java index 14d850a7..ef297522 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java @@ -5,6 +5,7 @@ import com.roboclub.robobuggy.ros.NodeChannel; import java.util.ArrayList; +import java.util.Date; /** * Created by vivaanbahl on 4/4/17. @@ -12,6 +13,18 @@ public class PolynomialPlanner extends PathPlannerNode { private ArrayList waypoints; + private Date expirationTime; + private double previousSteeringAngle; + private GPSPoseMessage currentPose; + private GpsMeasurement target; + private ArrayList checkInPoints; + + // switch checkin points if we are 2m away from the current target checkin + private static final int SWITCHING_TOLERANCE_M = 2; + // recompute the polynomial every 1000ms + private static final int POLYNOMIAL_RECOMPUTE_PERIOD = 1000; + // sample the polynomial to get checkin points every 50 ms along the curve + private static final int CHECKIN_SAMPLE_PERIOD = 50; /** * Construct a new {@link PathPlannerNode} @@ -20,15 +33,52 @@ public class PolynomialPlanner extends PathPlannerNode { public PolynomialPlanner(ArrayList waypoints) { super(NodeChannel.PATH_PLANNER); this.waypoints = waypoints; + previousSteeringAngle = 0; + expirationTime = new Date(); } @Override protected void updatePositionEstimate(GPSPoseMessage m) { + currentPose = m; + } @Override protected double getCommandedSteeringAngle() { + Date currentTime = new Date(); + long elapsedMilliseconds = expirationTime.getTime() - currentTime.getTime(); + + // if we've expired the current polynomial curve either in time or in space recompute + if (elapsedMilliseconds < 0 || checkInPoints.size() == 0) { + + ArrayList samplePoints = new ArrayList<>(); + samplePoints.add(currentPose); + + // get all the waypoints within the recompute period away from us (velocity times period) + double lookaheadDist = currentPose.getCurrentState().get(2, 0) * POLYNOMIAL_RECOMPUTE_PERIOD; + int lookaheadIndex = 0; + for (GpsMeasurement candidate : waypoints) { + if (GPSPoseMessage.getDistance(currentPose, candidate.toGpsPoseMessage(0)) < lookaheadDist) { + samplePoints.add(candidate.toGpsPoseMessage(0)); + } + } + + // build our regression fit matrix + double[] yDouble = new double[samplePoints.size()]; + double[][] xDouble = new double[samplePoints.size()][samplePoints.size()]; + // TODO: 4/6/17 fill this in tomorrow + + } + // otherwise we still should be following this curve, check whether we're too close to this current check-in point + else { + GPSPoseMessage targetAsPose = target.toGpsPoseMessage(0); + double targetDistance = GPSPoseMessage.getDistance(currentPose, targetAsPose); + if (targetDistance <= SWITCHING_TOLERANCE_M) { + target = checkInPoints.remove(0); + } + } + return 0; } From fac6ceb611e4910586587686267cee74bd04728b Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 6 Apr 2017 22:22:16 -0400 Subject: [PATCH 097/149] finished polynomial planner --- .../robobuggy/main/RobobuggyMainFile.java | 6 +- .../nodes/planners/PolynomialPlanner.java | 89 ++++++++++++++++--- .../robots/ControllerTesterRobot.java | 5 +- 3 files changed, 80 insertions(+), 20 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 381fc195..cc0b1158 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,9 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.PlayBackRobot; -import com.roboclub.robobuggy.robots.SimRobot; -import com.roboclub.robobuggy.robots.TransistorAuton; +import com.roboclub.robobuggy.robots.ControllerTesterRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -32,7 +30,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = PlayBackRobot.getInstance(); + robot = ControllerTesterRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java index ef297522..565c8911 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java @@ -1,7 +1,11 @@ package com.roboclub.robobuggy.nodes.planners; +import Jama.Matrix; +import com.roboclub.robobuggy.main.Util; import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; +import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.ros.NodeChannel; import java.util.ArrayList; @@ -23,8 +27,12 @@ public class PolynomialPlanner extends PathPlannerNode { private static final int SWITCHING_TOLERANCE_M = 2; // recompute the polynomial every 1000ms private static final int POLYNOMIAL_RECOMPUTE_PERIOD = 1000; - // sample the polynomial to get checkin points every 50 ms along the curve - private static final int CHECKIN_SAMPLE_PERIOD = 50; + // we want 20 points sampled from the polynomial + private static final int CHECKIN_SAMPLE_SIZE = 20; + // we want our points 2m away from each other (laterally) + private static final double CHECKIN_POINT_SAMPLE_DISTANCE = LocalizerUtil.convertMetersToLon(2); + // quadratic interpolation, need 3 terms (including x^0) + private static final int POLYNOMIAL_NUMBER_OF_TERMS = 5; /** * Construct a new {@link PathPlannerNode} @@ -35,6 +43,7 @@ public PolynomialPlanner(ArrayList waypoints) { this.waypoints = waypoints; previousSteeringAngle = 0; expirationTime = new Date(); + checkInPoints = new ArrayList<>(); } @Override @@ -56,8 +65,7 @@ protected double getCommandedSteeringAngle() { samplePoints.add(currentPose); // get all the waypoints within the recompute period away from us (velocity times period) - double lookaheadDist = currentPose.getCurrentState().get(2, 0) * POLYNOMIAL_RECOMPUTE_PERIOD; - int lookaheadIndex = 0; + double lookaheadDist = currentPose.getCurrentState().get(2, 0) * POLYNOMIAL_RECOMPUTE_PERIOD / 1000.0; for (GpsMeasurement candidate : waypoints) { if (GPSPoseMessage.getDistance(currentPose, candidate.toGpsPoseMessage(0)) < lookaheadDist) { samplePoints.add(candidate.toGpsPoseMessage(0)); @@ -65,21 +73,74 @@ protected double getCommandedSteeringAngle() { } // build our regression fit matrix - double[] yDouble = new double[samplePoints.size()]; - double[][] xDouble = new double[samplePoints.size()][samplePoints.size()]; - // TODO: 4/6/17 fill this in tomorrow + double[][] yDouble = new double[POLYNOMIAL_NUMBER_OF_TERMS][1]; + double[][] xDouble = new double[POLYNOMIAL_NUMBER_OF_TERMS][POLYNOMIAL_NUMBER_OF_TERMS]; + for (int i = 0; i < POLYNOMIAL_NUMBER_OF_TERMS; i++) { + + for (GPSPoseMessage samplePoint : samplePoints) { + yDouble[i][0] += samplePoint.getLatitude() * Math.pow(samplePoint.getLongitude(), i); + } + + for (int j = 0; j < POLYNOMIAL_NUMBER_OF_TERMS; j++) { + for (GPSPoseMessage samplePoint : samplePoints) { + xDouble[i][j] += Math.pow(samplePoint.getLongitude(), i + j); + } + } + } + // since y will be horizontal but we want a vertical vector + Matrix y = new Matrix(yDouble); + Matrix x = new Matrix(xDouble); + + // get the coefficients of the polynomial + Matrix a = x.inverse().times(y); + + // build up our check in array + checkInPoints = new ArrayList<>(); + + for (int i = -CHECKIN_SAMPLE_SIZE; i < CHECKIN_SAMPLE_SIZE; i++) { + double checkin_x = currentPose.getLongitude() + i*CHECKIN_POINT_SAMPLE_DISTANCE; + double checkin_y = 0; + for (int j = 0; j < POLYNOMIAL_NUMBER_OF_TERMS; j++) { + checkin_y += a.get(j, 0) * Math.pow(checkin_x, j); + } + + checkInPoints.add(new GpsMeasurement(checkin_x, checkin_y)); + } } - // otherwise we still should be following this curve, check whether we're too close to this current check-in point - else { - GPSPoseMessage targetAsPose = target.toGpsPoseMessage(0); - double targetDistance = GPSPoseMessage.getDistance(currentPose, targetAsPose); - if (targetDistance <= SWITCHING_TOLERANCE_M) { - target = checkInPoints.remove(0); + + // otherwise we still should be following this curve, get the closest checkin point + double minDistance = Double.MAX_VALUE; + for (GpsMeasurement candidate: checkInPoints) { + GPSPoseMessage candidatePose = candidate.toGpsPoseMessage(0); + + // figure out heading diff between current and candidate, if it's "behind" (> 45deg), skip + double buggyOrientation = currentPose.getHeading(); + double checkpointDx = LocalizerUtil.convertLonToMeters(currentPose.getLongitude()) - LocalizerUtil.convertLonToMeters(candidatePose.getLongitude()); + double checkpointDy = LocalizerUtil.convertLatToMeters(currentPose.getLatitude()) - LocalizerUtil.convertLatToMeters(candidatePose.getLatitude()); + double checkpointOrientation = Math.atan2(checkpointDy, checkpointDx); + + double deltaHeading = Math.abs(buggyOrientation - checkpointOrientation); + if (deltaHeading < Math.toRadians(45)) { + double candidateDistance = GPSPoseMessage.getDistance(currentPose, candidatePose); + if (candidateDistance < minDistance) { + minDistance = candidateDistance; + target = candidate; + } } + } - return 0; + + // steer towards the target + double commandedAngle = 0; + double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(currentPose.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(currentPose.getLatitude()); + double deltaHeading = Math.atan2(dy, dx) - currentPose.getHeading(); + commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), minDistance); + commandedAngle = Util.normalizeAngleRad(commandedAngle); + + return commandedAngle; } @Override diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index 91674c86..7b55c131 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -3,7 +3,7 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; -import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.PolynomialPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.simulation.ControllerTester; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -35,7 +35,8 @@ private ControllerTesterRobot() { ArrayList wayPoints = null; try { wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); - nodeList.add(new WayPointFollowerPlanner(wayPoints)); + nodeList.add(new PolynomialPlanner(wayPoints)); +// nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block e.printStackTrace(); From 71e2b7a3e99ce7ca4d3f556cb3a4329abc61e7cf Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 6 Apr 2017 23:49:11 -0400 Subject: [PATCH 098/149] turns out doing the max allowable angle diff thing makes the stanley controller completely freak out in simulation --- .../nodes/planners/PolynomialPlanner.java | 3 ++- .../planners/WayPointFollowerPlanner.java | 20 ++++++------------- .../robots/ControllerTesterRobot.java | 6 +++--- .../java/com/roboclub/robobuggy/ui/Map.java | 4 ++-- 4 files changed, 13 insertions(+), 20 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java index 565c8911..7f151ea0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java @@ -32,7 +32,7 @@ public class PolynomialPlanner extends PathPlannerNode { // we want our points 2m away from each other (laterally) private static final double CHECKIN_POINT_SAMPLE_DISTANCE = LocalizerUtil.convertMetersToLon(2); // quadratic interpolation, need 3 terms (including x^0) - private static final int POLYNOMIAL_NUMBER_OF_TERMS = 5; + private static final int POLYNOMIAL_NUMBER_OF_TERMS = 3; /** * Construct a new {@link PathPlannerNode} @@ -110,6 +110,7 @@ protected double getCommandedSteeringAngle() { } // otherwise we still should be following this curve, get the closest checkin point + target = checkInPoints.get(0); double minDistance = Double.MAX_VALUE; for (GpsMeasurement candidate: checkInPoints) { GPSPoseMessage candidatePose = candidate.toGpsPoseMessage(0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index fe0d70ec..13216f3d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -19,11 +19,9 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type private int lastClosestIndex = 0; - private double previousSteeringAngle = 0; // we only want to look at the next 10 waypoints as possible candidates private final static int WAYPOINT_LOOKAHEAD_MAX = 50; - private final static double MAX_ORIENTATION_ERROR = Math.toRadians(45); /** * @vivaanbahl TESTING CODE ONLY @@ -76,7 +74,6 @@ public double getCommandedSteeringAngle() { currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; - previousSteeringAngle = commandedAngle; return commandedAngle; } @@ -146,23 +143,18 @@ private double stanleyMethodController() { double pathy = LocalizerUtil.convertLatToMeters(ptB.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); + currentWaypoint = ptA; double pathHeading = Math.atan2(pathy, pathx); double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); double commandedAngle; - if (Math.abs(headingError) > MAX_ORIENTATION_ERROR) { - commandedAngle = previousSteeringAngle; - } - else { - - double determinant = (pathx * dy) - (pathy - dx); - double crosstrackError = -determinant / Math.sqrt(pathx * pathx + pathy * pathy); + double determinant = (pathx * dy) - (pathy - dx); + double crosstrackError = -determinant / Math.sqrt(pathx * pathx + pathy * pathy); - //Stanley steering controller - commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); - commandedAngle = Util.normalizeAngleRad(commandedAngle); - } + //Stanley steering controller + commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); + commandedAngle = Util.normalizeAngleRad(commandedAngle); return commandedAngle; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index 7b55c131..3cf45f33 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -3,7 +3,7 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; -import com.roboclub.robobuggy.nodes.planners.PolynomialPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.simulation.ControllerTester; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -35,8 +35,8 @@ private ControllerTesterRobot() { ArrayList wayPoints = null; try { wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); - nodeList.add(new PolynomialPlanner(wayPoints)); -// nodeList.add(new WayPointFollowerPlanner(wayPoints)); +// nodeList.add(new PolynomialPlanner(wayPoints)); + nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block e.printStackTrace(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index 6704a749..0e1e1c12 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -82,8 +82,8 @@ public void actionPerformed(String topicName, Message m) { addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); getMapTree().getViewer().removeMapMarker(currentWaypoint); -// currentWaypoint.setLat(WayPointFollowerPlanner.currentWaypoint.getLatitude()); -// currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); + currentWaypoint.setLat(WayPointFollowerPlanner.currentWaypoint.getLatitude()); + currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); getMapTree().getViewer().addMapMarker(currentWaypoint); getMapTree().getViewer().removeMapPolygon(desiredHeadingMapObj); From e792a5ade8a22253174cd7f4344a8e551a62b82e Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 7 Apr 2017 15:53:03 -0400 Subject: [PATCH 099/149] turns out the polynomial planner was a bust, we keep getting singular matrices My guess is that either the sample points are too close together, in which the precision goes to 0. Really banking on the Stanley controller right about now --- .../nodes/planners/PolynomialPlanner.java | 151 ------------------ .../robots/ControllerTesterRobot.java | 1 - 2 files changed, 152 deletions(-) delete mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java deleted file mode 100644 index 7f151ea0..00000000 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PolynomialPlanner.java +++ /dev/null @@ -1,151 +0,0 @@ -package com.roboclub.robobuggy.nodes.planners; - -import Jama.Matrix; -import com.roboclub.robobuggy.main.Util; -import com.roboclub.robobuggy.messages.GPSPoseMessage; -import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; -import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; -import com.roboclub.robobuggy.ros.NodeChannel; - -import java.util.ArrayList; -import java.util.Date; - -/** - * Created by vivaanbahl on 4/4/17. - */ -public class PolynomialPlanner extends PathPlannerNode { - - private ArrayList waypoints; - private Date expirationTime; - private double previousSteeringAngle; - private GPSPoseMessage currentPose; - private GpsMeasurement target; - private ArrayList checkInPoints; - - // switch checkin points if we are 2m away from the current target checkin - private static final int SWITCHING_TOLERANCE_M = 2; - // recompute the polynomial every 1000ms - private static final int POLYNOMIAL_RECOMPUTE_PERIOD = 1000; - // we want 20 points sampled from the polynomial - private static final int CHECKIN_SAMPLE_SIZE = 20; - // we want our points 2m away from each other (laterally) - private static final double CHECKIN_POINT_SAMPLE_DISTANCE = LocalizerUtil.convertMetersToLon(2); - // quadratic interpolation, need 3 terms (including x^0) - private static final int POLYNOMIAL_NUMBER_OF_TERMS = 3; - - /** - * Construct a new {@link PathPlannerNode} - * - */ - public PolynomialPlanner(ArrayList waypoints) { - super(NodeChannel.PATH_PLANNER); - this.waypoints = waypoints; - previousSteeringAngle = 0; - expirationTime = new Date(); - checkInPoints = new ArrayList<>(); - } - - @Override - protected void updatePositionEstimate(GPSPoseMessage m) { - - currentPose = m; - - } - - @Override - protected double getCommandedSteeringAngle() { - Date currentTime = new Date(); - long elapsedMilliseconds = expirationTime.getTime() - currentTime.getTime(); - - // if we've expired the current polynomial curve either in time or in space recompute - if (elapsedMilliseconds < 0 || checkInPoints.size() == 0) { - - ArrayList samplePoints = new ArrayList<>(); - samplePoints.add(currentPose); - - // get all the waypoints within the recompute period away from us (velocity times period) - double lookaheadDist = currentPose.getCurrentState().get(2, 0) * POLYNOMIAL_RECOMPUTE_PERIOD / 1000.0; - for (GpsMeasurement candidate : waypoints) { - if (GPSPoseMessage.getDistance(currentPose, candidate.toGpsPoseMessage(0)) < lookaheadDist) { - samplePoints.add(candidate.toGpsPoseMessage(0)); - } - } - - // build our regression fit matrix - double[][] yDouble = new double[POLYNOMIAL_NUMBER_OF_TERMS][1]; - double[][] xDouble = new double[POLYNOMIAL_NUMBER_OF_TERMS][POLYNOMIAL_NUMBER_OF_TERMS]; - for (int i = 0; i < POLYNOMIAL_NUMBER_OF_TERMS; i++) { - - for (GPSPoseMessage samplePoint : samplePoints) { - yDouble[i][0] += samplePoint.getLatitude() * Math.pow(samplePoint.getLongitude(), i); - } - - for (int j = 0; j < POLYNOMIAL_NUMBER_OF_TERMS; j++) { - for (GPSPoseMessage samplePoint : samplePoints) { - xDouble[i][j] += Math.pow(samplePoint.getLongitude(), i + j); - } - } - } - // since y will be horizontal but we want a vertical vector - Matrix y = new Matrix(yDouble); - Matrix x = new Matrix(xDouble); - - // get the coefficients of the polynomial - Matrix a = x.inverse().times(y); - - // build up our check in array - checkInPoints = new ArrayList<>(); - - for (int i = -CHECKIN_SAMPLE_SIZE; i < CHECKIN_SAMPLE_SIZE; i++) { - double checkin_x = currentPose.getLongitude() + i*CHECKIN_POINT_SAMPLE_DISTANCE; - double checkin_y = 0; - for (int j = 0; j < POLYNOMIAL_NUMBER_OF_TERMS; j++) { - checkin_y += a.get(j, 0) * Math.pow(checkin_x, j); - } - - checkInPoints.add(new GpsMeasurement(checkin_x, checkin_y)); - } - - } - - // otherwise we still should be following this curve, get the closest checkin point - target = checkInPoints.get(0); - double minDistance = Double.MAX_VALUE; - for (GpsMeasurement candidate: checkInPoints) { - GPSPoseMessage candidatePose = candidate.toGpsPoseMessage(0); - - // figure out heading diff between current and candidate, if it's "behind" (> 45deg), skip - double buggyOrientation = currentPose.getHeading(); - double checkpointDx = LocalizerUtil.convertLonToMeters(currentPose.getLongitude()) - LocalizerUtil.convertLonToMeters(candidatePose.getLongitude()); - double checkpointDy = LocalizerUtil.convertLatToMeters(currentPose.getLatitude()) - LocalizerUtil.convertLatToMeters(candidatePose.getLatitude()); - double checkpointOrientation = Math.atan2(checkpointDy, checkpointDx); - - double deltaHeading = Math.abs(buggyOrientation - checkpointOrientation); - if (deltaHeading < Math.toRadians(45)) { - double candidateDistance = GPSPoseMessage.getDistance(currentPose, candidatePose); - if (candidateDistance < minDistance) { - minDistance = candidateDistance; - target = candidate; - } - } - - } - - - // steer towards the target - double commandedAngle = 0; - double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(currentPose.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(currentPose.getLatitude()); - double deltaHeading = Math.atan2(dy, dx) - currentPose.getHeading(); - commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), minDistance); - commandedAngle = Util.normalizeAngleRad(commandedAngle); - - return commandedAngle; - } - - @Override - protected boolean getDeployBrakeValue() { - return false; - } -} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index 3cf45f33..91674c86 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -35,7 +35,6 @@ private ControllerTesterRobot() { ArrayList wayPoints = null; try { wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); -// nodeList.add(new PolynomialPlanner(wayPoints)); nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block From aaf2ccc3ba106c85d5839c9f32ccacc82bbc4184 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 7 Apr 2017 17:08:32 -0400 Subject: [PATCH 100/149] tuned stanley controller from the simulation --- .../nodes/planners/WayPointFollowerPlanner.java | 5 ++--- .../robobuggy/simulation/ControllerTester.java | 17 +++-------------- 2 files changed, 5 insertions(+), 17 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 13216f3d..d1c72bd7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -128,7 +128,7 @@ private double stanleyMethodController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 0.1; + double K = 0.01; double velocity = pose.getCurrentState().get(2, 0); //if we are out of points then just go straight @@ -149,8 +149,7 @@ private double stanleyMethodController() { double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); double commandedAngle; - double determinant = (pathx * dy) - (pathy - dx); - double crosstrackError = -determinant / Math.sqrt(pathx * pathx + pathy * pathy); + double crosstrackError = GPSPoseMessage.getDistance(currentWaypoint.toGpsPoseMessage(0), pose); //Stanley steering controller commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index d0d7717a..a495bdb2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -3,7 +3,6 @@ import Jama.Matrix; import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; -import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; import com.roboclub.robobuggy.nodes.localizers.LocTuple; @@ -25,7 +24,7 @@ public class ControllerTester extends PeriodicNode { // run every 10 ms private static final int SIM_PERIOD = 10; // controller runs every 10 iterations - private static final int CONTROLLER_PERIOD = 5; + private static final int CONTROLLER_PERIOD = 1; // wheelbase in meters private static final double WHEELBASE = 1.13; // assume a velocity of 8 m/s @@ -62,7 +61,7 @@ public ControllerTester(String name, LocTuple initialPosition) { new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); - targetHeading = X.get(3, 0) + commandedSteeringAngle; +// targetHeading = X.get(3, 0) + commandedSteeringAngle; })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); @@ -78,22 +77,12 @@ protected void update() { // update simulated position X = A.times(X); - double steeringIncrement = Math.toRadians(0.5); - double heading = X.get(3, 0); - if (heading > targetHeading) { - X.set(3, 0, heading - steeringIncrement); - } - else if (heading < targetHeading) { - X.set(3, 0, heading + steeringIncrement); - } - UTMTuple t = new UTMTuple(17, 'T', X.get(0, 0), X.get(1, 0)); LocTuple lt = LocalizerUtil.utm2Deg(t); // if it's time to run the controller, update the controller's understanding of where we are if (simCounter == CONTROLLER_PERIOD) { - simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), heading, X)); - gpspub.publish(new GpsMeasurement(lt.getLatitude(), lt.getLongitude())); + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), X.get(3, 0), X)); simCounter = 0; } From c70f54b33bff0e2182f1efab1bba08449b288e2c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 8 Apr 2017 18:02:13 -0400 Subject: [PATCH 101/149] fixed math error in the Stanley controller Basically we were calculating cross-track error, but we weren't calculating whether it was error to the left or the right. Also changed how we calculate cross-track error to actually calculate distance to the track rather than just the nearest waypoint --- .../planners/WayPointFollowerPlanner.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index d1c72bd7..46eafb95 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -128,7 +128,7 @@ private double stanleyMethodController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 0.01; + double K = 0.05; double velocity = pose.getCurrentState().get(2, 0); //if we are out of points then just go straight @@ -148,8 +148,20 @@ private double stanleyMethodController() { double pathHeading = Math.atan2(pathy, pathx); double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); double commandedAngle; - - double crosstrackError = GPSPoseMessage.getDistance(currentWaypoint.toGpsPoseMessage(0), pose); + double L = GPSPoseMessage.getDistance(pose, ptA.toGpsPoseMessage(0)); + double theta = Math.atan2(dy, dx); + + double crosstrackError = L * Math.sin(theta); + int direction; + if (headingError > 0) { + // steer left + direction = 1; + } + else { + // steer right + direction = -1; + } + crosstrackError = crosstrackError * direction; //Stanley steering controller commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); From fd867f87bee37bc481212321690d5ca2352766f1 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 8 Apr 2017 18:26:09 -0400 Subject: [PATCH 102/149] removed dumb import --- .../java/com/roboclub/robobuggy/simulation/PlayBackUtil.java | 1 - 1 file changed, 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 6a61a325..64a6cafa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -23,7 +23,6 @@ import com.roboclub.robobuggy.ros.Message; import com.roboclub.robobuggy.ros.NodeChannel; import com.roboclub.robobuggy.ros.Publisher; -import com.sun.org.apache.xml.internal.resolver.helpers.PublicId; /** * utilities for playback From 41a94b73467950affa04366707bc046ceeb2297c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 8 Apr 2017 18:58:23 -0400 Subject: [PATCH 103/149] rolls code --- .../robobuggy/main/RobobuggyMainFile.java | 4 ++- .../localizers/RobobuggyKFLocalizer.java | 36 +++++++++---------- .../planners/WayPointFollowerPlanner.java | 8 ++--- .../robobuggy/robots/TransistorAuton.java | 2 +- 4 files changed, 26 insertions(+), 24 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index cc0b1158..70f7370c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -2,6 +2,8 @@ import com.roboclub.robobuggy.robots.AbstractRobot; import com.roboclub.robobuggy.robots.ControllerTesterRobot; +import com.roboclub.robobuggy.robots.PlayBackRobot; +import com.roboclub.robobuggy.robots.TransistorAuton; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +32,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = ControllerTesterRobot.getInstance(); + robot = TransistorAuton.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index d92bfbca..ab2b3ced 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -192,24 +192,24 @@ private void setupGPSSubscriber() { // very crude dynamic covariance - if (gpsLoc.getNumSatellites() > 11) { - // want to trust the GPS more, since we have better lock - double[][] qGPS2D = { - {4, 0, 0}, - {0, 4, 0}, - {0, 0, 0.1}, - }; - Q_gps = new Matrix(qGPS2D); - } - else if (gpsLoc.getNumSatellites() <= 11) { - // want to trust the GPS less, since we have worse lock - double[][] qGPS2D = { - {5, 0, 0}, - {0, 5, 0}, - {0, 0, 0.5}, - }; - Q_gps = new Matrix(qGPS2D); - } +// if (gpsLoc.getNumSatellites() > 11) { +// // want to trust the GPS more, since we have better lock +// double[][] qGPS2D = { +// {4, 0, 0}, +// {0, 4, 0}, +// {0, 0, 0.1}, +// }; +// Q_gps = new Matrix(qGPS2D); +// } +// else if (gpsLoc.getNumSatellites() <= 11) { +// // want to trust the GPS less, since we have worse lock +// double[][] qGPS2D = { +// {5, 0, 0}, +// {0, 5, 0}, +// {0, 0, 0.5}, +// }; +// Q_gps = new Matrix(qGPS2D); +// } kalmanFilter(C_gps, Q_gps, z); })); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index d1c72bd7..14294629 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -69,8 +69,8 @@ public double getCommandedSteeringAngle() { // determines the angle at which to move every 50 milliseconds //PD control of DC steering motor handled by low level -// double commandedAngle = purePursuitController(); - double commandedAngle = stanleyMethodController(); + double commandedAngle = purePursuitController(); +// double commandedAngle = stanleyMethodController(); currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; @@ -83,7 +83,7 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 3.0; + double K = 2.5; double velocity = pose.getCurrentState().get(2, 0); double lookaheadLowerBound = 3.0; double lookaheadUpperBound = 25.0; @@ -128,7 +128,7 @@ private double stanleyMethodController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 0.01; + double K = 0.03; double velocity = pose.getCurrentState().get(2, 0); //if we are out of points then just go straight diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 45c8de37..92c7e0a7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -60,7 +60,6 @@ private TransistorAuton() { // Initialize Nodes nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); - nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, NodeChannel.getLoggingChannels())); @@ -70,6 +69,7 @@ private TransistorAuton() { // nodeList.add(new HillCrestImuNode()); try { +// ArrayList wayPoints = WayPointUtil.createWayPointsFromLog("logs/", "2017-04-07-18-15-27/sensors_2017-04-07-18-15-27.txt"); ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); //ArrayList wayPoints = WayPointUtil.createWaypointsFromOdomLocalizerLog(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); From 7c9d37de46273dd1ca8b85fb40de8bf3ce7192c9 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 9 Apr 2017 11:59:50 -0400 Subject: [PATCH 104/149] added a Config robot to directly edit the config file --- .../robobuggy/main/RobobuggyMainFile.java | 4 ++ .../robobuggy/robots/ConfigRobot.java | 43 +++++++++++++++++++ 2 files changed, 47 insertions(+) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 70f7370c..b4123d97 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,9 +1,13 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; +<<<<<<< Updated upstream import com.roboclub.robobuggy.robots.ControllerTesterRobot; import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.robots.TransistorAuton; +======= +import com.roboclub.robobuggy.robots.ConfigRobot; +>>>>>>> Stashed changes import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java new file mode 100644 index 00000000..f4e1f507 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ConfigRobot.java @@ -0,0 +1,43 @@ +package com.roboclub.robobuggy.robots; + +import com.roboclub.robobuggy.ui.ConfigurationPanel; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.PathPanel; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; + +/** + * Runs playback + * + * @author Trevor Decker + */ +public final class ConfigRobot extends AbstractRobot { + + private static ConfigRobot instance; + + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ + public static AbstractRobot getInstance() { + if (instance == null) { + instance = new ConfigRobot(); + } + return instance; + } + + private ConfigRobot() { + super(); + + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); + Gui.getInstance().addWindow(mainWindow); + RobobuggyGUITabs tabs = new RobobuggyGUITabs(); + tabs.addTab(new ConfigurationPanel(), "Configuration"); + tabs.addTab(new PathPanel(), "Path Visualizer"); + mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); + Gui.getInstance().fixPaint(); + } +} + From 79ad673a2210983d6b3075287efa66633c51fcf4 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 9 Apr 2017 12:00:22 -0400 Subject: [PATCH 105/149] forgot about other conflicts --- .../java/com/roboclub/robobuggy/main/RobobuggyMainFile.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index b4123d97..f65b3269 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,13 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -<<<<<<< Updated upstream -import com.roboclub.robobuggy.robots.ControllerTesterRobot; -import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.robots.TransistorAuton; -======= -import com.roboclub.robobuggy.robots.ConfigRobot; ->>>>>>> Stashed changes import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; From 4efb3505c3cbf75bf5d5ae738e679587bc81aa5a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 10 Apr 2017 14:45:41 -0400 Subject: [PATCH 106/149] fixed playback --- .../robobuggy/main/RobobuggyMainFile.java | 5 ++-- .../localizers/RobobuggyKFLocalizer.java | 23 ------------------- .../robobuggy/robots/PlayBackRobot.java | 4 +++- .../robobuggy/simulation/PlayBackUtil.java | 2 +- .../com/roboclub/robobuggy/ui/DataPanel.java | 5 ++-- 5 files changed, 10 insertions(+), 29 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index f65b3269..8a213810 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.TransistorAuton; +import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +30,8 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = TransistorAuton.getInstance(); + robot = PlayBackRobot.getInstance(); +// robot = ConfigRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index ab2b3ced..3253d6a4 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -147,8 +147,6 @@ private void setupEncoderSubscriber() { lastEncoderTime = currentTime; lastEncoder = currentEncoder; - double headingChange = Math.tan(steeringAngle) / WHEELBASE_IN_METERS; - // measurement double[][] z2D = { { bodySpeed }, @@ -190,27 +188,6 @@ private void setupGPSSubscriber() { Matrix z = new Matrix(z2D); - - // very crude dynamic covariance -// if (gpsLoc.getNumSatellites() > 11) { -// // want to trust the GPS more, since we have better lock -// double[][] qGPS2D = { -// {4, 0, 0}, -// {0, 4, 0}, -// {0, 0, 0.1}, -// }; -// Q_gps = new Matrix(qGPS2D); -// } -// else if (gpsLoc.getNumSatellites() <= 11) { -// // want to trust the GPS less, since we have worse lock -// double[][] qGPS2D = { -// {5, 0, 0}, -// {0, 5, 0}, -// {0, 0, 0.5}, -// }; -// Q_gps = new Matrix(qGPS2D); -// } - kalmanFilter(C_gps, Q_gps, z); })); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index 7ff2c8cb..004cc51f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -1,7 +1,8 @@ package com.roboclub.robobuggy.robots; import com.roboclub.robobuggy.main.RobobuggyConfigFile; -import com.roboclub.robobuggy.nodes.localizers.KfLocalizer; +import com.roboclub.robobuggy.nodes.localizers.LocTuple; +import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; import com.roboclub.robobuggy.simulation.LineByLineSensorPlayer; import com.roboclub.robobuggy.ui.AutonomousPanel; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -36,6 +37,7 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 64a6cafa..6779965c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -160,7 +160,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case GPSPoseMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, GPSPoseMessage.class); - getPrivateInstance().posePub.publish(transmitMessage); +// getPrivateInstance().posePub.publish(transmitMessage); break; case RemoteWheelAngleRequest.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, RemoteWheelAngleRequest.class); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java index 48f6cc51..f505199f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/DataPanel.java @@ -1,6 +1,7 @@ package com.roboclub.robobuggy.ui; import com.roboclub.robobuggy.messages.BrakeControlMessage; +import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.EncoderMeasurement; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.messages.ImuMeasurement; @@ -195,10 +196,10 @@ public void actionPerformed(String topicName, Message m) { panel.add(commandAng); // Subscriber for drive control updates - new Subscriber("uiDataPan", NodeChannel.STEERING_COMMANDED.getMsgPath(), new MessageListener() { + new Subscriber("uiDataPan", NodeChannel.DRIVE_CTRL.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { - commandAng.setText(Double.toString(((SteeringMeasurement) m).getAngle())); + commandAng.setText(Double.toString(Math.toDegrees(((DriveControlMessage) m).getAngleDouble()))); Gui.getInstance().fixPaint(); } }); From 957d26e24d0e833fbcc1f8bbaeed34673eb91b25 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 10 Apr 2017 18:11:36 -0400 Subject: [PATCH 107/149] added in version 2 of the pure pursuit contoller --- .../planners/WayPointFollowerPlanner.java | 68 ++++++++++++++++++- .../robobuggy/robots/PlayBackRobot.java | 11 ++- .../robobuggy/simulation/PlayBackUtil.java | 2 +- 3 files changed, 77 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index d25b73f8..43c5e2b7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -19,6 +19,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private ArrayList wayPoints; private GPSPoseMessage pose; //TODO change this to a reasonable type private int lastClosestIndex = 0; + private GpsMeasurement target; // we only want to look at the next 10 waypoints as possible candidates private final static int WAYPOINT_LOOKAHEAD_MAX = 50; @@ -37,6 +38,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { public WayPointFollowerPlanner(ArrayList wayPoints) { super(NodeChannel.PATH_PLANNER); this.wayPoints = wayPoints; + target = wayPoints.get(0); pose = new GPSPoseMessage(new Date(0), 0, 0, 0);// temp measurment } @@ -69,14 +71,76 @@ public double getCommandedSteeringAngle() { // determines the angle at which to move every 50 milliseconds //PD control of DC steering motor handled by low level - double commandedAngle = purePursuitController(); -// double commandedAngle = stanleyMethodController(); + double commandedAngle; + commandedAngle = purePursuitController(); +// commandedAngle = stanleyMethodController(); +// commandedAngle = purePursuitV2(); + currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; return commandedAngle; } + private double purePursuitV2() { + + // check if we should get a new waypoint + boolean newWaypoint = false; + // if we don't have a target then we need one + if (target == wayPoints.get(0)) { + newWaypoint = true; + } + else { + double waypointDist = GPSPoseMessage.getDistance(pose, target.toGpsPoseMessage(0)); + // 3m is too close, pick a new one + if (waypointDist < 3) { + newWaypoint = true; + } + + double currentOrientation = pose.getCurrentState().get(3, 0); + double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); + double desiredHeading = Math.atan2(dy, dx); + + if (Math.abs(currentOrientation - desiredHeading) > Math.toRadians(45)){ + newWaypoint = true; + } + } + + // if we do, pick one which is 3 seconds away + if (newWaypoint) { + double lookaheadTime = 3.0; + // pick a point between 3 & 10m + double lookaheadLB = 3.0; + double lookaheadUB = 10.0; + double lookahead = pose.getCurrentState().get(2, 0) * lookaheadTime; + if (lookahead < lookaheadLB) { lookahead = lookaheadLB; } + if (lookahead > lookaheadUB) { lookahead = lookaheadUB; } + + int closestIndex = getClosestIndex(wayPoints, pose); + for (int i = closestIndex; i < closestIndex + WAYPOINT_LOOKAHEAD_MAX; i++) { + // pick the first point that is at least lookahead away + if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) > lookahead) { + target = wayPoints.get(i); + currentWaypoint = target; + break; + } + } + } + + // steer towards it + double currentOrientation = pose.getCurrentState().get(3, 0); + double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); + double desiredHeading = Math.atan2(dy, dx); + desiredHeading = Util.normalizeAngleRad(desiredHeading); + + double alpha = desiredHeading - currentOrientation; + double L = RobobuggyKFLocalizer.WHEELBASE_IN_METERS; + double ld = GPSPoseMessage.getDistance(target.toGpsPoseMessage(0), pose) + L / 2; + return Math.atan2(2*L*Math.sin(alpha), ld); + } + private double purePursuitController() { // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf // section 2.2 diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index 004cc51f..a5ad5b4b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -3,6 +3,8 @@ import com.roboclub.robobuggy.main.RobobuggyConfigFile; import com.roboclub.robobuggy.nodes.localizers.LocTuple; import com.roboclub.robobuggy.nodes.localizers.RobobuggyKFLocalizer; +import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; +import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.simulation.LineByLineSensorPlayer; import com.roboclub.robobuggy.ui.AutonomousPanel; import com.roboclub.robobuggy.ui.ConfigurationPanel; @@ -12,6 +14,8 @@ import com.roboclub.robobuggy.ui.RobobuggyGUITabs; import com.roboclub.robobuggy.ui.RobobuggyJFrame; +import java.io.FileNotFoundException; + /** * Runs playback * @@ -37,7 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); - nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); + try { + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); + nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 6779965c..30c03c67 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,7 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); - getPrivateInstance().driveCtrlPub.publish(transmitMessage); +// getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); From ebfaf926ceb5861597fac101cc295e529388a574 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 10 Apr 2017 20:41:10 -0400 Subject: [PATCH 108/149] added more stanley fixes --- .../planners/WayPointFollowerPlanner.java | 21 +++++-------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 43c5e2b7..44d248e0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -72,11 +72,10 @@ public double getCommandedSteeringAngle() { //PD control of DC steering motor handled by low level double commandedAngle; - commandedAngle = purePursuitController(); -// commandedAngle = stanleyMethodController(); +// commandedAngle = purePursuitController(); + commandedAngle = stanleyMethodController(); // commandedAngle = purePursuitV2(); - currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; return commandedAngle; @@ -192,7 +191,7 @@ private double stanleyMethodController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 0.03; + double K = 0.1; double velocity = pose.getCurrentState().get(2, 0); //if we are out of points then just go straight @@ -201,13 +200,13 @@ private double stanleyMethodController() { return 0; } - GpsMeasurement ptA = wayPoints.get(closestIndex); + GpsMeasurement ptA = wayPoints.get(closestIndex - 1); GpsMeasurement ptB = wayPoints.get(closestIndex + 1); double pathx = LocalizerUtil.convertLonToMeters(ptB.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); double pathy = LocalizerUtil.convertLatToMeters(ptB.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); - currentWaypoint = ptA; + currentWaypoint = wayPoints.get(closestIndex); double pathHeading = Math.atan2(pathy, pathx); double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); @@ -216,16 +215,6 @@ private double stanleyMethodController() { double theta = Math.atan2(dy, dx); double crosstrackError = L * Math.sin(theta); - int direction; - if (headingError > 0) { - // steer left - direction = 1; - } - else { - // steer right - direction = -1; - } - crosstrackError = crosstrackError * direction; //Stanley steering controller commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); From d5c2331555fd8a55c2c3f7c49fc8f9ea57362aa8 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 12 Apr 2017 13:01:02 -0400 Subject: [PATCH 109/149] localizer fixes --- .../localizers/RobobuggyKFLocalizer.java | 22 ++++++++++++++++--- .../planners/WayPointFollowerPlanner.java | 12 +++++----- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 3253d6a4..0028cdf3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -92,7 +92,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { }; x = new Matrix(x2D); - double[] rArray = {4, 4, 0.25, 0.01, 0.01}; + double[] rArray = {4, 4, 0.25, 0.07, 0.01}; double[] pArray = {25, 25, 0.25, 2.46, 2.46}; R = arrayToMatrix(rArray); @@ -101,7 +101,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] qGPS2D = { {4, 0, 0}, {0, 4, 0}, - {0, 0, 0.1}, + {0, 0, 0.07}, }; Q_gps = new Matrix(qGPS2D); @@ -135,7 +135,7 @@ private void setupEncoderSubscriber() { long currentTime = new Date().getTime(); long dt = currentTime - lastEncoderTime; // to remove numeric instability, limit rate to 10ms, 100Hz - if (dt < 10) { + if (dt < 100) { return; } @@ -286,7 +286,23 @@ This produces your next state (x[k]) and your next noise estimation (p[k]), K = K.plus(Q); K = P_pre.times(C.transpose()).times(K.inverse()); + double prev_heading = x.get(HEADING_GLOBAL_ROW, 0); x = x_pre.plus(K.times(residual)); + double new_heading = x.get(HEADING_GLOBAL_ROW, 0); + +// if (Math.abs(prev_heading - new_heading) > Math.toRadians(45)) { +// if (x.get(2, 0) > 0.5) { +// if (new_heading > prev_heading) { +// x.set(HEADING_GLOBAL_ROW, 0, prev_heading - Math.toRadians(10)); +// } else { +// x.set(HEADING_GLOBAL_ROW, 0, prev_heading + Math.toRadians(10)); +// } +// } +// else { +// x.set(HEADING_GLOBAL_ROW, 0, prev_heading); +// } +// } + P = Matrix.identity(5, 5).minus(K.times(C)); P = P.times(P_pre); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 44d248e0..17389e5f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -73,8 +73,8 @@ public double getCommandedSteeringAngle() { double commandedAngle; // commandedAngle = purePursuitController(); - commandedAngle = stanleyMethodController(); -// commandedAngle = purePursuitV2(); +// commandedAngle = stanleyMethodController(); + commandedAngle = purePursuitV2(); currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; @@ -191,7 +191,7 @@ private double stanleyMethodController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 0.1; + double K = 0.3; double velocity = pose.getCurrentState().get(2, 0); //if we are out of points then just go straight @@ -204,14 +204,14 @@ private double stanleyMethodController() { GpsMeasurement ptB = wayPoints.get(closestIndex + 1); double pathx = LocalizerUtil.convertLonToMeters(ptB.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); double pathy = LocalizerUtil.convertLatToMeters(ptB.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); - double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); + double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptB.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptB.getLatitude()); currentWaypoint = wayPoints.get(closestIndex); double pathHeading = Math.atan2(pathy, pathx); double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); double commandedAngle; - double L = GPSPoseMessage.getDistance(pose, ptA.toGpsPoseMessage(0)); + double L = GPSPoseMessage.getDistance(pose, ptB.toGpsPoseMessage(0)); double theta = Math.atan2(dy, dx); double crosstrackError = L * Math.sin(theta); From a2f77d59e79d4ad2061e55bc83c7cfa57ad7effc Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 13 Apr 2017 02:02:26 -0400 Subject: [PATCH 110/149] the gps has been actually fairly accurate, so we are choosing to trust it more --- .../roboclub/robobuggy/main/RobobuggyMainFile.java | 5 ++--- .../nodes/localizers/RobobuggyKFLocalizer.java | 10 +++++----- .../nodes/planners/WayPointFollowerPlanner.java | 14 +++++++++++--- .../roboclub/robobuggy/robots/PlayBackRobot.java | 10 +++++----- .../robobuggy/simulation/PlayBackUtil.java | 2 +- 5 files changed, 24 insertions(+), 17 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 8a213810..94a558e5 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,6 @@ package com.roboclub.robobuggy.main; -import com.roboclub.robobuggy.robots.AbstractRobot; -import com.roboclub.robobuggy.robots.PlayBackRobot; +import com.roboclub.robobuggy.robots.*; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,8 +29,8 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = PlayBackRobot.getInstance(); // robot = ConfigRobot.getInstance(); + robot = PlayBackRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 0028cdf3..47b97205 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -92,16 +92,16 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { }; x = new Matrix(x2D); - double[] rArray = {4, 4, 0.25, 0.07, 0.01}; + double[] rArray = {4, 4, 0.25, 0.01, 0.01}; double[] pArray = {25, 25, 0.25, 2.46, 2.46}; R = arrayToMatrix(rArray); P = arrayToMatrix(pArray); double[][] qGPS2D = { - {4, 0, 0}, - {0, 4, 0}, - {0, 0, 0.07}, + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 0.01}, }; Q_gps = new Matrix(qGPS2D); @@ -135,7 +135,7 @@ private void setupEncoderSubscriber() { long currentTime = new Date().getTime(); long dt = currentTime - lastEncoderTime; // to remove numeric instability, limit rate to 10ms, 100Hz - if (dt < 100) { + if (dt < 50) { return; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 17389e5f..4dd260cc 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -91,7 +91,7 @@ private double purePursuitV2() { } else { double waypointDist = GPSPoseMessage.getDistance(pose, target.toGpsPoseMessage(0)); - // 3m is too close, pick a new one + // 5m is too close, pick a new one if (waypointDist < 3) { newWaypoint = true; } @@ -118,8 +118,15 @@ private double purePursuitV2() { int closestIndex = getClosestIndex(wayPoints, pose); for (int i = closestIndex; i < closestIndex + WAYPOINT_LOOKAHEAD_MAX; i++) { + double dx = LocalizerUtil.convertLonToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); + + double ch = pose.getCurrentState().get(3, 0); + double dh = Math.atan2(dy, dx); + + double theta = dh-ch; // pick the first point that is at least lookahead away - if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) > lookahead) { + if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) * Math.cos(theta) > lookahead/2) { target = wayPoints.get(i); currentWaypoint = target; break; @@ -137,7 +144,8 @@ private double purePursuitV2() { double alpha = desiredHeading - currentOrientation; double L = RobobuggyKFLocalizer.WHEELBASE_IN_METERS; double ld = GPSPoseMessage.getDistance(target.toGpsPoseMessage(0), pose) + L / 2; - return Math.atan2(2*L*Math.sin(alpha), ld); +// return Math.atan2(2*L*Math.sin(alpha), ld); + return alpha; } private double purePursuitController() { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index a5ad5b4b..bdc50a29 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -41,12 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); - try { +// try { nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); - nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } +// nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); +// } catch (FileNotFoundException e) { +// e.printStackTrace(); +// } // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 30c03c67..6779965c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,7 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); -// getPrivateInstance().driveCtrlPub.publish(transmitMessage); + getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); From 7a68aa5c9a0964d7b01db4663d4da188cc2528b9 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 13 Apr 2017 02:12:09 -0400 Subject: [PATCH 111/149] pretty ballsy move here but if it works itll be legend --- .../robobuggy/nodes/localizers/RobobuggyKFLocalizer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 47b97205..85d604f0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -101,7 +101,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] qGPS2D = { {1, 0, 0}, {0, 1, 0}, - {0, 0, 0.01}, + {0, 0, 0.0001}, }; Q_gps = new Matrix(qGPS2D); From 0f3dd514530f135481f5371487210507c1c56ef0 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 13 Apr 2017 11:48:18 -0400 Subject: [PATCH 112/149] more tuning fixes --- .../nodes/planners/WayPointFollowerPlanner.java | 8 +++++--- .../com/roboclub/robobuggy/robots/PlayBackRobot.java | 10 +++++----- .../roboclub/robobuggy/simulation/PlayBackUtil.java | 2 +- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 4dd260cc..ab737f4e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -118,15 +118,17 @@ private double purePursuitV2() { int closestIndex = getClosestIndex(wayPoints, pose); for (int i = closestIndex; i < closestIndex + WAYPOINT_LOOKAHEAD_MAX; i++) { - double dx = LocalizerUtil.convertLonToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); + double dx = LocalizerUtil.convertLonToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLongitude()) - LocalizerUtil.convertLonToMeters + (wayPoints.get(closestIndex).getLongitude()); + double dy = LocalizerUtil.convertLatToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLatitude()) - LocalizerUtil.convertLatToMeters + (wayPoints.get(closestIndex).getLatitude()); double ch = pose.getCurrentState().get(3, 0); double dh = Math.atan2(dy, dx); double theta = dh-ch; // pick the first point that is at least lookahead away - if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) * Math.cos(theta) > lookahead/2) { + if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) * Math.cos(theta) > lookahead) { target = wayPoints.get(i); currentWaypoint = target; break; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index bdc50a29..a5ad5b4b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -41,12 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); -// try { + try { nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); -// nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); -// } catch (FileNotFoundException e) { -// e.printStackTrace(); -// } + nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 6779965c..30c03c67 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,7 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); - getPrivateInstance().driveCtrlPub.publish(transmitMessage); +// getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); From f61f47996c8121ab62bd69d5a5c407efc4ffe3d3 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 13 Apr 2017 11:50:49 -0400 Subject: [PATCH 113/149] forgot to reinclude ackermann steering --- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index ab737f4e..4be08235 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -146,8 +146,7 @@ private double purePursuitV2() { double alpha = desiredHeading - currentOrientation; double L = RobobuggyKFLocalizer.WHEELBASE_IN_METERS; double ld = GPSPoseMessage.getDistance(target.toGpsPoseMessage(0), pose) + L / 2; -// return Math.atan2(2*L*Math.sin(alpha), ld); - return alpha; + return Math.atan2(2*L*Math.sin(alpha), ld); } private double purePursuitController() { From b7c14532fe82dc58bd6e60ff1094a12ce505d262 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 18 Apr 2017 16:24:55 -0400 Subject: [PATCH 114/149] testing fixes --- .../roboclub/robobuggy/main/RobobuggyMainFile.java | 3 ++- .../nodes/localizers/RobobuggyKFLocalizer.java | 2 +- .../nodes/planners/WayPointFollowerPlanner.java | 12 ++++++------ .../com/roboclub/robobuggy/robots/PlayBackRobot.java | 12 ++++++------ .../roboclub/robobuggy/simulation/PlayBackUtil.java | 4 ++-- .../src/main/java/com/roboclub/robobuggy/ui/Map.java | 1 + 6 files changed, 18 insertions(+), 16 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 94a558e5..d63c0bc6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -30,7 +30,8 @@ public static void main(String[] args) { new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); // robot = ConfigRobot.getInstance(); - robot = PlayBackRobot.getInstance(); +// robot = PlayBackRobot.getInstance(); + robot = TransistorAuton.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 85d604f0..47b97205 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -101,7 +101,7 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[][] qGPS2D = { {1, 0, 0}, {0, 1, 0}, - {0, 0, 0.0001}, + {0, 0, 0.01}, }; Q_gps = new Matrix(qGPS2D); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 4be08235..09b56e29 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -72,9 +72,9 @@ public double getCommandedSteeringAngle() { //PD control of DC steering motor handled by low level double commandedAngle; -// commandedAngle = purePursuitController(); + commandedAngle = purePursuitController(); // commandedAngle = stanleyMethodController(); - commandedAngle = purePursuitV2(); +// commandedAngle = purePursuitV2(); currentCommandedAngle = commandedAngle; currentDesiredHeading = pose.getHeading() + commandedAngle; @@ -92,7 +92,7 @@ private double purePursuitV2() { else { double waypointDist = GPSPoseMessage.getDistance(pose, target.toGpsPoseMessage(0)); // 5m is too close, pick a new one - if (waypointDist < 3) { + if (waypointDist < 5) { newWaypoint = true; } @@ -101,7 +101,7 @@ private double purePursuitV2() { double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double desiredHeading = Math.atan2(dy, dx); - if (Math.abs(currentOrientation - desiredHeading) > Math.toRadians(45)){ + if (Math.abs(currentOrientation - desiredHeading) > Math.toRadians(40)){ newWaypoint = true; } } @@ -155,9 +155,9 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 2.5; + double K = 3.0; double velocity = pose.getCurrentState().get(2, 0); - double lookaheadLowerBound = 3.0; + double lookaheadLowerBound = 5.0; double lookaheadUpperBound = 25.0; double lookahead = K * velocity; if(lookahead < lookaheadLowerBound) { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index a5ad5b4b..e34b429d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -41,12 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); - try { - nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); - nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } +// try { +// nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); +// nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); +// } catch (FileNotFoundException e) { +// e.printStackTrace(); +// } // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 30c03c67..64a6cafa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,7 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); -// getPrivateInstance().driveCtrlPub.publish(transmitMessage); + getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); @@ -160,7 +160,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case GPSPoseMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, GPSPoseMessage.class); -// getPrivateInstance().posePub.publish(transmitMessage); + getPrivateInstance().posePub.publish(transmitMessage); break; case RemoteWheelAngleRequest.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, RemoteWheelAngleRequest.class); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index 0e1e1c12..7fee3f89 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -128,6 +128,7 @@ public void actionPerformed(String topicName, Message m) { DriveControlMessage dcm = ((DriveControlMessage) m); currentWaypoint.setLat(dcm.getWaypoint().getLatitude()); currentWaypoint.setLon(dcm.getWaypoint().getLongitude()); + WayPointFollowerPlanner.currentCommandedAngle = dcm.getAngleDouble(); }); } From 7db4bf1475112db25d3131645556a6930dcf91cb Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 18 Apr 2017 16:36:48 -0400 Subject: [PATCH 115/149] added untested course correction code --- .../nodes/planners/WayPointFollowerPlanner.java | 12 ++++++++++++ .../com/roboclub/robobuggy/robots/PlayBackRobot.java | 10 +++++----- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 09b56e29..96f35906 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -188,6 +188,18 @@ else if(lookahead > lookaheadUpperBound) { double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); + double closestWaypointDist = GPSPoseMessage.getDistance(pose, wayPoints.get(closestIndex).toGpsPoseMessage(0)); + if (closestWaypointDist > 3) { + // orientation is probably wacked, have path correction take over instead + double dpathx = LocalizerUtil.convertLonToMeters(wayPoints.get(closestIndex + 1).getLongitude()) - LocalizerUtil.convertLonToMeters(wayPoints.get + (closestIndex).getLongitude()); + double dpathy = LocalizerUtil.convertLatToMeters(wayPoints.get(closestIndex + 1).getLatitude()) - LocalizerUtil.convertLatToMeters(wayPoints.get + (closestIndex).getLatitude()); + + double pathHeading = Math.atan2(dpathy, dpathx); + deltaHeading = Math.atan2(dy, dx) - pathHeading; + } + //Pure Pursuit steering controller double commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), lookahead); commandedAngle = Util.normalizeAngleRad(commandedAngle); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index e34b429d..a1a338b2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -41,12 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); -// try { + try { // nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); -// nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); -// } catch (FileNotFoundException e) { -// e.printStackTrace(); -// } + nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } // new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); // new HighTrustGPSLocalizer(); RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); From cca6f42a96e3c6126f5f5840944a6849df6f1ce9 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 18 Apr 2017 16:46:26 -0400 Subject: [PATCH 116/149] made the switchover more aggressive --- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 2 +- .../java/com/roboclub/robobuggy/simulation/PlayBackUtil.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 96f35906..eb54f6a7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -189,7 +189,7 @@ else if(lookahead > lookaheadUpperBound) { double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); double closestWaypointDist = GPSPoseMessage.getDistance(pose, wayPoints.get(closestIndex).toGpsPoseMessage(0)); - if (closestWaypointDist > 3) { + if (closestWaypointDist > 2) { // orientation is probably wacked, have path correction take over instead double dpathx = LocalizerUtil.convertLonToMeters(wayPoints.get(closestIndex + 1).getLongitude()) - LocalizerUtil.convertLonToMeters(wayPoints.get (closestIndex).getLongitude()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 64a6cafa..a2afd636 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,7 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); - getPrivateInstance().driveCtrlPub.publish(transmitMessage); +// getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); From 06d5777f4d7f659c1a1cfa5e315caf43c1f0712c Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 19 Apr 2017 16:38:53 -0400 Subject: [PATCH 117/149] this will go down as one of the ballsiest moves in the history of the project added untested better path tracker --- .../planners/WayPointFollowerPlanner.java | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 96f35906..ef3d1360 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -188,17 +188,27 @@ else if(lookahead > lookaheadUpperBound) { double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); - double closestWaypointDist = GPSPoseMessage.getDistance(pose, wayPoints.get(closestIndex).toGpsPoseMessage(0)); - if (closestWaypointDist > 3) { - // orientation is probably wacked, have path correction take over instead - double dpathx = LocalizerUtil.convertLonToMeters(wayPoints.get(closestIndex + 1).getLongitude()) - LocalizerUtil.convertLonToMeters(wayPoints.get - (closestIndex).getLongitude()); - double dpathy = LocalizerUtil.convertLatToMeters(wayPoints.get(closestIndex + 1).getLatitude()) - LocalizerUtil.convertLatToMeters(wayPoints.get - (closestIndex).getLatitude()); - - double pathHeading = Math.atan2(dpathy, dpathx); - deltaHeading = Math.atan2(dy, dx) - pathHeading; - } + GpsMeasurement B = wayPoints.get(closestIndex); + GpsMeasurement A = wayPoints.get(closestIndex + 1); + GPSPoseMessage P = pose; + + double padx = LocalizerUtil.convertLonToMeters(P.getLongitude()) - LocalizerUtil.convertLonToMeters(A.getLongitude()); + double pady = LocalizerUtil.convertLatToMeters(P.getLatitude()) - LocalizerUtil.convertLatToMeters(A.getLatitude()); + double phi = Math.atan2(pady, padx); + + double badx = LocalizerUtil.convertLonToMeters(B.getLongitude()) - LocalizerUtil.convertLonToMeters(A.getLongitude()); + double bady = LocalizerUtil.convertLatToMeters(B.getLatitude()) - LocalizerUtil.convertLatToMeters(A.getLatitude()); + double psi = Math.atan2(bady, badx); + + double theta = phi - psi; + + double L = GPSPoseMessage.getDistance(P, A.toGpsPoseMessage(0)); + double E = L * Math.sin(theta); + + double thetaDelta = psi - Math.atan2(dy, dx); + thetaDelta *= E; + + deltaHeading += thetaDelta; //Pure Pursuit steering controller double commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), lookahead); From 3162d7ea7cd2dd85c0b6770bcd645bca59f3a32b Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 19 Apr 2017 18:17:19 -0400 Subject: [PATCH 118/149] ran simulations and fixed issues that came up proceed with caution, but I think this will work --- .../robobuggy/main/RobobuggyMainFile.java | 4 ++-- .../nodes/planners/WayPointFollowerPlanner.java | 17 +++++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index d63c0bc6..b046c7a9 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -30,8 +30,8 @@ public static void main(String[] args) { new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); // robot = ConfigRobot.getInstance(); -// robot = PlayBackRobot.getInstance(); - robot = TransistorAuton.getInstance(); + robot = PlayBackRobot.getInstance(); +// robot = TransistorAuton.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index ef3d1360..26a86d4c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -155,7 +155,7 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 3.0; + double K = 2.5; double velocity = pose.getCurrentState().get(2, 0); double lookaheadLowerBound = 5.0; double lookaheadUpperBound = 25.0; @@ -192,21 +192,22 @@ else if(lookahead > lookaheadUpperBound) { GpsMeasurement A = wayPoints.get(closestIndex + 1); GPSPoseMessage P = pose; - double padx = LocalizerUtil.convertLonToMeters(P.getLongitude()) - LocalizerUtil.convertLonToMeters(A.getLongitude()); - double pady = LocalizerUtil.convertLatToMeters(P.getLatitude()) - LocalizerUtil.convertLatToMeters(A.getLatitude()); + double padx = LocalizerUtil.convertLonToMeters(A.getLongitude()) - LocalizerUtil.convertLonToMeters(P.getLongitude()); + double pady = LocalizerUtil.convertLatToMeters(A.getLatitude()) - LocalizerUtil.convertLatToMeters(P.getLatitude()); double phi = Math.atan2(pady, padx); - double badx = LocalizerUtil.convertLonToMeters(B.getLongitude()) - LocalizerUtil.convertLonToMeters(A.getLongitude()); - double bady = LocalizerUtil.convertLatToMeters(B.getLatitude()) - LocalizerUtil.convertLatToMeters(A.getLatitude()); + double badx = LocalizerUtil.convertLonToMeters(A.getLongitude()) - LocalizerUtil.convertLonToMeters(B.getLongitude()); + double bady = LocalizerUtil.convertLatToMeters(A.getLatitude()) - LocalizerUtil.convertLatToMeters(B.getLatitude()); double psi = Math.atan2(bady, badx); double theta = phi - psi; - double L = GPSPoseMessage.getDistance(P, A.toGpsPoseMessage(0)); + double L = GPSPoseMessage.getDistance(P, B.toGpsPoseMessage(0)); double E = L * Math.sin(theta); - double thetaDelta = psi - Math.atan2(dy, dx); - thetaDelta *= E; + double thetaDelta = phi - psi; + thetaDelta *= E/10.0; + deltaHeading += thetaDelta; From 3281dde8e4f92f84577e34e60d754e14c22209e0 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Wed, 26 Apr 2017 19:53:34 -0400 Subject: [PATCH 119/149] raceday commit --- .../com/roboclub/robobuggy/main/RobobuggyMainFile.java | 6 ++++-- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 8 ++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index b046c7a9..6ed40272 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,6 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.*; +import com.roboclub.robobuggy.simulation.FullSimRunner; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,8 +31,9 @@ public static void main(String[] args) { new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); // robot = ConfigRobot.getInstance(); - robot = PlayBackRobot.getInstance(); -// robot = TransistorAuton.getInstance(); +// robot = PlayBackRobot.getInstance(); + robot = TransistorAuton.getInstance(); +// robot = CommTestRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 26a86d4c..8f12ec4d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -206,10 +206,14 @@ else if(lookahead > lookaheadUpperBound) { double E = L * Math.sin(theta); double thetaDelta = phi - psi; - thetaDelta *= E/10.0; + if (Math.signum(thetaDelta) != Math.signum(thetaDelta*E/10.0)){ + E=1; + } - deltaHeading += thetaDelta; + thetaDelta *= E/10.0; + +// deltaHeading += thetaDelta; //Pure Pursuit steering controller double commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), lookahead); From d5cbc4fb2c2817a7a7a3197837f9d18a6ea305a5 Mon Sep 17 00:00:00 2001 From: sbhotika Date: Fri, 28 Apr 2017 18:13:01 -0400 Subject: [PATCH 120/149] Stupid stuff --- offline/controller/controller.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/offline/controller/controller.m b/offline/controller/controller.m index fe0594a0..f785a122 100644 --- a/offline/controller/controller.m +++ b/offline/controller/controller.m @@ -107,7 +107,7 @@ end function [u] = control(desired_traj, X) - pos = X(1:2)'; + pos = X(1:2); b = repmat(pos, size(desired_traj, 1), 1); delta = 15*15; possible = find(sum((desired_traj-b).^2, 2) < delta); From 70e66c6b1b6adc23b100b3ddc4d5a107b726aa29 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 29 Apr 2017 22:24:34 -0400 Subject: [PATCH 121/149] moved all the data from the current state matrix into the rest of the fields of the GPSPoseMessage --- .../robobuggy/messages/GPSPoseMessage.java | 16 +++++++++------- .../nodes/localizers/RobobuggyKFLocalizer.java | 4 ++-- .../nodes/planners/WayPointFollowerPlanner.java | 12 ++++++------ .../robobuggy/simulation/ControllerTester.java | 2 +- 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java index c0fd3a11..b026f0f5 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java @@ -1,6 +1,5 @@ package com.roboclub.robobuggy.messages; -import Jama.Matrix; import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil; import com.roboclub.robobuggy.ros.Message; @@ -17,11 +16,11 @@ public class GPSPoseMessage extends BaseMessage { private final double latitude; private final double longitude; + private final double velocity; private final double heading; - private final Matrix currentState; public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading) { - this(timestamp, latitude, longitude, heading, null); + this(timestamp, latitude, longitude, heading, 0.0); } /** @@ -32,12 +31,12 @@ public GPSPoseMessage(Date timestamp, double latitude, double longitude, double * @param longitude of the buggy (negative is West) * @param heading of the buggy (in degrees from North) */ - public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, Matrix currentState) { + public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, double velocity) { this.latitude = latitude; this.longitude = longitude; this.heading = heading; this.timestamp = new Date(timestamp.getTime()).getTime(); - this.currentState = currentState; + this.velocity = velocity; } /** @@ -67,8 +66,11 @@ public double getHeading() { return heading; } - public Matrix getCurrentState() { - return currentState; + /** + * @return the current estimated velocity (m/s) + */ + public double getVelocity() { + return velocity; } /** diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 47b97205..177d40f8 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -25,7 +25,7 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // constants we use throughout the file private static final int X_GLOBAL_ROW = 0; private static final int Y_GLOBAL_ROW = 1; - private static final int Y_BODY_ROW = 2; + private static final int VELOCITY_ROW = 2; private static final int HEADING_GLOBAL_ROW = 3; private static final int HEADING_VEL_ROW = 4; @@ -208,7 +208,7 @@ protected void update() { LocTuple latLon = LocalizerUtil.utm2Deg(utm); if (gpsMessagesReceived) { posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), - latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x)); + latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x_predict.get(VELOCITY_ROW, 0))); } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 8f12ec4d..415d53e1 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -96,7 +96,7 @@ private double purePursuitV2() { newWaypoint = true; } - double currentOrientation = pose.getCurrentState().get(3, 0); + double currentOrientation = pose.getHeading(); double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double desiredHeading = Math.atan2(dy, dx); @@ -112,7 +112,7 @@ private double purePursuitV2() { // pick a point between 3 & 10m double lookaheadLB = 3.0; double lookaheadUB = 10.0; - double lookahead = pose.getCurrentState().get(2, 0) * lookaheadTime; + double lookahead = pose.getVelocity() * lookaheadTime; if (lookahead < lookaheadLB) { lookahead = lookaheadLB; } if (lookahead > lookaheadUB) { lookahead = lookaheadUB; } @@ -123,7 +123,7 @@ private double purePursuitV2() { double dy = LocalizerUtil.convertLatToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLatitude()) - LocalizerUtil.convertLatToMeters (wayPoints.get(closestIndex).getLatitude()); - double ch = pose.getCurrentState().get(3, 0); + double ch = pose.getHeading(); double dh = Math.atan2(dy, dx); double theta = dh-ch; @@ -137,7 +137,7 @@ private double purePursuitV2() { } // steer towards it - double currentOrientation = pose.getCurrentState().get(3, 0); + double currentOrientation = pose.getHeading(); double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double desiredHeading = Math.atan2(dy, dx); @@ -156,7 +156,7 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); double K = 2.5; - double velocity = pose.getCurrentState().get(2, 0); + double velocity = pose.getVelocity(); double lookaheadLowerBound = 5.0; double lookaheadUpperBound = 25.0; double lookahead = K * velocity; @@ -228,7 +228,7 @@ private double stanleyMethodController() { int closestIndex = getClosestIndex(wayPoints, pose); double K = 0.3; - double velocity = pose.getCurrentState().get(2, 0); + double velocity = pose.getVelocity(); //if we are out of points then just go straight if (closestIndex >= (wayPoints.size() - 1)) { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index a495bdb2..3f258d83 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -82,7 +82,7 @@ protected void update() { // if it's time to run the controller, update the controller's understanding of where we are if (simCounter == CONTROLLER_PERIOD) { - simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), X.get(3, 0), X)); + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), X.get(3, 0), X.get(2, 0))); simCounter = 0; } From 66db548330d1a8ef237cf8e67cbb46de6f2cc5c5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sun, 30 Apr 2017 15:19:15 -0400 Subject: [PATCH 122/149] forgot about this one --- .../Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index 7fee3f89..30abdf53 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -96,7 +96,7 @@ public void actionPerformed(String topicName, Message m) { desiredHeadingMapObj.setColor(Color.GREEN); getMapTree().getViewer().addMapPolygon(desiredHeadingMapObj); - double currentHeading = gpsM.getCurrentState().get(3, 0); + double currentHeading = gpsM.getHeading(); getMapTree().getViewer().removeMapPolygon(currentHeadingMapObj); currentHeadingMapObj = new MapPolygonImpl( new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), From 085d06a096d1c37145d6cbd684c686737bb664ac Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 5 May 2017 01:37:49 -0400 Subject: [PATCH 123/149] cleaned up comments and moved them to more appropriate place --- .../nodes/localizers/RobobuggyKFLocalizer.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 177d40f8..82b44b3e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -46,18 +46,18 @@ public class RobobuggyKFLocalizer extends PeriodicNode { private Matrix Q_gps; // model noise covariance matrix private Matrix Q_encoder; - // Q is variance of new measurements - // P is variance of old measurements - // C is observation matrix - // Rk = covariance of measurements - // S-1 models confidence of new measurements - // if Rk is 0, that means kalman gain is high, means you weight measurement more than model - // if Rk is large, kalman gain low, weight model more than measurements - // output matrices private Matrix C_gps; // a description of how the GPS impacts x private Matrix C_encoder; // how the encoder affects x + // Q is the variance of new measurements + // P is the variance of old measurements + // C is the observation matrix - what variables can we measure, since we can't directly measure everything + // in the state + // R = covariance of measurements + // if R is small, that means the "kalman gain" is high, means you weight measurement more than model + // if R is large, kalman gain low, weight model more than measurements + private long lastTime; // the last time we updated the current position estimate private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement private double lastEncoder; // deadreckoning value From a4d8dc9fd5a6eb6d85e9d9b98730d55968ee24c5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 5 May 2017 01:38:27 -0400 Subject: [PATCH 124/149] removed cancerous commented code --- .../nodes/localizers/RobobuggyKFLocalizer.java | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 82b44b3e..4e7867c0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -290,19 +290,6 @@ This produces your next state (x[k]) and your next noise estimation (p[k]), x = x_pre.plus(K.times(residual)); double new_heading = x.get(HEADING_GLOBAL_ROW, 0); -// if (Math.abs(prev_heading - new_heading) > Math.toRadians(45)) { -// if (x.get(2, 0) > 0.5) { -// if (new_heading > prev_heading) { -// x.set(HEADING_GLOBAL_ROW, 0, prev_heading - Math.toRadians(10)); -// } else { -// x.set(HEADING_GLOBAL_ROW, 0, prev_heading + Math.toRadians(10)); -// } -// } -// else { -// x.set(HEADING_GLOBAL_ROW, 0, prev_heading); -// } -// } - P = Matrix.identity(5, 5).minus(K.times(C)); P = P.times(P_pre); From 3704bea39fe6a6d9b6106466a1640770c9b797df Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 5 May 2017 01:45:51 -0400 Subject: [PATCH 125/149] fixes to Robots --- .../roboclub/robobuggy/nodes/planners/SweepNode.java | 1 - .../com/roboclub/robobuggy/robots/CommTestRobot.java | 7 ++----- .../com/roboclub/robobuggy/robots/PlayBackRobot.java | 11 +---------- .../roboclub/robobuggy/robots/TransistorAuton.java | 11 ++--------- 4 files changed, 5 insertions(+), 25 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java index 14c85e2b..c160d46b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/SweepNode.java @@ -75,7 +75,6 @@ protected void updatePositionEstimate(GPSPoseMessage m) { @Override protected double getCommandedSteeringAngle() { - System.out.println("requested steering angle"); return currentCommandedSteeringAngle; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java index f3b3d9f4..a29cb429 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java @@ -11,6 +11,7 @@ */ public class CommTestRobot extends AbstractRobot { private static CommTestRobot instance; + private static final int ARDUINO_BOOTLOADER_TIMEOUT_MS = 2000; public static CommTestRobot getInstance() { if (instance == null) { @@ -22,7 +23,7 @@ public static CommTestRobot getInstance() { private CommTestRobot() { super(); try { - Thread.sleep(2000); + Thread.sleep(ARDUINO_BOOTLOADER_TIMEOUT_MS); } catch (InterruptedException e) { e.printStackTrace(); } @@ -37,10 +38,6 @@ private CommTestRobot() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new AutonomousPanel(),"Autonomous"); - tabs.add(new PathPanel(), "Path Visualizer"); - tabs.addTab(new ConfigurationPanel(), "Configuration"); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java index a1a338b2..83d9d4fa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/PlayBackRobot.java @@ -41,21 +41,12 @@ public static AbstractRobot getInstance() { private PlayBackRobot() { super(); new LineByLineSensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); - try { -// nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); - nodeList.add(new WayPointFollowerPlanner(WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()))); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } -// new SensorPlayer(RobobuggyConfigFile.getPlayBackSourceFile(), 1); -// new HighTrustGPSLocalizer(); + RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); Gui.getInstance().addWindow(mainWindow); RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - tabs.addTab(new AutonomousPanel(), "Autonomous"); tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 92c7e0a7..40c55068 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -57,21 +57,16 @@ private TransistorAuton() { shutDown(); } new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); - // Initialize Nodes + // Initialize Nodes nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, NodeChannel.getLoggingChannels())); nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); -// nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); -// nodeList.add(new HillCrestImuNode()); - try { -// ArrayList wayPoints = WayPointUtil.createWayPointsFromLog("logs/", "2017-04-07-18-15-27/sensors_2017-04-07-18-15-27.txt"); - ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); - //ArrayList wayPoints = WayPointUtil.createWaypointsFromOdomLocalizerLog(RobobuggyConfigFile.getWaypointSourceLogFile()); + ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); } catch (IOException e) { // TODO Auto-generated catch block @@ -84,8 +79,6 @@ private TransistorAuton() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new AutonomousPanel(),"Autonomous"); tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); From bd0bc496421cb212b7eee01d5423216eaf352475 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 5 May 2017 01:48:15 -0400 Subject: [PATCH 126/149] fixed sim tools --- .../roboclub/robobuggy/robots/LocalizerTesterRobot.java | 4 +--- .../com/roboclub/robobuggy/simulation/FullSimRunner.java | 8 ++------ .../roboclub/robobuggy/simulation/LocalizerTester.java | 9 ++++----- 3 files changed, 7 insertions(+), 14 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java index 5b28ea88..d11a2ef7 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java @@ -33,7 +33,7 @@ protected LocalizerTesterRobot() { e.printStackTrace(); } - nodeList.add(new LocalizerTester("localizer tester", waypoints, 1)); + nodeList.add(new LocalizerTester("localizer tester", waypoints)); nodeList.add(new RobobuggyKFLocalizer(10, "localizer", new LocTuple(40.441670, -79.9416362))); //setup the gui @@ -42,8 +42,6 @@ protected LocalizerTesterRobot() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new AutonomousPanel(),"Autonomous"); tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index 0c7f1671..2043bca0 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -102,14 +102,10 @@ public FullSimRunner(String name, LocTuple initialPos) { gpsTimer.scheduleAtFixedRate(new TimerTask() { @Override public void run() { - // 50-50 shot that we get noise on the left or right -// double noise = Math.random() > 0.5? 0.5 : -0.5; - double noise = 0; - Matrix nextStep = generateNextStep(); UTMTuple nextLocUTM = new UTMTuple(UTMZONE, 'T', state.get(0, 0), state.get(1, 0)); - nextLocUTM.setEasting(nextLocUTM.getEasting() + Math.random() * noise); - nextLocUTM.setNorthing(nextLocUTM.getNorthing() + Math.random() * noise); + nextLocUTM.setEasting(nextLocUTM.getEasting()); + nextLocUTM.setNorthing(nextLocUTM.getNorthing()); LocTuple nextLocLL = LocalizerUtil.utm2Deg(nextLocUTM); gpsPub.publish(new GpsMeasurement(new Date(), nextLocLL.getLatitude(), true, nextLocLL.getLongitude(), true, 0, 0, 0, 0, 0, 0)); state = nextStep; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index b16ae44a..3f65ce99 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -28,7 +28,7 @@ public class LocalizerTester extends BuggyDecoratorNode { private Timer odomTimer; private LocTuple currentPosition = new LocTuple(40.441670, -79.9416362); private double heading = Math.toRadians(90); - private double noise; // todo insert noise + // TODO simulate noise using Gaussians and actual covariances private Publisher gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); private Publisher odomPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); @@ -38,12 +38,11 @@ public class LocalizerTester extends BuggyDecoratorNode { * * @param name the name we want for this node to store so that it can be referenced later */ - public LocalizerTester(String name, ArrayList waypoints, double noise) { + public LocalizerTester(String name, ArrayList waypoints) { super(new BuggyBaseNode(NodeChannel.POSE), name); gpsTimer = new Timer("GPS"); odomTimer = new Timer("odom"); - this.noise = noise; this.waypoints = waypoints; } @@ -64,8 +63,8 @@ private GpsMeasurement updateSimulatedPosition() { double dlon = currentPosition.getLongitude() - targetWaypoint.getLongitude(); heading = Math.atan2(dlat, -dlon) + Math.toRadians(90); - double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random() * noise); - double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random() * noise); + double updateLat = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.cos(heading + Math.random()); + double updateLon = LocalizerUtil.convertMetersToLat(POSITION_UPDATE_M) * Math.sin(heading + Math.random()); double newLat = currentPosition.getLatitude() + updateLat; double newLon = currentPosition.getLongitude() + updateLon; currentPosition = new LocTuple(newLat, newLon); From a411bd44acb9937a39c0c67610222972186bfb5b Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 5 May 2017 02:10:30 -0400 Subject: [PATCH 127/149] fixed testing code kerfuffle --- .../nodes/planners/PathPlannerNode.java | 7 +++++- .../planners/WayPointFollowerPlanner.java | 18 ++++----------- .../java/com/roboclub/robobuggy/ui/Map.java | 23 ++++--------------- 3 files changed, 15 insertions(+), 33 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java index 147e6ba2..ab9a61d2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java @@ -3,6 +3,7 @@ import com.roboclub.robobuggy.messages.BrakeControlMessage; import com.roboclub.robobuggy.messages.DriveControlMessage; import com.roboclub.robobuggy.messages.GPSPoseMessage; +import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; @@ -49,7 +50,7 @@ protected final boolean startDecoratorNode() { public void actionPerformed(String topicName, Message m) { updatePositionEstimate((GPSPoseMessage) m); steeringCommandPub.publish(new DriveControlMessage(new Date(), - getCommandedSteeringAngle(), WayPointFollowerPlanner.currentWaypoint)); + getCommandedSteeringAngle(), getTargetWaypoint())); brakingCommandPub.publish(new BrakeControlMessage(new Date(), getDeployBrakeValue())); } @@ -90,4 +91,8 @@ protected boolean shutdownDecoratorNode() { * @return desired commanded brake value (true is deployed) */ protected abstract boolean getDeployBrakeValue(); + + protected GpsMeasurement getTargetWaypoint() { + return null; + } } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 415d53e1..20b14c06 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -24,14 +24,6 @@ public class WayPointFollowerPlanner extends PathPlannerNode { // we only want to look at the next 10 waypoints as possible candidates private final static int WAYPOINT_LOOKAHEAD_MAX = 50; - /** - * @vivaanbahl TESTING CODE ONLY - * REMOVE FOR PROD PUSH - */ - public static GpsMeasurement currentWaypoint = new GpsMeasurement(0, 0); - public static double currentCommandedAngle = 0.0; - public static double currentDesiredHeading = 0.0; - /** * @param wayPoints the list of waypoints to follow */ @@ -48,6 +40,11 @@ public void updatePositionEstimate(GPSPoseMessage m) { } + @Override + protected GpsMeasurement getTargetWaypoint() { + return target; + } + //find the closest way point private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage currentLocation) { double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this @@ -76,8 +73,6 @@ public double getCommandedSteeringAngle() { // commandedAngle = stanleyMethodController(); // commandedAngle = purePursuitV2(); - currentCommandedAngle = commandedAngle; - currentDesiredHeading = pose.getHeading() + commandedAngle; return commandedAngle; } @@ -130,7 +125,6 @@ private double purePursuitV2() { // pick the first point that is at least lookahead away if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) * Math.cos(theta) > lookahead) { target = wayPoints.get(i); - currentWaypoint = target; break; } } @@ -183,7 +177,6 @@ else if(lookahead > lookaheadUpperBound) { //find a path from our current location to that point GpsMeasurement target = wayPoints.get(lookaheadIndex); - currentWaypoint = target; double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); @@ -242,7 +235,6 @@ private double stanleyMethodController() { double pathy = LocalizerUtil.convertLatToMeters(ptB.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptB.getLongitude()); double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptB.getLatitude()); - currentWaypoint = wayPoints.get(closestIndex); double pathHeading = Math.atan2(pathy, pathx); double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java index 30abdf53..3ab3ac55 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/ui/Map.java @@ -6,7 +6,6 @@ import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.localizers.LocTuple; -import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.ros.Message; import com.roboclub.robobuggy.ros.MessageListener; import com.roboclub.robobuggy.ros.NodeChannel; @@ -54,7 +53,7 @@ public class Map extends JPanel { private MapMarkerDot currentWaypoint = new MapMarkerDot(0, 0); private MapPolygonImpl currentSteeringCommandMapObj = new MapPolygonImpl(); private MapPolygonImpl currentHeadingMapObj = new MapPolygonImpl(); - private MapPolygonImpl desiredHeadingMapObj = new MapPolygonImpl(); + private double currentCommandedAngle; /** * initializes a new Map with cache loaded @@ -68,7 +67,6 @@ public Map() { getMapTree().getViewer().addMapMarker(currentWaypoint); getMapTree().getViewer().addMapPolygon(currentSteeringCommandMapObj); getMapTree().getViewer().addMapPolygon(currentHeadingMapObj); - getMapTree().getViewer().addMapPolygon(desiredHeadingMapObj); //adds track buggy new Subscriber("Map", NodeChannel.POSE.getMsgPath(), new MessageListener() { @@ -82,20 +80,8 @@ public void actionPerformed(String topicName, Message m) { addPointsToMapTree(Color.RED, new LocTuple(gpsM.getLatitude(), gpsM.getLongitude())); getMapTree().getViewer().removeMapMarker(currentWaypoint); - currentWaypoint.setLat(WayPointFollowerPlanner.currentWaypoint.getLatitude()); - currentWaypoint.setLon(WayPointFollowerPlanner.currentWaypoint.getLongitude()); getMapTree().getViewer().addMapMarker(currentWaypoint); - getMapTree().getViewer().removeMapPolygon(desiredHeadingMapObj); - desiredHeadingMapObj = new MapPolygonImpl( - new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), - new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(WayPointFollowerPlanner.currentDesiredHeading), gpsM.getLongitude() + 0.0001 * - Math.cos(WayPointFollowerPlanner.currentDesiredHeading)), - new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) - ); - desiredHeadingMapObj.setColor(Color.GREEN); - getMapTree().getViewer().addMapPolygon(desiredHeadingMapObj); - double currentHeading = gpsM.getHeading(); getMapTree().getViewer().removeMapPolygon(currentHeadingMapObj); currentHeadingMapObj = new MapPolygonImpl( @@ -109,9 +95,8 @@ public void actionPerformed(String topicName, Message m) { getMapTree().getViewer().removeMapPolygon(currentSteeringCommandMapObj); currentSteeringCommandMapObj = new MapPolygonImpl( new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()), - new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(WayPointFollowerPlanner - .currentCommandedAngle + currentHeading), gpsM.getLongitude() + 0.0001 * Math.cos(WayPointFollowerPlanner - .currentCommandedAngle + currentHeading)), + new Coordinate(gpsM.getLatitude() + 0.0001 * Math.sin(currentCommandedAngle + currentHeading), + gpsM.getLongitude() + 0.0001 * Math.cos(currentCommandedAngle + currentHeading)), new Coordinate(gpsM.getLatitude(), gpsM.getLongitude()) ); currentSteeringCommandMapObj.setColor(Color.BLUE); @@ -128,7 +113,7 @@ public void actionPerformed(String topicName, Message m) { DriveControlMessage dcm = ((DriveControlMessage) m); currentWaypoint.setLat(dcm.getWaypoint().getLatitude()); currentWaypoint.setLon(dcm.getWaypoint().getLongitude()); - WayPointFollowerPlanner.currentCommandedAngle = dcm.getAngleDouble(); + currentCommandedAngle = dcm.getAngleDouble(); }); } From 1038e8ea62e80e496644a404697edfce43ef1534 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 5 May 2017 02:22:50 -0400 Subject: [PATCH 128/149] fixed static analysis inconsistencies we may want to consider disabling those checks --- .../localizers/RobobuggyKFLocalizer.java | 84 +++++++++---------- 1 file changed, 41 insertions(+), 43 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java index 4e7867c0..86e21af2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizer.java @@ -41,22 +41,22 @@ public class RobobuggyKFLocalizer extends PeriodicNode { // heading - heading, or yaw angle, in the world frame, in rad // dheading - angular velocity in the world frame, in rad/s private Matrix x; // current state - private Matrix R; // measurement noise covariance matrix - private Matrix P; // covariance matrix - private Matrix Q_gps; // model noise covariance matrix - private Matrix Q_encoder; + private Matrix rMatrix; // measurement noise covariance matrix + private Matrix pMatrix; // covariance matrix + private Matrix qGPS; // model noise covariance matrix + private Matrix qEncoder; // output matrices - private Matrix C_gps; // a description of how the GPS impacts x - private Matrix C_encoder; // how the encoder affects x + private Matrix cGPS; // a description of how the GPS impacts x + private Matrix cEncoder; // how the encoder affects x // Q is the variance of new measurements - // P is the variance of old measurements + // pMatrix is the variance of old measurements // C is the observation matrix - what variables can we measure, since we can't directly measure everything // in the state - // R = covariance of measurements - // if R is small, that means the "kalman gain" is high, means you weight measurement more than model - // if R is large, kalman gain low, weight model more than measurements + // rMatrix = covariance of measurements + // if rMatrix is small, that means the "kalman gain" is high, means you weight measurement more than model + // if rMatrix is large, kalman gain low, weight model more than measurements private long lastTime; // the last time we updated the current position estimate private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement @@ -95,32 +95,32 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) { double[] rArray = {4, 4, 0.25, 0.01, 0.01}; double[] pArray = {25, 25, 0.25, 2.46, 2.46}; - R = arrayToMatrix(rArray); - P = arrayToMatrix(pArray); + rMatrix = arrayToMatrix(rArray); + pMatrix = arrayToMatrix(pArray); double[][] qGPS2D = { {1, 0, 0}, {0, 1, 0}, {0, 0, 0.01}, }; - Q_gps = new Matrix(qGPS2D); + qGPS = new Matrix(qGPS2D); double[][] qEncoder2D = { {0.25}, }; - Q_encoder = new Matrix(qEncoder2D); + qEncoder = new Matrix(qEncoder2D); double[][] cGPS2D = { {1, 0, 0, 0, 0}, {0, 1, 0, 0, 0}, {0, 0, 0, 1, 0}, }; - C_gps = new Matrix(cGPS2D); + cGPS = new Matrix(cGPS2D); double[][] cEncoder2D = { {0, 0, 1, 0, 0}, }; - C_encoder = new Matrix(cEncoder2D); + cEncoder = new Matrix(cEncoder2D); // add all our subscribers for our current state update stream // Every time we get a new sensor update, trigger the new kalman update @@ -153,7 +153,7 @@ private void setupEncoderSubscriber() { }; Matrix z = new Matrix(z2D); - kalmanFilter(C_encoder, Q_encoder, z); + kalmanFilter(cEncoder, qEncoder, z); })); } @@ -188,7 +188,7 @@ private void setupGPSSubscriber() { Matrix z = new Matrix(z2D); - kalmanFilter(C_gps, Q_gps, z); + kalmanFilter(cGPS, qGPS, z); })); } @@ -201,14 +201,14 @@ private void setupSteeringSubscriber() { @Override protected void update() { - Matrix x_predict = propagate(); + Matrix xPredict = propagate(); - UTMTuple utm = new UTMTuple(UTMZONE, 'T', x_predict.get(X_GLOBAL_ROW, 0), - x_predict.get(Y_GLOBAL_ROW, 0)); + UTMTuple utm = new UTMTuple(UTMZONE, 'T', xPredict.get(X_GLOBAL_ROW, 0), + xPredict.get(Y_GLOBAL_ROW, 0)); LocTuple latLon = LocalizerUtil.utm2Deg(utm); if (gpsMessagesReceived) { posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(), - latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x_predict.get(VELOCITY_ROW, 0))); + latLon.getLongitude(), xPredict.get(HEADING_GLOBAL_ROW, 0), xPredict.get(VELOCITY_ROW, 0))); } } @@ -238,13 +238,13 @@ private Matrix getMotionModel(double dt) { | | ------------------------------------------------------------------------------------| */ - private void kalmanFilter(Matrix C, Matrix Q, Matrix z) { + private void kalmanFilter(Matrix cMatrix, Matrix qMatrix, Matrix z) { // update time Date now = new Date(); double dt = (now.getTime() - lastTime) / 1000.0; lastTime = now.getTime(); - Matrix A = getMotionModel(dt); + Matrix aMatrix = getMotionModel(dt); /* the predict step is responsible for determining the estimate of the next state @@ -254,14 +254,14 @@ prediction state (xhat[k]) and our prediction noise (phat[k]) Predict: x_pre = A * x - P_pre = A * P * A' + R + P_pre = A * pMatrix * A' + rMatrix */ - Matrix x_pre = A.times(x); - Matrix P_pre = A.times(P).times(A.transpose()); - P_pre = P_pre.plus(R); + Matrix xPre = aMatrix.times(x); + Matrix pPre = aMatrix.times(pMatrix).times(aMatrix.transpose()); + pPre = pPre.plus(rMatrix); - x_pre.set(HEADING_GLOBAL_ROW, 0, clampAngle(x_pre.get(HEADING_GLOBAL_ROW, 0))); - x_pre.set(HEADING_VEL_ROW, 0, clampAngle(x_pre.get(HEADING_VEL_ROW, 0))); + xPre.set(HEADING_GLOBAL_ROW, 0, clampAngle(xPre.get(HEADING_GLOBAL_ROW, 0))); + xPre.set(HEADING_VEL_ROW, 0, clampAngle(xPre.get(HEADING_VEL_ROW, 0))); /* the update step is responsible for updating the current state, and resolving @@ -278,20 +278,18 @@ This produces your next state (x[k]) and your next noise estimation (p[k]), r = z - (C * x_pre) K = P_pre * C' * inv((C * P_pre * C') + Q) // gain x = x_pre + (K * r) - P = (I - (K * C)) * P_pre + pMatrix = (I - (K * C)) * P_pre */ - Matrix residual = z.minus(C.times(x_pre)); - Matrix K = C.times(P_pre); - K = K.times(C.transpose()); - K = K.plus(Q); - K = P_pre.times(C.transpose()).times(K.inverse()); + Matrix residual = z.minus(cMatrix.times(xPre)); + Matrix kMatrix = cMatrix.times(pPre); + kMatrix = kMatrix.times(cMatrix.transpose()); + kMatrix = kMatrix.plus(qMatrix); + kMatrix = pPre.times(cMatrix.transpose()).times(kMatrix.inverse()); - double prev_heading = x.get(HEADING_GLOBAL_ROW, 0); - x = x_pre.plus(K.times(residual)); - double new_heading = x.get(HEADING_GLOBAL_ROW, 0); + x = xPre.plus(kMatrix.times(residual)); - P = Matrix.identity(5, 5).minus(K.times(C)); - P = P.times(P_pre); + pMatrix = Matrix.identity(5, 5).minus(kMatrix.times(cMatrix)); + pMatrix = pMatrix.times(pPre); x.set(HEADING_GLOBAL_ROW, 0, clampAngle(x.get(HEADING_GLOBAL_ROW, 0))); x.set(HEADING_VEL_ROW, 0, clampAngle(x.get(HEADING_VEL_ROW, 0))); @@ -304,8 +302,8 @@ private Matrix propagate() { double dt = (now.getTime() - lastTime) / 1000.0; // x_pre = A * x - Matrix A = getMotionModel(dt); - return A.times(x); + Matrix aMatrix = getMotionModel(dt); + return aMatrix.times(x); } From aa9c9290f238c5bc00800cd622fdf7fc8b89be1f Mon Sep 17 00:00:00 2001 From: Darshan Patil Date: Fri, 5 May 2017 16:29:29 -0400 Subject: [PATCH 129/149] Cleaned up RobobuggyMainFile --- .../roboclub/robobuggy/main/RobobuggyMainFile.java | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 58f92f02..92962166 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,7 +1,7 @@ package com.roboclub.robobuggy.main; -import com.roboclub.robobuggy.robots.*; -import com.roboclub.robobuggy.simulation.FullSimRunner; +import com.roboclub.robobuggy.robots.AbstractRobot; +import com.roboclub.robobuggy.robots.TransistorAuton; import com.roboclub.robobuggy.ui.Gui; @@ -29,10 +29,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); -// robot = ConfigRobot.getInstance(); -// robot = PlayBackRobot.getInstance(); robot = TransistorAuton.getInstance(); -// robot = CommTestRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); @@ -61,11 +58,6 @@ public static AbstractRobot getCurrentRobot() { */ public static void resetSystem() { robot.shutDown(); - // Gui.close(); - // Gui.getInstance(); -// robot.getInstance(); - //TODO make this work for real - } From 3670b8867cc6d82c12f0843b665ca51008228fe2 Mon Sep 17 00:00:00 2001 From: Darshan Patil Date: Fri, 5 May 2017 16:34:36 -0400 Subject: [PATCH 130/149] Removed star import in Util.java --- .../src/main/java/com/roboclub/robobuggy/main/Util.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java index 465c83e8..5ebee217 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/Util.java @@ -6,7 +6,11 @@ import com.google.gson.JsonObject; import purejavacomm.CommPortIdentifier; -import java.io.*; +import java.io.File; +import java.io.FileInputStream; +import java.io.FileNotFoundException; +import java.io.InputStreamReader; +import java.io.UnsupportedEncodingException; import java.util.ArrayList; import java.util.Enumeration; import java.util.List; From a305d6d087c5bca6521614dfd43561b40fbf606e Mon Sep 17 00:00:00 2001 From: Darshan Patil Date: Fri, 5 May 2017 16:38:52 -0400 Subject: [PATCH 131/149] Removed unused controller code from WayPointFollowerPlanner --- .../planners/WayPointFollowerPlanner.java | 132 ------------------ 1 file changed, 132 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 20b14c06..d8c1ec36 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -70,79 +70,10 @@ public double getCommandedSteeringAngle() { double commandedAngle; commandedAngle = purePursuitController(); -// commandedAngle = stanleyMethodController(); -// commandedAngle = purePursuitV2(); return commandedAngle; } - private double purePursuitV2() { - - // check if we should get a new waypoint - boolean newWaypoint = false; - // if we don't have a target then we need one - if (target == wayPoints.get(0)) { - newWaypoint = true; - } - else { - double waypointDist = GPSPoseMessage.getDistance(pose, target.toGpsPoseMessage(0)); - // 5m is too close, pick a new one - if (waypointDist < 5) { - newWaypoint = true; - } - - double currentOrientation = pose.getHeading(); - double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); - double desiredHeading = Math.atan2(dy, dx); - - if (Math.abs(currentOrientation - desiredHeading) > Math.toRadians(40)){ - newWaypoint = true; - } - } - - // if we do, pick one which is 3 seconds away - if (newWaypoint) { - double lookaheadTime = 3.0; - // pick a point between 3 & 10m - double lookaheadLB = 3.0; - double lookaheadUB = 10.0; - double lookahead = pose.getVelocity() * lookaheadTime; - if (lookahead < lookaheadLB) { lookahead = lookaheadLB; } - if (lookahead > lookaheadUB) { lookahead = lookaheadUB; } - - int closestIndex = getClosestIndex(wayPoints, pose); - for (int i = closestIndex; i < closestIndex + WAYPOINT_LOOKAHEAD_MAX; i++) { - double dx = LocalizerUtil.convertLonToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLongitude()) - LocalizerUtil.convertLonToMeters - (wayPoints.get(closestIndex).getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(wayPoints.get(i).toGpsPoseMessage(0).getLatitude()) - LocalizerUtil.convertLatToMeters - (wayPoints.get(closestIndex).getLatitude()); - - double ch = pose.getHeading(); - double dh = Math.atan2(dy, dx); - - double theta = dh-ch; - // pick the first point that is at least lookahead away - if (GPSPoseMessage.getDistance(pose, wayPoints.get(i).toGpsPoseMessage(0)) * Math.cos(theta) > lookahead) { - target = wayPoints.get(i); - break; - } - } - } - - // steer towards it - double currentOrientation = pose.getHeading(); - double dx = LocalizerUtil.convertLonToMeters(target.getLongitude()) - LocalizerUtil.convertLonToMeters(pose.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); - double desiredHeading = Math.atan2(dy, dx); - desiredHeading = Util.normalizeAngleRad(desiredHeading); - - double alpha = desiredHeading - currentOrientation; - double L = RobobuggyKFLocalizer.WHEELBASE_IN_METERS; - double ld = GPSPoseMessage.getDistance(target.toGpsPoseMessage(0), pose) + L / 2; - return Math.atan2(2*L*Math.sin(alpha), ld); - } - private double purePursuitController() { // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf // section 2.2 @@ -181,75 +112,12 @@ else if(lookahead > lookaheadUpperBound) { double dy = LocalizerUtil.convertLatToMeters(target.getLatitude()) - LocalizerUtil.convertLatToMeters(pose.getLatitude()); double deltaHeading = Math.atan2(dy, dx) - pose.getHeading(); - GpsMeasurement B = wayPoints.get(closestIndex); - GpsMeasurement A = wayPoints.get(closestIndex + 1); - GPSPoseMessage P = pose; - - double padx = LocalizerUtil.convertLonToMeters(A.getLongitude()) - LocalizerUtil.convertLonToMeters(P.getLongitude()); - double pady = LocalizerUtil.convertLatToMeters(A.getLatitude()) - LocalizerUtil.convertLatToMeters(P.getLatitude()); - double phi = Math.atan2(pady, padx); - - double badx = LocalizerUtil.convertLonToMeters(A.getLongitude()) - LocalizerUtil.convertLonToMeters(B.getLongitude()); - double bady = LocalizerUtil.convertLatToMeters(A.getLatitude()) - LocalizerUtil.convertLatToMeters(B.getLatitude()); - double psi = Math.atan2(bady, badx); - - double theta = phi - psi; - - double L = GPSPoseMessage.getDistance(P, B.toGpsPoseMessage(0)); - double E = L * Math.sin(theta); - - double thetaDelta = phi - psi; - - if (Math.signum(thetaDelta) != Math.signum(thetaDelta*E/10.0)){ - E=1; - } - - thetaDelta *= E/10.0; - -// deltaHeading += thetaDelta; - //Pure Pursuit steering controller double commandedAngle = Math.atan2(2 * RobobuggyKFLocalizer.WHEELBASE_IN_METERS * Math.sin(deltaHeading), lookahead); commandedAngle = Util.normalizeAngleRad(commandedAngle); return commandedAngle; } - private double stanleyMethodController() { - // https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf - // section 2.3 - - int closestIndex = getClosestIndex(wayPoints, pose); - - double K = 0.3; - double velocity = pose.getVelocity(); - - //if we are out of points then just go straight - if (closestIndex >= (wayPoints.size() - 1)) { - new RobobuggyLogicNotification("HELP out of points", RobobuggyMessageLevel.EXCEPTION); - return 0; - } - - GpsMeasurement ptA = wayPoints.get(closestIndex - 1); - GpsMeasurement ptB = wayPoints.get(closestIndex + 1); - double pathx = LocalizerUtil.convertLonToMeters(ptB.getLongitude()) - LocalizerUtil.convertLonToMeters(ptA.getLongitude()); - double pathy = LocalizerUtil.convertLatToMeters(ptB.getLatitude()) - LocalizerUtil.convertLatToMeters(ptA.getLatitude()); - double dx = LocalizerUtil.convertLonToMeters(pose.getLongitude()) - LocalizerUtil.convertLonToMeters(ptB.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(pose.getLatitude()) - LocalizerUtil.convertLatToMeters(ptB.getLatitude()); - - double pathHeading = Math.atan2(pathy, pathx); - double headingError = Util.normalizeAngleRad(pathHeading) - Util.normalizeAngleRad(pose.getHeading()); - double commandedAngle; - double L = GPSPoseMessage.getDistance(pose, ptB.toGpsPoseMessage(0)); - double theta = Math.atan2(dy, dx); - - double crosstrackError = L * Math.sin(theta); - - //Stanley steering controller - commandedAngle = headingError + Math.atan2(K * crosstrackError, velocity); - commandedAngle = Util.normalizeAngleRad(commandedAngle); - return commandedAngle; - } - @Override protected boolean getDeployBrakeValue() { // need to brake when 15 m off course From 8e0708e160b33f6ef3d7de1568d087ce3153c292 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sat, 20 May 2017 15:27:53 -0400 Subject: [PATCH 132/149] removing waypoint .txt files --- offline/controller/waypoints_course.txt | 134 ---- offline/controller/waypoints_course_v2.txt | 733 --------------------- offline/controller/waypoints_tri.txt | 372 ----------- 3 files changed, 1239 deletions(-) delete mode 100644 offline/controller/waypoints_course.txt delete mode 100644 offline/controller/waypoints_course_v2.txt delete mode 100644 offline/controller/waypoints_tri.txt diff --git a/offline/controller/waypoints_course.txt b/offline/controller/waypoints_course.txt deleted file mode 100644 index 248b92ee..00000000 --- a/offline/controller/waypoints_course.txt +++ /dev/null @@ -1,134 +0,0 @@ -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44161120375703,"north":true,"longitude":-79.94159013032913,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463072} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441554046160554,"north":true,"longitude":-79.9416196346283,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463080} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441490764479234,"north":true,"longitude":-79.94164109230042,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463082} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44141931734805,"north":true,"longitude":-79.9416732788086,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463082} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44135195284055,"north":true,"longitude":-79.94171351194382,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463083} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44127234015373,"north":true,"longitude":-79.94175642728806,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463083} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44118864466336,"north":true,"longitude":-79.94180202484131,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463083} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441111073140206,"north":true,"longitude":-79.94184225797653,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44104166696468,"north":true,"longitude":-79.94187980890274,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44095592982538,"north":true,"longitude":-79.94191735982895,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440876316669566,"north":true,"longitude":-79.94195222854614,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463084} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440798744786015,"north":true,"longitude":-79.94198441505432,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44072729691936,"north":true,"longitude":-79.9420166015625,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4406436007504,"north":true,"longitude":-79.94205951690674,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440561945850995,"north":true,"longitude":-79.94209706783295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463085} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44050070461135,"north":true,"longitude":-79.94212120771408,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44043946331594,"north":true,"longitude":-79.94215339422226,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440374139205986,"north":true,"longitude":-79.94220435619354,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440312897795266,"north":true,"longitude":-79.94224458932877,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463086} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44026390462652,"north":true,"longitude":-79.94227945804596,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440227159726525,"north":true,"longitude":-79.94231969118118,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44021082865341,"north":true,"longitude":-79.94237333536148,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440198580345964,"north":true,"longitude":-79.94244039058685,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44018020788062,"north":true,"longitude":-79.94255304336548,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463087} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44016387679611,"north":true,"longitude":-79.9426656961441,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44015571125236,"north":true,"longitude":-79.9427729845047,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44014142154843,"north":true,"longitude":-79.94286686182022,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44010875935659,"north":true,"longitude":-79.94298756122589,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440061807428016,"north":true,"longitude":-79.94308948516846,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463088} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44000873129533,"north":true,"longitude":-79.9431699514389,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439937282589064,"north":true,"longitude":-79.94327992200851,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43987195799114,"north":true,"longitude":-79.94336575269699,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43978621936003,"north":true,"longitude":-79.94344890117645,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463089} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439702522019445,"north":true,"longitude":-79.94351863861084,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439629031585696,"north":true,"longitude":-79.9435830116272,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4395432926448,"north":true,"longitude":-79.94367152452469,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439463677816036,"north":true,"longitude":-79.94375199079514,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439394269939655,"north":true,"longitude":-79.94381904602051,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463090} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43932486199164,"north":true,"longitude":-79.94389146566391,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4392472883179,"north":true,"longitude":-79.9439612030983,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43918604588067,"north":true,"longitude":-79.94402289390564,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43911255488232,"north":true,"longitude":-79.94409531354904,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4390635608388,"north":true,"longitude":-79.94415432214737,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439012525338825,"north":true,"longitude":-79.94422674179077,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463091} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43896148980012,"north":true,"longitude":-79.9443045258522,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43892678561167,"north":true,"longitude":-79.94437962770462,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43889616425403,"north":true,"longitude":-79.9444654583931,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438867584307665,"north":true,"longitude":-79.94453519582748,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438849211478576,"north":true,"longitude":-79.94461566209793,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4388185900856,"north":true,"longitude":-79.94473099708557,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43879001010625,"north":true,"longitude":-79.94485437870026,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463092} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43876143011477,"north":true,"longitude":-79.94500458240509,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438736932969526,"north":true,"longitude":-79.94514405727386,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43871039438544,"north":true,"longitude":-79.94529157876968,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43868793865227,"north":true,"longitude":-79.94544982910156,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43866956577409,"north":true,"longitude":-79.94559735059738,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43867364863634,"north":true,"longitude":-79.94572341442108,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463093} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438685897221596,"north":true,"longitude":-79.94586557149887,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43871039438544,"north":true,"longitude":-79.94601845741272,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43873489154035,"north":true,"longitude":-79.94612842798233,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438769595827864,"north":true,"longitude":-79.94625717401505,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43883288007072,"north":true,"longitude":-79.94634568691254,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.438918619917665,"north":true,"longitude":-79.946428835392,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463094} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439020691021426,"north":true,"longitude":-79.9465361237526,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43916359030634,"north":true,"longitude":-79.94663268327713,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43928199234091,"north":true,"longitude":-79.94669169187546,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43943509811091,"north":true,"longitude":-79.94676142930984,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439573913707655,"north":true,"longitude":-79.94682043790817,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439663735411635,"north":true,"longitude":-79.94687139987946,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43973518440866,"north":true,"longitude":-79.94691699743271,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463095} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.439818881708604,"north":true,"longitude":-79.94697600603104,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43992095144552,"north":true,"longitude":-79.94706451892853,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.43999648295108,"north":true,"longitude":-79.94715839624405,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44007201437179,"north":true,"longitude":-79.94724959135056,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44012713184144,"north":true,"longitude":-79.94733273983002,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44020062173069,"north":true,"longitude":-79.94741320610046,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44026798739194,"north":true,"longitude":-79.94751244783401,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463096} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440327187462756,"north":true,"longitude":-79.94759827852249,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44037618058537,"north":true,"longitude":-79.94765996932983,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44041088402565,"north":true,"longitude":-79.94772166013718,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44044967020238,"north":true,"longitude":-79.94777262210846,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440488456356746,"north":true,"longitude":-79.94782358407974,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44052520111392,"north":true,"longitude":-79.94786113500595,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463097} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44056602859833,"north":true,"longitude":-79.94789868593216,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44061706291898,"north":true,"longitude":-79.94792819023132,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4406619730891,"north":true,"longitude":-79.94794696569443,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440708924598525,"north":true,"longitude":-79.94796574115753,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44075791744295,"north":true,"longitude":-79.94798719882965,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44080486888535,"north":true,"longitude":-79.94800597429276,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463098} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440857944389435,"north":true,"longitude":-79.9480140209198,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440902854398644,"north":true,"longitude":-79.94800329208374,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4409457230158,"north":true,"longitude":-79.94798183441162,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44098042616205,"north":true,"longitude":-79.94795501232147,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44101104657035,"north":true,"longitude":-79.94791477918625,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44104370832381,"north":true,"longitude":-79.94787722826004,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44110290771151,"north":true,"longitude":-79.94780480861664,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463099} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44117639653405,"north":true,"longitude":-79.94771093130112,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44124171986446,"north":true,"longitude":-79.9476170539856,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44131112583342,"north":true,"longitude":-79.94753658771515,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44136624228716,"north":true,"longitude":-79.94746148586273,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44140094521634,"north":true,"longitude":-79.9474024772644,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44143156543307,"north":true,"longitude":-79.94734078645706,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441449937556435,"north":true,"longitude":-79.94729518890381,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44146830967477,"north":true,"longitude":-79.94722545146942,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44147443371306,"north":true,"longitude":-79.94716376066208,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441478516404985,"north":true,"longitude":-79.94710475206375,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44147239236704,"north":true,"longitude":-79.94702160358429,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441451978903146,"north":true,"longitude":-79.9468794465065,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441421358695706,"north":true,"longitude":-79.94673192501068,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441392779822834,"north":true,"longitude":-79.94658708572388,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44136011823899,"north":true,"longitude":-79.94641810655594,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44132745663924,"north":true,"longitude":-79.94626522064209,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44129275367215,"north":true,"longitude":-79.94611233472824,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44125805068712,"north":true,"longitude":-79.94595408439636,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44123559580491,"north":true,"longitude":-79.94583070278168,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441202934144684,"north":true,"longitude":-79.94568049907684,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44117231382381,"north":true,"longitude":-79.94553834199905,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44113965213282,"north":true,"longitude":-79.94537472724915,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.441109031783114,"north":true,"longitude":-79.94521915912628,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44105799783596,"north":true,"longitude":-79.94498312473297,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44102125337,"north":true,"longitude":-79.94480341672897,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44098859160563,"north":true,"longitude":-79.94462370872498,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.4409457230158,"north":true,"longitude":-79.94444668292999,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440908978488494,"north":true,"longitude":-79.94428038597107,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440872233941086,"north":true,"longitude":-79.94408994913101,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440839572104295,"north":true,"longitude":-79.94391292333603,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440798744786015,"north":true,"longitude":-79.94370639324188,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44076200017835,"north":true,"longitude":-79.94353741407394,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44071913144405,"north":true,"longitude":-79.94336307048798,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463104} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440684428162896,"north":true,"longitude":-79.94319140911102,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44063951800779,"north":true,"longitude":-79.94299829006195,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44059869056801,"north":true,"longitude":-79.942826628685,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.440572152718836,"north":true,"longitude":-79.94267910718918,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 18, 2016 6:01:03 PM","latitude":40.44054153211064,"north":true,"longitude":-79.94251817464828,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458338463105} diff --git a/offline/controller/waypoints_course_v2.txt b/offline/controller/waypoints_course_v2.txt deleted file mode 100644 index 737889f0..00000000 --- a/offline/controller/waypoints_course_v2.txt +++ /dev/null @@ -1,733 +0,0 @@ -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:21 AM","latitude":40.44164466666667,"north":true,"longitude":-79.9415915,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49868,"rawGPSLong":7956.49549,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433081768,"topicName":"sensors/gps","sequenceNumber":40} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:22 AM","latitude":40.4416445,"north":true,"longitude":-79.94159133333334,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49867,"rawGPSLong":7956.49548,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433082284,"topicName":"sensors/gps","sequenceNumber":41} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:22 AM","latitude":40.4416445,"north":true,"longitude":-79.94159166666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49867,"rawGPSLong":7956.4955,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433082773,"topicName":"sensors/gps","sequenceNumber":42} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:23 AM","latitude":40.44164416666667,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49865,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433083267,"topicName":"sensors/gps","sequenceNumber":43} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:23 AM","latitude":40.441643666666664,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.49862,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433083786,"topicName":"sensors/gps","sequenceNumber":44} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:24 AM","latitude":40.44164333333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.6,"rawGPSLat":4026.4986,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433084267,"topicName":"sensors/gps","sequenceNumber":45} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:24 AM","latitude":40.441643,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.3,"rawGPSLat":4026.49858,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433084782,"topicName":"sensors/gps","sequenceNumber":46} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:25 AM","latitude":40.441642333333334,"north":true,"longitude":-79.94159283333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.1,"rawGPSLat":4026.49854,"rawGPSLong":7956.49557,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433085280,"topicName":"sensors/gps","sequenceNumber":47} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:25 AM","latitude":40.441641833333335,"north":true,"longitude":-79.94159316666666,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.0,"rawGPSLat":4026.49851,"rawGPSLong":7956.49559,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433085772,"topicName":"sensors/gps","sequenceNumber":48} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:26 AM","latitude":40.4416415,"north":true,"longitude":-79.941593,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49849,"rawGPSLong":7956.49558,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433086270,"topicName":"sensors/gps","sequenceNumber":49} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:26 AM","latitude":40.44164116666666,"north":true,"longitude":-79.94159266666666,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49847,"rawGPSLong":7956.49556,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433086794,"topicName":"sensors/gps","sequenceNumber":50} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:27 AM","latitude":40.441640666666665,"north":true,"longitude":-79.9415925,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49844,"rawGPSLong":7956.49555,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433087293,"topicName":"sensors/gps","sequenceNumber":51} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:27 AM","latitude":40.4416405,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.2,"rawGPSLat":4026.49843,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433087782,"topicName":"sensors/gps","sequenceNumber":52} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:28 AM","latitude":40.4416405,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.1,"rawGPSLat":4026.49843,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433088270,"topicName":"sensors/gps","sequenceNumber":53} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:28 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":291.0,"rawGPSLat":4026.49842,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433088779,"topicName":"sensors/gps","sequenceNumber":54} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:29 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.8,"rawGPSLat":4026.49842,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433089283,"topicName":"sensors/gps","sequenceNumber":55} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:29 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.7,"rawGPSLat":4026.49842,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433089780,"topicName":"sensors/gps","sequenceNumber":56} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:30 AM","latitude":40.44164033333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.7,"rawGPSLat":4026.49842,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433090275,"topicName":"sensors/gps","sequenceNumber":57} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:30 AM","latitude":40.44164033333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.6,"rawGPSLat":4026.49842,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433090779,"topicName":"sensors/gps","sequenceNumber":58} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:31 AM","latitude":40.44164033333333,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.5,"rawGPSLat":4026.49842,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433091283,"topicName":"sensors/gps","sequenceNumber":59} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:31 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941592,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.64,"antennaAltitude":290.4,"rawGPSLat":4026.49841,"rawGPSLong":7956.49552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433091790,"topicName":"sensors/gps","sequenceNumber":60} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:32 AM","latitude":40.441640166666666,"north":true,"longitude":-79.94159216666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.2,"rawGPSLat":4026.49841,"rawGPSLong":7956.49553,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433092290,"topicName":"sensors/gps","sequenceNumber":61} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:32 AM","latitude":40.441640166666666,"north":true,"longitude":-79.94159233333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.0,"rawGPSLat":4026.49841,"rawGPSLong":7956.49554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433092787,"topicName":"sensors/gps","sequenceNumber":62} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:33 AM","latitude":40.441640166666666,"north":true,"longitude":-79.94159283333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.7,"rawGPSLat":4026.49841,"rawGPSLong":7956.49557,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433093285,"topicName":"sensors/gps","sequenceNumber":63} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:33 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159316666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.5,"rawGPSLat":4026.49842,"rawGPSLong":7956.49559,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433093787,"topicName":"sensors/gps","sequenceNumber":64} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:34 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159383333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.2,"rawGPSLat":4026.49842,"rawGPSLong":7956.49563,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433094289,"topicName":"sensors/gps","sequenceNumber":65} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:34 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159383333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.0,"rawGPSLat":4026.49844,"rawGPSLong":7956.49563,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433094780,"topicName":"sensors/gps","sequenceNumber":66} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:35 AM","latitude":40.4416405,"north":true,"longitude":-79.94159433333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.8,"rawGPSLat":4026.49843,"rawGPSLong":7956.49566,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433095282,"topicName":"sensors/gps","sequenceNumber":67} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:35 AM","latitude":40.4416405,"north":true,"longitude":-79.94159466666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.7,"rawGPSLat":4026.49843,"rawGPSLong":7956.49568,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433095780,"topicName":"sensors/gps","sequenceNumber":68} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:36 AM","latitude":40.441640166666666,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.5,"rawGPSLat":4026.49841,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433096281,"topicName":"sensors/gps","sequenceNumber":69} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:36 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.3,"rawGPSLat":4026.49841,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433096790,"topicName":"sensors/gps","sequenceNumber":70} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:37 AM","latitude":40.441640166666666,"north":true,"longitude":-79.9415965,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.2,"rawGPSLat":4026.49841,"rawGPSLong":7956.49579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433097291,"topicName":"sensors/gps","sequenceNumber":71} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:37 AM","latitude":40.44164,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.1,"rawGPSLat":4026.4984,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433097795,"topicName":"sensors/gps","sequenceNumber":72} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:38 AM","latitude":40.44164,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.0,"rawGPSLat":4026.4984,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433098306,"topicName":"sensors/gps","sequenceNumber":73} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:38 AM","latitude":40.44163983333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.9,"rawGPSLat":4026.49839,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433098793,"topicName":"sensors/gps","sequenceNumber":74} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:39 AM","latitude":40.44164,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.9,"rawGPSLat":4026.4984,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433099280,"topicName":"sensors/gps","sequenceNumber":75} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:39 AM","latitude":40.441640166666666,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49841,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433099773,"topicName":"sensors/gps","sequenceNumber":76} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:40 AM","latitude":40.4416405,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49843,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433100275,"topicName":"sensors/gps","sequenceNumber":77} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:40 AM","latitude":40.441640666666665,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49844,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433100771,"topicName":"sensors/gps","sequenceNumber":78} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:41 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.7,"rawGPSLat":4026.49845,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433101290,"topicName":"sensors/gps","sequenceNumber":79} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:41 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.7,"rawGPSLat":4026.49845,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433101789,"topicName":"sensors/gps","sequenceNumber":80} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:42 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433102281,"topicName":"sensors/gps","sequenceNumber":81} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:42 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433102776,"topicName":"sensors/gps","sequenceNumber":82} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:43 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433103289,"topicName":"sensors/gps","sequenceNumber":83} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:43 AM","latitude":40.44164083333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49845,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433103810,"topicName":"sensors/gps","sequenceNumber":84} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:44 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.49845,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433104289,"topicName":"sensors/gps","sequenceNumber":85} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:44 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.49844,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433104790,"topicName":"sensors/gps","sequenceNumber":86} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:45 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49844,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433105283,"topicName":"sensors/gps","sequenceNumber":87} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:45 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49844,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433105789,"topicName":"sensors/gps","sequenceNumber":88} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:46 AM","latitude":40.44164083333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49845,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433106294,"topicName":"sensors/gps","sequenceNumber":89} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:46 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49844,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433106795,"topicName":"sensors/gps","sequenceNumber":90} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:47 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49844,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433107286,"topicName":"sensors/gps","sequenceNumber":91} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:47 AM","latitude":40.441640666666665,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49844,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433107780,"topicName":"sensors/gps","sequenceNumber":92} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:48 AM","latitude":40.4416405,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49843,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433108293,"topicName":"sensors/gps","sequenceNumber":93} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:48 AM","latitude":40.4416405,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.3,"rawGPSLat":4026.49843,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433108786,"topicName":"sensors/gps","sequenceNumber":94} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:49 AM","latitude":40.4416405,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49843,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433109284,"topicName":"sensors/gps","sequenceNumber":95} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:49 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.4,"rawGPSLat":4026.49841,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433109795,"topicName":"sensors/gps","sequenceNumber":96} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:50 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.49841,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433110282,"topicName":"sensors/gps","sequenceNumber":97} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:50 AM","latitude":40.44164,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.5,"rawGPSLat":4026.4984,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433110782,"topicName":"sensors/gps","sequenceNumber":98} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:51 AM","latitude":40.44163983333333,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.6,"rawGPSLat":4026.49839,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433111285,"topicName":"sensors/gps","sequenceNumber":99} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:51 AM","latitude":40.44163966666667,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.7,"rawGPSLat":4026.49838,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433111791,"topicName":"sensors/gps","sequenceNumber":100} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:52 AM","latitude":40.44163966666667,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.8,"rawGPSLat":4026.49838,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433112288,"topicName":"sensors/gps","sequenceNumber":101} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:52 AM","latitude":40.44163983333333,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":287.9,"rawGPSLat":4026.49839,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433112797,"topicName":"sensors/gps","sequenceNumber":102} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:53 AM","latitude":40.4416395,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.0,"rawGPSLat":4026.49837,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433113289,"topicName":"sensors/gps","sequenceNumber":103} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:53 AM","latitude":40.441639333333335,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.1,"rawGPSLat":4026.49836,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433113796,"topicName":"sensors/gps","sequenceNumber":104} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:54 AM","latitude":40.44163916666667,"north":true,"longitude":-79.94159766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.3,"rawGPSLat":4026.49835,"rawGPSLong":7956.49586,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433114290,"topicName":"sensors/gps","sequenceNumber":105} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:54 AM","latitude":40.44163916666667,"north":true,"longitude":-79.94159766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.3,"rawGPSLat":4026.49835,"rawGPSLong":7956.49586,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433114791,"topicName":"sensors/gps","sequenceNumber":106} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:55 AM","latitude":40.44163916666667,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.4,"rawGPSLat":4026.49835,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433115284,"topicName":"sensors/gps","sequenceNumber":107} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:55 AM","latitude":40.44163916666667,"north":true,"longitude":-79.9415975,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.5,"rawGPSLat":4026.49835,"rawGPSLong":7956.49585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433115798,"topicName":"sensors/gps","sequenceNumber":108} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:56 AM","latitude":40.4416395,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.7,"rawGPSLat":4026.49837,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433116288,"topicName":"sensors/gps","sequenceNumber":109} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:56 AM","latitude":40.44163983333333,"north":true,"longitude":-79.94159733333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.8,"rawGPSLat":4026.49839,"rawGPSLong":7956.49584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433116788,"topicName":"sensors/gps","sequenceNumber":110} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:57 AM","latitude":40.44164,"north":true,"longitude":-79.94159716666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":288.9,"rawGPSLat":4026.4984,"rawGPSLong":7956.49583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433117288,"topicName":"sensors/gps","sequenceNumber":111} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:57 AM","latitude":40.441640166666666,"north":true,"longitude":-79.941597,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.1,"rawGPSLat":4026.49841,"rawGPSLong":7956.49582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433117789,"topicName":"sensors/gps","sequenceNumber":112} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:58 AM","latitude":40.44164033333333,"north":true,"longitude":-79.94159666666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.2,"rawGPSLat":4026.49842,"rawGPSLong":7956.4958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433118281,"topicName":"sensors/gps","sequenceNumber":113} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:58 AM","latitude":40.4416405,"north":true,"longitude":-79.9415965,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.3,"rawGPSLat":4026.49843,"rawGPSLong":7956.49579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433118776,"topicName":"sensors/gps","sequenceNumber":114} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:59 AM","latitude":40.441640666666665,"north":true,"longitude":-79.94159616666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.4,"rawGPSLat":4026.49844,"rawGPSLong":7956.49577,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433119296,"topicName":"sensors/gps","sequenceNumber":115} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:51:59 AM","latitude":40.441641,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.5,"rawGPSLat":4026.49846,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433119789,"topicName":"sensors/gps","sequenceNumber":116} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:00 AM","latitude":40.44164116666666,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.6,"rawGPSLat":4026.49847,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433120296,"topicName":"sensors/gps","sequenceNumber":117} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:00 AM","latitude":40.44164133333334,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.6,"rawGPSLat":4026.49848,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433120788,"topicName":"sensors/gps","sequenceNumber":118} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:01 AM","latitude":40.44164166666667,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.7,"rawGPSLat":4026.4985,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433121290,"topicName":"sensors/gps","sequenceNumber":119} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:01 AM","latitude":40.441641833333335,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.7,"rawGPSLat":4026.49851,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433121820,"topicName":"sensors/gps","sequenceNumber":120} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:02 AM","latitude":40.44164216666667,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.8,"rawGPSLat":4026.49853,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433122287,"topicName":"sensors/gps","sequenceNumber":121} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:02 AM","latitude":40.441642333333334,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.9,"rawGPSLat":4026.49854,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433122787,"topicName":"sensors/gps","sequenceNumber":122} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:03 AM","latitude":40.44164266666667,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":289.9,"rawGPSLat":4026.49856,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433123282,"topicName":"sensors/gps","sequenceNumber":123} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:03 AM","latitude":40.441643166666665,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.0,"rawGPSLat":4026.49859,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433123788,"topicName":"sensors/gps","sequenceNumber":124} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:04 AM","latitude":40.4416435,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.0,"rawGPSLat":4026.49861,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433124290,"topicName":"sensors/gps","sequenceNumber":125} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:04 AM","latitude":40.44164383333333,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.1,"rawGPSLat":4026.49863,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433124782,"topicName":"sensors/gps","sequenceNumber":126} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:05 AM","latitude":40.441644,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.1,"rawGPSLat":4026.49864,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433125276,"topicName":"sensors/gps","sequenceNumber":127} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:05 AM","latitude":40.441644333333336,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.1,"rawGPSLat":4026.49866,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433125789,"topicName":"sensors/gps","sequenceNumber":128} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:06 AM","latitude":40.44164466666667,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.2,"rawGPSLat":4026.49868,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433126289,"topicName":"sensors/gps","sequenceNumber":129} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:06 AM","latitude":40.441645,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.2,"rawGPSLat":4026.4987,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433126790,"topicName":"sensors/gps","sequenceNumber":130} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:07 AM","latitude":40.441645333333334,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.3,"rawGPSLat":4026.49872,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433127291,"topicName":"sensors/gps","sequenceNumber":131} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:07 AM","latitude":40.44164583333333,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.3,"rawGPSLat":4026.49875,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433127788,"topicName":"sensors/gps","sequenceNumber":132} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:08 AM","latitude":40.441646166666665,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.4,"rawGPSLat":4026.49877,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433128292,"topicName":"sensors/gps","sequenceNumber":133} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:08 AM","latitude":40.441646666666664,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.4,"rawGPSLat":4026.4988,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433128789,"topicName":"sensors/gps","sequenceNumber":134} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:09 AM","latitude":40.441647,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.5,"rawGPSLat":4026.49882,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433129274,"topicName":"sensors/gps","sequenceNumber":135} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:09 AM","latitude":40.441647333333336,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.5,"rawGPSLat":4026.49884,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433129774,"topicName":"sensors/gps","sequenceNumber":136} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:10 AM","latitude":40.441647833333334,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.6,"rawGPSLat":4026.49887,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433130291,"topicName":"sensors/gps","sequenceNumber":137} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:10 AM","latitude":40.44164816666667,"north":true,"longitude":-79.94159516666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.7,"rawGPSLat":4026.49889,"rawGPSLong":7956.49571,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433130782,"topicName":"sensors/gps","sequenceNumber":138} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:11 AM","latitude":40.4416485,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.8,"rawGPSLat":4026.49891,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433131274,"topicName":"sensors/gps","sequenceNumber":139} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:11 AM","latitude":40.44164883333333,"north":true,"longitude":-79.94159533333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.8,"rawGPSLat":4026.49893,"rawGPSLong":7956.49572,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433131789,"topicName":"sensors/gps","sequenceNumber":140} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:12 AM","latitude":40.441649,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.9,"rawGPSLat":4026.49894,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433132287,"topicName":"sensors/gps","sequenceNumber":141} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:12 AM","latitude":40.44164933333333,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":290.9,"rawGPSLat":4026.49896,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433132789,"topicName":"sensors/gps","sequenceNumber":142} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:13 AM","latitude":40.4416495,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.0,"rawGPSLat":4026.49897,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433133282,"topicName":"sensors/gps","sequenceNumber":143} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:13 AM","latitude":40.44164983333334,"north":true,"longitude":-79.9415955,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.0,"rawGPSLat":4026.49899,"rawGPSLong":7956.49573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433133785,"topicName":"sensors/gps","sequenceNumber":144} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:14 AM","latitude":40.44165,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.1,"rawGPSLat":4026.499,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433134289,"topicName":"sensors/gps","sequenceNumber":145} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:14 AM","latitude":40.44165016666667,"north":true,"longitude":-79.94159566666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.1,"rawGPSLat":4026.49901,"rawGPSLong":7956.49574,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433134788,"topicName":"sensors/gps","sequenceNumber":146} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:15 AM","latitude":40.441650333333335,"north":true,"longitude":-79.94159583333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.2,"rawGPSLat":4026.49902,"rawGPSLong":7956.49575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433135286,"topicName":"sensors/gps","sequenceNumber":147} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:15 AM","latitude":40.4416505,"north":true,"longitude":-79.941596,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.2,"rawGPSLat":4026.49903,"rawGPSLong":7956.49576,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433135784,"topicName":"sensors/gps","sequenceNumber":148} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:16 AM","latitude":40.44165066666667,"north":true,"longitude":-79.94159616666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.3,"rawGPSLat":4026.49904,"rawGPSLong":7956.49577,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433136281,"topicName":"sensors/gps","sequenceNumber":149} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:16 AM","latitude":40.44165066666667,"north":true,"longitude":-79.94159633333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.3,"rawGPSLat":4026.49904,"rawGPSLong":7956.49578,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433136785,"topicName":"sensors/gps","sequenceNumber":150} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:17 AM","latitude":40.441650833333334,"north":true,"longitude":-79.9415965,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.3,"rawGPSLat":4026.49905,"rawGPSLong":7956.49579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433137275,"topicName":"sensors/gps","sequenceNumber":151} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:17 AM","latitude":40.441651,"north":true,"longitude":-79.94159666666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.4,"rawGPSLat":4026.49906,"rawGPSLong":7956.4958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433137778,"topicName":"sensors/gps","sequenceNumber":152} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:18 AM","latitude":40.44165066666667,"north":true,"longitude":-79.94159683333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.5,"rawGPSLat":4026.49904,"rawGPSLong":7956.49581,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433138280,"topicName":"sensors/gps","sequenceNumber":153} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:18 AM","latitude":40.441646666666664,"north":true,"longitude":-79.9415985,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.5,"rawGPSLat":4026.4988,"rawGPSLong":7956.49591,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433138780,"topicName":"sensors/gps","sequenceNumber":154} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:19 AM","latitude":40.44163916666667,"north":true,"longitude":-79.94160133333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49835,"rawGPSLong":7956.49608,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433139279,"topicName":"sensors/gps","sequenceNumber":155} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:19 AM","latitude":40.44162866666667,"north":true,"longitude":-79.941605,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49772,"rawGPSLong":7956.4963,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433139797,"topicName":"sensors/gps","sequenceNumber":156} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:20 AM","latitude":40.4416165,"north":true,"longitude":-79.94161,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49699,"rawGPSLong":7956.4966,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433140289,"topicName":"sensors/gps","sequenceNumber":157} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:20 AM","latitude":40.44160316666667,"north":true,"longitude":-79.94161616666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49619,"rawGPSLong":7956.49697,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433140790,"topicName":"sensors/gps","sequenceNumber":158} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:21 AM","latitude":40.441588333333335,"north":true,"longitude":-79.941623,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.4953,"rawGPSLong":7956.49738,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433141287,"topicName":"sensors/gps","sequenceNumber":159} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:21 AM","latitude":40.44157283333333,"north":true,"longitude":-79.941629,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.6,"rawGPSLat":4026.49437,"rawGPSLong":7956.49774,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433141792,"topicName":"sensors/gps","sequenceNumber":160} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:22 AM","latitude":40.44155583333333,"north":true,"longitude":-79.94163616666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.7,"rawGPSLat":4026.49335,"rawGPSLong":7956.49817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433142284,"topicName":"sensors/gps","sequenceNumber":161} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:22 AM","latitude":40.4415395,"north":true,"longitude":-79.94164433333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.49237,"rawGPSLong":7956.49866,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433142778,"topicName":"sensors/gps","sequenceNumber":162} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:23 AM","latitude":40.44152283333333,"north":true,"longitude":-79.94165233333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.49137,"rawGPSLong":7956.49914,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433143289,"topicName":"sensors/gps","sequenceNumber":163} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:23 AM","latitude":40.4415035,"north":true,"longitude":-79.94165816666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.49021,"rawGPSLong":7956.49949,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433143785,"topicName":"sensors/gps","sequenceNumber":164} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:24 AM","latitude":40.44148583333333,"north":true,"longitude":-79.94166333333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":291.8,"rawGPSLat":4026.48915,"rawGPSLong":7956.4998,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433144273,"topicName":"sensors/gps","sequenceNumber":165} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:24 AM","latitude":40.441469166666664,"north":true,"longitude":-79.9416695,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.0,"rawGPSLat":4026.48815,"rawGPSLong":7956.50017,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433144779,"topicName":"sensors/gps","sequenceNumber":166} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:25 AM","latitude":40.441450833333334,"north":true,"longitude":-79.9416785,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.1,"rawGPSLat":4026.48705,"rawGPSLong":7956.50071,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433145290,"topicName":"sensors/gps","sequenceNumber":167} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:25 AM","latitude":40.44143366666667,"north":true,"longitude":-79.941686,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.2,"rawGPSLat":4026.48602,"rawGPSLong":7956.50116,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433145798,"topicName":"sensors/gps","sequenceNumber":168} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:26 AM","latitude":40.441416833333335,"north":true,"longitude":-79.94169283333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.3,"rawGPSLat":4026.48501,"rawGPSLong":7956.50157,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433146293,"topicName":"sensors/gps","sequenceNumber":169} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:26 AM","latitude":40.44139833333333,"north":true,"longitude":-79.9417005,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.4,"rawGPSLat":4026.4839,"rawGPSLong":7956.50203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433146790,"topicName":"sensors/gps","sequenceNumber":170} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:27 AM","latitude":40.44138133333333,"north":true,"longitude":-79.94170766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.5,"rawGPSLat":4026.48288,"rawGPSLong":7956.50246,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433147290,"topicName":"sensors/gps","sequenceNumber":171} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:27 AM","latitude":40.441364,"north":true,"longitude":-79.941714,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.7,"rawGPSLat":4026.48184,"rawGPSLong":7956.50284,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433147794,"topicName":"sensors/gps","sequenceNumber":172} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:28 AM","latitude":40.44134533333333,"north":true,"longitude":-79.94172333333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.7,"rawGPSLat":4026.48072,"rawGPSLong":7956.5034,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433148289,"topicName":"sensors/gps","sequenceNumber":173} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:28 AM","latitude":40.441328,"north":true,"longitude":-79.94173083333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":292.9,"rawGPSLat":4026.47968,"rawGPSLong":7956.50385,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433148790,"topicName":"sensors/gps","sequenceNumber":174} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:29 AM","latitude":40.44131183333333,"north":true,"longitude":-79.94173766666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.0,"rawGPSLat":4026.47871,"rawGPSLong":7956.50426,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433149296,"topicName":"sensors/gps","sequenceNumber":175} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:29 AM","latitude":40.44129266666667,"north":true,"longitude":-79.941745,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.2,"rawGPSLat":4026.47756,"rawGPSLong":7956.5047,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433149788,"topicName":"sensors/gps","sequenceNumber":176} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:30 AM","latitude":40.44127483333333,"north":true,"longitude":-79.9417525,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.3,"rawGPSLat":4026.47649,"rawGPSLong":7956.50515,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433150286,"topicName":"sensors/gps","sequenceNumber":177} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:30 AM","latitude":40.441259,"north":true,"longitude":-79.94176083333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.4,"rawGPSLat":4026.47554,"rawGPSLong":7956.50565,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433150787,"topicName":"sensors/gps","sequenceNumber":178} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:31 AM","latitude":40.441241833333336,"north":true,"longitude":-79.94176916666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.6,"rawGPSLat":4026.47451,"rawGPSLong":7956.50615,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433151289,"topicName":"sensors/gps","sequenceNumber":179} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:31 AM","latitude":40.4412245,"north":true,"longitude":-79.941778,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":293.9,"rawGPSLat":4026.47347,"rawGPSLong":7956.50668,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433151795,"topicName":"sensors/gps","sequenceNumber":180} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:32 AM","latitude":40.441208333333336,"north":true,"longitude":-79.94178583333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.0,"rawGPSLat":4026.4725,"rawGPSLong":7956.50715,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433152287,"topicName":"sensors/gps","sequenceNumber":181} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:32 AM","latitude":40.441191833333335,"north":true,"longitude":-79.94179266666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.1,"rawGPSLat":4026.47151,"rawGPSLong":7956.50756,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433152790,"topicName":"sensors/gps","sequenceNumber":182} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:33 AM","latitude":40.44117333333333,"north":true,"longitude":-79.94179933333334,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.1,"rawGPSLat":4026.4704,"rawGPSLong":7956.50796,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433153289,"topicName":"sensors/gps","sequenceNumber":183} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:33 AM","latitude":40.441156,"north":true,"longitude":-79.941806,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.4,"rawGPSLat":4026.46936,"rawGPSLong":7956.50836,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433153794,"topicName":"sensors/gps","sequenceNumber":184} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:34 AM","latitude":40.441139666666665,"north":true,"longitude":-79.94181216666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":294.6,"rawGPSLat":4026.46838,"rawGPSLong":7956.50873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433154289,"topicName":"sensors/gps","sequenceNumber":185} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:34 AM","latitude":40.44112116666667,"north":true,"longitude":-79.94182116666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":294.8,"rawGPSLat":4026.46727,"rawGPSLong":7956.50927,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433154788,"topicName":"sensors/gps","sequenceNumber":186} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:35 AM","latitude":40.44110383333334,"north":true,"longitude":-79.94183066666666,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.0,"rawGPSLat":4026.46623,"rawGPSLong":7956.50984,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433155292,"topicName":"sensors/gps","sequenceNumber":187} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:35 AM","latitude":40.44108816666667,"north":true,"longitude":-79.94183916666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.1,"rawGPSLat":4026.46529,"rawGPSLong":7956.51035,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433155790,"topicName":"sensors/gps","sequenceNumber":188} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:36 AM","latitude":40.441070333333336,"north":true,"longitude":-79.941848,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.4,"rawGPSLat":4026.46422,"rawGPSLong":7956.51088,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433156291,"topicName":"sensors/gps","sequenceNumber":189} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:36 AM","latitude":40.44105183333333,"north":true,"longitude":-79.941856,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":295.6,"rawGPSLat":4026.46311,"rawGPSLong":7956.51136,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433156792,"topicName":"sensors/gps","sequenceNumber":190} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:37 AM","latitude":40.441035,"north":true,"longitude":-79.94186233333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":296.0,"rawGPSLat":4026.4621,"rawGPSLong":7956.51174,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433157294,"topicName":"sensors/gps","sequenceNumber":191} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:37 AM","latitude":40.441017333333335,"north":true,"longitude":-79.94186883333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":296.1,"rawGPSLat":4026.46104,"rawGPSLong":7956.51213,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433157795,"topicName":"sensors/gps","sequenceNumber":192} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:38 AM","latitude":40.440998666666665,"north":true,"longitude":-79.941876,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":296.5,"rawGPSLat":4026.45992,"rawGPSLong":7956.51256,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433158292,"topicName":"sensors/gps","sequenceNumber":193} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:38 AM","latitude":40.4409815,"north":true,"longitude":-79.94188266666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":296.8,"rawGPSLat":4026.45889,"rawGPSLong":7956.51296,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433158788,"topicName":"sensors/gps","sequenceNumber":194} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:39 AM","latitude":40.440966,"north":true,"longitude":-79.94189016666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":296.9,"rawGPSLat":4026.45796,"rawGPSLong":7956.51341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433159290,"topicName":"sensors/gps","sequenceNumber":195} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:39 AM","latitude":40.4409485,"north":true,"longitude":-79.941899,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.65,"antennaAltitude":297.1,"rawGPSLat":4026.45691,"rawGPSLong":7956.51394,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433159791,"topicName":"sensors/gps","sequenceNumber":196} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:40 AM","latitude":40.440932,"north":true,"longitude":-79.941907,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.2,"rawGPSLat":4026.45592,"rawGPSLong":7956.51442,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433160291,"topicName":"sensors/gps","sequenceNumber":197} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:40 AM","latitude":40.440917,"north":true,"longitude":-79.94191416666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.3,"rawGPSLat":4026.45502,"rawGPSLong":7956.51485,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433160791,"topicName":"sensors/gps","sequenceNumber":198} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:41 AM","latitude":40.440900666666664,"north":true,"longitude":-79.94192183333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.4,"rawGPSLat":4026.45404,"rawGPSLong":7956.51531,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433161290,"topicName":"sensors/gps","sequenceNumber":199} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:41 AM","latitude":40.44088333333333,"north":true,"longitude":-79.94193033333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.8,"rawGPSLat":4026.453,"rawGPSLong":7956.51582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433161790,"topicName":"sensors/gps","sequenceNumber":200} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:42 AM","latitude":40.44086733333334,"north":true,"longitude":-79.94193833333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.2,"rawGPSLat":4026.45204,"rawGPSLong":7956.5163,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433162293,"topicName":"sensors/gps","sequenceNumber":201} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:42 AM","latitude":40.44085183333333,"north":true,"longitude":-79.9419455,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.4,"rawGPSLat":4026.45111,"rawGPSLong":7956.51673,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433162793,"topicName":"sensors/gps","sequenceNumber":202} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:43 AM","latitude":40.440834,"north":true,"longitude":-79.941953,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.7,"rawGPSLat":4026.45004,"rawGPSLong":7956.51718,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433163291,"topicName":"sensors/gps","sequenceNumber":203} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:43 AM","latitude":40.44081766666667,"north":true,"longitude":-79.941959,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.8,"rawGPSLat":4026.44906,"rawGPSLong":7956.51754,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433163802,"topicName":"sensors/gps","sequenceNumber":204} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:44 AM","latitude":40.4408025,"north":true,"longitude":-79.9419645,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.1,"rawGPSLat":4026.44815,"rawGPSLong":7956.51787,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433164292,"topicName":"sensors/gps","sequenceNumber":205} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:44 AM","latitude":40.44078533333333,"north":true,"longitude":-79.94197116666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44712,"rawGPSLong":7956.51827,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433164797,"topicName":"sensors/gps","sequenceNumber":206} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:45 AM","latitude":40.440769,"north":true,"longitude":-79.94197883333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44614,"rawGPSLong":7956.51873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433165293,"topicName":"sensors/gps","sequenceNumber":207} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:45 AM","latitude":40.4407545,"north":true,"longitude":-79.94198566666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44527,"rawGPSLong":7956.51914,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433165796,"topicName":"sensors/gps","sequenceNumber":208} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:46 AM","latitude":40.44073983333333,"north":true,"longitude":-79.94199333333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.6,"rawGPSLat":4026.44439,"rawGPSLong":7956.5196,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433166293,"topicName":"sensors/gps","sequenceNumber":209} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:46 AM","latitude":40.440724833333334,"north":true,"longitude":-79.9420015,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.5,"rawGPSLat":4026.44349,"rawGPSLong":7956.52009,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433166795,"topicName":"sensors/gps","sequenceNumber":210} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:47 AM","latitude":40.44070933333333,"north":true,"longitude":-79.94200983333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44256,"rawGPSLong":7956.52059,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433167293,"topicName":"sensors/gps","sequenceNumber":211} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:47 AM","latitude":40.440695166666664,"north":true,"longitude":-79.942017,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.44171,"rawGPSLong":7956.52102,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433167792,"topicName":"sensors/gps","sequenceNumber":212} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:48 AM","latitude":40.4406805,"north":true,"longitude":-79.9420235,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.44083,"rawGPSLong":7956.52141,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433168297,"topicName":"sensors/gps","sequenceNumber":213} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:48 AM","latitude":40.440665,"north":true,"longitude":-79.94203083333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.2,"rawGPSLat":4026.4399,"rawGPSLong":7956.52185,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433168801,"topicName":"sensors/gps","sequenceNumber":214} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:49 AM","latitude":40.44064866666667,"north":true,"longitude":-79.9420385,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43892,"rawGPSLong":7956.52231,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433169289,"topicName":"sensors/gps","sequenceNumber":215} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:49 AM","latitude":40.440634,"north":true,"longitude":-79.94204483333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43804,"rawGPSLong":7956.52269,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433169806,"topicName":"sensors/gps","sequenceNumber":216} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:50 AM","latitude":40.44062066666667,"north":true,"longitude":-79.94205033333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.5,"rawGPSLat":4026.43724,"rawGPSLong":7956.52302,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433170293,"topicName":"sensors/gps","sequenceNumber":217} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:50 AM","latitude":40.44060666666667,"north":true,"longitude":-79.9420565,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.4364,"rawGPSLong":7956.52339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433170802,"topicName":"sensors/gps","sequenceNumber":218} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:51 AM","latitude":40.44059133333333,"north":true,"longitude":-79.94206466666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43548,"rawGPSLong":7956.52388,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433171298,"topicName":"sensors/gps","sequenceNumber":219} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:51 AM","latitude":40.44057733333333,"north":true,"longitude":-79.94207183333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43464,"rawGPSLong":7956.52431,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433171800,"topicName":"sensors/gps","sequenceNumber":220} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:52 AM","latitude":40.4405635,"north":true,"longitude":-79.9420795,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43381,"rawGPSLong":7956.52477,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433172292,"topicName":"sensors/gps","sequenceNumber":221} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:52 AM","latitude":40.440546166666664,"north":true,"longitude":-79.942088,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43277,"rawGPSLong":7956.52528,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433172798,"topicName":"sensors/gps","sequenceNumber":222} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:53 AM","latitude":40.440529166666664,"north":true,"longitude":-79.94209666666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43175,"rawGPSLong":7956.5258,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433173297,"topicName":"sensors/gps","sequenceNumber":223} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:53 AM","latitude":40.440513833333334,"north":true,"longitude":-79.94210516666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.43083,"rawGPSLong":7956.52631,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433173798,"topicName":"sensors/gps","sequenceNumber":224} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:54 AM","latitude":40.4405,"north":true,"longitude":-79.942114,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.4,"rawGPSLat":4026.43,"rawGPSLong":7956.52684,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433174298,"topicName":"sensors/gps","sequenceNumber":225} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:54 AM","latitude":40.44048516666667,"north":true,"longitude":-79.94212583333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.3,"rawGPSLat":4026.42911,"rawGPSLong":7956.52755,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433174785,"topicName":"sensors/gps","sequenceNumber":226} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:55 AM","latitude":40.440468833333334,"north":true,"longitude":-79.9421365,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.1,"rawGPSLat":4026.42813,"rawGPSLong":7956.52819,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433175294,"topicName":"sensors/gps","sequenceNumber":227} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:55 AM","latitude":40.440453166666664,"north":true,"longitude":-79.94214683333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.2,"rawGPSLat":4026.42719,"rawGPSLong":7956.52881,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433175789,"topicName":"sensors/gps","sequenceNumber":228} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:56 AM","latitude":40.440438666666665,"north":true,"longitude":-79.942156,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":299.1,"rawGPSLat":4026.42632,"rawGPSLong":7956.52936,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433176300,"topicName":"sensors/gps","sequenceNumber":229} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:56 AM","latitude":40.44042366666667,"north":true,"longitude":-79.94216566666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.9,"rawGPSLat":4026.42542,"rawGPSLong":7956.52994,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433176789,"topicName":"sensors/gps","sequenceNumber":230} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:57 AM","latitude":40.44040716666667,"north":true,"longitude":-79.94217833333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.5,"rawGPSLat":4026.42443,"rawGPSLong":7956.5307,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433177297,"topicName":"sensors/gps","sequenceNumber":231} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:57 AM","latitude":40.44039083333333,"north":true,"longitude":-79.9421905,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42345,"rawGPSLong":7956.53143,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433177783,"topicName":"sensors/gps","sequenceNumber":232} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:58 AM","latitude":40.4403755,"north":true,"longitude":-79.94220233333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42253,"rawGPSLong":7956.53214,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433178301,"topicName":"sensors/gps","sequenceNumber":233} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:58 AM","latitude":40.44036033333333,"north":true,"longitude":-79.94221516666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42162,"rawGPSLong":7956.53291,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433178802,"topicName":"sensors/gps","sequenceNumber":234} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:59 AM","latitude":40.440344333333336,"north":true,"longitude":-79.94223283333334,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.42066,"rawGPSLong":7956.53397,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433179297,"topicName":"sensors/gps","sequenceNumber":235} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:52:59 AM","latitude":40.44032766666667,"north":true,"longitude":-79.94224916666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.41966,"rawGPSLong":7956.53495,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433179795,"topicName":"sensors/gps","sequenceNumber":236} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:00 AM","latitude":40.440310833333335,"north":true,"longitude":-79.94226416666666,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.3,"rawGPSLat":4026.41865,"rawGPSLong":7956.53585,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433180295,"topicName":"sensors/gps","sequenceNumber":237} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:00 AM","latitude":40.440294333333334,"north":true,"longitude":-79.94227916666667,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.2,"rawGPSLat":4026.41766,"rawGPSLong":7956.53675,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433180795,"topicName":"sensors/gps","sequenceNumber":238} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:01 AM","latitude":40.44027666666667,"north":true,"longitude":-79.94229833333333,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":298.1,"rawGPSLat":4026.4166,"rawGPSLong":7956.5379,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433181309,"topicName":"sensors/gps","sequenceNumber":239} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:01 AM","latitude":40.44025966666667,"north":true,"longitude":-79.942322,"west":true,"qualityValue":2,"numSatellites":7,"horizontalDilutionOfPrecision":1.26,"antennaAltitude":297.9,"rawGPSLat":4026.41558,"rawGPSLong":7956.53932,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433181810,"topicName":"sensors/gps","sequenceNumber":240} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:02 AM","latitude":40.440244666666665,"north":true,"longitude":-79.94234933333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.9,"rawGPSLat":4026.41468,"rawGPSLong":7956.54096,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433182339,"topicName":"sensors/gps","sequenceNumber":241} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:02 AM","latitude":40.440232,"north":true,"longitude":-79.942379,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.8,"rawGPSLat":4026.41392,"rawGPSLong":7956.54274,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433182823,"topicName":"sensors/gps","sequenceNumber":242} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:03 AM","latitude":40.440220833333335,"north":true,"longitude":-79.94241033333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.7,"rawGPSLat":4026.41325,"rawGPSLong":7956.54462,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433183308,"topicName":"sensors/gps","sequenceNumber":243} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:03 AM","latitude":40.44021266666667,"north":true,"longitude":-79.94244366666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.6,"rawGPSLat":4026.41276,"rawGPSLong":7956.54662,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433183808,"topicName":"sensors/gps","sequenceNumber":244} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:04 AM","latitude":40.44020583333333,"north":true,"longitude":-79.94247883333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.3,"rawGPSLat":4026.41235,"rawGPSLong":7956.54873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433184312,"topicName":"sensors/gps","sequenceNumber":245} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:04 AM","latitude":40.4402,"north":true,"longitude":-79.94251483333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.2,"rawGPSLat":4026.412,"rawGPSLong":7956.55089,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433184824,"topicName":"sensors/gps","sequenceNumber":246} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:05 AM","latitude":40.44019216666667,"north":true,"longitude":-79.94255066666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":297.1,"rawGPSLat":4026.41153,"rawGPSLong":7956.55304,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433185324,"topicName":"sensors/gps","sequenceNumber":247} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:05 AM","latitude":40.4401825,"north":true,"longitude":-79.94258716666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.9,"rawGPSLat":4026.41095,"rawGPSLong":7956.55523,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433185826,"topicName":"sensors/gps","sequenceNumber":248} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:06 AM","latitude":40.440174166666665,"north":true,"longitude":-79.94262533333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.8,"rawGPSLat":4026.41045,"rawGPSLong":7956.55752,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433186311,"topicName":"sensors/gps","sequenceNumber":249} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:06 AM","latitude":40.44016766666667,"north":true,"longitude":-79.94266466666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.4,"rawGPSLat":4026.41006,"rawGPSLong":7956.55988,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433186815,"topicName":"sensors/gps","sequenceNumber":250} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:07 AM","latitude":40.44016166666667,"north":true,"longitude":-79.94270616666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.2,"rawGPSLat":4026.4097,"rawGPSLong":7956.56237,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433187316,"topicName":"sensors/gps","sequenceNumber":251} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:07 AM","latitude":40.44015366666667,"north":true,"longitude":-79.9427485,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.0,"rawGPSLat":4026.40922,"rawGPSLong":7956.56491,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433187839,"topicName":"sensors/gps","sequenceNumber":252} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:08 AM","latitude":40.44014383333333,"north":true,"longitude":-79.942791,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.40863,"rawGPSLong":7956.56746,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433188329,"topicName":"sensors/gps","sequenceNumber":253} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:08 AM","latitude":40.440135166666664,"north":true,"longitude":-79.94283483333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.5,"rawGPSLat":4026.40811,"rawGPSLong":7956.57009,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433188820,"topicName":"sensors/gps","sequenceNumber":254} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:09 AM","latitude":40.440125333333334,"north":true,"longitude":-79.94287833333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.1,"rawGPSLat":4026.40752,"rawGPSLong":7956.5727,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433189327,"topicName":"sensors/gps","sequenceNumber":255} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:09 AM","latitude":40.440113,"north":true,"longitude":-79.94292216666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.7,"rawGPSLat":4026.40678,"rawGPSLong":7956.57533,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433189820,"topicName":"sensors/gps","sequenceNumber":256} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:10 AM","latitude":40.440096,"north":true,"longitude":-79.94296516666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.5,"rawGPSLat":4026.40576,"rawGPSLong":7956.57791,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433190333,"topicName":"sensors/gps","sequenceNumber":257} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:10 AM","latitude":40.44007783333333,"north":true,"longitude":-79.94300816666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.3,"rawGPSLat":4026.40467,"rawGPSLong":7956.58049,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433190825,"topicName":"sensors/gps","sequenceNumber":258} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:11 AM","latitude":40.44006016666667,"north":true,"longitude":-79.943053,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.0,"rawGPSLat":4026.40361,"rawGPSLong":7956.58318,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433191329,"topicName":"sensors/gps","sequenceNumber":259} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:11 AM","latitude":40.44004,"north":true,"longitude":-79.943097,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.8,"rawGPSLat":4026.4024,"rawGPSLong":7956.58582,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433191825,"topicName":"sensors/gps","sequenceNumber":260} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:12 AM","latitude":40.440016,"north":true,"longitude":-79.9431385,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.5,"rawGPSLat":4026.40096,"rawGPSLong":7956.58831,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433192327,"topicName":"sensors/gps","sequenceNumber":261} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:12 AM","latitude":40.43999,"north":true,"longitude":-79.94317916666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.0,"rawGPSLat":4026.3994,"rawGPSLong":7956.59075,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433192827,"topicName":"sensors/gps","sequenceNumber":262} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:13 AM","latitude":40.439963,"north":true,"longitude":-79.94322033333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.7,"rawGPSLat":4026.39778,"rawGPSLong":7956.59322,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433193325,"topicName":"sensors/gps","sequenceNumber":263} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:13 AM","latitude":40.439936833333334,"north":true,"longitude":-79.94326316666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.5,"rawGPSLat":4026.39621,"rawGPSLong":7956.59579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433193838,"topicName":"sensors/gps","sequenceNumber":264} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:14 AM","latitude":40.43991116666667,"north":true,"longitude":-79.94330766666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.1,"rawGPSLat":4026.39467,"rawGPSLong":7956.59846,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433194329,"topicName":"sensors/gps","sequenceNumber":265} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:14 AM","latitude":40.4398795,"north":true,"longitude":-79.9433445,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.4,"rawGPSLat":4026.39277,"rawGPSLong":7956.60067,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433194828,"topicName":"sensors/gps","sequenceNumber":266} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:15 AM","latitude":40.4398465,"north":true,"longitude":-79.94337916666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.0,"rawGPSLat":4026.39079,"rawGPSLong":7956.60275,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433195326,"topicName":"sensors/gps","sequenceNumber":267} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:15 AM","latitude":40.4398105,"north":true,"longitude":-79.94340883333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.6,"rawGPSLat":4026.38863,"rawGPSLong":7956.60453,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433195816,"topicName":"sensors/gps","sequenceNumber":268} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:16 AM","latitude":40.439777166666666,"north":true,"longitude":-79.943441,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.38663,"rawGPSLong":7956.60646,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433196325,"topicName":"sensors/gps","sequenceNumber":269} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:16 AM","latitude":40.439743,"north":true,"longitude":-79.94347516666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.38458,"rawGPSLong":7956.60851,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433196835,"topicName":"sensors/gps","sequenceNumber":270} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:17 AM","latitude":40.43970816666667,"north":true,"longitude":-79.94350783333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.38249,"rawGPSLong":7956.61047,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433197323,"topicName":"sensors/gps","sequenceNumber":271} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:17 AM","latitude":40.439676666666664,"north":true,"longitude":-79.9435455,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.3,"rawGPSLat":4026.3806,"rawGPSLong":7956.61273,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433197812,"topicName":"sensors/gps","sequenceNumber":272} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:18 AM","latitude":40.439645166666665,"north":true,"longitude":-79.94358316666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.1,"rawGPSLat":4026.37871,"rawGPSLong":7956.61499,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433198322,"topicName":"sensors/gps","sequenceNumber":273} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:18 AM","latitude":40.43961066666667,"north":true,"longitude":-79.94361533333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.3,"rawGPSLat":4026.37664,"rawGPSLong":7956.61692,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433198823,"topicName":"sensors/gps","sequenceNumber":274} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:19 AM","latitude":40.43957416666667,"north":true,"longitude":-79.943644,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.37445,"rawGPSLong":7956.61864,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433199309,"topicName":"sensors/gps","sequenceNumber":275} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:19 AM","latitude":40.439538166666665,"north":true,"longitude":-79.94367216666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.37229,"rawGPSLong":7956.62033,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433199831,"topicName":"sensors/gps","sequenceNumber":276} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:20 AM","latitude":40.43950316666667,"north":true,"longitude":-79.94370033333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.37019,"rawGPSLong":7956.62202,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433200328,"topicName":"sensors/gps","sequenceNumber":277} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:20 AM","latitude":40.439469333333335,"north":true,"longitude":-79.94373166666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.36816,"rawGPSLong":7956.6239,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433200827,"topicName":"sensors/gps","sequenceNumber":278} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:21 AM","latitude":40.439438333333335,"north":true,"longitude":-79.9437665,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.3663,"rawGPSLong":7956.62599,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433201329,"topicName":"sensors/gps","sequenceNumber":279} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:21 AM","latitude":40.439407333333335,"north":true,"longitude":-79.9438,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.36444,"rawGPSLong":7956.628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433201830,"topicName":"sensors/gps","sequenceNumber":280} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:22 AM","latitude":40.439378,"north":true,"longitude":-79.94383533333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.36268,"rawGPSLong":7956.63012,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433202326,"topicName":"sensors/gps","sequenceNumber":281} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:22 AM","latitude":40.439348833333334,"north":true,"longitude":-79.94387033333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.36093,"rawGPSLong":7956.63222,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433202829,"topicName":"sensors/gps","sequenceNumber":282} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:23 AM","latitude":40.439318,"north":true,"longitude":-79.94390133333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.35908,"rawGPSLong":7956.63408,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433203327,"topicName":"sensors/gps","sequenceNumber":283} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:23 AM","latitude":40.439287,"north":true,"longitude":-79.94393183333334,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.35722,"rawGPSLong":7956.63591,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433203816,"topicName":"sensors/gps","sequenceNumber":284} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:24 AM","latitude":40.43925766666667,"north":true,"longitude":-79.94396416666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.35546,"rawGPSLong":7956.63785,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433204321,"topicName":"sensors/gps","sequenceNumber":285} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:24 AM","latitude":40.43922966666667,"north":true,"longitude":-79.94399766666666,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":1.08,"antennaAltitude":289.5,"rawGPSLat":4026.35378,"rawGPSLong":7956.63986,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433204832,"topicName":"sensors/gps","sequenceNumber":286} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:25 AM","latitude":40.43919966666667,"north":true,"longitude":-79.94402866666667,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.4,"rawGPSLat":4026.35198,"rawGPSLong":7956.64172,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433205328,"topicName":"sensors/gps","sequenceNumber":287} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:25 AM","latitude":40.439172,"north":true,"longitude":-79.944061,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.2,"rawGPSLat":4026.35032,"rawGPSLong":7956.64366,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433205814,"topicName":"sensors/gps","sequenceNumber":288} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:26 AM","latitude":40.439146,"north":true,"longitude":-79.9440955,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.34876,"rawGPSLong":7956.64573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433206329,"topicName":"sensors/gps","sequenceNumber":289} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:26 AM","latitude":40.439120833333334,"north":true,"longitude":-79.94413033333333,"west":true,"qualityValue":2,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.34725,"rawGPSLong":7956.64782,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433206837,"topicName":"sensors/gps","sequenceNumber":290} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:27 AM","latitude":40.43909433333334,"north":true,"longitude":-79.94416316666667,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.34566,"rawGPSLong":7956.64979,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433207332,"topicName":"sensors/gps","sequenceNumber":291} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:27 AM","latitude":40.439068166666665,"north":true,"longitude":-79.9441955,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.8,"rawGPSLat":4026.34409,"rawGPSLong":7956.65173,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433207817,"topicName":"sensors/gps","sequenceNumber":292} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:28 AM","latitude":40.4390435,"north":true,"longitude":-79.94423,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.7,"rawGPSLat":4026.34261,"rawGPSLong":7956.6538,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433208315,"topicName":"sensors/gps","sequenceNumber":293} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:28 AM","latitude":40.439020666666664,"north":true,"longitude":-79.944266,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.6,"rawGPSLat":4026.34124,"rawGPSLong":7956.65596,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433208833,"topicName":"sensors/gps","sequenceNumber":294} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:29 AM","latitude":40.43900083333333,"north":true,"longitude":-79.94430416666667,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.34005,"rawGPSLong":7956.65825,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433209333,"topicName":"sensors/gps","sequenceNumber":295} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:29 AM","latitude":40.4389825,"north":true,"longitude":-79.94434366666667,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.4,"rawGPSLat":4026.33895,"rawGPSLong":7956.66062,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433209826,"topicName":"sensors/gps","sequenceNumber":296} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:30 AM","latitude":40.4389655,"north":true,"longitude":-79.94438333333333,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.2,"rawGPSLat":4026.33793,"rawGPSLong":7956.663,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433210342,"topicName":"sensors/gps","sequenceNumber":297} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:30 AM","latitude":40.43895033333333,"north":true,"longitude":-79.94442416666666,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.33702,"rawGPSLong":7956.66545,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433210949,"topicName":"sensors/gps","sequenceNumber":298} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:31 AM","latitude":40.43893633333333,"north":true,"longitude":-79.94446566666667,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.33618,"rawGPSLong":7956.66794,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433211330,"topicName":"sensors/gps","sequenceNumber":299} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:32 AM","latitude":40.438923,"north":true,"longitude":-79.94450633333334,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.0,"rawGPSLat":4026.33538,"rawGPSLong":7956.67038,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433212132,"topicName":"sensors/gps","sequenceNumber":300} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:32 AM","latitude":40.438908166666664,"north":true,"longitude":-79.94454633333334,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.9,"rawGPSLat":4026.33449,"rawGPSLong":7956.67278,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433212339,"topicName":"sensors/gps","sequenceNumber":301} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:32 AM","latitude":40.438893,"north":true,"longitude":-79.9445855,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.33358,"rawGPSLong":7956.67513,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433212838,"topicName":"sensors/gps","sequenceNumber":302} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:33 AM","latitude":40.43887933333333,"north":true,"longitude":-79.94462466666667,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.33276,"rawGPSLong":7956.67748,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433213326,"topicName":"sensors/gps","sequenceNumber":303} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:33 AM","latitude":40.4388675,"north":true,"longitude":-79.9446655,"west":true,"qualityValue":2,"numSatellites":11,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.7,"rawGPSLat":4026.33205,"rawGPSLong":7956.67993,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433213822,"topicName":"sensors/gps","sequenceNumber":304} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:34 AM","latitude":40.43885683333333,"north":true,"longitude":-79.94470616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.5,"rawGPSLat":4026.33141,"rawGPSLong":7956.68237,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433214331,"topicName":"sensors/gps","sequenceNumber":305} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:34 AM","latitude":40.438846833333336,"north":true,"longitude":-79.94474333333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.1,"rawGPSLat":4026.33081,"rawGPSLong":7956.6846,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433214813,"topicName":"sensors/gps","sequenceNumber":306} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:35 AM","latitude":40.43883916666667,"north":true,"longitude":-79.94478483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.8,"rawGPSLat":4026.33035,"rawGPSLong":7956.68709,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433215326,"topicName":"sensors/gps","sequenceNumber":307} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:35 AM","latitude":40.43883133333333,"north":true,"longitude":-79.94482433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.5,"rawGPSLat":4026.32988,"rawGPSLong":7956.68946,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433215823,"topicName":"sensors/gps","sequenceNumber":308} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:36 AM","latitude":40.43882283333333,"north":true,"longitude":-79.944864,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.3,"rawGPSLat":4026.32937,"rawGPSLong":7956.69184,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433216320,"topicName":"sensors/gps","sequenceNumber":309} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:36 AM","latitude":40.438814,"north":true,"longitude":-79.94490366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.3,"rawGPSLat":4026.32884,"rawGPSLong":7956.69422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433216811,"topicName":"sensors/gps","sequenceNumber":310} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:37 AM","latitude":40.438806,"north":true,"longitude":-79.94494333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.1,"rawGPSLat":4026.32836,"rawGPSLong":7956.6966,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433217324,"topicName":"sensors/gps","sequenceNumber":311} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:37 AM","latitude":40.43879916666667,"north":true,"longitude":-79.944983,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.8,"rawGPSLat":4026.32795,"rawGPSLong":7956.69898,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433217833,"topicName":"sensors/gps","sequenceNumber":312} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:38 AM","latitude":40.438790833333336,"north":true,"longitude":-79.94502216666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.9,"rawGPSLat":4026.32745,"rawGPSLong":7956.70133,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433218329,"topicName":"sensors/gps","sequenceNumber":313} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:38 AM","latitude":40.4387815,"north":true,"longitude":-79.945061,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.9,"rawGPSLat":4026.32689,"rawGPSLong":7956.70366,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433218825,"topicName":"sensors/gps","sequenceNumber":314} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:39 AM","latitude":40.438772666666665,"north":true,"longitude":-79.94509916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.7,"rawGPSLat":4026.32636,"rawGPSLong":7956.70595,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433219328,"topicName":"sensors/gps","sequenceNumber":315} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:39 AM","latitude":40.438763333333334,"north":true,"longitude":-79.94513733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.7,"rawGPSLat":4026.3258,"rawGPSLong":7956.70824,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433219815,"topicName":"sensors/gps","sequenceNumber":316} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:40 AM","latitude":40.43875516666667,"north":true,"longitude":-79.94517583333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.8,"rawGPSLat":4026.32531,"rawGPSLong":7956.71055,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433220320,"topicName":"sensors/gps","sequenceNumber":317} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:40 AM","latitude":40.43874816666667,"north":true,"longitude":-79.94521483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.8,"rawGPSLat":4026.32489,"rawGPSLong":7956.71289,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433220803,"topicName":"sensors/gps","sequenceNumber":318} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:41 AM","latitude":40.4387435,"north":true,"longitude":-79.945253,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.4,"rawGPSLat":4026.32461,"rawGPSLong":7956.71518,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433221324,"topicName":"sensors/gps","sequenceNumber":319} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:41 AM","latitude":40.4387375,"north":true,"longitude":-79.94529033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":285.0,"rawGPSLat":4026.32425,"rawGPSLong":7956.71742,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433221809,"topicName":"sensors/gps","sequenceNumber":320} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:42 AM","latitude":40.438729333333335,"north":true,"longitude":-79.945328,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.9,"rawGPSLat":4026.32376,"rawGPSLong":7956.71968,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433222328,"topicName":"sensors/gps","sequenceNumber":321} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:42 AM","latitude":40.4387245,"north":true,"longitude":-79.9453655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.5,"rawGPSLat":4026.32347,"rawGPSLong":7956.72193,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433222817,"topicName":"sensors/gps","sequenceNumber":322} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:43 AM","latitude":40.43872,"north":true,"longitude":-79.94540366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.3,"rawGPSLat":4026.3232,"rawGPSLong":7956.72422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433223326,"topicName":"sensors/gps","sequenceNumber":323} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:43 AM","latitude":40.4387165,"north":true,"longitude":-79.94544083333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.9,"rawGPSLat":4026.32299,"rawGPSLong":7956.72645,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433223829,"topicName":"sensors/gps","sequenceNumber":324} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:44 AM","latitude":40.43871266666667,"north":true,"longitude":-79.9454785,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.9,"rawGPSLat":4026.32276,"rawGPSLong":7956.72871,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433224324,"topicName":"sensors/gps","sequenceNumber":325} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:44 AM","latitude":40.43870916666667,"north":true,"longitude":-79.945516,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.9,"rawGPSLat":4026.32255,"rawGPSLong":7956.73096,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433224812,"topicName":"sensors/gps","sequenceNumber":326} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:45 AM","latitude":40.438706333333336,"north":true,"longitude":-79.94555316666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.6,"rawGPSLat":4026.32238,"rawGPSLong":7956.73319,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433225323,"topicName":"sensors/gps","sequenceNumber":327} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:45 AM","latitude":40.438703333333336,"north":true,"longitude":-79.94559016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.6,"rawGPSLat":4026.3222,"rawGPSLong":7956.73541,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433225802,"topicName":"sensors/gps","sequenceNumber":328} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:46 AM","latitude":40.43870183333333,"north":true,"longitude":-79.945627,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":283.3,"rawGPSLat":4026.32211,"rawGPSLong":7956.73762,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433226328,"topicName":"sensors/gps","sequenceNumber":329} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:46 AM","latitude":40.43870066666667,"north":true,"longitude":-79.94566333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.8,"rawGPSLat":4026.32204,"rawGPSLong":7956.7398,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433226816,"topicName":"sensors/gps","sequenceNumber":330} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:47 AM","latitude":40.4386995,"north":true,"longitude":-79.94569933333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.6,"rawGPSLat":4026.32197,"rawGPSLong":7956.74196,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433227315,"topicName":"sensors/gps","sequenceNumber":331} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:47 AM","latitude":40.438701,"north":true,"longitude":-79.94573566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.3,"rawGPSLat":4026.32206,"rawGPSLong":7956.74414,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433227838,"topicName":"sensors/gps","sequenceNumber":332} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:48 AM","latitude":40.438704333333334,"north":true,"longitude":-79.9457715,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.32226,"rawGPSLong":7956.74629,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433228318,"topicName":"sensors/gps","sequenceNumber":333} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:48 AM","latitude":40.438709333333335,"north":true,"longitude":-79.94580666666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32256,"rawGPSLong":7956.7484,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433228816,"topicName":"sensors/gps","sequenceNumber":334} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:49 AM","latitude":40.438715333333334,"north":true,"longitude":-79.94584166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32292,"rawGPSLong":7956.7505,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433229316,"topicName":"sensors/gps","sequenceNumber":335} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:49 AM","latitude":40.43872316666667,"north":true,"longitude":-79.9458755,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32339,"rawGPSLong":7956.75253,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433229813,"topicName":"sensors/gps","sequenceNumber":336} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:50 AM","latitude":40.4387315,"north":true,"longitude":-79.94590883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32389,"rawGPSLong":7956.75453,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433230304,"topicName":"sensors/gps","sequenceNumber":337} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:50 AM","latitude":40.43874016666667,"north":true,"longitude":-79.945942,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.6,"rawGPSLat":4026.32441,"rawGPSLong":7956.75652,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433230800,"topicName":"sensors/gps","sequenceNumber":338} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:51 AM","latitude":40.43874733333333,"north":true,"longitude":-79.9459755,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32484,"rawGPSLong":7956.75853,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433231319,"topicName":"sensors/gps","sequenceNumber":339} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:51 AM","latitude":40.43875383333334,"north":true,"longitude":-79.9460085,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32523,"rawGPSLong":7956.76051,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433231818,"topicName":"sensors/gps","sequenceNumber":340} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:52 AM","latitude":40.438760333333335,"north":true,"longitude":-79.946041,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.32562,"rawGPSLong":7956.76246,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433232317,"topicName":"sensors/gps","sequenceNumber":341} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:52 AM","latitude":40.43876783333333,"north":true,"longitude":-79.94607216666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32607,"rawGPSLong":7956.76433,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433232819,"topicName":"sensors/gps","sequenceNumber":342} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:53 AM","latitude":40.438777,"north":true,"longitude":-79.94610166666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.6,"rawGPSLat":4026.32662,"rawGPSLong":7956.7661,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433233313,"topicName":"sensors/gps","sequenceNumber":343} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:53 AM","latitude":40.43878683333333,"north":true,"longitude":-79.94613083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.32721,"rawGPSLong":7956.76785,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433233823,"topicName":"sensors/gps","sequenceNumber":344} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:54 AM","latitude":40.438796833333335,"north":true,"longitude":-79.94615916666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.9,"rawGPSLat":4026.32781,"rawGPSLong":7956.76955,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433234314,"topicName":"sensors/gps","sequenceNumber":345} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:54 AM","latitude":40.4388075,"north":true,"longitude":-79.94618583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.8,"rawGPSLat":4026.32845,"rawGPSLong":7956.77115,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433234810,"topicName":"sensors/gps","sequenceNumber":346} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:55 AM","latitude":40.438818833333336,"north":true,"longitude":-79.94621166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.9,"rawGPSLat":4026.32913,"rawGPSLong":7956.7727,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433235305,"topicName":"sensors/gps","sequenceNumber":347} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:55 AM","latitude":40.438831,"north":true,"longitude":-79.94623583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.32986,"rawGPSLong":7956.77415,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433235815,"topicName":"sensors/gps","sequenceNumber":348} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:56 AM","latitude":40.4388445,"north":true,"longitude":-79.94625816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33067,"rawGPSLong":7956.77549,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433236299,"topicName":"sensors/gps","sequenceNumber":349} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:56 AM","latitude":40.438858333333336,"north":true,"longitude":-79.94627933333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.3315,"rawGPSLong":7956.77676,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433236809,"topicName":"sensors/gps","sequenceNumber":350} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:57 AM","latitude":40.43887216666667,"north":true,"longitude":-79.94630016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33233,"rawGPSLong":7956.77801,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433237307,"topicName":"sensors/gps","sequenceNumber":351} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:57 AM","latitude":40.43888583333333,"north":true,"longitude":-79.94632116666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33315,"rawGPSLong":7956.77927,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433237809,"topicName":"sensors/gps","sequenceNumber":352} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:58 AM","latitude":40.438899666666664,"north":true,"longitude":-79.94634116666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33398,"rawGPSLong":7956.78047,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433238295,"topicName":"sensors/gps","sequenceNumber":353} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:58 AM","latitude":40.4389145,"north":true,"longitude":-79.94636016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33487,"rawGPSLong":7956.78161,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433238810,"topicName":"sensors/gps","sequenceNumber":354} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:59 AM","latitude":40.43892916666667,"north":true,"longitude":-79.94637883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":282.0,"rawGPSLat":4026.33575,"rawGPSLong":7956.78273,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433239307,"topicName":"sensors/gps","sequenceNumber":355} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:53:59 AM","latitude":40.4389435,"north":true,"longitude":-79.94639783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.9,"rawGPSLat":4026.33661,"rawGPSLong":7956.78387,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433239810,"topicName":"sensors/gps","sequenceNumber":356} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:00 AM","latitude":40.438958166666666,"north":true,"longitude":-79.94641616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.8,"rawGPSLat":4026.33749,"rawGPSLong":7956.78497,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433240308,"topicName":"sensors/gps","sequenceNumber":357} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:00 AM","latitude":40.438974333333334,"north":true,"longitude":-79.9464325,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.33846,"rawGPSLong":7956.78595,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433240797,"topicName":"sensors/gps","sequenceNumber":358} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:01 AM","latitude":40.438991,"north":true,"longitude":-79.94644833333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.33946,"rawGPSLong":7956.7869,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433241319,"topicName":"sensors/gps","sequenceNumber":359} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:01 AM","latitude":40.43900816666667,"north":true,"longitude":-79.94646333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.6,"rawGPSLat":4026.34049,"rawGPSLong":7956.7878,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433241811,"topicName":"sensors/gps","sequenceNumber":360} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:02 AM","latitude":40.439025,"north":true,"longitude":-79.94647883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.3415,"rawGPSLong":7956.78873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433242309,"topicName":"sensors/gps","sequenceNumber":361} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:02 AM","latitude":40.43904216666667,"north":true,"longitude":-79.946494,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.34253,"rawGPSLong":7956.78964,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433242810,"topicName":"sensors/gps","sequenceNumber":362} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:03 AM","latitude":40.43905933333333,"north":true,"longitude":-79.94650916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.34356,"rawGPSLong":7956.79055,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433243306,"topicName":"sensors/gps","sequenceNumber":363} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:03 AM","latitude":40.43907683333333,"north":true,"longitude":-79.9465235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.7,"rawGPSLat":4026.34461,"rawGPSLong":7956.79141,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433243798,"topicName":"sensors/gps","sequenceNumber":364} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:04 AM","latitude":40.439095333333334,"north":true,"longitude":-79.94653566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.4,"rawGPSLat":4026.34572,"rawGPSLong":7956.79214,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433244307,"topicName":"sensors/gps","sequenceNumber":365} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:04 AM","latitude":40.439113166666665,"north":true,"longitude":-79.94654866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.4,"rawGPSLat":4026.34679,"rawGPSLong":7956.79292,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433244795,"topicName":"sensors/gps","sequenceNumber":366} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:05 AM","latitude":40.439131333333336,"north":true,"longitude":-79.9465615,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.34788,"rawGPSLong":7956.79369,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433245309,"topicName":"sensors/gps","sequenceNumber":367} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:05 AM","latitude":40.439149,"north":true,"longitude":-79.94657516666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.34894,"rawGPSLong":7956.79451,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433245792,"topicName":"sensors/gps","sequenceNumber":368} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:06 AM","latitude":40.4391675,"north":true,"longitude":-79.9465865,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35005,"rawGPSLong":7956.79519,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433246297,"topicName":"sensors/gps","sequenceNumber":369} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:06 AM","latitude":40.439186,"north":true,"longitude":-79.94659733333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35116,"rawGPSLong":7956.79584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433246793,"topicName":"sensors/gps","sequenceNumber":370} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:07 AM","latitude":40.439204333333336,"north":true,"longitude":-79.94660833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35226,"rawGPSLong":7956.7965,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433247307,"topicName":"sensors/gps","sequenceNumber":371} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:07 AM","latitude":40.4392225,"north":true,"longitude":-79.94661916666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35335,"rawGPSLong":7956.79715,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433247824,"topicName":"sensors/gps","sequenceNumber":372} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:08 AM","latitude":40.43924066666667,"north":true,"longitude":-79.94663,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.35444,"rawGPSLong":7956.7978,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433248307,"topicName":"sensors/gps","sequenceNumber":373} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:08 AM","latitude":40.439259166666666,"north":true,"longitude":-79.94663983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.35555,"rawGPSLong":7956.79839,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433248811,"topicName":"sensors/gps","sequenceNumber":374} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:09 AM","latitude":40.4392775,"north":true,"longitude":-79.94664983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.35665,"rawGPSLong":7956.79899,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433249301,"topicName":"sensors/gps","sequenceNumber":375} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:09 AM","latitude":40.439295666666666,"north":true,"longitude":-79.94665966666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.1,"rawGPSLat":4026.35774,"rawGPSLong":7956.79958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433249808,"topicName":"sensors/gps","sequenceNumber":376} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:10 AM","latitude":40.43931383333334,"north":true,"longitude":-79.94666983333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35883,"rawGPSLong":7956.80019,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433250310,"topicName":"sensors/gps","sequenceNumber":377} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:10 AM","latitude":40.439331833333334,"north":true,"longitude":-79.94668016666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.35991,"rawGPSLong":7956.80081,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433250810,"topicName":"sensors/gps","sequenceNumber":378} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:11 AM","latitude":40.439349666666665,"north":true,"longitude":-79.94669066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.36098,"rawGPSLong":7956.80144,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433251313,"topicName":"sensors/gps","sequenceNumber":379} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:11 AM","latitude":40.43936766666667,"north":true,"longitude":-79.9467005,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.36206,"rawGPSLong":7956.80203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433251803,"topicName":"sensors/gps","sequenceNumber":380} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:12 AM","latitude":40.43938583333333,"north":true,"longitude":-79.94671,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.36315,"rawGPSLong":7956.8026,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433252303,"topicName":"sensors/gps","sequenceNumber":381} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:12 AM","latitude":40.43940416666667,"north":true,"longitude":-79.946719,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.36425,"rawGPSLong":7956.80314,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433252807,"topicName":"sensors/gps","sequenceNumber":382} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:13 AM","latitude":40.43942283333333,"north":true,"longitude":-79.9467275,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.2,"rawGPSLat":4026.36537,"rawGPSLong":7956.80365,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433253297,"topicName":"sensors/gps","sequenceNumber":383} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:13 AM","latitude":40.43944166666667,"north":true,"longitude":-79.94673566666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.1,"rawGPSLat":4026.3665,"rawGPSLong":7956.80414,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433253812,"topicName":"sensors/gps","sequenceNumber":384} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:14 AM","latitude":40.43946016666666,"north":true,"longitude":-79.9467435,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.41,"antennaAltitude":281.0,"rawGPSLat":4026.36761,"rawGPSLong":7956.80461,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433254300,"topicName":"sensors/gps","sequenceNumber":385} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:14 AM","latitude":40.439479,"north":true,"longitude":-79.94675183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":281.0,"rawGPSLat":4026.36874,"rawGPSLong":7956.80511,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433254807,"topicName":"sensors/gps","sequenceNumber":386} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:15 AM","latitude":40.439497833333334,"north":true,"longitude":-79.946761,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.9,"rawGPSLat":4026.36987,"rawGPSLong":7956.80566,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433255301,"topicName":"sensors/gps","sequenceNumber":387} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:15 AM","latitude":40.439516166666664,"north":true,"longitude":-79.94677133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.8,"rawGPSLat":4026.37097,"rawGPSLong":7956.80628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433255784,"topicName":"sensors/gps","sequenceNumber":388} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:16 AM","latitude":40.439534333333334,"north":true,"longitude":-79.94678316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.8,"rawGPSLat":4026.37206,"rawGPSLong":7956.80699,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433256307,"topicName":"sensors/gps","sequenceNumber":389} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:16 AM","latitude":40.439552166666665,"north":true,"longitude":-79.9467965,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.7,"rawGPSLat":4026.37313,"rawGPSLong":7956.80779,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433256800,"topicName":"sensors/gps","sequenceNumber":390} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:17 AM","latitude":40.4395705,"north":true,"longitude":-79.94680983333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.5,"rawGPSLat":4026.37423,"rawGPSLong":7956.80859,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433257317,"topicName":"sensors/gps","sequenceNumber":391} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:17 AM","latitude":40.439589166666664,"north":true,"longitude":-79.9468225,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.5,"rawGPSLat":4026.37535,"rawGPSLong":7956.80935,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433257808,"topicName":"sensors/gps","sequenceNumber":392} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:18 AM","latitude":40.439608,"north":true,"longitude":-79.94683616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.4,"rawGPSLat":4026.37648,"rawGPSLong":7956.81017,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433258318,"topicName":"sensors/gps","sequenceNumber":393} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:18 AM","latitude":40.43962766666667,"north":true,"longitude":-79.94684883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.3,"rawGPSLat":4026.37766,"rawGPSLong":7956.81093,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433258802,"topicName":"sensors/gps","sequenceNumber":394} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:19 AM","latitude":40.439648,"north":true,"longitude":-79.94686133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.3,"rawGPSLat":4026.37888,"rawGPSLong":7956.81168,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433259316,"topicName":"sensors/gps","sequenceNumber":395} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:19 AM","latitude":40.43966833333333,"north":true,"longitude":-79.9468745,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.1,"rawGPSLat":4026.3801,"rawGPSLong":7956.81247,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433259814,"topicName":"sensors/gps","sequenceNumber":396} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:20 AM","latitude":40.4396885,"north":true,"longitude":-79.946889,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.0,"rawGPSLat":4026.38131,"rawGPSLong":7956.81334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433260315,"topicName":"sensors/gps","sequenceNumber":397} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:20 AM","latitude":40.439708833333334,"north":true,"longitude":-79.946905,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.9,"rawGPSLat":4026.38253,"rawGPSLong":7956.8143,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433260812,"topicName":"sensors/gps","sequenceNumber":398} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:21 AM","latitude":40.439729,"north":true,"longitude":-79.94692233333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.8,"rawGPSLat":4026.38374,"rawGPSLong":7956.81534,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433261327,"topicName":"sensors/gps","sequenceNumber":399} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:21 AM","latitude":40.4397495,"north":true,"longitude":-79.94694083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.6,"rawGPSLat":4026.38497,"rawGPSLong":7956.81645,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433261825,"topicName":"sensors/gps","sequenceNumber":400} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:22 AM","latitude":40.4397705,"north":true,"longitude":-79.94695916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.5,"rawGPSLat":4026.38623,"rawGPSLong":7956.81755,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433262318,"topicName":"sensors/gps","sequenceNumber":401} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:22 AM","latitude":40.439791666666665,"north":true,"longitude":-79.9469785,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.3,"rawGPSLat":4026.3875,"rawGPSLong":7956.81871,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433262825,"topicName":"sensors/gps","sequenceNumber":402} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:23 AM","latitude":40.439812833333335,"north":true,"longitude":-79.94699866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.2,"rawGPSLat":4026.38877,"rawGPSLong":7956.81992,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433263329,"topicName":"sensors/gps","sequenceNumber":403} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:23 AM","latitude":40.43983433333333,"north":true,"longitude":-79.94701933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.0,"rawGPSLat":4026.39006,"rawGPSLong":7956.82116,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433263831,"topicName":"sensors/gps","sequenceNumber":404} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:24 AM","latitude":40.43985633333333,"north":true,"longitude":-79.9470395,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.39138,"rawGPSLong":7956.82237,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433264326,"topicName":"sensors/gps","sequenceNumber":405} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:24 AM","latitude":40.43987883333333,"north":true,"longitude":-79.9470595,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.8,"rawGPSLat":4026.39273,"rawGPSLong":7956.82357,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433264820,"topicName":"sensors/gps","sequenceNumber":406} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:25 AM","latitude":40.439901166666665,"north":true,"longitude":-79.9470805,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.7,"rawGPSLat":4026.39407,"rawGPSLong":7956.82483,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433265322,"topicName":"sensors/gps","sequenceNumber":407} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:25 AM","latitude":40.439922833333334,"north":true,"longitude":-79.94710283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.6,"rawGPSLat":4026.39537,"rawGPSLong":7956.82617,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433265834,"topicName":"sensors/gps","sequenceNumber":408} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:26 AM","latitude":40.43994433333334,"north":true,"longitude":-79.94712616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.39666,"rawGPSLong":7956.82757,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433266325,"topicName":"sensors/gps","sequenceNumber":409} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:26 AM","latitude":40.43996516666667,"north":true,"longitude":-79.947151,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.39791,"rawGPSLong":7956.82906,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433266818,"topicName":"sensors/gps","sequenceNumber":410} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:27 AM","latitude":40.439987333333335,"north":true,"longitude":-79.94717416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.4,"rawGPSLat":4026.39924,"rawGPSLong":7956.83045,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433267321,"topicName":"sensors/gps","sequenceNumber":411} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:27 AM","latitude":40.44001033333333,"north":true,"longitude":-79.94719683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.40062,"rawGPSLong":7956.83181,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433267819,"topicName":"sensors/gps","sequenceNumber":412} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:28 AM","latitude":40.440032333333335,"north":true,"longitude":-79.94722083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.2,"rawGPSLat":4026.40194,"rawGPSLong":7956.83325,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433268320,"topicName":"sensors/gps","sequenceNumber":413} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:28 AM","latitude":40.4400535,"north":true,"longitude":-79.94724683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.1,"rawGPSLat":4026.40321,"rawGPSLong":7956.83481,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433268816,"topicName":"sensors/gps","sequenceNumber":414} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:29 AM","latitude":40.4400745,"north":true,"longitude":-79.9472735,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.0,"rawGPSLat":4026.40447,"rawGPSLong":7956.83641,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433269313,"topicName":"sensors/gps","sequenceNumber":415} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:29 AM","latitude":40.44009633333334,"north":true,"longitude":-79.94729883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.0,"rawGPSLat":4026.40578,"rawGPSLong":7956.83793,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433269814,"topicName":"sensors/gps","sequenceNumber":416} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:30 AM","latitude":40.44011916666667,"north":true,"longitude":-79.94732383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.9,"rawGPSLat":4026.40715,"rawGPSLong":7956.83943,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433270316,"topicName":"sensors/gps","sequenceNumber":417} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:30 AM","latitude":40.44014166666667,"north":true,"longitude":-79.94734933333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.7,"rawGPSLat":4026.4085,"rawGPSLong":7956.84096,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433270806,"topicName":"sensors/gps","sequenceNumber":418} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:31 AM","latitude":40.440164833333334,"north":true,"longitude":-79.9473735,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.7,"rawGPSLat":4026.40989,"rawGPSLong":7956.84241,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433271321,"topicName":"sensors/gps","sequenceNumber":419} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:31 AM","latitude":40.44018833333333,"north":true,"longitude":-79.94739766666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.6,"rawGPSLat":4026.4113,"rawGPSLong":7956.84386,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433271826,"topicName":"sensors/gps","sequenceNumber":420} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:32 AM","latitude":40.44021166666667,"north":true,"longitude":-79.94742266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.6,"rawGPSLat":4026.4127,"rawGPSLong":7956.84536,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433272322,"topicName":"sensors/gps","sequenceNumber":421} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:32 AM","latitude":40.4402355,"north":true,"longitude":-79.94744666666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.5,"rawGPSLat":4026.41413,"rawGPSLong":7956.8468,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433272827,"topicName":"sensors/gps","sequenceNumber":422} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:33 AM","latitude":40.4402595,"north":true,"longitude":-79.9474705,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.4,"rawGPSLat":4026.41557,"rawGPSLong":7956.84823,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433273322,"topicName":"sensors/gps","sequenceNumber":423} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:33 AM","latitude":40.440283666666666,"north":true,"longitude":-79.94749466666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.4,"rawGPSLat":4026.41702,"rawGPSLong":7956.84968,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433273809,"topicName":"sensors/gps","sequenceNumber":424} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:34 AM","latitude":40.440306666666665,"north":true,"longitude":-79.94752033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.4,"rawGPSLat":4026.4184,"rawGPSLong":7956.85122,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433274315,"topicName":"sensors/gps","sequenceNumber":425} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:34 AM","latitude":40.44033016666667,"north":true,"longitude":-79.9475455,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.3,"rawGPSLat":4026.41981,"rawGPSLong":7956.85273,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433274817,"topicName":"sensors/gps","sequenceNumber":426} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:35 AM","latitude":40.44035383333333,"north":true,"longitude":-79.94757083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.2,"rawGPSLat":4026.42123,"rawGPSLong":7956.85425,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433275324,"topicName":"sensors/gps","sequenceNumber":427} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:35 AM","latitude":40.440376666666666,"north":true,"longitude":-79.94759716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.1,"rawGPSLat":4026.4226,"rawGPSLong":7956.85583,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433275829,"topicName":"sensors/gps","sequenceNumber":428} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:36 AM","latitude":40.4404,"north":true,"longitude":-79.9476235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.1,"rawGPSLat":4026.424,"rawGPSLong":7956.85741,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433276318,"topicName":"sensors/gps","sequenceNumber":429} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:36 AM","latitude":40.4404245,"north":true,"longitude":-79.94764783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.0,"rawGPSLat":4026.42547,"rawGPSLong":7956.85887,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433276811,"topicName":"sensors/gps","sequenceNumber":430} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:37 AM","latitude":40.44045,"north":true,"longitude":-79.94767133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.8,"rawGPSLat":4026.427,"rawGPSLong":7956.86028,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433277316,"topicName":"sensors/gps","sequenceNumber":431} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:37 AM","latitude":40.440475166666666,"north":true,"longitude":-79.94769566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.7,"rawGPSLat":4026.42851,"rawGPSLong":7956.86174,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433277839,"topicName":"sensors/gps","sequenceNumber":432} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:38 AM","latitude":40.44050083333333,"north":true,"longitude":-79.9477195,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.6,"rawGPSLat":4026.43005,"rawGPSLong":7956.86317,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433278321,"topicName":"sensors/gps","sequenceNumber":433} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:38 AM","latitude":40.440528,"north":true,"longitude":-79.94774116666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.4,"rawGPSLat":4026.43168,"rawGPSLong":7956.86447,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433278821,"topicName":"sensors/gps","sequenceNumber":434} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:39 AM","latitude":40.44055516666667,"north":true,"longitude":-79.94776216666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.3,"rawGPSLat":4026.43331,"rawGPSLong":7956.86573,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433279323,"topicName":"sensors/gps","sequenceNumber":435} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:39 AM","latitude":40.440581333333334,"north":true,"longitude":-79.94778583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.2,"rawGPSLat":4026.43488,"rawGPSLong":7956.86715,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433279828,"topicName":"sensors/gps","sequenceNumber":436} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:40 AM","latitude":40.440606333333335,"north":true,"longitude":-79.94781183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.0,"rawGPSLat":4026.43638,"rawGPSLong":7956.86871,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433280323,"topicName":"sensors/gps","sequenceNumber":437} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:40 AM","latitude":40.440632,"north":true,"longitude":-79.9478365,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.9,"rawGPSLat":4026.43792,"rawGPSLong":7956.87019,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433280810,"topicName":"sensors/gps","sequenceNumber":438} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:41 AM","latitude":40.440659333333336,"north":true,"longitude":-79.9478585,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.7,"rawGPSLat":4026.43956,"rawGPSLong":7956.87151,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433281317,"topicName":"sensors/gps","sequenceNumber":439} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:41 AM","latitude":40.44068816666667,"north":true,"longitude":-79.9478785,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.6,"rawGPSLat":4026.44129,"rawGPSLong":7956.87271,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433281818,"topicName":"sensors/gps","sequenceNumber":440} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:42 AM","latitude":40.440718333333336,"north":true,"longitude":-79.94789633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.4,"rawGPSLat":4026.4431,"rawGPSLong":7956.87378,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433282315,"topicName":"sensors/gps","sequenceNumber":441} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:42 AM","latitude":40.4407485,"north":true,"longitude":-79.94791483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.3,"rawGPSLat":4026.44491,"rawGPSLong":7956.87489,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433282818,"topicName":"sensors/gps","sequenceNumber":442} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:43 AM","latitude":40.4407795,"north":true,"longitude":-79.9479315,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.2,"rawGPSLat":4026.44677,"rawGPSLong":7956.87589,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433283323,"topicName":"sensors/gps","sequenceNumber":443} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:43 AM","latitude":40.440810666666664,"north":true,"longitude":-79.94794616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.1,"rawGPSLat":4026.44864,"rawGPSLong":7956.87677,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433283835,"topicName":"sensors/gps","sequenceNumber":444} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:44 AM","latitude":40.44084333333333,"north":true,"longitude":-79.94795433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.0,"rawGPSLat":4026.4506,"rawGPSLong":7956.87726,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433284317,"topicName":"sensors/gps","sequenceNumber":445} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:44 AM","latitude":40.440876,"north":true,"longitude":-79.947957,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.0,"rawGPSLat":4026.45256,"rawGPSLong":7956.87742,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433284823,"topicName":"sensors/gps","sequenceNumber":446} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:45 AM","latitude":40.44090833333333,"north":true,"longitude":-79.94795483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":274.9,"rawGPSLat":4026.4545,"rawGPSLong":7956.87729,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433285307,"topicName":"sensors/gps","sequenceNumber":447} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:45 AM","latitude":40.44093933333333,"north":true,"longitude":-79.94794733333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":274.9,"rawGPSLat":4026.45636,"rawGPSLong":7956.87684,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433285794,"topicName":"sensors/gps","sequenceNumber":448} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:46 AM","latitude":40.44096783333333,"north":true,"longitude":-79.94793366666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.0,"rawGPSLat":4026.45807,"rawGPSLong":7956.87602,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433286307,"topicName":"sensors/gps","sequenceNumber":449} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:46 AM","latitude":40.44099333333333,"north":true,"longitude":-79.94791483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.1,"rawGPSLat":4026.4596,"rawGPSLong":7956.87489,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433286795,"topicName":"sensors/gps","sequenceNumber":450} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:47 AM","latitude":40.44101666666667,"north":true,"longitude":-79.947894,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.2,"rawGPSLat":4026.461,"rawGPSLong":7956.87364,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433287302,"topicName":"sensors/gps","sequenceNumber":451} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:47 AM","latitude":40.441039,"north":true,"longitude":-79.94787333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.2,"rawGPSLat":4026.46234,"rawGPSLong":7956.8724,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433287795,"topicName":"sensors/gps","sequenceNumber":452} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:48 AM","latitude":40.44105966666667,"north":true,"longitude":-79.94785283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.1,"rawGPSLat":4026.46358,"rawGPSLong":7956.87117,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433288310,"topicName":"sensors/gps","sequenceNumber":453} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:48 AM","latitude":40.441078,"north":true,"longitude":-79.94783016666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":275.2,"rawGPSLat":4026.46468,"rawGPSLong":7956.86981,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433288807,"topicName":"sensors/gps","sequenceNumber":454} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:49 AM","latitude":40.44109533333334,"north":true,"longitude":-79.94780783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.3,"rawGPSLat":4026.46572,"rawGPSLong":7956.86847,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433289310,"topicName":"sensors/gps","sequenceNumber":455} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:49 AM","latitude":40.4411115,"north":true,"longitude":-79.94778716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.5,"rawGPSLat":4026.46669,"rawGPSLong":7956.86723,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433289807,"topicName":"sensors/gps","sequenceNumber":456} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:50 AM","latitude":40.441126833333335,"north":true,"longitude":-79.94776683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.6,"rawGPSLat":4026.46761,"rawGPSLong":7956.86601,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433290296,"topicName":"sensors/gps","sequenceNumber":457} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:50 AM","latitude":40.44114166666667,"north":true,"longitude":-79.94774783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.7,"rawGPSLat":4026.4685,"rawGPSLong":7956.86487,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433290805,"topicName":"sensors/gps","sequenceNumber":458} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:51 AM","latitude":40.441156166666666,"north":true,"longitude":-79.94773,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":275.9,"rawGPSLat":4026.46937,"rawGPSLong":7956.8638,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433291309,"topicName":"sensors/gps","sequenceNumber":459} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:51 AM","latitude":40.441169333333335,"north":true,"longitude":-79.94771316666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":276.2,"rawGPSLat":4026.47016,"rawGPSLong":7956.86279,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433291800,"topicName":"sensors/gps","sequenceNumber":460} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:52 AM","latitude":40.44118133333333,"north":true,"longitude":-79.947696,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.4,"rawGPSLat":4026.47088,"rawGPSLong":7956.86176,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433292308,"topicName":"sensors/gps","sequenceNumber":461} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:52 AM","latitude":40.44119283333333,"north":true,"longitude":-79.9476795,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.6,"rawGPSLat":4026.47157,"rawGPSLong":7956.86077,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433292807,"topicName":"sensors/gps","sequenceNumber":462} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:53 AM","latitude":40.441204,"north":true,"longitude":-79.94766483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":276.8,"rawGPSLat":4026.47224,"rawGPSLong":7956.85989,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433293303,"topicName":"sensors/gps","sequenceNumber":463} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:53 AM","latitude":40.441214333333335,"north":true,"longitude":-79.947651,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.0,"rawGPSLat":4026.47286,"rawGPSLong":7956.85906,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433293810,"topicName":"sensors/gps","sequenceNumber":464} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:54 AM","latitude":40.44122383333333,"north":true,"longitude":-79.9476375,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.1,"rawGPSLat":4026.47343,"rawGPSLong":7956.85825,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433294296,"topicName":"sensors/gps","sequenceNumber":465} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:54 AM","latitude":40.44123333333334,"north":true,"longitude":-79.94762533333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.3,"rawGPSLat":4026.474,"rawGPSLong":7956.85752,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433294809,"topicName":"sensors/gps","sequenceNumber":466} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:55 AM","latitude":40.441241833333336,"north":true,"longitude":-79.947614,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.5,"rawGPSLat":4026.47451,"rawGPSLong":7956.85684,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433295308,"topicName":"sensors/gps","sequenceNumber":467} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:55 AM","latitude":40.44125016666667,"north":true,"longitude":-79.94760316666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":277.8,"rawGPSLat":4026.47501,"rawGPSLong":7956.85619,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433295809,"topicName":"sensors/gps","sequenceNumber":468} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:56 AM","latitude":40.4412595,"north":true,"longitude":-79.94759183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.0,"rawGPSLat":4026.47557,"rawGPSLong":7956.85551,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433296297,"topicName":"sensors/gps","sequenceNumber":469} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:56 AM","latitude":40.44126883333333,"north":true,"longitude":-79.94757983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.1,"rawGPSLat":4026.47613,"rawGPSLong":7956.85479,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433296814,"topicName":"sensors/gps","sequenceNumber":470} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:57 AM","latitude":40.44127916666667,"north":true,"longitude":-79.9475655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.2,"rawGPSLat":4026.47675,"rawGPSLong":7956.85393,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433297300,"topicName":"sensors/gps","sequenceNumber":471} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:57 AM","latitude":40.44129066666667,"north":true,"longitude":-79.94754816666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.47744,"rawGPSLong":7956.85289,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433297808,"topicName":"sensors/gps","sequenceNumber":472} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:58 AM","latitude":40.441303,"north":true,"longitude":-79.94752783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.47818,"rawGPSLong":7956.85167,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433298308,"topicName":"sensors/gps","sequenceNumber":473} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:58 AM","latitude":40.44131733333333,"north":true,"longitude":-79.947508,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.4,"rawGPSLat":4026.47904,"rawGPSLong":7956.85048,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433298789,"topicName":"sensors/gps","sequenceNumber":474} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:59 AM","latitude":40.44133133333333,"north":true,"longitude":-79.94748383333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.6,"rawGPSLat":4026.47988,"rawGPSLong":7956.84903,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433299302,"topicName":"sensors/gps","sequenceNumber":475} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:54:59 AM","latitude":40.4413445,"north":true,"longitude":-79.94745766666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.7,"rawGPSLat":4026.48067,"rawGPSLong":7956.84746,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433299806,"topicName":"sensors/gps","sequenceNumber":476} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:00 AM","latitude":40.4413575,"north":true,"longitude":-79.94742983333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48145,"rawGPSLong":7956.84579,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433300306,"topicName":"sensors/gps","sequenceNumber":477} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:00 AM","latitude":40.44137216666667,"north":true,"longitude":-79.94740383333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48233,"rawGPSLong":7956.84423,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433300812,"topicName":"sensors/gps","sequenceNumber":478} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:01 AM","latitude":40.44138816666667,"north":true,"longitude":-79.9473805,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48329,"rawGPSLong":7956.84283,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433301300,"topicName":"sensors/gps","sequenceNumber":479} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:01 AM","latitude":40.441402833333335,"north":true,"longitude":-79.94735833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.8,"rawGPSLat":4026.48417,"rawGPSLong":7956.8415,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433301804,"topicName":"sensors/gps","sequenceNumber":480} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:02 AM","latitude":40.441414333333334,"north":true,"longitude":-79.9473345,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.5,"rawGPSLat":4026.48486,"rawGPSLong":7956.84007,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433302289,"topicName":"sensors/gps","sequenceNumber":481} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:02 AM","latitude":40.44142516666667,"north":true,"longitude":-79.94730883333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.48551,"rawGPSLong":7956.83853,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433302805,"topicName":"sensors/gps","sequenceNumber":482} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:03 AM","latitude":40.44143533333333,"north":true,"longitude":-79.947281,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.3,"rawGPSLat":4026.48612,"rawGPSLong":7956.83686,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433303305,"topicName":"sensors/gps","sequenceNumber":483} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:03 AM","latitude":40.4414435,"north":true,"longitude":-79.947252,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.4,"rawGPSLat":4026.48661,"rawGPSLong":7956.83512,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433303801,"topicName":"sensors/gps","sequenceNumber":484} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:04 AM","latitude":40.44145016666667,"north":true,"longitude":-79.947221,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.7,"rawGPSLat":4026.48701,"rawGPSLong":7956.83326,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433304288,"topicName":"sensors/gps","sequenceNumber":485} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:04 AM","latitude":40.441456333333335,"north":true,"longitude":-79.94719066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":278.9,"rawGPSLat":4026.48738,"rawGPSLong":7956.83144,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433304793,"topicName":"sensors/gps","sequenceNumber":486} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:05 AM","latitude":40.44146116666667,"north":true,"longitude":-79.94716033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.3,"rawGPSLat":4026.48767,"rawGPSLong":7956.82962,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433305284,"topicName":"sensors/gps","sequenceNumber":487} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:05 AM","latitude":40.441464,"north":true,"longitude":-79.94713033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.6,"rawGPSLat":4026.48784,"rawGPSLong":7956.82782,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433305807,"topicName":"sensors/gps","sequenceNumber":488} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:06 AM","latitude":40.4414635,"north":true,"longitude":-79.94710083333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":279.9,"rawGPSLat":4026.48781,"rawGPSLong":7956.82605,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433306305,"topicName":"sensors/gps","sequenceNumber":489} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:06 AM","latitude":40.441460666666664,"north":true,"longitude":-79.94707166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.3,"rawGPSLat":4026.48764,"rawGPSLong":7956.8243,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433306808,"topicName":"sensors/gps","sequenceNumber":490} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:07 AM","latitude":40.44145816666666,"north":true,"longitude":-79.94704366666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.4,"rawGPSLat":4026.48749,"rawGPSLong":7956.82262,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433307293,"topicName":"sensors/gps","sequenceNumber":491} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:07 AM","latitude":40.441455833333336,"north":true,"longitude":-79.94701566666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.7,"rawGPSLat":4026.48735,"rawGPSLong":7956.82094,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433307810,"topicName":"sensors/gps","sequenceNumber":492} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:08 AM","latitude":40.44145416666667,"north":true,"longitude":-79.94698816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.7,"rawGPSLat":4026.48725,"rawGPSLong":7956.81929,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433308293,"topicName":"sensors/gps","sequenceNumber":493} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:08 AM","latitude":40.441452,"north":true,"longitude":-79.94696066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":280.9,"rawGPSLat":4026.48712,"rawGPSLong":7956.81764,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433308801,"topicName":"sensors/gps","sequenceNumber":494} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:09 AM","latitude":40.44144883333333,"north":true,"longitude":-79.94693333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.0,"rawGPSLat":4026.48693,"rawGPSLong":7956.816,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433309290,"topicName":"sensors/gps","sequenceNumber":495} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:09 AM","latitude":40.4414445,"north":true,"longitude":-79.94690616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.1,"rawGPSLat":4026.48667,"rawGPSLong":7956.81437,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433309801,"topicName":"sensors/gps","sequenceNumber":496} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:10 AM","latitude":40.4414395,"north":true,"longitude":-79.94687866666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.3,"rawGPSLat":4026.48637,"rawGPSLong":7956.81272,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433310312,"topicName":"sensors/gps","sequenceNumber":497} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:10 AM","latitude":40.441435166666665,"north":true,"longitude":-79.9468515,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.5,"rawGPSLat":4026.48611,"rawGPSLong":7956.81109,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433310795,"topicName":"sensors/gps","sequenceNumber":498} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:11 AM","latitude":40.441431666666666,"north":true,"longitude":-79.94682366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.6,"rawGPSLat":4026.4859,"rawGPSLong":7956.80942,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433311308,"topicName":"sensors/gps","sequenceNumber":499} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:11 AM","latitude":40.4414295,"north":true,"longitude":-79.946796,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.7,"rawGPSLat":4026.48577,"rawGPSLong":7956.80776,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433311802,"topicName":"sensors/gps","sequenceNumber":500} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:12 AM","latitude":40.44142633333333,"north":true,"longitude":-79.946769,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":281.9,"rawGPSLat":4026.48558,"rawGPSLong":7956.80614,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433312296,"topicName":"sensors/gps","sequenceNumber":501} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:12 AM","latitude":40.44142133333333,"north":true,"longitude":-79.9467425,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.1,"rawGPSLat":4026.48528,"rawGPSLong":7956.80455,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433312802,"topicName":"sensors/gps","sequenceNumber":502} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:13 AM","latitude":40.44141583333333,"north":true,"longitude":-79.94671683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.3,"rawGPSLat":4026.48495,"rawGPSLong":7956.80301,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433313311,"topicName":"sensors/gps","sequenceNumber":503} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:13 AM","latitude":40.441410166666664,"north":true,"longitude":-79.9466915,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.5,"rawGPSLat":4026.48461,"rawGPSLong":7956.80149,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433313817,"topicName":"sensors/gps","sequenceNumber":504} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:14 AM","latitude":40.44140516666667,"north":true,"longitude":-79.9466655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.6,"rawGPSLat":4026.48431,"rawGPSLong":7956.79993,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433314309,"topicName":"sensors/gps","sequenceNumber":505} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:14 AM","latitude":40.441401,"north":true,"longitude":-79.94663966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":282.8,"rawGPSLat":4026.48406,"rawGPSLong":7956.79838,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433314789,"topicName":"sensors/gps","sequenceNumber":506} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:15 AM","latitude":40.4413975,"north":true,"longitude":-79.94661366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.0,"rawGPSLat":4026.48385,"rawGPSLong":7956.79682,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433315300,"topicName":"sensors/gps","sequenceNumber":507} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:15 AM","latitude":40.441394,"north":true,"longitude":-79.94658783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.1,"rawGPSLat":4026.48364,"rawGPSLong":7956.79527,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433315785,"topicName":"sensors/gps","sequenceNumber":508} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:16 AM","latitude":40.4413905,"north":true,"longitude":-79.94656283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.2,"rawGPSLat":4026.48343,"rawGPSLong":7956.79377,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433316301,"topicName":"sensors/gps","sequenceNumber":509} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:16 AM","latitude":40.441386333333334,"north":true,"longitude":-79.946538,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.3,"rawGPSLat":4026.48318,"rawGPSLong":7956.79228,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433316801,"topicName":"sensors/gps","sequenceNumber":510} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:17 AM","latitude":40.4413825,"north":true,"longitude":-79.94651316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.4,"rawGPSLat":4026.48295,"rawGPSLong":7956.79079,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433317291,"topicName":"sensors/gps","sequenceNumber":511} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:17 AM","latitude":40.441379,"north":true,"longitude":-79.946488,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.6,"rawGPSLat":4026.48274,"rawGPSLong":7956.78928,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433317801,"topicName":"sensors/gps","sequenceNumber":512} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:18 AM","latitude":40.441374833333334,"north":true,"longitude":-79.94646266666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.7,"rawGPSLat":4026.48249,"rawGPSLong":7956.78776,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433318309,"topicName":"sensors/gps","sequenceNumber":513} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:18 AM","latitude":40.44136966666667,"north":true,"longitude":-79.94643816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":283.8,"rawGPSLat":4026.48218,"rawGPSLong":7956.78629,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433318787,"topicName":"sensors/gps","sequenceNumber":514} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:19 AM","latitude":40.441364,"north":true,"longitude":-79.94641333333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.0,"rawGPSLat":4026.48184,"rawGPSLong":7956.7848,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433319302,"topicName":"sensors/gps","sequenceNumber":515} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:19 AM","latitude":40.44135883333333,"north":true,"longitude":-79.94638883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.1,"rawGPSLat":4026.48153,"rawGPSLong":7956.78333,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433319808,"topicName":"sensors/gps","sequenceNumber":516} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:20 AM","latitude":40.441354,"north":true,"longitude":-79.94636416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.2,"rawGPSLat":4026.48124,"rawGPSLong":7956.78185,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433320312,"topicName":"sensors/gps","sequenceNumber":517} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:20 AM","latitude":40.4413505,"north":true,"longitude":-79.9463395,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.4,"rawGPSLat":4026.48103,"rawGPSLong":7956.78037,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433320796,"topicName":"sensors/gps","sequenceNumber":518} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:21 AM","latitude":40.44134733333333,"north":true,"longitude":-79.9463145,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.5,"rawGPSLat":4026.48084,"rawGPSLong":7956.77887,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433321304,"topicName":"sensors/gps","sequenceNumber":519} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:21 AM","latitude":40.44134433333333,"north":true,"longitude":-79.94629,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.6,"rawGPSLat":4026.48066,"rawGPSLong":7956.7774,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433321801,"topicName":"sensors/gps","sequenceNumber":520} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:22 AM","latitude":40.441340833333335,"north":true,"longitude":-79.94626583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":284.7,"rawGPSLat":4026.48045,"rawGPSLong":7956.77595,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433322310,"topicName":"sensors/gps","sequenceNumber":521} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:22 AM","latitude":40.441336166666666,"north":true,"longitude":-79.9462415,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":284.8,"rawGPSLat":4026.48017,"rawGPSLong":7956.77449,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433322804,"topicName":"sensors/gps","sequenceNumber":522} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:23 AM","latitude":40.4413305,"north":true,"longitude":-79.9462175,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.0,"rawGPSLat":4026.47983,"rawGPSLong":7956.77305,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433323301,"topicName":"sensors/gps","sequenceNumber":523} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:23 AM","latitude":40.44132466666667,"north":true,"longitude":-79.94619483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.2,"rawGPSLat":4026.47948,"rawGPSLong":7956.77169,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433323800,"topicName":"sensors/gps","sequenceNumber":524} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:24 AM","latitude":40.441319,"north":true,"longitude":-79.94617233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.4,"rawGPSLat":4026.47914,"rawGPSLong":7956.77034,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433324293,"topicName":"sensors/gps","sequenceNumber":525} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:24 AM","latitude":40.44131483333334,"north":true,"longitude":-79.94614916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.5,"rawGPSLat":4026.47889,"rawGPSLong":7956.76895,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433324800,"topicName":"sensors/gps","sequenceNumber":526} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:25 AM","latitude":40.44131083333333,"north":true,"longitude":-79.946127,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.7,"rawGPSLat":4026.47865,"rawGPSLong":7956.76762,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433325288,"topicName":"sensors/gps","sequenceNumber":527} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:25 AM","latitude":40.4413075,"north":true,"longitude":-79.94610266666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.9,"rawGPSLat":4026.47845,"rawGPSLong":7956.76616,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433325812,"topicName":"sensors/gps","sequenceNumber":528} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:26 AM","latitude":40.4413025,"north":true,"longitude":-79.94607733333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.1,"rawGPSLat":4026.47815,"rawGPSLong":7956.76464,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433326288,"topicName":"sensors/gps","sequenceNumber":529} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:26 AM","latitude":40.441297166666665,"north":true,"longitude":-79.94605416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.3,"rawGPSLat":4026.47783,"rawGPSLong":7956.76325,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433326799,"topicName":"sensors/gps","sequenceNumber":530} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:27 AM","latitude":40.4412935,"north":true,"longitude":-79.94603383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.4,"rawGPSLat":4026.47761,"rawGPSLong":7956.76203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433327301,"topicName":"sensors/gps","sequenceNumber":531} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:27 AM","latitude":40.44128966666667,"north":true,"longitude":-79.946015,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.6,"rawGPSLat":4026.47738,"rawGPSLong":7956.7609,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433327810,"topicName":"sensors/gps","sequenceNumber":532} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:28 AM","latitude":40.44128633333333,"north":true,"longitude":-79.9459965,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":286.8,"rawGPSLat":4026.47718,"rawGPSLong":7956.75979,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433328306,"topicName":"sensors/gps","sequenceNumber":533} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:28 AM","latitude":40.44128216666667,"north":true,"longitude":-79.9459765,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.3,"rawGPSLat":4026.47693,"rawGPSLong":7956.75859,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433328808,"topicName":"sensors/gps","sequenceNumber":534} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:29 AM","latitude":40.441278,"north":true,"longitude":-79.94595483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.5,"rawGPSLat":4026.47668,"rawGPSLong":7956.75729,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433329305,"topicName":"sensors/gps","sequenceNumber":535} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:29 AM","latitude":40.441274166666666,"north":true,"longitude":-79.9459345,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.7,"rawGPSLat":4026.47645,"rawGPSLong":7956.75607,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433329806,"topicName":"sensors/gps","sequenceNumber":536} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:30 AM","latitude":40.441270833333334,"north":true,"longitude":-79.94591616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.47625,"rawGPSLong":7956.75497,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433330301,"topicName":"sensors/gps","sequenceNumber":537} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:30 AM","latitude":40.44126633333333,"north":true,"longitude":-79.94589316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.9,"rawGPSLat":4026.47598,"rawGPSLong":7956.75359,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433330806,"topicName":"sensors/gps","sequenceNumber":538} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:31 AM","latitude":40.44126033333333,"north":true,"longitude":-79.94586883333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.2,"rawGPSLat":4026.47562,"rawGPSLong":7956.75213,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433331302,"topicName":"sensors/gps","sequenceNumber":539} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:31 AM","latitude":40.441254666666666,"north":true,"longitude":-79.945847,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.3,"rawGPSLat":4026.47528,"rawGPSLong":7956.75082,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433331808,"topicName":"sensors/gps","sequenceNumber":540} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:32 AM","latitude":40.44124933333333,"north":true,"longitude":-79.94582816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.6,"rawGPSLat":4026.47496,"rawGPSLong":7956.74969,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433332304,"topicName":"sensors/gps","sequenceNumber":541} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:32 AM","latitude":40.44124583333333,"north":true,"longitude":-79.94581033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.7,"rawGPSLat":4026.47475,"rawGPSLong":7956.74862,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433332805,"topicName":"sensors/gps","sequenceNumber":542} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:33 AM","latitude":40.44124083333333,"north":true,"longitude":-79.94579033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.9,"rawGPSLat":4026.47445,"rawGPSLong":7956.74742,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433333301,"topicName":"sensors/gps","sequenceNumber":543} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:33 AM","latitude":40.441236,"north":true,"longitude":-79.9457655,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.47416,"rawGPSLong":7956.74593,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433333802,"topicName":"sensors/gps","sequenceNumber":544} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:34 AM","latitude":40.441230166666664,"north":true,"longitude":-79.94574283333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.3,"rawGPSLat":4026.47381,"rawGPSLong":7956.74457,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433334309,"topicName":"sensors/gps","sequenceNumber":545} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:34 AM","latitude":40.441225333333335,"north":true,"longitude":-79.94572183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47352,"rawGPSLong":7956.74331,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433334797,"topicName":"sensors/gps","sequenceNumber":546} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:35 AM","latitude":40.44122216666667,"north":true,"longitude":-79.945702,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.47333,"rawGPSLong":7956.74212,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433335308,"topicName":"sensors/gps","sequenceNumber":547} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:35 AM","latitude":40.44121966666667,"north":true,"longitude":-79.94568366666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.47318,"rawGPSLong":7956.74102,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433335797,"topicName":"sensors/gps","sequenceNumber":548} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:36 AM","latitude":40.44121616666666,"north":true,"longitude":-79.94566333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.47297,"rawGPSLong":7956.7398,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433336304,"topicName":"sensors/gps","sequenceNumber":549} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:36 AM","latitude":40.441212166666666,"north":true,"longitude":-79.94563933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47273,"rawGPSLong":7956.73836,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433336802,"topicName":"sensors/gps","sequenceNumber":550} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:37 AM","latitude":40.44120816666667,"north":true,"longitude":-79.94561583333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47249,"rawGPSLong":7956.73695,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433337307,"topicName":"sensors/gps","sequenceNumber":551} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:37 AM","latitude":40.441205,"north":true,"longitude":-79.9455935,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.4723,"rawGPSLong":7956.73561,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433337803,"topicName":"sensors/gps","sequenceNumber":552} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:38 AM","latitude":40.44120216666666,"north":true,"longitude":-79.94557383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.5,"rawGPSLat":4026.47213,"rawGPSLong":7956.73443,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433338310,"topicName":"sensors/gps","sequenceNumber":553} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:38 AM","latitude":40.44119966666667,"north":true,"longitude":-79.945556,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.47198,"rawGPSLong":7956.73336,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433338808,"topicName":"sensors/gps","sequenceNumber":554} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:39 AM","latitude":40.441196833333336,"north":true,"longitude":-79.94553833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.47181,"rawGPSLong":7956.7323,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433339308,"topicName":"sensors/gps","sequenceNumber":555} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:39 AM","latitude":40.4411935,"north":true,"longitude":-79.945517,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.47161,"rawGPSLong":7956.73102,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433339808,"topicName":"sensors/gps","sequenceNumber":556} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:40 AM","latitude":40.441190166666665,"north":true,"longitude":-79.94549383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.47141,"rawGPSLong":7956.72963,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433340308,"topicName":"sensors/gps","sequenceNumber":557} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:40 AM","latitude":40.441187166666666,"north":true,"longitude":-79.9454715,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.47123,"rawGPSLong":7956.72829,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433340804,"topicName":"sensors/gps","sequenceNumber":558} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:41 AM","latitude":40.44118316666667,"north":true,"longitude":-79.94545233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.47099,"rawGPSLong":7956.72714,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433341303,"topicName":"sensors/gps","sequenceNumber":559} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:41 AM","latitude":40.441179,"north":true,"longitude":-79.945435,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.7,"rawGPSLat":4026.47074,"rawGPSLong":7956.7261,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433341788,"topicName":"sensors/gps","sequenceNumber":560} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:42 AM","latitude":40.4411755,"north":true,"longitude":-79.94541866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.47053,"rawGPSLong":7956.72512,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433342301,"topicName":"sensors/gps","sequenceNumber":561} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:42 AM","latitude":40.441171,"north":true,"longitude":-79.9453975,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.47026,"rawGPSLong":7956.72385,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433342808,"topicName":"sensors/gps","sequenceNumber":562} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:43 AM","latitude":40.441166333333335,"north":true,"longitude":-79.945373,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.46998,"rawGPSLong":7956.72238,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433343298,"topicName":"sensors/gps","sequenceNumber":563} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:43 AM","latitude":40.4411625,"north":true,"longitude":-79.94534966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.0,"rawGPSLat":4026.46975,"rawGPSLong":7956.72098,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433343821,"topicName":"sensors/gps","sequenceNumber":564} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:44 AM","latitude":40.441159166666665,"north":true,"longitude":-79.94532783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.0,"rawGPSLat":4026.46955,"rawGPSLong":7956.71967,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433344300,"topicName":"sensors/gps","sequenceNumber":565} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:44 AM","latitude":40.441156666666664,"north":true,"longitude":-79.94530783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.0,"rawGPSLat":4026.4694,"rawGPSLong":7956.71847,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433344800,"topicName":"sensors/gps","sequenceNumber":566} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:45 AM","latitude":40.44115383333333,"north":true,"longitude":-79.94528783333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.1,"rawGPSLat":4026.46923,"rawGPSLong":7956.71727,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433345301,"topicName":"sensors/gps","sequenceNumber":567} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:45 AM","latitude":40.441149,"north":true,"longitude":-79.945264,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.1,"rawGPSLat":4026.46894,"rawGPSLong":7956.71584,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433345791,"topicName":"sensors/gps","sequenceNumber":568} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:46 AM","latitude":40.44114366666667,"north":true,"longitude":-79.94523866666667,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.14,"antennaAltitude":290.0,"rawGPSLat":4026.46862,"rawGPSLong":7956.71432,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433346294,"topicName":"sensors/gps","sequenceNumber":569} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:46 AM","latitude":40.441139166666666,"north":true,"longitude":-79.94521483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.8,"rawGPSLat":4026.46835,"rawGPSLong":7956.71289,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433346798,"topicName":"sensors/gps","sequenceNumber":570} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:47 AM","latitude":40.441135,"north":true,"longitude":-79.9451925,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.4681,"rawGPSLong":7956.71155,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433347300,"topicName":"sensors/gps","sequenceNumber":571} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:47 AM","latitude":40.441130333333334,"north":true,"longitude":-79.94517183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.46782,"rawGPSLong":7956.71031,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433347803,"topicName":"sensors/gps","sequenceNumber":572} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:48 AM","latitude":40.44112583333333,"north":true,"longitude":-79.94515266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.46755,"rawGPSLong":7956.70916,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433348314,"topicName":"sensors/gps","sequenceNumber":573} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:48 AM","latitude":40.44112116666667,"north":true,"longitude":-79.94513333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.0,"rawGPSLat":4026.46727,"rawGPSLong":7956.708,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433348792,"topicName":"sensors/gps","sequenceNumber":574} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:49 AM","latitude":40.4411165,"north":true,"longitude":-79.945109,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.46699,"rawGPSLong":7956.70654,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433349307,"topicName":"sensors/gps","sequenceNumber":575} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:49 AM","latitude":40.44111183333333,"north":true,"longitude":-79.94508483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.46671,"rawGPSLong":7956.70509,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433349809,"topicName":"sensors/gps","sequenceNumber":576} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:50 AM","latitude":40.44110766666667,"north":true,"longitude":-79.94506133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.2,"rawGPSLat":4026.46646,"rawGPSLong":7956.70368,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433350301,"topicName":"sensors/gps","sequenceNumber":577} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:50 AM","latitude":40.4411025,"north":true,"longitude":-79.94504033333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.2,"rawGPSLat":4026.46615,"rawGPSLong":7956.70242,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433350803,"topicName":"sensors/gps","sequenceNumber":578} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:51 AM","latitude":40.44109783333333,"north":true,"longitude":-79.94502033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.3,"rawGPSLat":4026.46587,"rawGPSLong":7956.70122,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433351307,"topicName":"sensors/gps","sequenceNumber":579} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:51 AM","latitude":40.44109366666667,"north":true,"longitude":-79.945,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.3,"rawGPSLat":4026.46562,"rawGPSLong":7956.7,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433351803,"topicName":"sensors/gps","sequenceNumber":580} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:52 AM","latitude":40.44108933333333,"north":true,"longitude":-79.94497566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.3,"rawGPSLat":4026.46536,"rawGPSLong":7956.69854,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433352303,"topicName":"sensors/gps","sequenceNumber":581} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:52 AM","latitude":40.4410845,"north":true,"longitude":-79.94494966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.4,"rawGPSLat":4026.46507,"rawGPSLong":7956.69698,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433352792,"topicName":"sensors/gps","sequenceNumber":582} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:53 AM","latitude":40.44107916666667,"north":true,"longitude":-79.94492566666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.5,"rawGPSLat":4026.46475,"rawGPSLong":7956.69554,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433353298,"topicName":"sensors/gps","sequenceNumber":583} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:53 AM","latitude":40.44107316666667,"north":true,"longitude":-79.944903,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.7,"rawGPSLat":4026.46439,"rawGPSLong":7956.69418,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433353803,"topicName":"sensors/gps","sequenceNumber":584} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:54 AM","latitude":40.44106766666667,"north":true,"longitude":-79.94488183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.8,"rawGPSLat":4026.46406,"rawGPSLong":7956.69291,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433354288,"topicName":"sensors/gps","sequenceNumber":585} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:54 AM","latitude":40.441063166666666,"north":true,"longitude":-79.94486166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.8,"rawGPSLat":4026.46379,"rawGPSLong":7956.6917,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433354801,"topicName":"sensors/gps","sequenceNumber":586} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:55 AM","latitude":40.44105916666667,"north":true,"longitude":-79.94484066666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.7,"rawGPSLat":4026.46355,"rawGPSLong":7956.69044,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433355302,"topicName":"sensors/gps","sequenceNumber":587} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:55 AM","latitude":40.441055166666665,"north":true,"longitude":-79.94481516666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.46331,"rawGPSLong":7956.68891,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433355814,"topicName":"sensors/gps","sequenceNumber":588} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:56 AM","latitude":40.4410515,"north":true,"longitude":-79.94478883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.46309,"rawGPSLong":7956.68733,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433356296,"topicName":"sensors/gps","sequenceNumber":589} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:56 AM","latitude":40.441047833333336,"north":true,"longitude":-79.9447635,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.46287,"rawGPSLong":7956.68581,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433356809,"topicName":"sensors/gps","sequenceNumber":590} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:57 AM","latitude":40.44104316666667,"north":true,"longitude":-79.94473966666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.8,"rawGPSLat":4026.46259,"rawGPSLong":7956.68438,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433357297,"topicName":"sensors/gps","sequenceNumber":591} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:57 AM","latitude":40.4410385,"north":true,"longitude":-79.94471683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.8,"rawGPSLat":4026.46231,"rawGPSLong":7956.68301,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433357812,"topicName":"sensors/gps","sequenceNumber":592} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:58 AM","latitude":40.44103416666667,"north":true,"longitude":-79.944695,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.9,"rawGPSLat":4026.46205,"rawGPSLong":7956.6817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433358298,"topicName":"sensors/gps","sequenceNumber":593} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:58 AM","latitude":40.441030166666664,"north":true,"longitude":-79.94467183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.0,"rawGPSLat":4026.46181,"rawGPSLong":7956.68031,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433358813,"topicName":"sensors/gps","sequenceNumber":594} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:59 AM","latitude":40.4410255,"north":true,"longitude":-79.944647,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.1,"rawGPSLat":4026.46153,"rawGPSLong":7956.67882,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433359305,"topicName":"sensors/gps","sequenceNumber":595} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:55:59 AM","latitude":40.441019,"north":true,"longitude":-79.9446215,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.1,"rawGPSLat":4026.46114,"rawGPSLong":7956.67729,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433359811,"topicName":"sensors/gps","sequenceNumber":596} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:00 AM","latitude":40.441011833333334,"north":true,"longitude":-79.94459583333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.2,"rawGPSLat":4026.46071,"rawGPSLong":7956.67575,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433360295,"topicName":"sensors/gps","sequenceNumber":597} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:00 AM","latitude":40.44100566666667,"north":true,"longitude":-79.94457033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.3,"rawGPSLat":4026.46034,"rawGPSLong":7956.67422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433360807,"topicName":"sensors/gps","sequenceNumber":598} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:01 AM","latitude":40.44100016666667,"north":true,"longitude":-79.94454616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.4,"rawGPSLat":4026.46001,"rawGPSLong":7956.67277,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433361322,"topicName":"sensors/gps","sequenceNumber":599} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:01 AM","latitude":40.440995666666666,"north":true,"longitude":-79.94452233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.6,"rawGPSLat":4026.45974,"rawGPSLong":7956.67134,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433361799,"topicName":"sensors/gps","sequenceNumber":600} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:02 AM","latitude":40.440991833333335,"north":true,"longitude":-79.94449966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.6,"rawGPSLat":4026.45951,"rawGPSLong":7956.66998,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433362296,"topicName":"sensors/gps","sequenceNumber":601} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:02 AM","latitude":40.44098833333334,"north":true,"longitude":-79.94447883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.5,"rawGPSLat":4026.4593,"rawGPSLong":7956.66873,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433362808,"topicName":"sensors/gps","sequenceNumber":602} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:03 AM","latitude":40.440984666666665,"north":true,"longitude":-79.94445616666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.4,"rawGPSLat":4026.45908,"rawGPSLong":7956.66737,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433363309,"topicName":"sensors/gps","sequenceNumber":603} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:03 AM","latitude":40.440979166666665,"north":true,"longitude":-79.944428,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.5,"rawGPSLat":4026.45875,"rawGPSLong":7956.66568,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433363807,"topicName":"sensors/gps","sequenceNumber":604} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:04 AM","latitude":40.44097466666667,"north":true,"longitude":-79.944401,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.45848,"rawGPSLong":7956.66406,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433364313,"topicName":"sensors/gps","sequenceNumber":605} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:04 AM","latitude":40.44097,"north":true,"longitude":-79.94437433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.4582,"rawGPSLong":7956.66246,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433364784,"topicName":"sensors/gps","sequenceNumber":606} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:05 AM","latitude":40.440964,"north":true,"longitude":-79.94434866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.2,"rawGPSLat":4026.45784,"rawGPSLong":7956.66092,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433365308,"topicName":"sensors/gps","sequenceNumber":607} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:05 AM","latitude":40.4409585,"north":true,"longitude":-79.94432166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.45751,"rawGPSLong":7956.6593,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433365801,"topicName":"sensors/gps","sequenceNumber":608} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:06 AM","latitude":40.440953666666665,"north":true,"longitude":-79.944289,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.6,"rawGPSLat":4026.45722,"rawGPSLong":7956.65734,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433366323,"topicName":"sensors/gps","sequenceNumber":609} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:06 AM","latitude":40.440949333333336,"north":true,"longitude":-79.944256,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.4,"rawGPSLat":4026.45696,"rawGPSLong":7956.65536,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433366808,"topicName":"sensors/gps","sequenceNumber":610} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:07 AM","latitude":40.4409405,"north":true,"longitude":-79.944226,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.5,"rawGPSLat":4026.45643,"rawGPSLong":7956.65356,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433367305,"topicName":"sensors/gps","sequenceNumber":611} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:07 AM","latitude":40.44092916666666,"north":true,"longitude":-79.94419883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.7,"rawGPSLat":4026.45575,"rawGPSLong":7956.65193,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433367832,"topicName":"sensors/gps","sequenceNumber":612} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:08 AM","latitude":40.44091916666667,"north":true,"longitude":-79.94417233333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.45515,"rawGPSLong":7956.65034,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433368305,"topicName":"sensors/gps","sequenceNumber":613} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:08 AM","latitude":40.440911666666665,"north":true,"longitude":-79.94414483333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.4547,"rawGPSLong":7956.64869,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433368792,"topicName":"sensors/gps","sequenceNumber":614} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:09 AM","latitude":40.44090516666667,"north":true,"longitude":-79.94411733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.45431,"rawGPSLong":7956.64704,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433369300,"topicName":"sensors/gps","sequenceNumber":615} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:09 AM","latitude":40.44089916666667,"north":true,"longitude":-79.94408866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.1,"rawGPSLat":4026.45395,"rawGPSLong":7956.64532,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433369800,"topicName":"sensors/gps","sequenceNumber":616} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:10 AM","latitude":40.44089016666667,"north":true,"longitude":-79.94405933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.45341,"rawGPSLong":7956.64356,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433370318,"topicName":"sensors/gps","sequenceNumber":617} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:10 AM","latitude":40.440880166666666,"north":true,"longitude":-79.94402933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.45281,"rawGPSLong":7956.64176,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433370816,"topicName":"sensors/gps","sequenceNumber":618} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:11 AM","latitude":40.440870833333335,"north":true,"longitude":-79.94400083333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.6,"rawGPSLat":4026.45225,"rawGPSLong":7956.64005,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433371315,"topicName":"sensors/gps","sequenceNumber":619} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:11 AM","latitude":40.44086333333333,"north":true,"longitude":-79.94397166666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.4518,"rawGPSLong":7956.6383,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433371816,"topicName":"sensors/gps","sequenceNumber":620} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:12 AM","latitude":40.44085583333333,"north":true,"longitude":-79.943942,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.45135,"rawGPSLong":7956.63652,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433372316,"topicName":"sensors/gps","sequenceNumber":621} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:12 AM","latitude":40.440848333333335,"north":true,"longitude":-79.9439125,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.4509,"rawGPSLong":7956.63475,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433372805,"topicName":"sensors/gps","sequenceNumber":622} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:13 AM","latitude":40.440839,"north":true,"longitude":-79.9438845,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.2,"rawGPSLat":4026.45034,"rawGPSLong":7956.63307,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433373307,"topicName":"sensors/gps","sequenceNumber":623} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:13 AM","latitude":40.440833833333336,"north":true,"longitude":-79.94385833333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.1,"rawGPSLat":4026.45003,"rawGPSLong":7956.6315,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433373821,"topicName":"sensors/gps","sequenceNumber":624} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:14 AM","latitude":40.4408295,"north":true,"longitude":-79.94383216666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.44977,"rawGPSLong":7956.62993,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433374295,"topicName":"sensors/gps","sequenceNumber":625} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:14 AM","latitude":40.44082216666666,"north":true,"longitude":-79.94380283333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.44933,"rawGPSLong":7956.62817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433374823,"topicName":"sensors/gps","sequenceNumber":626} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:15 AM","latitude":40.440814,"north":true,"longitude":-79.94377433333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.6,"rawGPSLat":4026.44884,"rawGPSLong":7956.62646,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433375316,"topicName":"sensors/gps","sequenceNumber":627} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:15 AM","latitude":40.440804166666666,"north":true,"longitude":-79.94374866666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.6,"rawGPSLat":4026.44825,"rawGPSLong":7956.62492,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433375807,"topicName":"sensors/gps","sequenceNumber":628} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:16 AM","latitude":40.4407955,"north":true,"longitude":-79.94371916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.5,"rawGPSLat":4026.44773,"rawGPSLong":7956.62315,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433376305,"topicName":"sensors/gps","sequenceNumber":629} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:16 AM","latitude":40.440788833333336,"north":true,"longitude":-79.94369266666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.3,"rawGPSLat":4026.44733,"rawGPSLong":7956.62156,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433376804,"topicName":"sensors/gps","sequenceNumber":630} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:17 AM","latitude":40.440782166666665,"north":true,"longitude":-79.943666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.0,"rawGPSLat":4026.44693,"rawGPSLong":7956.61996,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433377300,"topicName":"sensors/gps","sequenceNumber":631} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:17 AM","latitude":40.44077816666667,"north":true,"longitude":-79.94363683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":290.4,"rawGPSLat":4026.44669,"rawGPSLong":7956.61821,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433377796,"topicName":"sensors/gps","sequenceNumber":632} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:18 AM","latitude":40.440773666666665,"north":true,"longitude":-79.94360633333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":289.7,"rawGPSLat":4026.44642,"rawGPSLong":7956.61638,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433378315,"topicName":"sensors/gps","sequenceNumber":633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:18 AM","latitude":40.44076866666666,"north":true,"longitude":-79.94357816666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.44612,"rawGPSLong":7956.61469,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433378800,"topicName":"sensors/gps","sequenceNumber":634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:19 AM","latitude":40.440764333333334,"north":true,"longitude":-79.94354966666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.44586,"rawGPSLong":7956.61298,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433379302,"topicName":"sensors/gps","sequenceNumber":635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:19 AM","latitude":40.440760166666664,"north":true,"longitude":-79.94352183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.44561,"rawGPSLong":7956.61131,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433379787,"topicName":"sensors/gps","sequenceNumber":636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:20 AM","latitude":40.440755,"north":true,"longitude":-79.943493,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.7,"rawGPSLat":4026.4453,"rawGPSLong":7956.60958,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433380306,"topicName":"sensors/gps","sequenceNumber":637} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:20 AM","latitude":40.4407505,"north":true,"longitude":-79.9434615,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.0,"rawGPSLat":4026.44503,"rawGPSLong":7956.60769,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433380819,"topicName":"sensors/gps","sequenceNumber":638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:21 AM","latitude":40.440745666666665,"north":true,"longitude":-79.94343183333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.7,"rawGPSLat":4026.44474,"rawGPSLong":7956.60591,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433381323,"topicName":"sensors/gps","sequenceNumber":639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:21 AM","latitude":40.44074033333333,"north":true,"longitude":-79.94340283333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.6,"rawGPSLat":4026.44442,"rawGPSLong":7956.60417,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433381791,"topicName":"sensors/gps","sequenceNumber":640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:22 AM","latitude":40.440734,"north":true,"longitude":-79.94337416666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.44404,"rawGPSLong":7956.60245,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433382304,"topicName":"sensors/gps","sequenceNumber":641} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:22 AM","latitude":40.440728166666666,"north":true,"longitude":-79.94334733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.44369,"rawGPSLong":7956.60084,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433382809,"topicName":"sensors/gps","sequenceNumber":642} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:23 AM","latitude":40.440721833333335,"north":true,"longitude":-79.94332183333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.3,"rawGPSLat":4026.44331,"rawGPSLong":7956.59931,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433383293,"topicName":"sensors/gps","sequenceNumber":643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:23 AM","latitude":40.440717166666666,"north":true,"longitude":-79.94329516666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.1,"rawGPSLat":4026.44303,"rawGPSLong":7956.59771,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433383796,"topicName":"sensors/gps","sequenceNumber":644} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:24 AM","latitude":40.440712,"north":true,"longitude":-79.94326483333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.7,"rawGPSLat":4026.44272,"rawGPSLong":7956.59589,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433384311,"topicName":"sensors/gps","sequenceNumber":645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:24 AM","latitude":40.44070516666667,"north":true,"longitude":-79.94323633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.44231,"rawGPSLong":7956.59418,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433384808,"topicName":"sensors/gps","sequenceNumber":646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:25 AM","latitude":40.44069833333333,"north":true,"longitude":-79.94320933333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.9,"rawGPSLat":4026.4419,"rawGPSLong":7956.59256,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433385309,"topicName":"sensors/gps","sequenceNumber":647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:25 AM","latitude":40.44069283333333,"north":true,"longitude":-79.94318233333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.3,"rawGPSLat":4026.44157,"rawGPSLong":7956.59094,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433385799,"topicName":"sensors/gps","sequenceNumber":648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:26 AM","latitude":40.4406865,"north":true,"longitude":-79.94315783333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.44119,"rawGPSLong":7956.58947,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433386312,"topicName":"sensors/gps","sequenceNumber":649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:26 AM","latitude":40.44068033333333,"north":true,"longitude":-79.943134,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.5,"rawGPSLat":4026.44082,"rawGPSLong":7956.58804,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433386801,"topicName":"sensors/gps","sequenceNumber":650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:27 AM","latitude":40.44067166666667,"north":true,"longitude":-79.943112,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.3,"rawGPSLat":4026.4403,"rawGPSLong":7956.58672,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433387307,"topicName":"sensors/gps","sequenceNumber":651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:27 AM","latitude":40.44066183333333,"north":true,"longitude":-79.9430875,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.2,"rawGPSLat":4026.43971,"rawGPSLong":7956.58525,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433387801,"topicName":"sensors/gps","sequenceNumber":652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:28 AM","latitude":40.4406565,"north":true,"longitude":-79.94305983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.0,"rawGPSLat":4026.43939,"rawGPSLong":7956.58359,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433388304,"topicName":"sensors/gps","sequenceNumber":653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:28 AM","latitude":40.44065233333333,"north":true,"longitude":-79.943031,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.9,"rawGPSLat":4026.43914,"rawGPSLong":7956.58186,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433388797,"topicName":"sensors/gps","sequenceNumber":654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:29 AM","latitude":40.44064783333334,"north":true,"longitude":-79.943005,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.8,"rawGPSLat":4026.43887,"rawGPSLong":7956.5803,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433389301,"topicName":"sensors/gps","sequenceNumber":655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:29 AM","latitude":40.440644,"north":true,"longitude":-79.94297866666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.6,"rawGPSLat":4026.43864,"rawGPSLong":7956.57872,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433389786,"topicName":"sensors/gps","sequenceNumber":656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:30 AM","latitude":40.44063933333333,"north":true,"longitude":-79.94295033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":287.5,"rawGPSLat":4026.43836,"rawGPSLong":7956.57702,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433390300,"topicName":"sensors/gps","sequenceNumber":657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:30 AM","latitude":40.44063583333333,"north":true,"longitude":-79.94292133333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43815,"rawGPSLong":7956.57528,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433390791,"topicName":"sensors/gps","sequenceNumber":658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:31 AM","latitude":40.44063,"north":true,"longitude":-79.94289383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.9,"rawGPSLat":4026.4378,"rawGPSLong":7956.57363,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433391300,"topicName":"sensors/gps","sequenceNumber":659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:31 AM","latitude":40.4406235,"north":true,"longitude":-79.94286716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43741,"rawGPSLong":7956.57203,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433391803,"topicName":"sensors/gps","sequenceNumber":660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:32 AM","latitude":40.4406195,"north":true,"longitude":-79.94284066666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.4,"rawGPSLat":4026.43717,"rawGPSLong":7956.57044,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433392308,"topicName":"sensors/gps","sequenceNumber":661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:32 AM","latitude":40.440615,"north":true,"longitude":-79.94281716666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.1,"rawGPSLat":4026.4369,"rawGPSLong":7956.56903,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433392799,"topicName":"sensors/gps","sequenceNumber":662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:33 AM","latitude":40.440608833333336,"north":true,"longitude":-79.94279633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.9,"rawGPSLat":4026.43653,"rawGPSLong":7956.56778,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433393303,"topicName":"sensors/gps","sequenceNumber":663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:33 AM","latitude":40.4406045,"north":true,"longitude":-79.94277133333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.4,"rawGPSLat":4026.43627,"rawGPSLong":7956.56628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433393796,"topicName":"sensors/gps","sequenceNumber":664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:34 AM","latitude":40.4406005,"north":true,"longitude":-79.94274683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.5,"rawGPSLat":4026.43603,"rawGPSLong":7956.56481,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433394304,"topicName":"sensors/gps","sequenceNumber":665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:34 AM","latitude":40.440596,"north":true,"longitude":-79.94272133333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.9,"rawGPSLat":4026.43576,"rawGPSLong":7956.56328,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433394803,"topicName":"sensors/gps","sequenceNumber":666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:35 AM","latitude":40.440591166666664,"north":true,"longitude":-79.94269683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43547,"rawGPSLong":7956.56181,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433395302,"topicName":"sensors/gps","sequenceNumber":667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:35 AM","latitude":40.44058533333333,"north":true,"longitude":-79.942672,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.6,"rawGPSLat":4026.43512,"rawGPSLong":7956.56032,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433395809,"topicName":"sensors/gps","sequenceNumber":668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:36 AM","latitude":40.44058066666667,"north":true,"longitude":-79.942646,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.8,"rawGPSLat":4026.43484,"rawGPSLong":7956.55876,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433396299,"topicName":"sensors/gps","sequenceNumber":669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:36 AM","latitude":40.44057766666667,"north":true,"longitude":-79.942618,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.2,"rawGPSLat":4026.43466,"rawGPSLong":7956.55708,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433396799,"topicName":"sensors/gps","sequenceNumber":670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:37 AM","latitude":40.44057433333333,"north":true,"longitude":-79.942592,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.8,"rawGPSLat":4026.43446,"rawGPSLong":7956.55552,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433397299,"topicName":"sensors/gps","sequenceNumber":671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:37 AM","latitude":40.44057083333333,"north":true,"longitude":-79.94256833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":285.9,"rawGPSLat":4026.43425,"rawGPSLong":7956.5541,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433397817,"topicName":"sensors/gps","sequenceNumber":672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:38 AM","latitude":40.4405675,"north":true,"longitude":-79.942548,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.3,"rawGPSLat":4026.43405,"rawGPSLong":7956.55288,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433398285,"topicName":"sensors/gps","sequenceNumber":673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:38 AM","latitude":40.440565166666666,"north":true,"longitude":-79.942532,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.2,"rawGPSLat":4026.43391,"rawGPSLong":7956.55192,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433398810,"topicName":"sensors/gps","sequenceNumber":674} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:39 AM","latitude":40.44056416666667,"north":true,"longitude":-79.94251983333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.2,"rawGPSLat":4026.43385,"rawGPSLong":7956.55119,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433399284,"topicName":"sensors/gps","sequenceNumber":675} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:39 AM","latitude":40.440563833333336,"north":true,"longitude":-79.94251166666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.3,"rawGPSLat":4026.43383,"rawGPSLong":7956.5507,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433399793,"topicName":"sensors/gps","sequenceNumber":676} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:40 AM","latitude":40.440564,"north":true,"longitude":-79.9425045,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.4,"rawGPSLat":4026.43384,"rawGPSLong":7956.55027,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433400299,"topicName":"sensors/gps","sequenceNumber":677} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:40 AM","latitude":40.440563,"north":true,"longitude":-79.94249766666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.6,"rawGPSLat":4026.43378,"rawGPSLong":7956.54986,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433400801,"topicName":"sensors/gps","sequenceNumber":678} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:41 AM","latitude":40.44056283333333,"north":true,"longitude":-79.94249016666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":286.7,"rawGPSLat":4026.43377,"rawGPSLong":7956.54941,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433401301,"topicName":"sensors/gps","sequenceNumber":679} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:41 AM","latitude":40.44055983333333,"north":true,"longitude":-79.94248366666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":287.8,"rawGPSLat":4026.43359,"rawGPSLong":7956.54902,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433401807,"topicName":"sensors/gps","sequenceNumber":680} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:42 AM","latitude":40.440558833333334,"north":true,"longitude":-79.94247633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":288.0,"rawGPSLat":4026.43353,"rawGPSLong":7956.54858,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433402302,"topicName":"sensors/gps","sequenceNumber":681} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:42 AM","latitude":40.440557166666665,"north":true,"longitude":-79.9424695,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":288.2,"rawGPSLat":4026.43343,"rawGPSLong":7956.54817,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433402791,"topicName":"sensors/gps","sequenceNumber":682} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:43 AM","latitude":40.44055566666667,"north":true,"longitude":-79.94246033333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.2,"rawGPSLat":4026.43334,"rawGPSLong":7956.54762,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433403309,"topicName":"sensors/gps","sequenceNumber":683} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:43 AM","latitude":40.4405545,"north":true,"longitude":-79.942452,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.6,"rawGPSLat":4026.43327,"rawGPSLong":7956.54712,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433403805,"topicName":"sensors/gps","sequenceNumber":684} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:44 AM","latitude":40.44055316666667,"north":true,"longitude":-79.9424435,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.8,"rawGPSLat":4026.43319,"rawGPSLong":7956.54661,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433404307,"topicName":"sensors/gps","sequenceNumber":685} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:44 AM","latitude":40.440551666666664,"north":true,"longitude":-79.94243533333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.4331,"rawGPSLong":7956.54612,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433404796,"topicName":"sensors/gps","sequenceNumber":686} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:45 AM","latitude":40.440550333333334,"north":true,"longitude":-79.94242683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.1,"rawGPSLat":4026.43302,"rawGPSLong":7956.54561,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433405304,"topicName":"sensors/gps","sequenceNumber":687} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:45 AM","latitude":40.440549833333336,"north":true,"longitude":-79.94241883333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":289.9,"rawGPSLat":4026.43299,"rawGPSLong":7956.54513,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433405805,"topicName":"sensors/gps","sequenceNumber":688} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:46 AM","latitude":40.44054833333333,"north":true,"longitude":-79.942411,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":290.3,"rawGPSLat":4026.4329,"rawGPSLong":7956.54466,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433406309,"topicName":"sensors/gps","sequenceNumber":689} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:46 AM","latitude":40.44054716666667,"north":true,"longitude":-79.942403,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":290.5,"rawGPSLat":4026.43283,"rawGPSLong":7956.54418,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433406808,"topicName":"sensors/gps","sequenceNumber":690} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:47 AM","latitude":40.44054633333333,"north":true,"longitude":-79.94239633333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":290.7,"rawGPSLat":4026.43278,"rawGPSLong":7956.54378,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433407311,"topicName":"sensors/gps","sequenceNumber":691} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:47 AM","latitude":40.440546,"north":true,"longitude":-79.942391,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":290.7,"rawGPSLat":4026.43276,"rawGPSLong":7956.54346,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433407800,"topicName":"sensors/gps","sequenceNumber":692} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:48 AM","latitude":40.440546,"north":true,"longitude":-79.9423865,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.0,"rawGPSLat":4026.43276,"rawGPSLong":7956.54319,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433408314,"topicName":"sensors/gps","sequenceNumber":693} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:48 AM","latitude":40.440546166666664,"north":true,"longitude":-79.94238383333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.1,"rawGPSLat":4026.43277,"rawGPSLong":7956.54303,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433408823,"topicName":"sensors/gps","sequenceNumber":694} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:49 AM","latitude":40.440546166666664,"north":true,"longitude":-79.942382,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":291.2,"rawGPSLat":4026.43277,"rawGPSLong":7956.54292,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433409313,"topicName":"sensors/gps","sequenceNumber":695} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:49 AM","latitude":40.440546166666664,"north":true,"longitude":-79.94238016666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.4,"rawGPSLat":4026.43277,"rawGPSLong":7956.54281,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433409801,"topicName":"sensors/gps","sequenceNumber":696} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:50 AM","latitude":40.440545666666665,"north":true,"longitude":-79.94237866666667,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":291.7,"rawGPSLat":4026.43274,"rawGPSLong":7956.54272,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433410303,"topicName":"sensors/gps","sequenceNumber":697} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:50 AM","latitude":40.440545166666666,"north":true,"longitude":-79.94237766666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":291.9,"rawGPSLat":4026.43271,"rawGPSLong":7956.54266,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433410808,"topicName":"sensors/gps","sequenceNumber":698} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:51 AM","latitude":40.440545,"north":true,"longitude":-79.9423755,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.1,"rawGPSLat":4026.4327,"rawGPSLong":7956.54253,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433411310,"topicName":"sensors/gps","sequenceNumber":699} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:51 AM","latitude":40.44054466666667,"north":true,"longitude":-79.94237066666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.3,"rawGPSLat":4026.43268,"rawGPSLong":7956.54224,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433411803,"topicName":"sensors/gps","sequenceNumber":700} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:52 AM","latitude":40.440544,"north":true,"longitude":-79.94236516666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.4,"rawGPSLat":4026.43264,"rawGPSLong":7956.54191,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433412303,"topicName":"sensors/gps","sequenceNumber":701} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:52 AM","latitude":40.44054333333333,"north":true,"longitude":-79.94235833333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.5,"rawGPSLat":4026.4326,"rawGPSLong":7956.5415,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433412796,"topicName":"sensors/gps","sequenceNumber":702} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:53 AM","latitude":40.440542666666666,"north":true,"longitude":-79.94235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.6,"rawGPSLat":4026.43256,"rawGPSLong":7956.541,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433413303,"topicName":"sensors/gps","sequenceNumber":703} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:53 AM","latitude":40.44054216666667,"north":true,"longitude":-79.942341,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.7,"rawGPSLat":4026.43253,"rawGPSLong":7956.54046,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433413803,"topicName":"sensors/gps","sequenceNumber":704} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:54 AM","latitude":40.440541833333334,"north":true,"longitude":-79.942332,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":292.8,"rawGPSLat":4026.43251,"rawGPSLong":7956.53992,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433414309,"topicName":"sensors/gps","sequenceNumber":705} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:54 AM","latitude":40.44054166666667,"north":true,"longitude":-79.9423225,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":292.9,"rawGPSLat":4026.4325,"rawGPSLong":7956.53935,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433414815,"topicName":"sensors/gps","sequenceNumber":706} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:55 AM","latitude":40.44054166666667,"north":true,"longitude":-79.94231266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.0,"rawGPSLat":4026.4325,"rawGPSLong":7956.53876,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433415307,"topicName":"sensors/gps","sequenceNumber":707} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:55 AM","latitude":40.440542,"north":true,"longitude":-79.942303,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.0,"rawGPSLat":4026.43252,"rawGPSLong":7956.53818,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433415805,"topicName":"sensors/gps","sequenceNumber":708} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:56 AM","latitude":40.44054216666667,"north":true,"longitude":-79.94229466666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.1,"rawGPSLat":4026.43253,"rawGPSLong":7956.53768,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433416307,"topicName":"sensors/gps","sequenceNumber":709} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:56 AM","latitude":40.4405425,"north":true,"longitude":-79.94228683333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.1,"rawGPSLat":4026.43255,"rawGPSLong":7956.53721,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433416811,"topicName":"sensors/gps","sequenceNumber":710} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:57 AM","latitude":40.44054283333333,"north":true,"longitude":-79.94227916666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.2,"rawGPSLat":4026.43257,"rawGPSLong":7956.53675,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433417293,"topicName":"sensors/gps","sequenceNumber":711} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:57 AM","latitude":40.440543166666664,"north":true,"longitude":-79.94227133333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.3,"rawGPSLat":4026.43259,"rawGPSLong":7956.53628,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433417808,"topicName":"sensors/gps","sequenceNumber":712} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:58 AM","latitude":40.4405435,"north":true,"longitude":-79.9422645,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.4,"rawGPSLat":4026.43261,"rawGPSLong":7956.53587,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433418298,"topicName":"sensors/gps","sequenceNumber":713} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:58 AM","latitude":40.440544,"north":true,"longitude":-79.94225816666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.5,"rawGPSLat":4026.43264,"rawGPSLong":7956.53549,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433418812,"topicName":"sensors/gps","sequenceNumber":714} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:59 AM","latitude":40.4405445,"north":true,"longitude":-79.94225166666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.6,"rawGPSLat":4026.43267,"rawGPSLong":7956.5351,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433419299,"topicName":"sensors/gps","sequenceNumber":715} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:56:59 AM","latitude":40.440545,"north":true,"longitude":-79.94224616666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":293.8,"rawGPSLat":4026.4327,"rawGPSLong":7956.53477,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433419797,"topicName":"sensors/gps","sequenceNumber":716} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:00 AM","latitude":40.44054533333333,"north":true,"longitude":-79.942242,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":293.9,"rawGPSLat":4026.43272,"rawGPSLong":7956.53452,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433420305,"topicName":"sensors/gps","sequenceNumber":717} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:00 AM","latitude":40.4405455,"north":true,"longitude":-79.9422395,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.0,"rawGPSLat":4026.43273,"rawGPSLong":7956.53437,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433420815,"topicName":"sensors/gps","sequenceNumber":718} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:01 AM","latitude":40.4405455,"north":true,"longitude":-79.94223833333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.2,"rawGPSLat":4026.43273,"rawGPSLong":7956.5343,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433421321,"topicName":"sensors/gps","sequenceNumber":719} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:01 AM","latitude":40.4405455,"north":true,"longitude":-79.94223733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.3,"rawGPSLat":4026.43273,"rawGPSLong":7956.53424,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433421816,"topicName":"sensors/gps","sequenceNumber":720} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:02 AM","latitude":40.44054533333333,"north":true,"longitude":-79.942237,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.5,"rawGPSLat":4026.43272,"rawGPSLong":7956.53422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433422309,"topicName":"sensors/gps","sequenceNumber":721} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:02 AM","latitude":40.44054533333333,"north":true,"longitude":-79.942237,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.5,"rawGPSLat":4026.43272,"rawGPSLong":7956.53422,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433422809,"topicName":"sensors/gps","sequenceNumber":722} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:03 AM","latitude":40.4405455,"north":true,"longitude":-79.94223733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.6,"rawGPSLat":4026.43273,"rawGPSLong":7956.53424,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433423303,"topicName":"sensors/gps","sequenceNumber":723} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:03 AM","latitude":40.44054633333333,"north":true,"longitude":-79.94223733333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":294.6,"rawGPSLat":4026.43278,"rawGPSLong":7956.53424,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433423805,"topicName":"sensors/gps","sequenceNumber":724} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:04 AM","latitude":40.440549,"north":true,"longitude":-79.9422355,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.07,"antennaAltitude":294.6,"rawGPSLat":4026.43294,"rawGPSLong":7956.53413,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433424293,"topicName":"sensors/gps","sequenceNumber":725} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:04 AM","latitude":40.44055216666667,"north":true,"longitude":-79.94223383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.7,"rawGPSLat":4026.43313,"rawGPSLong":7956.53403,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433424815,"topicName":"sensors/gps","sequenceNumber":726} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:05 AM","latitude":40.440556,"north":true,"longitude":-79.94223266666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.7,"rawGPSLat":4026.43336,"rawGPSLong":7956.53396,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433425295,"topicName":"sensors/gps","sequenceNumber":727} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:05 AM","latitude":40.44055933333333,"north":true,"longitude":-79.9422305,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.8,"rawGPSLat":4026.43356,"rawGPSLong":7956.53383,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433425810,"topicName":"sensors/gps","sequenceNumber":728} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:06 AM","latitude":40.4405625,"north":true,"longitude":-79.94222866666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":294.9,"rawGPSLat":4026.43375,"rawGPSLong":7956.53372,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433426295,"topicName":"sensors/gps","sequenceNumber":729} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:06 AM","latitude":40.4405645,"north":true,"longitude":-79.94222683333334,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.0,"rawGPSLat":4026.43387,"rawGPSLong":7956.53361,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433426808,"topicName":"sensors/gps","sequenceNumber":730} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:07 AM","latitude":40.4405655,"north":true,"longitude":-79.94222566666667,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.1,"rawGPSLat":4026.43393,"rawGPSLong":7956.53354,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433427315,"topicName":"sensors/gps","sequenceNumber":731} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:07 AM","latitude":40.440566,"north":true,"longitude":-79.94222466666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.3,"rawGPSLat":4026.43396,"rawGPSLong":7956.53348,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433427826,"topicName":"sensors/gps","sequenceNumber":732} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:08 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222433333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.4,"rawGPSLat":4026.43395,"rawGPSLong":7956.53346,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433428310,"topicName":"sensors/gps","sequenceNumber":733} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:08 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942224,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.5,"rawGPSLat":4026.43395,"rawGPSLong":7956.53344,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433428794,"topicName":"sensors/gps","sequenceNumber":734} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:09 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222383333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.6,"rawGPSLat":4026.43395,"rawGPSLong":7956.53343,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433429312,"topicName":"sensors/gps","sequenceNumber":735} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:09 AM","latitude":40.44056583333333,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.7,"rawGPSLat":4026.43395,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433429810,"topicName":"sensors/gps","sequenceNumber":736} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:10 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":295.7,"rawGPSLat":4026.43394,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433430306,"topicName":"sensors/gps","sequenceNumber":737} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:10 AM","latitude":40.440565666666664,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43394,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433430810,"topicName":"sensors/gps","sequenceNumber":738} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:11 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43395,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433431319,"topicName":"sensors/gps","sequenceNumber":739} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:11 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43395,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433431801,"topicName":"sensors/gps","sequenceNumber":740} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:12 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433432313,"topicName":"sensors/gps","sequenceNumber":741} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:12 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.8,"rawGPSLat":4026.43394,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433432809,"topicName":"sensors/gps","sequenceNumber":742} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:13 AM","latitude":40.440565666666664,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.9,"rawGPSLat":4026.43394,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433433314,"topicName":"sensors/gps","sequenceNumber":743} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:13 AM","latitude":40.440565666666664,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":295.9,"rawGPSLat":4026.43394,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433433827,"topicName":"sensors/gps","sequenceNumber":744} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:14 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.0,"rawGPSLat":4026.43394,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433434311,"topicName":"sensors/gps","sequenceNumber":745} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:14 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.0,"rawGPSLat":4026.43394,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433434806,"topicName":"sensors/gps","sequenceNumber":746} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:15 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":9,"horizontalDilutionOfPrecision":0.99,"antennaAltitude":296.1,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433435309,"topicName":"sensors/gps","sequenceNumber":747} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:15 AM","latitude":40.44056583333333,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.1,"rawGPSLat":4026.43395,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433435800,"topicName":"sensors/gps","sequenceNumber":748} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:16 AM","latitude":40.440565666666664,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43394,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433436309,"topicName":"sensors/gps","sequenceNumber":749} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:16 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433436800,"topicName":"sensors/gps","sequenceNumber":750} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:17 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433437305,"topicName":"sensors/gps","sequenceNumber":751} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:17 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433437783,"topicName":"sensors/gps","sequenceNumber":752} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:18 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433438294,"topicName":"sensors/gps","sequenceNumber":753} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:18 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433438802,"topicName":"sensors/gps","sequenceNumber":754} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:19 AM","latitude":40.44056583333333,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.2,"rawGPSLat":4026.43395,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433439295,"topicName":"sensors/gps","sequenceNumber":755} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:19 AM","latitude":40.440566,"north":true,"longitude":-79.94222316666666,"west":true,"qualityValue":1,"numSatellites":8,"horizontalDilutionOfPrecision":1.42,"antennaAltitude":296.3,"rawGPSLat":4026.43396,"rawGPSLong":7956.53339,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433439810,"topicName":"sensors/gps","sequenceNumber":756} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:20 AM","latitude":40.440566,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.7,"antennaAltitude":296.3,"rawGPSLat":4026.43396,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433440299,"topicName":"sensors/gps","sequenceNumber":757} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:20 AM","latitude":40.44056633333334,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":6,"horizontalDilutionOfPrecision":1.7,"antennaAltitude":296.3,"rawGPSLat":4026.43398,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433440789,"topicName":"sensors/gps","sequenceNumber":758} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:21 AM","latitude":40.4405665,"north":true,"longitude":-79.9422235,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.4,"rawGPSLat":4026.43399,"rawGPSLong":7956.53341,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433441295,"topicName":"sensors/gps","sequenceNumber":759} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:21 AM","latitude":40.44056666666667,"north":true,"longitude":-79.94222333333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.5,"rawGPSLat":4026.434,"rawGPSLong":7956.5334,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433441798,"topicName":"sensors/gps","sequenceNumber":760} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:22 AM","latitude":40.440566833333335,"north":true,"longitude":-79.942223,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.25,"antennaAltitude":296.6,"rawGPSLat":4026.43401,"rawGPSLong":7956.53338,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433442292,"topicName":"sensors/gps","sequenceNumber":761} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:22 AM","latitude":40.44056716666667,"north":true,"longitude":-79.94222283333333,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.7,"rawGPSLat":4026.43403,"rawGPSLong":7956.53337,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433442793,"topicName":"sensors/gps","sequenceNumber":762} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:23 AM","latitude":40.440567333333334,"north":true,"longitude":-79.94222266666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.7,"rawGPSLat":4026.43404,"rawGPSLong":7956.53336,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433443283,"topicName":"sensors/gps","sequenceNumber":763} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:23 AM","latitude":40.440567333333334,"north":true,"longitude":-79.9422225,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.8,"rawGPSLat":4026.43404,"rawGPSLong":7956.53335,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433443793,"topicName":"sensors/gps","sequenceNumber":764} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:24 AM","latitude":40.4405675,"north":true,"longitude":-79.94222216666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":296.9,"rawGPSLat":4026.43405,"rawGPSLong":7956.53333,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433444293,"topicName":"sensors/gps","sequenceNumber":765} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:24 AM","latitude":40.44056766666667,"north":true,"longitude":-79.94222166666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.0,"rawGPSLat":4026.43406,"rawGPSLong":7956.5333,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433444802,"topicName":"sensors/gps","sequenceNumber":766} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:25 AM","latitude":40.44056766666667,"north":true,"longitude":-79.94222116666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.1,"rawGPSLat":4026.43406,"rawGPSLong":7956.53327,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433445292,"topicName":"sensors/gps","sequenceNumber":767} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:25 AM","latitude":40.44056783333333,"north":true,"longitude":-79.94222083333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.1,"rawGPSLat":4026.43407,"rawGPSLong":7956.53325,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433445801,"topicName":"sensors/gps","sequenceNumber":768} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:26 AM","latitude":40.440568,"north":true,"longitude":-79.94222033333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.2,"rawGPSLat":4026.43408,"rawGPSLong":7956.53322,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433446288,"topicName":"sensors/gps","sequenceNumber":769} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:26 AM","latitude":40.440568166666665,"north":true,"longitude":-79.94222033333334,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.2,"rawGPSLat":4026.43409,"rawGPSLong":7956.53322,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433446795,"topicName":"sensors/gps","sequenceNumber":770} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:27 AM","latitude":40.440568166666665,"north":true,"longitude":-79.94222,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.71,"antennaAltitude":297.3,"rawGPSLat":4026.43409,"rawGPSLong":7956.5332,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433447297,"topicName":"sensors/gps","sequenceNumber":771} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Nov 6, 2016 6:57:27 AM","latitude":40.440568,"north":true,"longitude":-79.94221966666667,"west":true,"qualityValue":1,"numSatellites":7,"horizontalDilutionOfPrecision":1.25,"antennaAltitude":297.3,"rawGPSLat":4026.43408,"rawGPSLong":7956.53318,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1478433447788,"topicName":"sensors/gps","sequenceNumber":772} diff --git a/offline/controller/waypoints_tri.txt b/offline/controller/waypoints_tri.txt deleted file mode 100644 index 764280d0..00000000 --- a/offline/controller/waypoints_tri.txt +++ /dev/null @@ -1,372 +0,0 @@ -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286683333333,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022628} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286683333333,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022632} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022632} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022632} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4428675,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867666666665,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867666666665,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286783333333,"north":true,"longitude":-79.94273966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442868,"north":true,"longitude":-79.94273933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022633} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442868,"north":true,"longitude":-79.942739,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286833333334,"north":true,"longitude":-79.94273866666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.9427385,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286833333334,"north":true,"longitude":-79.94273833333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286833333334,"north":true,"longitude":-79.9427385,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.9427385,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022634} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.94273883333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286816666666,"north":true,"longitude":-79.942739,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442868,"north":true,"longitude":-79.942739,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286783333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442867666666665,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022635} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286733333333,"north":true,"longitude":-79.94273916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286683333333,"north":true,"longitude":-79.9427395,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44286533333333,"north":true,"longitude":-79.94274033333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442863,"north":true,"longitude":-79.94274166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442860333333336,"north":true,"longitude":-79.9427435,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022636} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442856666666664,"north":true,"longitude":-79.9427455,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44285266666667,"north":true,"longitude":-79.94274783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442848,"north":true,"longitude":-79.9427505,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44284283333333,"north":true,"longitude":-79.9427535,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44283733333333,"north":true,"longitude":-79.94275683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022637} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44283166666666,"north":true,"longitude":-79.94276066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44282616666667,"north":true,"longitude":-79.94276483333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4428205,"north":true,"longitude":-79.94276883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442815,"north":true,"longitude":-79.942773,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44280933333334,"north":true,"longitude":-79.942777,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44280366666667,"north":true,"longitude":-79.942781,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442797666666664,"north":true,"longitude":-79.94278533333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442791666666665,"north":true,"longitude":-79.94278966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427855,"north":true,"longitude":-79.942794,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022638} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44277316666667,"north":true,"longitude":-79.9428025,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442767,"north":true,"longitude":-79.94280666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442761,"north":true,"longitude":-79.9428105,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442755,"north":true,"longitude":-79.9428145,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427485,"north":true,"longitude":-79.94281866666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442742,"north":true,"longitude":-79.94282283333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442735666666664,"north":true,"longitude":-79.94282683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442729166666666,"north":true,"longitude":-79.94283083333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44272266666667,"north":true,"longitude":-79.942835,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442715666666665,"north":true,"longitude":-79.94283916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442709,"north":true,"longitude":-79.94284333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022639} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44270216666666,"north":true,"longitude":-79.94284716666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426955,"north":true,"longitude":-79.94285116666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44268866666667,"north":true,"longitude":-79.94285516666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44268183333333,"north":true,"longitude":-79.942859,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442675,"north":true,"longitude":-79.94286333333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442668,"north":true,"longitude":-79.94286733333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022640} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442660833333335,"north":true,"longitude":-79.94287066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44265383333333,"north":true,"longitude":-79.94287383333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426465,"north":true,"longitude":-79.94287683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44263933333333,"north":true,"longitude":-79.94287966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022641} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442632,"north":true,"longitude":-79.94288266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022642} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442625,"north":true,"longitude":-79.94288533333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022642} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261783333334,"north":true,"longitude":-79.942888,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022642} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261066666667,"north":true,"longitude":-79.94289066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44260333333333,"north":true,"longitude":-79.94289316666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44259616666667,"north":true,"longitude":-79.9428955,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258883333333,"north":true,"longitude":-79.94289783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442581833333335,"north":true,"longitude":-79.94290016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425745,"north":true,"longitude":-79.942902,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022643} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425675,"north":true,"longitude":-79.942904,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44256016666667,"north":true,"longitude":-79.94290583333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44255283333333,"north":true,"longitude":-79.9429075,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44254566666667,"north":true,"longitude":-79.942909,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44253833333333,"north":true,"longitude":-79.94291066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022644} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442531,"north":true,"longitude":-79.94291183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442524,"north":true,"longitude":-79.94291316666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442517,"north":true,"longitude":-79.94291416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425095,"north":true,"longitude":-79.94291516666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44250233333333,"north":true,"longitude":-79.9429165,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44249533333333,"north":true,"longitude":-79.94291766666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44248833333333,"north":true,"longitude":-79.942919,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424815,"north":true,"longitude":-79.94292033333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022645} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424745,"north":true,"longitude":-79.94292166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44246733333333,"north":true,"longitude":-79.94292266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44246016666666,"north":true,"longitude":-79.94292383333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442452833333334,"north":true,"longitude":-79.942925,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44244533333333,"north":true,"longitude":-79.94292583333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44243783333334,"north":true,"longitude":-79.94292683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022646} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44243016666667,"north":true,"longitude":-79.9429275,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44242233333333,"north":true,"longitude":-79.94292783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442415,"north":true,"longitude":-79.94292866666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44240683333334,"north":true,"longitude":-79.9429285,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44239983333333,"north":true,"longitude":-79.94292883333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423925,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44238516666667,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022647} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423785,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44236383333333,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44235716666667,"north":true,"longitude":-79.94292966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44235066666667,"north":true,"longitude":-79.94292966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442344166666665,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423375,"north":true,"longitude":-79.9429295,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423305,"north":true,"longitude":-79.94292916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022648} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44232366666667,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44231666666667,"north":true,"longitude":-79.942929,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423095,"north":true,"longitude":-79.94292916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422945,"north":true,"longitude":-79.94293033333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442287666666665,"north":true,"longitude":-79.94293133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44228066666667,"north":true,"longitude":-79.94293266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442274,"north":true,"longitude":-79.94293416666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442267333333334,"north":true,"longitude":-79.94293616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022649} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442260833333336,"north":true,"longitude":-79.94293833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44225433333333,"north":true,"longitude":-79.942941,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224783333333,"north":true,"longitude":-79.94294333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224116666667,"north":true,"longitude":-79.94294566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442234,"north":true,"longitude":-79.942948,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442227,"north":true,"longitude":-79.94295016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442220666666664,"north":true,"longitude":-79.9429525,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44221433333333,"north":true,"longitude":-79.94295466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422085,"north":true,"longitude":-79.9429575,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022650} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422025,"north":true,"longitude":-79.94296066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442196,"north":true,"longitude":-79.94296416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218983333333,"north":true,"longitude":-79.9429685,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421835,"north":true,"longitude":-79.942973,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421775,"north":true,"longitude":-79.94297833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217233333333,"north":true,"longitude":-79.9429845,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442168333333335,"north":true,"longitude":-79.94299083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216466666666,"north":true,"longitude":-79.94299733333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022651} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216133333333,"north":true,"longitude":-79.94300416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442158166666665,"north":true,"longitude":-79.943012,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215533333333,"north":true,"longitude":-79.94302083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442153,"north":true,"longitude":-79.94302966666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442151333333335,"north":true,"longitude":-79.943039,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442150166666664,"north":true,"longitude":-79.94304816666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421485,"north":true,"longitude":-79.94305716666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214733333333,"north":true,"longitude":-79.94306616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214616666667,"north":true,"longitude":-79.94307566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022652} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214566666667,"north":true,"longitude":-79.943085,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214633333333,"north":true,"longitude":-79.94310033333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442147166666665,"north":true,"longitude":-79.94310633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44214816666667,"north":true,"longitude":-79.94311216666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421495,"north":true,"longitude":-79.94311933333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215116666667,"north":true,"longitude":-79.9431275,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215283333333,"north":true,"longitude":-79.94313633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442154333333335,"north":true,"longitude":-79.9431455,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442156,"north":true,"longitude":-79.943155,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44215766666667,"north":true,"longitude":-79.9431645,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421595,"north":true,"longitude":-79.94317433333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022653} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216133333333,"north":true,"longitude":-79.943184,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421635,"north":true,"longitude":-79.94319366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442165333333335,"north":true,"longitude":-79.94320283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216683333333,"north":true,"longitude":-79.94321183333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44216866666667,"north":true,"longitude":-79.94322116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217066666667,"north":true,"longitude":-79.94323066666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217233333333,"north":true,"longitude":-79.94323983333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421745,"north":true,"longitude":-79.94324933333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442176833333335,"north":true,"longitude":-79.94325883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44217866666666,"north":true,"longitude":-79.943269,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022654} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421805,"north":true,"longitude":-79.9432785,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218216666667,"north":true,"longitude":-79.94328783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218383333333,"north":true,"longitude":-79.94329683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44218583333333,"north":true,"longitude":-79.94330616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421875,"north":true,"longitude":-79.9433155,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421895,"north":true,"longitude":-79.943325,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442191666666666,"north":true,"longitude":-79.94333466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421935,"north":true,"longitude":-79.943344,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4421955,"north":true,"longitude":-79.94335383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44219733333333,"north":true,"longitude":-79.94336366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442199333333335,"north":true,"longitude":-79.94337366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022655} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220166666667,"north":true,"longitude":-79.94338333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442203666666664,"north":true,"longitude":-79.94339316666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220583333333,"north":true,"longitude":-79.94340233333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220766666667,"north":true,"longitude":-79.94341183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44220966666666,"north":true,"longitude":-79.94342133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44221183333333,"north":true,"longitude":-79.94343066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442214,"north":true,"longitude":-79.94344016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442216,"north":true,"longitude":-79.9434495,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442218,"north":true,"longitude":-79.94345933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44222,"north":true,"longitude":-79.94346883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022656} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442221833333335,"north":true,"longitude":-79.94347866666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44222383333334,"north":true,"longitude":-79.94348816666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442225666666666,"north":true,"longitude":-79.94349733333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442227833333334,"north":true,"longitude":-79.94350683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223,"north":true,"longitude":-79.94351583333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223216666666,"north":true,"longitude":-79.94352533333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022657} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442234166666665,"north":true,"longitude":-79.94353416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223616666667,"north":true,"longitude":-79.943543,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44223816666667,"north":true,"longitude":-79.94355183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224033333333,"north":true,"longitude":-79.94356083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224233333333,"north":true,"longitude":-79.94356983333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224416666667,"north":true,"longitude":-79.94357866666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022658} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44224583333333,"north":true,"longitude":-79.94358783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422475,"north":true,"longitude":-79.94359666666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442249333333336,"north":true,"longitude":-79.94360566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442251,"north":true,"longitude":-79.9436145,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442252833333335,"north":true,"longitude":-79.94362366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422545,"north":true,"longitude":-79.9436325,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422565,"north":true,"longitude":-79.94364166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442258333333335,"north":true,"longitude":-79.94365083333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442260166666664,"north":true,"longitude":-79.94366,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442262166666666,"north":true,"longitude":-79.943669,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022659} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442264333333334,"north":true,"longitude":-79.94367783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442267,"north":true,"longitude":-79.9436865,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44227,"north":true,"longitude":-79.94369466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4422735,"north":true,"longitude":-79.94370216666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44227766666667,"north":true,"longitude":-79.94370816666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44228233333333,"north":true,"longitude":-79.94371333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442287666666665,"north":true,"longitude":-79.94371783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442293666666664,"north":true,"longitude":-79.94372133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442307166666666,"north":true,"longitude":-79.94372616666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022660} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44231416666667,"north":true,"longitude":-79.943727,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442321166666666,"north":true,"longitude":-79.94372666666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442328,"north":true,"longitude":-79.94372466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442335166666666,"north":true,"longitude":-79.94372166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44234183333333,"north":true,"longitude":-79.9437185,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442347833333336,"north":true,"longitude":-79.94371483333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423535,"north":true,"longitude":-79.943711,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44235883333333,"north":true,"longitude":-79.94370666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442364166666664,"north":true,"longitude":-79.9437005,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442369166666666,"north":true,"longitude":-79.94369316666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442373,"north":true,"longitude":-79.94368533333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442376333333335,"north":true,"longitude":-79.9436775,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022661} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423795,"north":true,"longitude":-79.94366916666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4423825,"north":true,"longitude":-79.94366016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44238633333333,"north":true,"longitude":-79.94365183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44239066666667,"north":true,"longitude":-79.943644,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442395166666664,"north":true,"longitude":-79.94363566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44239966666667,"north":true,"longitude":-79.94362783333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44240466666667,"north":true,"longitude":-79.94361983333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424095,"north":true,"longitude":-79.943612,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44241433333333,"north":true,"longitude":-79.94360383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442419,"north":true,"longitude":-79.94359583333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022662} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44242416666667,"north":true,"longitude":-79.943588,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442429,"north":true,"longitude":-79.94358016666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442433666666666,"north":true,"longitude":-79.94357216666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424385,"north":true,"longitude":-79.94356466666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44244333333334,"north":true,"longitude":-79.94355683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44244833333333,"north":true,"longitude":-79.943549,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442453666666665,"north":true,"longitude":-79.94354116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44245866666667,"north":true,"longitude":-79.94353366666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424635,"north":true,"longitude":-79.943526,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44246866666667,"north":true,"longitude":-79.94351833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442473666666665,"north":true,"longitude":-79.94351066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4424785,"north":true,"longitude":-79.94350283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022663} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442483333333335,"north":true,"longitude":-79.94349466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442488166666664,"north":true,"longitude":-79.94348666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442493166666665,"north":true,"longitude":-79.94347866666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442497833333334,"north":true,"longitude":-79.94347033333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442502833333336,"north":true,"longitude":-79.94346216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442512666666666,"north":true,"longitude":-79.943446,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442517333333335,"north":true,"longitude":-79.94343783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44252216666667,"north":true,"longitude":-79.94342983333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442527,"north":true,"longitude":-79.943422,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022664} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442531833333334,"north":true,"longitude":-79.943414,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442536833333335,"north":true,"longitude":-79.94340633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44254183333334,"north":true,"longitude":-79.94339883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442547,"north":true,"longitude":-79.94339133333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442552166666665,"north":true,"longitude":-79.943384,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44255733333333,"north":true,"longitude":-79.9433765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44256216666667,"north":true,"longitude":-79.943369,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442567,"north":true,"longitude":-79.94336116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4425715,"north":true,"longitude":-79.94335333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442575833333336,"north":true,"longitude":-79.9433455,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258,"north":true,"longitude":-79.94333783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022665} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258416666667,"north":true,"longitude":-79.94333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44258816666667,"north":true,"longitude":-79.94332233333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442592166666664,"north":true,"longitude":-79.94331433333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442596,"north":true,"longitude":-79.943306,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426,"north":true,"longitude":-79.94329783333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44260383333334,"north":true,"longitude":-79.94328966666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442607333333335,"north":true,"longitude":-79.94328133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442611166666666,"north":true,"longitude":-79.94327316666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261433333333,"north":true,"longitude":-79.943265,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44261783333334,"north":true,"longitude":-79.943257,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442621333333335,"north":true,"longitude":-79.94324883333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44262533333333,"north":true,"longitude":-79.943241,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022666} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442629333333336,"north":true,"longitude":-79.94323266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44263316666667,"north":true,"longitude":-79.94322466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442637,"north":true,"longitude":-79.9432165,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442640833333336,"north":true,"longitude":-79.943208,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442645166666665,"north":true,"longitude":-79.94319933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44264916666667,"north":true,"longitude":-79.94319066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44265333333333,"north":true,"longitude":-79.94318233333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426575,"north":true,"longitude":-79.94317433333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4426615,"north":true,"longitude":-79.94316616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44266533333333,"north":true,"longitude":-79.94315833333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442669333333335,"north":true,"longitude":-79.94315016666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022667} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442673,"north":true,"longitude":-79.94314183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442677,"north":true,"longitude":-79.94313333333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442685,"north":true,"longitude":-79.94311633333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44269283333333,"north":true,"longitude":-79.94309933333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442696833333336,"north":true,"longitude":-79.943091,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442700333333335,"north":true,"longitude":-79.94308233333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44270383333333,"north":true,"longitude":-79.94307383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442707666666664,"north":true,"longitude":-79.94306533333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442712,"north":true,"longitude":-79.9430575,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44272083333333,"north":true,"longitude":-79.94304116666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427255,"north":true,"longitude":-79.94303283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44272983333333,"north":true,"longitude":-79.94302483333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427345,"north":true,"longitude":-79.94301633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022668} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442739,"north":true,"longitude":-79.94300833333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442743,"north":true,"longitude":-79.9429995,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274733333334,"north":true,"longitude":-79.94299133333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427515,"north":true,"longitude":-79.94298283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427545,"north":true,"longitude":-79.942977,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442756333333335,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757,"north":true,"longitude":-79.9429725,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757,"north":true,"longitude":-79.9429725,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757,"north":true,"longitude":-79.94297283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275716666667,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275733333333,"north":true,"longitude":-79.94297366666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757666666665,"north":true,"longitude":-79.94297416666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757666666665,"north":true,"longitude":-79.9429745,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758,"north":true,"longitude":-79.94297483333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022669} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758166666664,"north":true,"longitude":-79.94297516666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758833333336,"north":true,"longitude":-79.9429755,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759,"north":true,"longitude":-79.942976,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759333333335,"north":true,"longitude":-79.94297633333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427595,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442759,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758833333336,"north":true,"longitude":-79.94297666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275866666667,"north":true,"longitude":-79.94297683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442758,"north":true,"longitude":-79.94297683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442757666666665,"north":true,"longitude":-79.94297683333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275716666667,"north":true,"longitude":-79.94297666666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442756833333334,"north":true,"longitude":-79.9429765,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022670} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442756333333335,"north":true,"longitude":-79.94297616666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275583333334,"north":true,"longitude":-79.942976,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275533333333,"north":true,"longitude":-79.94297566666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275483333333,"north":true,"longitude":-79.94297533333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275433333333,"north":true,"longitude":-79.942975,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275366666667,"north":true,"longitude":-79.94297466666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275316666667,"north":true,"longitude":-79.94297433333334,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442752666666664,"north":true,"longitude":-79.942974,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275233333333,"north":true,"longitude":-79.94297383333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275183333333,"north":true,"longitude":-79.94297366666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427515,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275116666667,"north":true,"longitude":-79.94297333333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442750833333335,"north":true,"longitude":-79.94297316666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022671} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275066666667,"north":true,"longitude":-79.942973,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442750333333336,"north":true,"longitude":-79.94297283333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275,"north":true,"longitude":-79.94297266666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274983333333,"north":true,"longitude":-79.94297233333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749166666665,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274883333333,"north":true,"longitude":-79.942972,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427485,"north":true,"longitude":-79.94297166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274816666667,"north":true,"longitude":-79.9429715,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442747833333335,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274766666667,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427475,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274766666667,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022672} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427475,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274766666667,"north":true,"longitude":-79.9429715,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442748,"north":true,"longitude":-79.94297183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274816666667,"north":true,"longitude":-79.94297183333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274866666667,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44274933333333,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749666666664,"north":true,"longitude":-79.94297216666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442749666666664,"north":true,"longitude":-79.942972,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.44275016666667,"north":true,"longitude":-79.94297166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.4427505,"north":true,"longitude":-79.94297166666666,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442750833333335,"north":true,"longitude":-79.94297133333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442751,"north":true,"longitude":-79.94297116666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022673} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442751333333334,"north":true,"longitude":-79.94297083333333,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442751666666666,"north":true,"longitude":-79.94297066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442752166666665,"north":true,"longitude":-79.94297066666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Mar 20, 2016 2:40:22 AM","latitude":40.442752,"north":true,"longitude":-79.94297016666667,"west":true,"qualityValue":0,"numSatellites":0,"horizontalDilutionOfPrecision":0.0,"antennaAltitude":0.0,"rawGPSLat":0.0,"rawGPSLong":0.0,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1458456022674} \ No newline at end of file From 500b0cc8ea420814e43e213082664285c6b35425 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sat, 20 May 2017 17:38:15 -0400 Subject: [PATCH 133/149] removing waypoint .txt files --- .../java/com/roboclub/robobuggy/main/RobobuggyMainFile.java | 3 ++- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java index 97a5a13f..8e864a23 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyMainFile.java @@ -1,6 +1,7 @@ package com.roboclub.robobuggy.main; import com.roboclub.robobuggy.robots.AbstractRobot; +import com.roboclub.robobuggy.robots.PlayBackRobot; import com.roboclub.robobuggy.robots.SimRobot; import com.roboclub.robobuggy.ui.Gui; import com.roboclub.robobuggy.utilities.JNISetup; @@ -30,7 +31,7 @@ public static void main(String[] args) { RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE); - robot = SimRobot.getInstance(); + robot = PlayBackRobot.getInstance(); new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE); Gui.getInstance(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 023bdfe7..1fb8aee9 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -87,7 +87,7 @@ private double purePursuitController() { double velocity = pose.getCurrentState().get(2, 0); double lookaheadLowerBound = 3.0; double lookaheadUpperBound = 25.0; - double lookahead = K * velocity; + double lookahead = (K * velocity)/2; if(lookahead < lookaheadLowerBound) { lookahead = lookaheadLowerBound; } From c1e15badc48cfc26b2567e1a4f39ea26c6a48dd3 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sat, 20 May 2017 18:42:33 -0400 Subject: [PATCH 134/149] removing duplicate method --- .../roboclub/robobuggy/messages/GpsMeasurement.java | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java index 671d2c1a..33e4f193 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GpsMeasurement.java @@ -163,17 +163,4 @@ public boolean getNorth() { public GPSPoseMessage toGpsPoseMessage(double heading) { return new GPSPoseMessage(gpsTimestamp, latitude, longitude, heading); } - - /** - * evaluates to the distance between two gps points based on an L2 metric - * - * @param a the first gps point - * @param b the second gps point - * @return the distance in meters - */ - public static double getDistance(GpsMeasurement a, GpsMeasurement b) { - double dx = LocalizerUtil.convertLonToMeters(a.getLongitude() - b.getLongitude()); - double dy = LocalizerUtil.convertLatToMeters(a.getLatitude() - b.getLatitude()); - return Math.sqrt(dx * dx + dy * dy); - } } \ No newline at end of file From 2482b7f0a2ff71e20a413be827e72acdf6a59605 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sat, 20 May 2017 18:59:50 -0400 Subject: [PATCH 135/149] eliminating magic nums in simrobot --- .../src/main/java/com/roboclub/robobuggy/robots/SimRobot.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java index d5431e07..2d44b189 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java @@ -34,6 +34,8 @@ */ public final class SimRobot extends AbstractRobot { private static SimRobot instance; + private static double INIT_LATITUDE = 40.441670; + private static double INIT_LONGITUDE = -79.9416362; /** * Returns a reference to the one instance of the {@link Robot} object. @@ -51,7 +53,7 @@ public static AbstractRobot getInstance() { private SimRobot() { super(); - nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(40.441670, -79.9416362))); + nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(INIT_LATITUDE, INIT_LONGITUDE))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); From 60ec6e6f2772e5e711a315ac4b00605f343f4e09 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sat, 20 May 2017 19:08:18 -0400 Subject: [PATCH 136/149] removed localizer.java --- offline/localizer/localizer.java | 355 ------------------------------- 1 file changed, 355 deletions(-) delete mode 100644 offline/localizer/localizer.java diff --git a/offline/localizer/localizer.java b/offline/localizer/localizer.java deleted file mode 100644 index c696bba7..00000000 --- a/offline/localizer/localizer.java +++ /dev/null @@ -1,355 +0,0 @@ -package com.roboclub.robobuggy.nodes.localizers; - -import Jama.Matrix; -import com.roboclub.robobuggy.messages.EncoderMeasurement; -import com.roboclub.robobuggy.messages.GPSPoseMessage; -import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.messages.SteeringMeasurement; -import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; -import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode; -import com.roboclub.robobuggy.ros.Message; -import com.roboclub.robobuggy.ros.MessageListener; -import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ros.Publisher; -import com.roboclub.robobuggy.ros.Subscriber; - -import java.util.Date; - - -/** - * localizes using a kalman filter - */ -public class KfLocalizer extends PeriodicNode { - private Publisher posePub; - private Matrix state; - private double lastEncoderReading; - private long lastEncoderReadingTime; - private Matrix covariance; - private UTMTuple lastGPS; - private Matrix motionMatrix; - private Date mostRecentUpdateTime; - private Matrix predictCovariance; - private double wheelBase; - - /** - * Constructor for the kfLocalizer, setup and starts the kf running at a - * fixed period - * - * @param period the period in milliseconds between pose outputs of the kf - */ - public KfLocalizer(int period) { - super(new BuggyBaseNode(NodeChannel.POSE), period, "KF"); - posePub = new Publisher(NodeChannel.POSE.getMsgPath()); - wheelBase = 1.13; // meters - - double[][] startCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_b - { 0, 0, 0, 1, 0, 0, 0 }, // y_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - covariance = new Matrix(startCovariance); - // state [x,y,x_b_dot,y_b_dot,th,th_dot,gamma] - LocTuple startLatLng = new LocTuple(40.441670, -79.9416362); - UTMTuple startUTM = LocalizerUtil.deg2UTM(startLatLng); - lastGPS = startUTM; - lastEncoderReadingTime = new Date().getTime(); - mostRecentUpdateTime = new Date(); - double startAngle = 250; - - double[][] start = { { startUTM.getEasting() }, // X meters 0 - { startUTM.getNorthing() }, // Y meters 1 - { 0 }, // x_b_dot 2 - { 0 }, // y_b_dot 3 - { startAngle }, // th degree 4 - { 0 }, // th_dot degrees/second 5 - { 0 } // heading degree 6 - }; - state = new Matrix(start); - - double[][] predictCovarianceArray = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_b - { 0, 0, 0, 1, 0, 0, 0 }, // y_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - predictCovariance = new Matrix(predictCovarianceArray); - - // Initialize subscriber to GPS measurements - new Subscriber("htGpsLoc", NodeChannel.GPS.getMsgPath(), - new MessageListener() { - @Override - public synchronized void actionPerformed(String topicName, Message m) { - GpsMeasurement newGPSData = (GpsMeasurement) m; - LocTuple gpsLatLng = new LocTuple(newGPSData - .getLatitude(), newGPSData.getLongitude()); - UTMTuple gpsUTM = LocalizerUtil.deg2UTM(gpsLatLng); - double dx = gpsUTM.getEasting() - lastGPS.getEasting(); - double dy = gpsUTM.getNorthing() - lastGPS.getNorthing(); - double th = Math.toDegrees(Math.atan2(dy, dx)); - lastGPS = gpsUTM; - - double[][] observationModel = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - // don't update angle if we did not move a lot - if (Math.sqrt(dx * dx + dy * dy) < .5) {// || - // Math.abs(th- - // state.get(4, - // 0)) > 90){ - observationModel[4][4] = 0; - } - - if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) - + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { - th = startAngle; - } - - double[][] meassurement = { { gpsUTM.getEasting() }, - { gpsUTM.getNorthing() }, { 0 }, { 0 }, { th }, - { 0 }, { 0 } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), new Matrix( - meassurement), new Matrix(updateCovariance)); - } - }); - - /* - new Subscriber( - "HighTrustGpsLoc", - NodeChannel.IMU_ANG_POS.getMsgPath(), - ((topicName, m) -> { - - IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); - // double y = mes.getRot()[0][1]; - // double x = mes.getRot()[0][0]; - double[][] xVar = { { 1 }, { 0 }, { 0 } }; - double[][] yVar = { { 0 }, { 1 }, { 0 } }; - Matrix xMat = new Matrix(xVar); - Matrix yMat = new Matrix(yVar); - Matrix rot = new Matrix(mes.getRot()); - double x = rot.times(xMat).get(0, 0); - double y = rot.times(yMat).get(0, 0); - double th = -(Math.toDegrees(Math.atan2(y, x)) - 90); - double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { th }, { 0 }, { 0 } }; - double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - // updateStep(new Matrix(observationModel),new - // Matrix(meassurement),new Matrix(updateCovariance)); - - })); - */ - - // TODO note that we will probably run into precision errors since the - // changes are so small - // would be good to batch up the encoder updates until we get a margin - // that we know can be represented proeprly - new Subscriber("htGpsLoc", NodeChannel.ENCODER.getMsgPath(), - new MessageListener() { - @Override - public synchronized void actionPerformed(String topicName, - Message m) { - EncoderMeasurement measurement = (EncoderMeasurement) m; - - // convert the feet from the last message into a delta - // degree, and update our position - double currentEncoderMeasurement = measurement - .getDistance(); - double deltaDistance = currentEncoderMeasurement - - lastEncoderReading; - long currentTime = new Date().getTime(); - long dt = currentTime - lastEncoderReadingTime; - if (dt > 1) { // to remove numeric instability - double bodySpeed = deltaDistance / (dt / 1000.0); - lastEncoderReadingTime = currentTime; - lastEncoderReading = currentEncoderMeasurement; - - double[][] observationModel = { - { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - double[][] meassurement = { { 0 }, { 0 }, - { bodySpeed }, { 0 }, { 0 }, { 0 }, { 0 } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), - new Matrix(meassurement), new Matrix( - updateCovariance)); - } - } - }); - - new Subscriber("htGpsLoc", NodeChannel.STEERING.getMsgPath(), - new MessageListener() { - - @Override - public synchronized void actionPerformed(String topicName, - Message m) { - SteeringMeasurement steerM = (SteeringMeasurement) m; - double[][] observationModel = { - { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // Heading - }; - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { 0 }, { 0 }, { steerM.getAngle() } }; - double[][] updateCovariance = { - { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - updateStep(new Matrix(observationModel), new Matrix( - meassurement), new Matrix(updateCovariance)); - } - }); - resume(); - - } - - private synchronized void updateStep(Matrix observationMatrix, - Matrix measurement, Matrix updateCovariance) { - predictStep(); - - // Matrix update = observationMatrix.times(state); - Matrix inovation = measurement.minus(observationMatrix.times(state)); - - for (int i = 4; i < 7; i++) { - if (inovation.get(i, 0) > 180) { - inovation.set(i, 0, -360 + inovation.get(i, 0)); - } else if (inovation.get(i, 0) < -180) { - inovation.set(i, 0, 360 + inovation.get(i, 0)); - } - } - - // Matrix innovation2 = - // observationMatrix.minus(measurement.times(state)); - // Matrix inovation = (innovation.normF() > innovation2.normF()) ? - // innovation2 : innovation; - Matrix innovationCovariance = observationMatrix.times(covariance) - .times(observationMatrix.transpose()).plus(updateCovariance); - Matrix kalmanGain = covariance.times(observationMatrix.transpose()) - .times(innovationCovariance.inverse()); - state = state.plus(kalmanGain.times(inovation)); - covariance = (Matrix.identity(covariance.getRowDimension(), - covariance.getColumnDimension()).minus(kalmanGain - .times(observationMatrix))); - - } - - @Override - protected synchronized void update() { - predictStep(); - // publish state - UTMTuple currentLatLng = new UTMTuple(17, 'T', state.get(0, 0), - state.get(1, 0)); - LocTuple latLng = LocalizerUtil.utm2Deg(currentLatLng); - posePub.publish(new GPSPoseMessage(new Date(), latLng.getLatitude(), - latLng.getLongitude(), state.get(4, 0))); - } - - @Override - protected boolean startDecoratorNode() { - // TODO Auto-generated method stub - return false; - } - - @Override - protected boolean shutdownDecoratorNode() { - // TODO Auto-generated method stub - return false; - } - - /** - * TODO - */ - // TODO - private synchronized void predictStep() { - Date now = new Date(); - double diff = (now.getTime() - mostRecentUpdateTime.getTime()) / 1000.0; - mostRecentUpdateTime = now; - double th = Math.toRadians(state.get(4, 0)); - double heading = Math.toRadians(state.get(6, 0)); - double[][] motionModel = { - // x y x_b y_b th th_dot heading - { 1, 0, Math.cos(th) * diff, -Math.sin(th) * diff, 0, 0, 0 }, // x - { 0, 1, Math.sin(th) * diff, Math.cos(th) * diff, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_b - { 0, 0, 0, 1, 0, 0, 0 }, // y_b - { 0, 0, 0, 0, 1, diff, 0 }, // th - { 0, 0, 180 / (Math.PI * (wheelBase / Math.sin(heading))), 0, - 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - motionMatrix = new Matrix(motionModel); - state = motionMatrix.times(state); - covariance = predictCovariance.times(covariance).times( - predictCovariance.transpose()); - for (int i = 4; i < 7; i++) { - while (state.get(i, 0) < -180) { - state.set(i, 0, state.get(i, 0) + 360); - } - while (state.get(i, 0) > 180) { - state.set(i, 0, state.get(i, 0) - 360); - } - } - } - -} From d484efbe79b357323d1c326d8e883646a84acb83 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sat, 20 May 2017 23:15:09 -0400 Subject: [PATCH 137/149] fixing magic nums again --- .../com/roboclub/robobuggy/main/RobobuggyConfigFile.java | 2 ++ .../main/java/com/roboclub/robobuggy/robots/SimRobot.java | 7 ++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java index c894d38b..6be9e7a4 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java @@ -32,6 +32,8 @@ public final class RobobuggyConfigFile { public static final boolean LOGGING = true; public static final String LOG_FILE_LOCATION = "logs"; public static final String LOG_FILE_NAME = "sensors"; + public static final double LATITUDE = 40.441670; + public static final double LONGITUDE = -79.9416362; //Autonomous controls public static final int RBSM_COMMAND_PERIOD = 50; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java index 2d44b189..cc46913f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java @@ -26,6 +26,9 @@ import java.io.FileNotFoundException; import java.util.ArrayList; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.LATITUDE; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.LONGITUDE; + /** * A robot file for a simulated robot that can be used for internal testing of nodes along simulated paths @@ -34,8 +37,6 @@ */ public final class SimRobot extends AbstractRobot { private static SimRobot instance; - private static double INIT_LATITUDE = 40.441670; - private static double INIT_LONGITUDE = -79.9416362; /** * Returns a reference to the one instance of the {@link Robot} object. @@ -53,7 +54,7 @@ public static AbstractRobot getInstance() { private SimRobot() { super(); - nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(INIT_LATITUDE, INIT_LONGITUDE))); + nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(LATITUDE, LONGITUDE))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); From c84c36b9f345cc251ed3deec61ad5a6dc7b8697b Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sun, 21 May 2017 12:00:59 -0400 Subject: [PATCH 138/149] renaming constants --- .../com/roboclub/robobuggy/main/RobobuggyConfigFile.java | 4 ++-- .../main/java/com/roboclub/robobuggy/robots/SimRobot.java | 5 ++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java index 6be9e7a4..02e136df 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/main/RobobuggyConfigFile.java @@ -32,8 +32,8 @@ public final class RobobuggyConfigFile { public static final boolean LOGGING = true; public static final String LOG_FILE_LOCATION = "logs"; public static final String LOG_FILE_NAME = "sensors"; - public static final double LATITUDE = 40.441670; - public static final double LONGITUDE = -79.9416362; + public static final double INITIAL_POS_LAT = 40.441670; + public static final double INITIAL_POS_LON = -79.9416362; //Autonomous controls public static final int RBSM_COMMAND_PERIOD = 50; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java index cc46913f..9c82b848 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java @@ -26,8 +26,7 @@ import java.io.FileNotFoundException; import java.util.ArrayList; -import static com.roboclub.robobuggy.main.RobobuggyConfigFile.LATITUDE; -import static com.roboclub.robobuggy.main.RobobuggyConfigFile.LONGITUDE; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.*; /** @@ -54,7 +53,7 @@ public static AbstractRobot getInstance() { private SimRobot() { super(); - nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(LATITUDE, LONGITUDE))); + nodeList.add(new FullSimRunner("Full Sim Toolbox", new LocTuple(INITIAL_POS_LAT, INITIAL_POS_LON))); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); From d9b6f1d2c6b42b9d34b8683f48e26d95dcacf046 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sun, 21 May 2017 12:07:45 -0400 Subject: [PATCH 139/149] took care of more constants --- .../java/com/roboclub/robobuggy/robots/TransistorAuton.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 40c55068..09262a75 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -22,7 +22,7 @@ import java.io.IOException; import java.util.ArrayList; - +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.*; /** * A robot class for having transistor drive itself * @@ -59,7 +59,7 @@ private TransistorAuton() { new RobobuggyLogicNotification("Logic Exception Setup properly", RobobuggyMessageLevel.NOTE); // Initialize Nodes - nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(40.441670, -79.9416362))); + nodeList.add(new RobobuggyKFLocalizer(10, "Robobuggy KF Localizer", new LocTuple(INITIAL_POS_LAT, INITIAL_POS_LON))); nodeList.add(new GpsNode(NodeChannel.GPS, RobobuggyConfigFile.getComPortGPS())); nodeList.add(new LoggingNode(NodeChannel.GUI_LOGGING_BUTTON, RobobuggyConfigFile.LOG_FILE_LOCATION, NodeChannel.getLoggingChannels())); From 0783c184be82473be0ee341cba2ac582ca6227f2 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sun, 21 May 2017 13:50:10 -0400 Subject: [PATCH 140/149] deleted unnecessary folder --- .../Demos/src/listStuff/ListDemo.java | 217 ------------------ 1 file changed, 217 deletions(-) delete mode 100644 real_time/surface_src/java_src/Demos/src/listStuff/ListDemo.java diff --git a/real_time/surface_src/java_src/Demos/src/listStuff/ListDemo.java b/real_time/surface_src/java_src/Demos/src/listStuff/ListDemo.java deleted file mode 100644 index 000dc8a1..00000000 --- a/real_time/surface_src/java_src/Demos/src/listStuff/ListDemo.java +++ /dev/null @@ -1,217 +0,0 @@ -package listStuff; - -import java.awt.*; -import java.awt.event.*; -import javax.swing.*; -import javax.swing.event.*; - -/* ListDemo.java requires no other files. */ -public class ListDemo extends JPanel - implements ListSelectionListener { - private JList list; - private DefaultListModel listModel; - - private static final String hireString = "Hire"; - private static final String fireString = "Fire"; - private JButton fireButton; - private JTextField employeeName; - - public ListDemo() { - super(new BorderLayout()); - - listModel = new DefaultListModel(); - listModel.addElement("Jane Doe"); - listModel.addElement("John Smith"); - listModel.addElement("Kathy Green"); - - //Create the list and put it in a scroll pane. - list = new JList(listModel); - list.setSelectionMode(ListSelectionModel.SINGLE_SELECTION); - list.setSelectedIndex(0); - list.addListSelectionListener(this); - list.setVisibleRowCount(5); - JScrollPane listScrollPane = new JScrollPane(list); - - JButton hireButton = new JButton(hireString); - HireListener hireListener = new HireListener(hireButton); - hireButton.setActionCommand(hireString); - hireButton.addActionListener(hireListener); - hireButton.setEnabled(false); - - fireButton = new JButton(fireString); - fireButton.setActionCommand(fireString); - fireButton.addActionListener(new FireListener()); - - employeeName = new JTextField(10); - employeeName.addActionListener(hireListener); - employeeName.getDocument().addDocumentListener(hireListener); - String name = listModel.getElementAt( - list.getSelectedIndex()).toString(); - - //Create a panel that uses BoxLayout. - JPanel buttonPane = new JPanel(); - buttonPane.setLayout(new BoxLayout(buttonPane, - BoxLayout.LINE_AXIS)); - buttonPane.add(fireButton); - buttonPane.add(Box.createHorizontalStrut(5)); - buttonPane.add(new JSeparator(SwingConstants.VERTICAL)); - buttonPane.add(Box.createHorizontalStrut(5)); - buttonPane.add(employeeName); - buttonPane.add(hireButton); - buttonPane.setBorder(BorderFactory.createEmptyBorder(5,5,5,5)); - - add(listScrollPane, BorderLayout.CENTER); - add(buttonPane, BorderLayout.PAGE_END); - } - - class FireListener implements ActionListener { - public void actionPerformed(ActionEvent e) { - //This method can be called only if - //there's a valid selection - //so go ahead and remove whatever's selected. - int index = list.getSelectedIndex(); - listModel.remove(index); - - int size = listModel.getSize(); - - if (size == 0) { //Nobody's left, disable firing. - fireButton.setEnabled(false); - - } else { //Select an index. - if (index == listModel.getSize()) { - //removed item in last position - index--; - } - - list.setSelectedIndex(index); - list.ensureIndexIsVisible(index); - } - } - } - - //This listener is shared by the text field and the hire button. - class HireListener implements ActionListener, DocumentListener { - private boolean alreadyEnabled = false; - private JButton button; - - public HireListener(JButton button) { - this.button = button; - } - - //Required by ActionListener. - public void actionPerformed(ActionEvent e) { - String name = employeeName.getText(); - - //User didn't type in a unique name... - if (name.equals("") || alreadyInList(name)) { - Toolkit.getDefaultToolkit().beep(); - employeeName.requestFocusInWindow(); - employeeName.selectAll(); - return; - } - - int index = list.getSelectedIndex(); //get selected index - if (index == -1) { //no selection, so insert at beginning - index = 0; - } else { //add after the selected item - index++; - } - - listModel.insertElementAt(employeeName.getText(), index); - //If we just wanted to add to the end, we'd do this: - //listModel.addElement(employeeName.getText()); - - //Reset the text field. - employeeName.requestFocusInWindow(); - employeeName.setText(""); - - //Select the new item and make it visible. - list.setSelectedIndex(index); - list.ensureIndexIsVisible(index); - } - - //This method tests for string equality. You could certainly - //get more sophisticated about the algorithm. For example, - //you might want to ignore white space and capitalization. - protected boolean alreadyInList(String name) { - return listModel.contains(name); - } - - //Required by DocumentListener. - public void insertUpdate(DocumentEvent e) { - enableButton(); - } - - //Required by DocumentListener. - public void removeUpdate(DocumentEvent e) { - handleEmptyTextField(e); - } - - //Required by DocumentListener. - public void changedUpdate(DocumentEvent e) { - if (!handleEmptyTextField(e)) { - enableButton(); - } - } - - private void enableButton() { - if (!alreadyEnabled) { - button.setEnabled(true); - } - } - - private boolean handleEmptyTextField(DocumentEvent e) { - if (e.getDocument().getLength() <= 0) { - button.setEnabled(false); - alreadyEnabled = false; - return true; - } - return false; - } - } - - //This method is required by ListSelectionListener. - public void valueChanged(ListSelectionEvent e) { - if (e.getValueIsAdjusting() == false) { - - if (list.getSelectedIndex() == -1) { - //No selection, disable fire button. - fireButton.setEnabled(false); - - } else { - //Selection, enable the fire button. - fireButton.setEnabled(true); - } - } - } - - /** - * Create the GUI and show it. For thread safety, - * this method should be invoked from the - * event-dispatching thread. - */ - private static void createAndShowGUI() { - //Create and set up the window. - JFrame frame = new JFrame("ListDemo"); - frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - - //Create and set up the content pane. - JComponent newContentPane = new ListDemo(); - newContentPane.setOpaque(true); //content panes must be opaque - frame.setContentPane(newContentPane); - - //Display the window. - frame.pack(); - frame.setVisible(true); - } - - public static void main(String[] args) { - //Schedule a job for the event-dispatching thread: - //creating and showing this application's GUI. - javax.swing.SwingUtilities.invokeLater(new Runnable() { - public void run() { - createAndShowGUI(); - } - }); - } -} From d82c62857a427ed8dfc14a275beede980c4de3d5 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Sun, 21 May 2017 23:19:57 -0400 Subject: [PATCH 141/149] changing localizer tester so correct getDistance method is used --- .../roboclub/robobuggy/simulation/LocalizerTester.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index 3f65ce99..2d6ca923 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -1,6 +1,7 @@ package com.roboclub.robobuggy.simulation; import com.roboclub.robobuggy.messages.EncoderMeasurement; +import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; @@ -48,9 +49,12 @@ public LocalizerTester(String name, ArrayList waypoints) { public GpsMeasurement getTargetWaypoint() { GpsMeasurement currentTarget = waypoints.get(targetWaypointIndex); - double distInM = (GpsMeasurement.getDistance(currentTarget, new GpsMeasurement(currentPosition.getLatitude(), - currentPosition - .getLongitude()))); + GpsMeasurement currentPositionMeas = new GpsMeasurement(currentPosition.getLatitude(), currentPosition + .getLongitude()); + GPSPoseMessage currentTargetPM = currentTarget.toGpsPoseMessage(0); + GPSPoseMessage currentPositionPM = currentPositionMeas.toGpsPoseMessage(0); + + double distInM = (GPSPoseMessage.getDistance(currentTargetPM, currentPositionPM )); if (distInM < 5) { targetWaypointIndex++; } From 5dd245ef51280f6bca2758ee68f513a7a9fca920 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Mon, 22 May 2017 23:29:27 -0400 Subject: [PATCH 142/149] fixing style on most things --- .../messages/DriveControlMessage.java | 14 ++++++++++++++ .../robobuggy/messages/GPSPoseMessage.java | 9 +++++++++ .../planners/WayPointFollowerPlanner.java | 8 ++++---- .../robobuggy/robots/CommTestRobot.java | 14 ++++++++++++-- .../robots/ControllerTesterRobot.java | 8 +++++++- .../robots/LocalizerTesterRobot.java | 10 ++++++++++ .../roboclub/robobuggy/robots/SimRobot.java | 3 ++- .../robobuggy/robots/TransistorAuton.java | 3 ++- .../simulation/ControllerTester.java | 19 ++++++++++--------- .../robobuggy/simulation/FullSimRunner.java | 3 ++- .../robobuggy/simulation/LocalizerTester.java | 5 +++++ .../localizers/RobobuggyKFLocalizerTest.java | 10 ++++++++-- 12 files changed, 85 insertions(+), 21 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java index ecce2953..ff2cc553 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/DriveControlMessage.java @@ -26,6 +26,13 @@ public DriveControlMessage(Date timestamp, double angle) { this.timestamp = new Date(timestamp.getTime()).getTime(); } + /** + * Construct a new DriveControlMessage + * + * @param timestamp {@link Date} representing the creation time + * @param angle the commanded angle of the front wheel + * @param currentWaypoint gps measurement representing the current waypoint + */ public DriveControlMessage(Date timestamp, double angle, GpsMeasurement currentWaypoint) { this.angle = angle; this.timestamp = new Date(timestamp.getTime()).getTime(); @@ -41,6 +48,13 @@ public double getAngleDouble() { return angle; } + /** + * Returns the GpsMeasurement representing the current waypoint + * from this DriveControlMessage + * + * @return the GpsMeasurement representing the current waypoint + * from this DriveControlMessage + */ public GpsMeasurement getWaypoint() { return currentWaypoint; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java index b026f0f5..6a8ff943 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/GPSPoseMessage.java @@ -19,6 +19,14 @@ public class GPSPoseMessage extends BaseMessage { private final double velocity; private final double heading; + /** + * Constructs a new {@link GPSPoseMessage} + * + * @param timestamp {@link Date} representing the creation time + * @param latitude of the buggy (negative is South) + * @param longitude of the buggy (negative is West) + * @param heading of the buggy (in degrees from North) + */ public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading) { this(timestamp, latitude, longitude, heading, 0.0); } @@ -30,6 +38,7 @@ public GPSPoseMessage(Date timestamp, double latitude, double longitude, double * @param latitude of the buggy (negative is South) * @param longitude of the buggy (negative is West) * @param heading of the buggy (in degrees from North) + * @param velocity of the buggy */ public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, double velocity) { this.latitude = latitude; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index fc009511..1bdcbfd2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -22,7 +22,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private GpsMeasurement target; // we only want to look at the next 10 waypoints as possible candidates - private final static int WAYPOINT_LOOKAHEAD_MAX = 50; + private final int wayPointLOOKAHEADMAX = 50; /** * @param wayPoints the list of waypoints to follow @@ -50,7 +50,7 @@ private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this int closestIndex = lastClosestIndex; - for (int i = lastClosestIndex; i < (lastClosestIndex + WAYPOINT_LOOKAHEAD_MAX) && i < wayPoints.size(); i++) { + for (int i = lastClosestIndex; i < (lastClosestIndex + wayPointLOOKAHEADMAX) && i < wayPoints.size(); i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { @@ -80,11 +80,11 @@ private double purePursuitController() { int closestIndex = getClosestIndex(wayPoints, pose); - double K = 2.5; + double k = 2.5; double velocity = pose.getVelocity(); double lookaheadLowerBound = 5.0; double lookaheadUpperBound = 25.0; - double lookahead = (K * velocity)/2; + double lookahead = (k * velocity)/2; if(lookahead < lookaheadLowerBound) { lookahead = lookaheadLowerBound; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java index a29cb429..647db616 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/CommTestRobot.java @@ -4,15 +4,25 @@ import com.roboclub.robobuggy.nodes.planners.SweepNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ui.*; +import com.roboclub.robobuggy.ui.RobobuggyJFrame; +import com.roboclub.robobuggy.ui.RobobuggyGUITabs; +import com.roboclub.robobuggy.ui.Gui; +import com.roboclub.robobuggy.ui.MainGuiWindow; + /** * Created by Robot on 1/24/2017. */ -public class CommTestRobot extends AbstractRobot { +public final class CommTestRobot extends AbstractRobot { private static CommTestRobot instance; private static final int ARDUINO_BOOTLOADER_TIMEOUT_MS = 2000; + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ public static CommTestRobot getInstance() { if (instance == null) { instance = new CommTestRobot(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index 91674c86..634604bc 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -19,9 +19,15 @@ /** * Created by vivaanbahl on 1/26/17. */ -public class ControllerTesterRobot extends AbstractRobot { +public final class ControllerTesterRobot extends AbstractRobot { private static ControllerTesterRobot instance; + /** + * Returns a reference to the one instance of the {@link Robot} object. + * If no instance exists, a new one is created. + * + * @return a reference to the one instance of the {@link Robot} object + */ public static ControllerTesterRobot getInstance() { if (instance == null) { instance = new ControllerTesterRobot(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java index d11a2ef7..d91082aa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/LocalizerTesterRobot.java @@ -23,6 +23,11 @@ public class LocalizerTesterRobot extends AbstractRobot { private static LocalizerTesterRobot instance; + /** + * Returns a reference to the one instance of the {@link Robot} object. + * + * @return a reference to the one instance of the {@link Robot} object + */ protected LocalizerTesterRobot() { super(); @@ -46,6 +51,11 @@ protected LocalizerTesterRobot() { tabs.addTab(new ConfigurationPanel(), "Configuration"); } + /** + * Returns a reference to the one instance of the {@link Robot} object. + * + * @return a reference to the one instance of the {@link Robot} object + */ public static LocalizerTesterRobot getInstance() { if (instance == null) { instance = new LocalizerTesterRobot(); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java index 9c82b848..baf05e51 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/SimRobot.java @@ -26,7 +26,8 @@ import java.io.FileNotFoundException; import java.util.ArrayList; -import static com.roboclub.robobuggy.main.RobobuggyConfigFile.*; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LAT; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LON; /** diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 09262a75..fc421c02 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -22,7 +22,8 @@ import java.io.IOException; import java.util.ArrayList; -import static com.roboclub.robobuggy.main.RobobuggyConfigFile.*; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LAT; +import static com.roboclub.robobuggy.main.RobobuggyConfigFile.INITIAL_POS_LON; /** * A robot class for having transistor drive itself * diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index 3f258d83..14646b66 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -32,8 +32,8 @@ public class ControllerTester extends PeriodicNode { // assume we are facing up hill 1 private static final double INITIAL_HEADING_RAD = Math.toRadians(250); - private Matrix X; - private Matrix A; + private Matrix matX; + private Matrix matA; private double commandedSteeringAngle = 0; private Publisher simulatedPosePub; private Publisher gpspub; @@ -43,13 +43,14 @@ public class ControllerTester extends PeriodicNode { /** * Create a new {@link PeriodicNode} decorator * - * @param name + * @param name - name of tester + * @param initialPosition - represents initial position of tester */ public ControllerTester(String name, LocTuple initialPosition) { super(new BuggyBaseNode(NodeChannel.AUTO), SIM_PERIOD, name); LocTuple firstPosition = initialPosition; - double[][] XAsDoubleArr = { + double[][] xAsDoubleArr = { { LocalizerUtil.deg2UTM(firstPosition).getEasting() }, { LocalizerUtil.deg2UTM(firstPosition).getNorthing() }, { VELOCITY }, @@ -57,7 +58,7 @@ public ControllerTester(String name, LocTuple initialPosition) { { 0 } }; - X = new Matrix(XAsDoubleArr); + matX = new Matrix(xAsDoubleArr); new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); @@ -72,17 +73,17 @@ public ControllerTester(String name, LocTuple initialPosition) { protected void update() { simCounter++; - A = getNewModel(X); + matA = getNewModel(matX); // update simulated position - X = A.times(X); + matX = matA.times(matX); - UTMTuple t = new UTMTuple(17, 'T', X.get(0, 0), X.get(1, 0)); + UTMTuple t = new UTMTuple(17, 'T', matX.get(0, 0), matX.get(1, 0)); LocTuple lt = LocalizerUtil.utm2Deg(t); // if it's time to run the controller, update the controller's understanding of where we are if (simCounter == CONTROLLER_PERIOD) { - simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), X.get(3, 0), X.get(2, 0))); + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), matX.get(3, 0), matX.get(2, 0))); simCounter = 0; } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index 2043bca0..afd9c5c8 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -55,7 +55,8 @@ public class FullSimRunner extends BuggyBaseNode { /** * Create a new {@link PeriodicNode} decorator * - * @param name + * @param name - name of full simulation runner + * @param initialPos - initial position of full simulation runner */ public FullSimRunner(String name, LocTuple initialPos) { super(NodeChannel.SIMULATION); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java index 2d6ca923..455570bb 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/LocalizerTester.java @@ -38,6 +38,7 @@ public class LocalizerTester extends BuggyDecoratorNode { * Creates a new decorator for the given Node * * @param name the name we want for this node to store so that it can be referenced later + * @param waypoints the array list of GpsMeasurements representing the waypoints on the map */ public LocalizerTester(String name, ArrayList waypoints) { super(new BuggyBaseNode(NodeChannel.POSE), name); @@ -47,6 +48,10 @@ public LocalizerTester(String name, ArrayList waypoints) { this.waypoints = waypoints; } + /** + * returns the target waypoint + * @return a GpsMeasurement representing the waypoint + */ public GpsMeasurement getTargetWaypoint() { GpsMeasurement currentTarget = waypoints.get(targetWaypointIndex); GpsMeasurement currentPositionMeas = new GpsMeasurement(currentPosition.getLatitude(), currentPosition diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java index 87a7cc8e..ed71317a 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java @@ -20,6 +20,9 @@ public class RobobuggyKFLocalizerTest { private static LinkedBlockingQueue poseMessages; @BeforeClass + /** + * creates a subscriber for testing the localizer + */ public static void oneTimeSetup() { new Subscriber("localizerTest", NodeChannel.POSE.getMsgPath(), ((topicName, m) -> { poseMessages.add(((GPSPoseMessage) m)); @@ -27,16 +30,19 @@ public static void oneTimeSetup() { } @Before + /** + * initializes the linked blocking queue of pose messages + */ public void setUp() { poseMessages = new LinkedBlockingQueue<>(); } /** * NOTE THIS IS NOT YET A VALID TEST CASE - * @throws InterruptedException + * @throws InterruptedException - need this here */ @Test - public void test_singleIteration() throws InterruptedException { + public void testSingleIteration() throws InterruptedException { RobobuggyKFLocalizer localizer = new RobobuggyKFLocalizer(10000, "testLocalizer", new LocTuple(40.441670, -79.9416362)); new Publisher(NodeChannel.GPS.getMsgPath()).publish(new GpsMeasurement(40.441670, -79.9512463)); Thread.sleep(3000); From 78f4631a6cf5f6a946405a29219a0b07f4424471 Mon Sep 17 00:00:00 2001 From: abhinavGirish Date: Mon, 22 May 2017 23:32:17 -0400 Subject: [PATCH 143/149] last changes to style --- .../nodes/localizers/RobobuggyKFLocalizerTest.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java index ed71317a..72efff24 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java @@ -19,20 +19,22 @@ public class RobobuggyKFLocalizerTest { private static LinkedBlockingQueue poseMessages; - @BeforeClass + /** * creates a subscriber for testing the localizer */ + @BeforeClass public static void oneTimeSetup() { new Subscriber("localizerTest", NodeChannel.POSE.getMsgPath(), ((topicName, m) -> { poseMessages.add(((GPSPoseMessage) m)); })); } - @Before + /** * initializes the linked blocking queue of pose messages */ + @Before public void setUp() { poseMessages = new LinkedBlockingQueue<>(); } From 91fc0aab9ba83213bc082367dec18054e401123d Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 22 May 2017 21:29:22 -0700 Subject: [PATCH 144/149] changed naming in the controller tester to be more accurate --- .../simulation/ControllerTester.java | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index 14646b66..eaecea9c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -32,8 +32,7 @@ public class ControllerTester extends PeriodicNode { // assume we are facing up hill 1 private static final double INITIAL_HEADING_RAD = Math.toRadians(250); - private Matrix matX; - private Matrix matA; + private Matrix x; private double commandedSteeringAngle = 0; private Publisher simulatedPosePub; private Publisher gpspub; @@ -49,20 +48,18 @@ public class ControllerTester extends PeriodicNode { public ControllerTester(String name, LocTuple initialPosition) { super(new BuggyBaseNode(NodeChannel.AUTO), SIM_PERIOD, name); - LocTuple firstPosition = initialPosition; double[][] xAsDoubleArr = { - { LocalizerUtil.deg2UTM(firstPosition).getEasting() }, - { LocalizerUtil.deg2UTM(firstPosition).getNorthing() }, + { LocalizerUtil.deg2UTM(initialPosition).getEasting() }, + { LocalizerUtil.deg2UTM(initialPosition).getNorthing() }, { VELOCITY }, { INITIAL_HEADING_RAD }, { 0 } }; - matX = new Matrix(xAsDoubleArr); + x = new Matrix(xAsDoubleArr); new Subscriber("controller tester", NodeChannel.DRIVE_CTRL.getMsgPath(), ((topicName, m) -> { commandedSteeringAngle = ((DriveControlMessage) m).getAngleDouble(); -// targetHeading = X.get(3, 0) + commandedSteeringAngle; })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); @@ -73,17 +70,17 @@ public ControllerTester(String name, LocTuple initialPosition) { protected void update() { simCounter++; - matA = getNewModel(matX); + Matrix motionModel = getNewModel(x); // update simulated position - matX = matA.times(matX); + x = motionModel.times(x); - UTMTuple t = new UTMTuple(17, 'T', matX.get(0, 0), matX.get(1, 0)); + UTMTuple t = new UTMTuple(17, 'T', x.get(0, 0), x.get(1, 0)); LocTuple lt = LocalizerUtil.utm2Deg(t); // if it's time to run the controller, update the controller's understanding of where we are if (simCounter == CONTROLLER_PERIOD) { - simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), matX.get(3, 0), matX.get(2, 0))); + simulatedPosePub.publish(new GPSPoseMessage(new Date(), lt.getLatitude(), lt.getLongitude(), x.get(3, 0), x.get(2, 0))); simCounter = 0; } From d0ec4f44e2c6e95d6d95570973a14ee1f661f145 Mon Sep 17 00:00:00 2001 From: sbhotika Date: Wed, 24 May 2017 22:42:42 -0400 Subject: [PATCH 145/149] Removed file notes.txt in offline/localizer per Vivaan's comments --- offline/localizer/notes.txt | 58 ------------------------------------- 1 file changed, 58 deletions(-) delete mode 100644 offline/localizer/notes.txt diff --git a/offline/localizer/notes.txt b/offline/localizer/notes.txt deleted file mode 100644 index 339c89e1..00000000 --- a/offline/localizer/notes.txt +++ /dev/null @@ -1,58 +0,0 @@ -double[][] motionModel = { - // x y x_b y_b th th_dot heading - { 1, 0, Math.cos(th) * diff, -Math.sin(th) * diff, 0, 0, 0 }, // x - { 0, 1, Math.sin(th) * diff, Math.cos(th) * diff, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_b - { 0, 0, 0, 1, 0, 0, 0 }, // y_b - { 0, 0, 0, 0, 1, diff, 0 }, // th - { 0, 0, 180 / (Math.PI * (wheelBase / Math.sin(heading))), 0, - 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading -}; - -Measurement stuff: - -GPS: -{"VERSION_ID":"gpsV0.2","gpsTimestamp":"Oct 9, 2016 6:17:15 AM","latitude":40.441633333333336,"north":true,"longitude":-79.941644,"west":true,"qualityValue":2,"numSatellites":10,"horizontalDilutionOfPrecision":1.18,"antennaAltitude":292.1,"rawGPSLat":4026.498,"rawGPSLong":7956.49864,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235951,"topicName":"sensors/gps","sequenceNumber":290}, - -Get x from latitude, y from longitude -x = x + x_dot*cos(theta)*dt - y_dot*sin(theta)*dt -y = y + y_dot*sin(theta)*dt + ydot*cos(theta)*dt - -Encoder: -{"VERSION_ID":"encoderV0.0","distance":0.4336513119533528,"velocity":NaN,"dataWord":8.0,"accel":NaN,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235950,"topicName":"sensors/encoder","sequenceNumber":14473}, - -Get x_dot from distance -x_dot = x_dot -y_dot = y_dot - -y_dot completely unaffected by measurement (if we're sliding sideways, something's going horribly wrong...) - -theta currently computed by the difference between our new GPS reading and the previous one: -if (Math.abs(gpsUTM.getEasting() - startUTM.getEasting()) - + Math.abs(gpsUTM.getNorthing() - startUTM.getNorthing()) < 10.0) { - th = startAngle; -} -theta_dot currently completely unaffected by measurement - -Ideally, we'd get theta and theta_dot from an IMU message. - -IMU Angular Position: - -{"VERSION_ID":"imuAngularPositionV0.0","rot":[[0.25653892010450363,0.030666396021842957,-0.01811222732067108],[0.032628461718559265,0.9959298074245453,-0.04417908191680908],[0.014278098940849304,-0.045562922954559326,0.25790490955114365]],"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235961,"topicName":"sensors/imu_ang_pos","sequenceNumber":18152}, - -IMU Angular Velocity: - -{"VERSION_ID":"imuAngularVelocityV0.0","x":0.14453125,"y":-0.361328125,"z":-0.3271484375,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235961,"topicName":"sensors/imu_ang_vel","sequenceNumber":18152}, - -Steering: - -{"VERSION_ID":"steering","angle":0.07,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235966,"topicName":"sensors/steering","sequenceNumber":14474}, - -phi is the angle field. Careful to check the topicName; we want sensors/steering, not sensors/commandedSteering - -IMU Temperature: -might be the compass angle - -{"VERSION_ID":"temperatureV0.1","temperature":13.53125,"DATE_FORMAT":"yyyy-MM-dd HH:mm:ss.SSS","timestamp":1476008235676,"topicName":"sensors/imu_temp","sequenceNumber":18117}, - From f5e568c93884f62b9ef6fa8846cc75e469ea9675 Mon Sep 17 00:00:00 2001 From: sbhotika Date: Wed, 24 May 2017 22:46:43 -0400 Subject: [PATCH 146/149] Removed some obsolete packages per earlier code review --- .../com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java | 1 - .../robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java | 2 -- 2 files changed, 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java index 147e6ba2..33185591 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/PathPlannerNode.java @@ -12,7 +12,6 @@ import com.roboclub.robobuggy.ros.Publisher; import com.roboclub.robobuggy.ros.Subscriber; -import java.time.temporal.WeekFields; import java.util.Date; /** diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java index 87a7cc8e..a238322b 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java @@ -1,5 +1,3 @@ -package com.roboclub.robobuggy.nodes.localizers; - import com.roboclub.robobuggy.messages.GPSPoseMessage; import com.roboclub.robobuggy.messages.GpsMeasurement; import com.roboclub.robobuggy.ros.NodeChannel; From 5938c1a6c8130dc6eac8bda0d91c4048a625c637 Mon Sep 17 00:00:00 2001 From: sbhotika Date: Wed, 24 May 2017 23:31:03 -0400 Subject: [PATCH 147/149] Cleaning up code --- .../surface_src/java_src/Alice/build.gradle | 2 +- .../nodes/localizers/KfLocalizer.java | 42 --------------- .../planners/RobobuggyWaypointFollower.java | 44 --------------- .../robots/ControllerTesterRobot.java | 2 - .../robobuggy/simulation/PlayBackUtil.java | 1 - .../localizers/RobobuggyKFLocalizerTest.java | 54 ------------------- 6 files changed, 1 insertion(+), 144 deletions(-) delete mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java delete mode 100644 real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java diff --git a/real_time/surface_src/java_src/Alice/build.gradle b/real_time/surface_src/java_src/Alice/build.gradle index c19bb136..10e4d46a 100644 --- a/real_time/surface_src/java_src/Alice/build.gradle +++ b/real_time/surface_src/java_src/Alice/build.gradle @@ -43,7 +43,7 @@ dependencies { compile project (':BuggyRos') //for jason object extraction from strings -// compile 'com.google.code.gson:gson:2.2.+' + compile group: 'com.google.code.gson', name: 'gson', version: '2.2.+' compile 'jfreechart:jfreechart:1.0.0' compile 'gov.nist.math:jama:1.0.2' diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java index 43f63fb4..396f3ec3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/localizers/KfLocalizer.java @@ -135,48 +135,6 @@ public synchronized void actionPerformed(String topicName, Message m) { } }); - /* - new Subscriber( - "HighTrustGpsLoc", - NodeChannel.IMU_ANG_POS.getMsgPath(), - ((topicName, m) -> { - - IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); - // double y = mes.getRot()[0][1]; - // double x = mes.getRot()[0][0]; - double[][] xVar = { { 1 }, { 0 }, { 0 } }; - double[][] yVar = { { 0 }, { 1 }, { 0 } }; - Matrix xMat = new Matrix(xVar); - Matrix yMat = new Matrix(yVar); - Matrix rot = new Matrix(mes.getRot()); - double x = rot.times(xMat).get(0, 0); - double y = rot.times(yMat).get(0, 0); - double th = -(Math.toDegrees(Math.atan2(y, x)) - 90); - double[][] observationModel = { { 0, 0, 0, 0, 0, 0, 0 }, // x - { 0, 0, 0, 0, 0, 0, 0 }, // y - { 0, 0, 0, 0, 0, 0, 0 }, // x_dot_b - { 0, 0, 0, 0, 0, 0, 0 }, // y_dot_b - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 0, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 0 } // Heading - }; - - double[][] meassurement = { { 0 }, { 0 }, { 0 }, { 0 }, - { th }, { 0 }, { 0 } }; - double[][] updateCovariance = { { 1, 0, 0, 0, 0, 0, 0 }, // x - { 0, 1, 0, 0, 0, 0, 0 }, // y - { 0, 0, 1, 0, 0, 0, 0 }, // x_dot - { 0, 0, 0, 1, 0, 0, 0 }, // y_dot - { 0, 0, 0, 0, 1, 0, 0 }, // th - { 0, 0, 0, 0, 0, 1, 0 }, // th_dot - { 0, 0, 0, 0, 0, 0, 1 } // heading - }; - // updateStep(new Matrix(observationModel),new - // Matrix(meassurement),new Matrix(updateCovariance)); - - })); - */ - // TODO note that we will probably run into precision errors since the // changes are so small // would be good to batch up the encoder updates until we get a margin diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java deleted file mode 100644 index 0e818332..00000000 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/RobobuggyWaypointFollower.java +++ /dev/null @@ -1,44 +0,0 @@ -package com.roboclub.robobuggy.nodes.planners; - -import com.roboclub.robobuggy.messages.GPSPoseMessage; -import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.ros.NodeChannel; - -import java.util.ArrayList; - -/** - * Created by vivaanbahl on 11/4/16. - */ -public class RobobuggyWaypointFollower extends PathPlannerNode { - private GPSPoseMessage currentPositionEsitmate; - private ArrayList waypoints; - - private double currentSteeringAngle; - private boolean currentBrakeStatus; - - /** - * Construct a new {@link PathPlannerNode} - * - * @param channel {@link NodeChannel} on which to broadcast status - * information about the node - */ - public RobobuggyWaypointFollower(NodeChannel channel) { - super(channel); - waypoints = new ArrayList<>(); - } - - @Override - protected void updatePositionEstimate(GPSPoseMessage m) { - currentPositionEsitmate = m; - } - - @Override - protected double getCommandedSteeringAngle() { - return currentSteeringAngle; - } - - @Override - protected boolean getDeployBrakeValue() { - return currentBrakeStatus; - } -} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index 634604bc..a504b49c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -54,8 +54,6 @@ private ControllerTesterRobot() { RobobuggyGUITabs tabs = new RobobuggyGUITabs(); mainWindow.addComponent(tabs, 0.0, 0.0, 1.0, 1.0); tabs.addTab(new MainGuiWindow(), "Home"); - // tabs.addTab(new PoseGraphsPanel(),"poses"); - // tabs.addTab(new AutonomousPanel(),"Autonomous"); tabs.add(new PathPanel(), "Path Visualizer"); tabs.addTab(new ConfigurationPanel(), "Configuration"); } diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index a2afd636..3ea917c6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,7 +137,6 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); -// getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java deleted file mode 100644 index 029d7221..00000000 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/localizers/RobobuggyKFLocalizerTest.java +++ /dev/null @@ -1,54 +0,0 @@ -import com.roboclub.robobuggy.messages.GPSPoseMessage; -import com.roboclub.robobuggy.messages.GpsMeasurement; -import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ros.Publisher; -import com.roboclub.robobuggy.ros.Subscriber; -import org.junit.Assert; -import org.junit.Before; -import org.junit.BeforeClass; -import org.junit.Test; - -import java.util.concurrent.LinkedBlockingQueue; - -/** - * Created by vivaanbahl on 11/16/16. - */ -public class RobobuggyKFLocalizerTest { - - private static LinkedBlockingQueue poseMessages; - - - /** - * creates a subscriber for testing the localizer - */ - @BeforeClass - public static void oneTimeSetup() { - new Subscriber("localizerTest", NodeChannel.POSE.getMsgPath(), ((topicName, m) -> { - poseMessages.add(((GPSPoseMessage) m)); - })); - } - - - /** - * initializes the linked blocking queue of pose messages - */ - @Before - public void setUp() { - poseMessages = new LinkedBlockingQueue<>(); - } - - /** - * NOTE THIS IS NOT YET A VALID TEST CASE - * @throws InterruptedException - need this here - */ - @Test - public void testSingleIteration() throws InterruptedException { - RobobuggyKFLocalizer localizer = new RobobuggyKFLocalizer(10000, "testLocalizer", new LocTuple(40.441670, -79.9416362)); - new Publisher(NodeChannel.GPS.getMsgPath()).publish(new GpsMeasurement(40.441670, -79.9512463)); - Thread.sleep(3000); - localizer.update(); - Thread.sleep(3000); - Assert.assertEquals(0.0, 1.0, 0.0); - } - -} \ No newline at end of file From 0658e07b0e5462df22bb3519e005ae19710e55e4 Mon Sep 17 00:00:00 2001 From: sbhotika Date: Wed, 24 May 2017 23:35:53 -0400 Subject: [PATCH 148/149] corrected a typo from 'jason' to 'json' --- real_time/surface_src/java_src/Alice/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/build.gradle b/real_time/surface_src/java_src/Alice/build.gradle index 10e4d46a..84aa75eb 100644 --- a/real_time/surface_src/java_src/Alice/build.gradle +++ b/real_time/surface_src/java_src/Alice/build.gradle @@ -42,7 +42,7 @@ dependencies { compile 'org.jcodec:jcodec-javase:0.1.9' compile project (':BuggyRos') - //for jason object extraction from strings + //for json object extraction from strings compile group: 'com.google.code.gson', name: 'gson', version: '2.2.+' compile 'jfreechart:jfreechart:1.0.0' From 4fe136cb51484a49f98927e3309970e83888b3d1 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 25 May 2017 22:47:50 -0700 Subject: [PATCH 149/149] fixed findbugs issues --- .../robobuggy/nodes/planners/WayPointFollowerPlanner.java | 4 ++-- .../com/roboclub/robobuggy/robots/ControllerTesterRobot.java | 2 +- .../com/roboclub/robobuggy/simulation/ControllerTester.java | 4 +--- .../java/com/roboclub/robobuggy/simulation/FullSimRunner.java | 3 +-- .../java/com/roboclub/robobuggy/simulation/PlayBackUtil.java | 1 + 5 files changed, 6 insertions(+), 8 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 1bdcbfd2..f9da87c6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -22,7 +22,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private GpsMeasurement target; // we only want to look at the next 10 waypoints as possible candidates - private final int wayPointLOOKAHEADMAX = 50; + private static final int WAY_POINT_LOOKAHEAD_MAX = 50; /** * @param wayPoints the list of waypoints to follow @@ -50,7 +50,7 @@ private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this int closestIndex = lastClosestIndex; - for (int i = lastClosestIndex; i < (lastClosestIndex + wayPointLOOKAHEADMAX) && i < wayPoints.size(); i++) { + for (int i = lastClosestIndex; i < (lastClosestIndex + WAY_POINT_LOOKAHEAD_MAX) && i < wayPoints.size(); i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index a504b49c..66ee000e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -38,7 +38,7 @@ public static ControllerTesterRobot getInstance() { private ControllerTesterRobot() { super(); - ArrayList wayPoints = null; + ArrayList wayPoints = new ArrayList<>(); try { wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index eaecea9c..1f9852f3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -35,9 +35,7 @@ public class ControllerTester extends PeriodicNode { private Matrix x; private double commandedSteeringAngle = 0; private Publisher simulatedPosePub; - private Publisher gpspub; private int simCounter; - private double targetHeading = INITIAL_HEADING_RAD; /** * Create a new {@link PeriodicNode} decorator @@ -63,7 +61,7 @@ public ControllerTester(String name, LocTuple initialPosition) { })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); - gpspub = new Publisher(NodeChannel.GPS.getMsgPath()); + new Publisher(NodeChannel.GPS.getMsgPath()); } @Override diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index afd9c5c8..5850c4fc 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -44,7 +44,6 @@ public class FullSimRunner extends BuggyBaseNode { private Publisher gpsPub; private Publisher encPub; private Publisher steerPub; - private Subscriber steerSub; private RobobuggyKFLocalizer localizer; private WayPointFollowerPlanner controller; @@ -84,7 +83,7 @@ public FullSimRunner(String name, LocTuple initialPos) { gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); encPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); steerPub = new Publisher(NodeChannel.STEERING.getMsgPath()); - steerSub = new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { + new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { updateMotionModel(((DriveControlMessage) m).getAngleDouble()); steerPub.publish(new SteeringMeasurement(Math.toDegrees(((DriveControlMessage) m).getAngleDouble()))); }); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 3ea917c6..64a6cafa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,6 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); + getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class);