diff --git a/map/CMakeLists.txt b/map/CMakeLists.txt index 46f919a8e0..357e703301 100644 --- a/map/CMakeLists.txt +++ b/map/CMakeLists.txt @@ -47,6 +47,8 @@ set( everywhere_search_callback.cpp everywhere_search_callback.hpp everywhere_search_params.hpp + extrapolation/extrapolator.cpp + extrapolation/extrapolator.hpp feature_vec_model.cpp feature_vec_model.hpp framework.cpp diff --git a/map/extrapolation/extrapolator.cpp b/map/extrapolation/extrapolator.cpp new file mode 100644 index 0000000000..fbab3cbc99 --- /dev/null +++ b/map/extrapolation/extrapolator.cpp @@ -0,0 +1,166 @@ +#include "map/extrapolation/extrapolator.hpp" + +#include "geometry/distance_on_sphere.hpp" + +#include "platform/platform.hpp" + +#include "base/logging.hpp" + +#include +#include + +namespace +{ +uint64_t constexpr kMaxExtrapolationTimeMs = 1000; +uint64_t constexpr kExtrapolationPeriodMs = 200; +double constexpr kMaxExtrapolationSpeedMPerS = 120.0; + +double LinearExtrapolationOfOneParam(double param1, double param2, uint64_t timeBetweenPointsMs, + uint64_t timeAfterPoint2Ms) +{ + return (param2 - param1) * timeAfterPoint2Ms / timeBetweenPointsMs; +} + +/// \brief Returns extrapolated position after |point2| in |timeAfterPoint2Ms|. +/// \note This function is assumed that between |point1| and |point2| passed one second. +/// \note |timeAfterPoint2Ms| should be relevantly small (several seconds maximum). +location::GpsInfo LinearExtrapolation(location::GpsInfo const & point1, location::GpsInfo & point2, + uint64_t timeAfterPoint2Ms) +{ + uint64_t const timeBetweenPointsMs = + static_cast((point2.m_timestamp - point1.m_timestamp) * 1000); + + location::GpsInfo extrapolated = point2; + + extrapolated.m_timestamp += timeAfterPoint2Ms; + + extrapolated.m_longitude += LinearExtrapolationOfOneParam( + point1.m_longitude, point2.m_longitude, timeBetweenPointsMs, timeAfterPoint2Ms); + + extrapolated.m_latitude += LinearExtrapolationOfOneParam(point1.m_latitude, point2.m_latitude, + timeBetweenPointsMs, timeAfterPoint2Ms); + + extrapolated.m_horizontalAccuracy += + LinearExtrapolationOfOneParam(point1.m_horizontalAccuracy, point2.m_horizontalAccuracy, + timeBetweenPointsMs, timeAfterPoint2Ms); + extrapolated.m_altitude += LinearExtrapolationOfOneParam(point1.m_altitude, point2.m_altitude, + timeBetweenPointsMs, timeAfterPoint2Ms); + + if (point1.m_verticalAccuracy != -1 && point2.m_verticalAccuracy != -1) + { + extrapolated.m_verticalAccuracy += + LinearExtrapolationOfOneParam(point1.m_verticalAccuracy, point2.m_verticalAccuracy, + timeBetweenPointsMs, timeAfterPoint2Ms); + } + + if (point1.m_bearing != -1 && point2.m_bearing != -1) + { + extrapolated.m_bearing += LinearExtrapolationOfOneParam(point1.m_bearing, point2.m_bearing, + timeBetweenPointsMs, timeAfterPoint2Ms); + } + + if (point1.m_speed != -1 && point2.m_speed != -1) + { + extrapolated.m_speed += LinearExtrapolationOfOneParam(point1.m_speed, point2.m_speed, + timeBetweenPointsMs, timeAfterPoint2Ms); + } + return extrapolated; +} +} // namespace + +namespace position_extrapolator +{ +using namespace std; + +// Extrapolator::Routine --------------------------------------------------------------------------- + +Extrapolator::Routine::Routine(ExtrapolatedLocationUpdate const & update) + : m_extrapolatedLocationUpdate(update) +{ +} + +void Extrapolator::Routine::Do() +{ + while (!IsCancelled()) + { + { + GetPlatform().RunTask(Platform::Thread::Gui, [this]() { + lock_guard guard(m_mutex); + uint64_t const extrapolationTimeMs = kExtrapolationPeriodMs * m_extrapolationCounter; + if (extrapolationTimeMs >= kMaxExtrapolationTimeMs) + return; + + if (DoesExtrapolationWork(extrapolationTimeMs)) + { + location::GpsInfo gpsInfo = + LinearExtrapolation(m_beforeLastGpsInfo, m_lastGpsInfo, extrapolationTimeMs); + m_extrapolatedLocationUpdate(gpsInfo); + } + else + { + if (m_lastGpsInfo.m_source != location::EUndefine) + { + location::GpsInfo gpsInfo = m_lastGpsInfo; + m_extrapolatedLocationUpdate(gpsInfo); + } + } + }); + + lock_guard guard(m_mutex); + if (m_extrapolationCounter != m_extrapolationCounterUndefined) + ++m_extrapolationCounter; + } + // @TODO(bykoinako) Method m_extrapolatedLocationUpdate() is run on gui thread every + // |kExtrapolationPeriodMs| milliseconds. But after changing GPS position + // (that means after a call of method Routine::SetGpsInfo()) + // m_extrapolatedLocationUpdate() should be run immediately on gui thread. + this_thread::sleep_for(std::chrono::milliseconds(kExtrapolationPeriodMs)); + } +} + +void Extrapolator::Routine::SetGpsInfo(location::GpsInfo const & gpsInfo) +{ + lock_guard guard(m_mutex); + m_beforeLastGpsInfo = m_lastGpsInfo; + m_lastGpsInfo = gpsInfo; + m_extrapolationCounter = 0; +} + +bool Extrapolator::Routine::DoesExtrapolationWork(uint64_t extrapolationTimeMs) const +{ + // Note. It's possible that m_beforeLastGpsInfo.m_timestamp >= m_lastGpsInfo.m_timestamp. + // It may happen in rare cases because GpsInfo::m_timestamp is not monotonic generally. + // Please see comment in declaration of class GpsInfo for details. + + if (m_extrapolationCounter == m_extrapolationCounterUndefined || + m_lastGpsInfo.m_source == location::EUndefine || + m_beforeLastGpsInfo.m_source == location::EUndefine || + m_beforeLastGpsInfo.m_timestamp >= m_lastGpsInfo.m_timestamp) + { + return false; + } + + double const distM = + ms::DistanceOnEarth(m_beforeLastGpsInfo.m_latitude, m_beforeLastGpsInfo.m_longitude, + m_lastGpsInfo.m_latitude, m_lastGpsInfo.m_longitude); + double const timeS = m_lastGpsInfo.m_timestamp - m_beforeLastGpsInfo.m_timestamp; + + // Switching off extrapolation based on speed. + return distM / timeS < kMaxExtrapolationSpeedMPerS; + // @TODO(bykoianko) Switching off extrapolation based on acceleration should be implemented. +} + +// Extrapolator ------------------------------------------------------------------------------------ +Extrapolator::Extrapolator(ExtrapolatedLocationUpdate const & update) + : m_extrapolatedLocationThread() +{ + m_extrapolatedLocationThread.Create(make_unique(update)); +} + +void Extrapolator::OnLocationUpdate(location::GpsInfo & info) +{ + auto * routine = m_extrapolatedLocationThread.GetRoutineAs(); + CHECK(routine, ()); + routine->SetGpsInfo(info); +} +} // namespace position_extrapolator diff --git a/map/extrapolation/extrapolator.hpp b/map/extrapolation/extrapolator.hpp new file mode 100644 index 0000000000..fe7c8b883b --- /dev/null +++ b/map/extrapolation/extrapolator.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "platform/location.hpp" + +#include "base/thread.hpp" + +#include +#include +#include +#include +#include + +namespace position_extrapolator +{ +class Extrapolator +{ +public: + using ExtrapolatedLocationUpdate = std::function; + + static uint64_t constexpr m_extrapolationCounterUndefined = std::numeric_limits::max(); + + /// \param update is a function which is called with params according to extrapolated position. + /// |update| will be called on gui thread. + explicit Extrapolator(ExtrapolatedLocationUpdate const & update); + void OnLocationUpdate(location::GpsInfo & info); + // @TODO(bykoianko) Gyroscope information should be taken into account as well for calculation + // extrapolated position. + + void Cancel() { m_extrapolatedLocationThread.Cancel(); } + +private: + class Routine : public threads::IRoutine + { + public: + explicit Routine(ExtrapolatedLocationUpdate const & update); + + // threads::IRoutine overrides: + void Do() override; + + void SetGpsInfo(location::GpsInfo const & gpsInfo); + + private: + bool DoesExtrapolationWork(uint64_t extrapolationTimeMs) const; + + ExtrapolatedLocationUpdate m_extrapolatedLocationUpdate; + + std::mutex m_mutex; + location::GpsInfo m_lastGpsInfo; + location::GpsInfo m_beforeLastGpsInfo; + uint64_t m_extrapolationCounter = m_extrapolationCounterUndefined; + }; + + threads::Thread m_extrapolatedLocationThread; +}; +} // namespace position_extrapolator diff --git a/map/framework.cpp b/map/framework.cpp index fcee28b10a..7c4309469f 100644 --- a/map/framework.cpp +++ b/map/framework.cpp @@ -216,7 +216,7 @@ void Framework::OnLocationUpdate(GpsInfo const & info) GpsInfo rInfo(info); #endif - m_routingManager.OnLocationUpdate(rInfo); + m_extrapolator.OnLocationUpdate(rInfo); } void Framework::OnCompassUpdate(CompassInfo const & info) @@ -356,18 +356,20 @@ Framework::Framework(FrameworkParams const & params) , m_storage(platform::migrate::NeedMigrate() ? COUNTRIES_OBSOLETE_FILE : COUNTRIES_FILE) , m_enabledDiffs(params.m_enableDiffs) , m_isRenderingEnabled(true) - , m_routingManager(RoutingManager::Callbacks([this]() -> Index & { return m_model.GetIndex(); }, - [this]() -> storage::CountryInfoGetter & { return GetCountryInfoGetter(); }, - [this](string const & id) -> string { - return m_storage.GetParentIdFor(id); - }, - [this](RoutingManager::Callbacks::FeatureCallback const & fn, - vector const & features) - { - return m_model.ReadFeatures(fn, features); - }, - [this]() -> StringsBundle const & { return m_stringsBundle; }), - static_cast(*this)) + , m_routingManager( + RoutingManager::Callbacks( + [this]() -> Index & { return m_model.GetIndex(); }, + [this]() -> storage::CountryInfoGetter & { return GetCountryInfoGetter(); }, + [this](string const & id) -> string { return m_storage.GetParentIdFor(id); }, + [this](RoutingManager::Callbacks::FeatureCallback const & fn, + vector const & features) { + return m_model.ReadFeatures(fn, features); + }, + [this]() -> StringsBundle const & { return m_stringsBundle; }), + static_cast(*this)) + , m_extrapolator([this](location::GpsInfo & gpsInfo) { + this->GetRoutingManager().OnLocationUpdate(gpsInfo); + }) , m_trafficManager(bind(&Framework::GetMwmsByRect, this, _1, false /* rough */), kMaxTrafficCacheSizeBytes, m_routingManager.RoutingSession()) , m_bookingFilterProcessor(m_model.GetIndex(), *m_bookingApi) @@ -495,6 +497,7 @@ Framework::~Framework() GetBookmarkManager().Teardown(); m_trafficManager.Teardown(); + m_extrapolator.Cancel(); DestroyDrapeEngine(); m_model.SetOnMapDeregisteredCallback(nullptr); diff --git a/map/framework.hpp b/map/framework.hpp index 5bd31099a2..f867ee64c5 100644 --- a/map/framework.hpp +++ b/map/framework.hpp @@ -7,6 +7,7 @@ #include "map/bookmark_manager.hpp" #include "map/discovery/discovery_manager.hpp" #include "map/displacement_mode_manager.hpp" +#include "map/extrapolation/extrapolator.hpp" #include "map/feature_vec_model.hpp" #include "map/local_ads_manager.hpp" #include "map/mwm_url.hpp" @@ -207,6 +208,7 @@ protected: // Note. |m_routingManager| should be declared before |m_trafficManager| RoutingManager m_routingManager; + position_extrapolator::Extrapolator m_extrapolator; TrafficManager m_trafficManager; diff --git a/platform/location.hpp b/platform/location.hpp index 1fc9ffd8c5..d927d7db21 100644 --- a/platform/location.hpp +++ b/platform/location.hpp @@ -27,6 +27,7 @@ namespace location enum TLocationSource { + EUndefine, EAppleNative, EWindowsNative, EAndroidNative, @@ -41,25 +42,25 @@ namespace location class GpsInfo { public: - GpsInfo() - : m_horizontalAccuracy(100.0), // use as a default accuracy - m_altitude(0.0), m_verticalAccuracy(-1.0), m_bearing(-1.0), m_speed(-1.0) - { - } + TLocationSource m_source = EUndefine; + /// \note |m_timestamp| is calculated based on platform methods which don't + /// guarantee that |m_timestamp| is monotonic. |m_monotonicTimeMs| should be added to + /// class |GpsInfo|. This time should be calculated based on Location::getElapsedRealtimeNanos() + /// method in case of Android. How to calculate such time in case of iOS should be + /// investigated. + /// \note For most cases |m_timestamp| is monotonic. + double m_timestamp = 0.0; //!< seconds from 1st Jan 1970 + double m_latitude = 0.0; //!< degrees + double m_longitude = 0.0; //!< degrees + double m_horizontalAccuracy = 100.0; //!< metres + double m_altitude = 0.0; //!< metres + double m_verticalAccuracy = -1.0; //!< metres + double m_bearing = -1.0; //!< positive degrees from the true North + double m_speed = -1.0; //!< metres per second - TLocationSource m_source; - double m_timestamp; //!< seconds from 1st Jan 1970 - double m_latitude; //!< degrees - double m_longitude; //!< degrees - double m_horizontalAccuracy; //!< metres - double m_altitude; //!< metres - double m_verticalAccuracy; //!< metres - double m_bearing; //!< positive degrees from the true North - double m_speed; //!< metres per second - - //bool HasAltitude() const { return m_verticalAccuracy >= 0.0; } - bool HasBearing() const { return m_bearing >= 0.0; } - bool HasSpeed() const { return m_speed >= 0.0; } + bool IsValid() const { return m_source != EUndefine; } + bool HasBearing() const { return m_bearing >= 0.0; } + bool HasSpeed() const { return m_speed >= 0.0; } }; /// GpsTrackInfo struct describes a point for GPS tracking diff --git a/xcode/map/map.xcodeproj/project.pbxproj b/xcode/map/map.xcodeproj/project.pbxproj index 7e32eba63c..657725d898 100644 --- a/xcode/map/map.xcodeproj/project.pbxproj +++ b/xcode/map/map.xcodeproj/project.pbxproj @@ -93,6 +93,8 @@ 45F6EE9F1FB1C77600019892 /* search_api.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 45F6EE9C1FB1C77500019892 /* search_api.cpp */; }; 47A9D82220A19E9E00E4671B /* libopen_location_code.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 47A9D82120A19E9E00E4671B /* libopen_location_code.a */; }; 47A9D82420A19EC300E4671B /* libkml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 47A9D82320A19EC300E4671B /* libkml.a */; }; + 56C116602090E5670068BBC0 /* extrapolator.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56C1165E2090E5670068BBC0 /* extrapolator.cpp */; }; + 56C116612090E5670068BBC0 /* extrapolator.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56C1165F2090E5670068BBC0 /* extrapolator.hpp */; }; 56EE14D31FE80E900036F20C /* libtransit.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56EE14D41FE80E900036F20C /* libtransit.a */; }; 56EE14D51FE80EBD0036F20C /* libmwm_diff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56EE14D61FE80EBD0036F20C /* libmwm_diff.a */; }; 56EE14D71FE80F290036F20C /* libbsdiff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56EE14D81FE80F290036F20C /* libbsdiff.a */; }; @@ -297,6 +299,8 @@ 45F6EE9C1FB1C77500019892 /* search_api.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = search_api.cpp; sourceTree = ""; }; 47A9D82120A19E9E00E4671B /* libopen_location_code.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libopen_location_code.a; sourceTree = BUILT_PRODUCTS_DIR; }; 47A9D82320A19EC300E4671B /* libkml.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libkml.a; sourceTree = BUILT_PRODUCTS_DIR; }; + 56C1165E2090E5670068BBC0 /* extrapolator.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = extrapolator.cpp; path = extrapolation/extrapolator.cpp; sourceTree = ""; }; + 56C1165F2090E5670068BBC0 /* extrapolator.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; name = extrapolator.hpp; path = extrapolation/extrapolator.hpp; sourceTree = ""; }; 56EE14D41FE80E900036F20C /* libtransit.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libtransit.a; sourceTree = BUILT_PRODUCTS_DIR; }; 56EE14D61FE80EBD0036F20C /* libmwm_diff.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libmwm_diff.a; sourceTree = BUILT_PRODUCTS_DIR; }; 56EE14D81FE80F290036F20C /* libbsdiff.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libbsdiff.a; sourceTree = BUILT_PRODUCTS_DIR; }; @@ -674,6 +678,8 @@ 675345BD1A4054AD00A0A8C3 /* map */ = { isa = PBXGroup; children = ( + 56C1165E2090E5670068BBC0 /* extrapolator.cpp */, + 56C1165F2090E5670068BBC0 /* extrapolator.hpp */, 675345CB1A4054E800A0A8C3 /* address_finder.cpp */, 45201E921CE4AC90008A4842 /* api_mark_point.cpp */, 34921F611BFA0A6900737D6E /* api_mark_point.hpp */, @@ -829,6 +835,7 @@ F63421F91DF9BF9100A96868 /* reachable_by_taxi_checker.hpp in Headers */, BBFC7E3B202D29C000531BE7 /* user_mark_layer.hpp in Headers */, 3D4E99A51FB4A6410025B48C /* booking_filter.hpp in Headers */, + 56C116612090E5670068BBC0 /* extrapolator.hpp in Headers */, 675346491A4054E800A0A8C3 /* bookmark_manager.hpp in Headers */, 3DA5714320B5CC80007BDE27 /* booking_filter_processor.hpp in Headers */, F6B2830A1C1B03320081957A /* gps_track.hpp in Headers */, @@ -1021,6 +1028,7 @@ BB4E5F281FCC664A00A77250 /* transit_reader.cpp in Sources */, 454649F11F2728CE00EF4064 /* local_ads_mark.cpp in Sources */, F63421F81DF9BF9100A96868 /* reachable_by_taxi_checker.cpp in Sources */, + 56C116602090E5670068BBC0 /* extrapolator.cpp in Sources */, 6753466A1A4054E800A0A8C3 /* geourl_process.cpp in Sources */, 348AB57C1D7EE0C6009F8301 /* chart_generator.cpp in Sources */, 342D833A1D5233E8000D8AEA /* displacement_mode_manager.cpp in Sources */,