From c59b63d784cc3491e7fb088950268f809f131b93 Mon Sep 17 00:00:00 2001 From: Henry Sternberg Date: Mon, 28 Jul 2025 17:48:44 +0100 Subject: [PATCH] [routing] OSM tag based time and turn penalties Signed-off-by: Henry Sternberg Co-Authored-By: eisa01 Co-Authored-By: x7z4w Co-Authored-By: Yannik Bloscheck --- defines.hpp | 2 + generator/CMakeLists.txt | 2 + generator/generator_tool/generator_tool.cpp | 5 +- generator/road_penalty_generator.cpp | 253 ++++++++++++++++ generator/road_penalty_generator.hpp | 70 +++++ generator/routing_helpers.cpp | 11 +- generator/translator_country.cpp | 3 + generator/way_nodes_mapper.hpp | 7 + libs/geometry/distance_on_sphere.cpp | 9 + libs/geometry/distance_on_sphere.hpp | 2 + libs/geometry/point3d.hpp | 6 + libs/indexer/road_shields_parser.cpp | 10 +- libs/routing/CMakeLists.txt | 3 + libs/routing/edge_estimator.cpp | 283 ++++++++++++++++-- libs/routing/edge_estimator.hpp | 12 +- libs/routing/index_graph.cpp | 58 +++- libs/routing/index_graph.hpp | 12 +- libs/routing/index_graph_loader.cpp | 42 ++- libs/routing/road_penalty.cpp | 82 +++++ libs/routing/road_penalty.hpp | 127 ++++++++ libs/routing/road_penalty_serialization.hpp | 110 +++++++ libs/routing/routing_tests/CMakeLists.txt | 1 + .../routing_tests/index_graph_tools.cpp | 5 + .../routing_tests/index_graph_tools.hpp | 4 +- .../routing_tests/road_penalty_test.cpp | 169 +++++++++++ libs/routing_common/car_model_coefs.hpp | 2 +- libs/routing_common/vehicle_model.cpp | 38 +++ libs/routing_common/vehicle_model.hpp | 1 + .../routing/routing.xcodeproj/project.pbxproj | 17 ++ 29 files changed, 1301 insertions(+), 45 deletions(-) create mode 100644 generator/road_penalty_generator.cpp create mode 100644 generator/road_penalty_generator.hpp create mode 100644 libs/routing/road_penalty.cpp create mode 100644 libs/routing/road_penalty.hpp create mode 100644 libs/routing/road_penalty_serialization.hpp create mode 100644 libs/routing/routing_tests/road_penalty_test.cpp diff --git a/defines.hpp b/defines.hpp index 081f461a8..71d18a375 100644 --- a/defines.hpp +++ b/defines.hpp @@ -42,6 +42,7 @@ #define METADATA_FILE_TAG "meta" #define ALTITUDES_FILE_TAG "altitudes" #define ROAD_ACCESS_FILE_TAG "roadaccess" +#define ROAD_PENALTY_FILE_TAG "roadpenalty" #define RESTRICTIONS_FILE_TAG "restrictions" #define ROUTING_FILE_TAG "routing" #define CROSS_MWM_FILE_TAG "cross_mwm" @@ -90,6 +91,7 @@ #define GPS_TRACK_FILENAME "gps_track.dat" #define RESTRICTIONS_FILENAME "restrictions.csv" #define ROAD_ACCESS_FILENAME "road_access.bin" +#define ROAD_PENALTY_FILENAME "road_penalty.bin" #define METALINES_FILENAME "metalines.bin" #define CAMERAS_TO_WAYS_FILENAME "cameras_to_ways.bin" diff --git a/generator/CMakeLists.txt b/generator/CMakeLists.txt index 9da16f131..b1af498d3 100644 --- a/generator/CMakeLists.txt +++ b/generator/CMakeLists.txt @@ -186,6 +186,8 @@ set(SRC restriction_writer.hpp road_access_generator.cpp road_access_generator.hpp + road_penalty_generator.cpp + road_penalty_generator.hpp routing_city_boundaries_processor.cpp routing_city_boundaries_processor.hpp routing_helpers.cpp diff --git a/generator/generator_tool/generator_tool.cpp b/generator/generator_tool/generator_tool.cpp index 2853d8158..c0dcc8cef 100644 --- a/generator/generator_tool/generator_tool.cpp +++ b/generator/generator_tool/generator_tool.cpp @@ -21,6 +21,7 @@ #include "generator/raw_generator.hpp" #include "generator/restriction_generator.hpp" #include "generator/road_access_generator.hpp" +#include "generator/road_penalty_generator.hpp" #include "generator/routing_index_generator.hpp" #include "generator/routing_world_roads_generator.hpp" #include "generator/search_index_builder.hpp" @@ -466,6 +467,7 @@ MAIN_WITH_ERROR_HANDLING([](int argc, char ** argv) string const restrictionsFilename = genInfo.GetIntermediateFileName(RESTRICTIONS_FILENAME); string const roadAccessFilename = genInfo.GetIntermediateFileName(ROAD_ACCESS_FILENAME); + string const roadPenaltyFilename = genInfo.GetIntermediateFileName(ROAD_PENALTY_FILENAME); BuildRoutingIndex(dataFile, country, *countryParentGetter); auto routingGraph = CreateIndexGraph(dataFile, country, *countryParentGetter); @@ -475,7 +477,8 @@ MAIN_WITH_ERROR_HANDLING([](int argc, char ** argv) /// @todo CHECK return result doesn't work now for some small countries like Somalie. if (!BuildRoadRestrictions(*routingGraph, dataFile, restrictionsFilename, osmToFeatureFilename) || - !BuildRoadAccessInfo(dataFile, roadAccessFilename, *osm2feature)) + !BuildRoadAccessInfo(dataFile, roadAccessFilename, *osm2feature) || + !BuildRoadPenalty(dataFile, roadPenaltyFilename, *osm2feature)) { LOG(LERROR, ("Routing build failed for", dataFile)); } diff --git a/generator/road_penalty_generator.cpp b/generator/road_penalty_generator.cpp new file mode 100644 index 000000000..91de5b2d1 --- /dev/null +++ b/generator/road_penalty_generator.cpp @@ -0,0 +1,253 @@ +#include "generator/road_penalty_generator.hpp" + +#include "generator/feature_builder.hpp" +#include "generator/intermediate_data.hpp" +#include "generator/osm_element.hpp" +#include "generator/routing_helpers.hpp" + +#include "routing/road_penalty_serialization.hpp" +#include "routing/routing_helpers.hpp" +#include "routing/vehicle_mask.hpp" + +#include "coding/file_reader.hpp" +#include "coding/file_writer.hpp" +#include "coding/files_container.hpp" +#include "coding/internal/file_data.hpp" +#include "coding/varint.hpp" +#include "coding/write_to_sink.hpp" + +#include "base/assert.hpp" +#include "base/logging.hpp" + +#include + +namespace routing_builder +{ +using namespace feature; +using namespace generator; +using namespace routing; +using std::string, std::vector; + +namespace +{ +// Unified penalty mapping for all OSM tags that create road penalties +std::map const kUnifiedPenaltyMapping = { + // Traffic calming measures + + {OsmElement::Tag("traffic_calming", "rumble_strip"), RoadPenalty::Type::SmallCalming}, + {OsmElement::Tag("traffic_calming", "island"), RoadPenalty::Type::SmallCalming}, + + {OsmElement::Tag("traffic_calming", "cushion"), RoadPenalty::Type::MediumCalming}, + {OsmElement::Tag("traffic_calming", "chicane"), RoadPenalty::Type::MediumCalming}, + {OsmElement::Tag("traffic_calming", "choker"), RoadPenalty::Type::MediumCalming}, + {OsmElement::Tag("traffic_calming", "table"), RoadPenalty::Type::MediumCalming}, + + {OsmElement::Tag("traffic_calming", "bump"), RoadPenalty::Type::LargeCalming}, + {OsmElement::Tag("traffic_calming", "hump"), RoadPenalty::Type::LargeCalming}, + + {OsmElement::Tag("crossing", "unmarked"), RoadPenalty::Type::LargeCalming}, + + // Barrier penalties + {OsmElement::Tag("barrier", "gate"), RoadPenalty::Type::Gate}, + {OsmElement::Tag("barrier", "lift_gate"), RoadPenalty::Type::Gate}, + {OsmElement::Tag("barrier", "swing_gate"), RoadPenalty::Type::Gate}, + + // Junction penalties + {OsmElement::Tag("highway", "stop"), RoadPenalty::Type::UncontrolledJunction}, + {OsmElement::Tag("highway", "give_way"), RoadPenalty::Type::UncontrolledJunction}, + {OsmElement::Tag("crossing", "uncontrolled"), RoadPenalty::Type::UncontrolledJunction}, + {OsmElement::Tag("crossing", "marked"), RoadPenalty::Type::UncontrolledJunction}, + // highway=crossing ommitted as often accompanied by traffic_signals + + {OsmElement::Tag("highway", "traffic_signals"), RoadPenalty::Type::ControlledJunction}, + {OsmElement::Tag("railway", "crossing"), RoadPenalty::Type::ControlledJunction}, + {OsmElement::Tag("railway", "level_crossing"), RoadPenalty::Type::ControlledJunction}, +}; + +std::optional GetPenaltyByMapping(OsmElement const & elem) +{ + for (auto const & tag : elem.m_tags) + { + auto const it = kUnifiedPenaltyMapping.find(tag); + if (it != kUnifiedPenaltyMapping.cend()) + return it->second; + } + return {}; +} + +void ReadRoadPenalty(std::string const & penaltyPath, routing::OsmWay2FeaturePoint & way2Feature, + RoadPenaltyByVehicleType & penalties) +{ + FileReader reader(penaltyPath); + ReaderSource src(reader); + + // Read node penalties grouped by way + uint64_t nodeWayCount = ReadPrimitiveFromSource(src); + while (nodeWayCount-- > 0) + { + uint64_t wayID = ReadPrimitiveFromSource(src); + uint32_t nodeCount = ReadPrimitiveFromSource(src); + + for (uint32_t i = 0; i < nodeCount; ++i) + { + uint32_t nodeIdx = ReadPrimitiveFromSource(src); + m2::PointU coord; + coord.x = ReadPrimitiveFromSource(src); + coord.y = ReadPrimitiveFromSource(src); + VehicleMask vehicleMask = ReadPrimitiveFromSource(src); + + // Read penalty type and derive time based on vehicle type + RoadPenalty::Type type; + routing::Load(src, type); + + // For each vehicle type, create penalty with vehicle-specific time + for (uint8_t vType = 0; vType < static_cast(VehicleType::Count); ++vType) + { + if (vehicleMask & (1 << vType)) + { + // Create penalty with time specific to this vehicle type + auto vehicleSpecificPenalty = RoadPenalty::Penalty(type, static_cast(vType)); + + // Process this location only once + RoadPenalty::PointToPenalty pointToPenalty; + way2Feature.ForEachNodeIdx(wayID, nodeIdx, coord, [&](uint32_t featureID, uint32_t nodeIdx) + { pointToPenalty[RoadPoint(featureID, nodeIdx)] = vehicleSpecificPenalty; }); + + if (!pointToPenalty.empty()) + { + auto existingPenalties = penalties[vType].GetPointToPenalty(); + existingPenalties.insert(pointToPenalty.begin(), pointToPenalty.end()); + penalties[vType].SetPointPenalties(std::move(existingPenalties)); + } + } + } + } + } +} +} // namespace + +// RoadPenaltyCollector implementation +RoadPenaltyCollector::RoadPenaltyCollector(string const & filename, IDRInterfacePtr cache) + : generator::CollectorInterface(filename) + , m_cache(std::move(cache)) +{ + // Empty - initialization handled in member initializer list +} + +std::shared_ptr RoadPenaltyCollector::Clone(IDRInterfacePtr const & cache) const +{ + return std::make_shared(GetFilename(), cache ? cache : m_cache); +} + +void RoadPenaltyCollector::CollectFeature(feature::FeatureBuilder const & fb, OsmElement const & elem) +{ + // Track roads for barrier processing + if (elem.IsWay() && routing::IsCarRoad(fb.GetTypes())) + m_roads.AddWay(elem); + + // Process only nodes for penalty types + if (elem.IsNode()) + { + auto penaltyType = GetPenaltyByMapping(elem); + if (penaltyType) + m_nodesWithType.Add(elem.m_id, ms::LatLon(elem.m_lat, elem.m_lon), *penaltyType); + } +} + +void RoadPenaltyCollector::MergeInto(RoadPenaltyCollector & collector) const +{ + m_nodesWithType.MergeInto(collector.m_nodesWithType); + m_roads.MergeInto(collector.m_roads); +} + +void RoadPenaltyCollector::Save() +{ + auto const fileName = GetFilename(); + LOG(LINFO, ("Saving road penalty values to", fileName)); + FileWriter writer(fileName); + + // All vehicles use the same penalty types + + // Group node penalties by way + struct NodePenaltyEntry + { + uint32_t nodeIdx = 0; + m2::PointU coord; + RoadPenalty::Type type = RoadPenalty::Type::None; + }; + + std::map> penaltiesByWay; + + m_roads.ForEachWayWithIndex([&](uint64_t wayID, std::vector const & nodes, size_t idx) + { + std::vector wayNodePenalties; + + for (uint32_t nodeIdx = 0; nodeIdx < nodes.size(); ++nodeIdx) + { + uint64_t const nodeID = nodes[nodeIdx]; + auto const * barrierEntry = m_nodesWithType.Find(nodeID); + if (barrierEntry == nullptr) + continue; + + NodePenaltyEntry entry; + entry.nodeIdx = nodeIdx; + entry.coord = barrierEntry->m_coord; + entry.type = barrierEntry->m_t; + wayNodePenalties.push_back(entry); + } + + if (!wayNodePenalties.empty()) + penaltiesByWay[wayID] = std::move(wayNodePenalties); + }, m_cache); + + // Write node penalties grouped by way + uint64_t const wayCount = penaltiesByWay.size(); + WriteToSink(writer, wayCount); + for (auto const & [wayID, entries] : penaltiesByWay) + { + WriteToSink(writer, wayID); + WriteToSink(writer, static_cast(entries.size())); + + for (auto const & entry : entries) + { + WriteToSink(writer, entry.nodeIdx); + WriteToSink(writer, entry.coord.x); + WriteToSink(writer, entry.coord.y); + WriteToSink(writer, kAllVehiclesMask); + + // Store only penalty type (time derived from vehicle type when loading) + routing::Save(writer, entry.type); + } + } +} + +bool BuildRoadPenalty(string const & dataFilePath, string const & roadPenaltyPath, + routing::OsmWay2FeaturePoint & way2feature) +{ + LOG(LINFO, ("Generating road penalty info for", dataFilePath)); + + try + { + RoadPenaltyByVehicleType penalties; + ReadRoadPenalty(roadPenaltyPath, way2feature, penalties); + + FilesContainerW cont(dataFilePath, FileWriter::OP_WRITE_EXISTING); + auto writer = cont.GetWriter(ROAD_PENALTY_FILE_TAG); + + // Write number of vehicle types + uint32_t const vehicleTypeCount = penalties.size(); + WriteToSink(*writer, vehicleTypeCount); + + // Write penalty data for each vehicle type + for (size_t i = 0; i < penalties.size(); ++i) + RoadPenaltySerializer::Serialize(*writer, penalties[i]); + + return true; + } + catch (RootException const & ex) + { + LOG(LWARNING, ("No road penalty created:", ex.Msg())); + return false; + } +} +} // namespace routing_builder diff --git a/generator/road_penalty_generator.hpp b/generator/road_penalty_generator.hpp new file mode 100644 index 000000000..e42dde562 --- /dev/null +++ b/generator/road_penalty_generator.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include "generator/collector_interface.hpp" +#include "generator/feature_builder.hpp" +#include "generator/osm_element.hpp" +#include "generator/way_nodes_mapper.hpp" + +#include "routing/road_penalty.hpp" +#include "routing/vehicle_mask.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace routing +{ +class OsmWay2FeaturePoint; + +template +void Save(Sink & sink, RoadPenalty::Type const & type) +{ + WriteToSink(sink, static_cast(type)); +} + +template +void Load(Source & src, RoadPenalty::Type & type) +{ + uint8_t const res = ReadPrimitiveFromSource(src); + CHECK_LESS(res, static_cast(RoadPenalty::Type::Count), ()); + type = static_cast(res); +} +} // namespace routing + +namespace routing_builder +{ +using RoadPenalty = routing::RoadPenalty; +using VehicleType = routing::VehicleType; + +using RoadPenaltyByVehicleType = std::array(VehicleType::Count)>; + +class RoadPenaltyCollector : public generator::CollectorInterface +{ +public: + RoadPenaltyCollector(std::string const & filename, IDRInterfacePtr cache); + + std::shared_ptr Clone(IDRInterfacePtr const & cache = {}) const override; + + void CollectFeature(feature::FeatureBuilder const & fb, OsmElement const & elem) override; + + IMPLEMENT_COLLECTOR_IFACE(RoadPenaltyCollector); + void MergeInto(RoadPenaltyCollector & collector) const; + +protected: + void Save() override; + +private: + IDRInterfacePtr m_cache; + + // Store only penalty types during collection phase + generator::WayNodesMapper m_nodesWithType; + generator::WaysIDHolder m_roads; +}; + +bool BuildRoadPenalty(std::string const & dataFilePath, std::string const & roadPenaltyPath, + routing::OsmWay2FeaturePoint & way2feature); +} // namespace routing_builder diff --git a/generator/routing_helpers.cpp b/generator/routing_helpers.cpp index dabce7fe2..35d2d801c 100644 --- a/generator/routing_helpers.cpp +++ b/generator/routing_helpers.cpp @@ -88,9 +88,16 @@ public: { auto ft = m_featureGetter.GetFeatureByIndex(featureID); CHECK(ft, (featureID)); - CHECK(ft->GetGeomType() == feature::GeomType::Line, (featureID)); - // Converison should work with the same logic as in WayNodesMapper::EncodePoint. + // Skip non-line features (e.g., barriers on area boundaries) + if (ft->GetGeomType() != feature::GeomType::Line) + { + // Use LDEBUG to reduce log spam for features processed multiple times per vehicle type + LOG(LDEBUG, ("Skipping non-line feature", featureID, "for node penalty mapping")); + continue; + } + + // Conversion should work with the same logic as in WayNodesMapper::EncodePoint. auto const mercatorPt = PointUToPointD(pt, kPointCoordBits, fullRect); double minSquareDist = 1.0E6; diff --git a/generator/translator_country.cpp b/generator/translator_country.cpp index 442d28459..cd47c9a05 100644 --- a/generator/translator_country.cpp +++ b/generator/translator_country.cpp @@ -21,6 +21,7 @@ // #include "generator/node_mixer.hpp" #include "generator/restriction_writer.hpp" #include "generator/road_access_generator.hpp" +#include "generator/road_penalty_generator.hpp" #include "platform/platform.hpp" @@ -107,6 +108,8 @@ TranslatorCountry::TranslatorCountry(std::shared_ptr info.GetIntermediateFileName(RESTRICTIONS_FILENAME), cache->GetCache())); collectors->Append(std::make_shared( info.GetIntermediateFileName(ROAD_ACCESS_FILENAME), cache->GetCache())); + collectors->Append(std::make_shared( + info.GetIntermediateFileName(ROAD_PENALTY_FILENAME), cache->GetCache())); collectors->Append(std::make_shared( info.GetIntermediateFileName(CAMERAS_TO_WAYS_FILENAME), cache->GetCache())); collectors->Append(std::make_shared(info.GetIntermediateFileName(MINI_ROUNDABOUTS_FILENAME), diff --git a/generator/way_nodes_mapper.hpp b/generator/way_nodes_mapper.hpp index 4a3b35563..09e8751bc 100644 --- a/generator/way_nodes_mapper.hpp +++ b/generator/way_nodes_mapper.hpp @@ -146,6 +146,13 @@ public: void MergeInto(WaysMapper & mapper) const { mapper.m_ways.insert(m_ways.begin(), m_ways.end()); } + template + void ForEach(Fn && fn) const + { + for (auto const & [id, value] : m_ways) + fn(id, value); + } + template void Serialize(Sink & sink) const { diff --git a/libs/geometry/distance_on_sphere.cpp b/libs/geometry/distance_on_sphere.cpp index 760ded13d..5e97187ee 100644 --- a/libs/geometry/distance_on_sphere.cpp +++ b/libs/geometry/distance_on_sphere.cpp @@ -26,4 +26,13 @@ double DistanceOnEarth(LatLon const & ll1, LatLon const & ll2) { return DistanceOnEarth(ll1.m_lat, ll1.m_lon, ll2.m_lat, ll2.m_lon); } + +m3::Point ToVector(LatLon const & ll) +{ + double const lat = math::DegToRad(ll.m_lat); + double const lon = math::DegToRad(ll.m_lon); + return {kEarthRadiusMeters * cos(lat) * cos(lon), kEarthRadiusMeters * cos(lat) * sin(lon), + kEarthRadiusMeters * sin(lat)}; +} + } // namespace ms diff --git a/libs/geometry/distance_on_sphere.hpp b/libs/geometry/distance_on_sphere.hpp index 48fca4948..88e2c9317 100644 --- a/libs/geometry/distance_on_sphere.hpp +++ b/libs/geometry/distance_on_sphere.hpp @@ -1,6 +1,7 @@ #pragma once #include "geometry/latlon.hpp" +#include "geometry/point3d.hpp" // namespace ms - "math on sphere", similar to namespace m2. namespace ms @@ -14,4 +15,5 @@ double DistanceOnSphere(double lat1Deg, double lon1Deg, double lat2Deg, double l // lat1, lat2, lon1, lon2 - in degrees. double DistanceOnEarth(double lat1Deg, double lon1Deg, double lat2Deg, double lon2Deg); double DistanceOnEarth(LatLon const & ll1, LatLon const & ll2); +m3::Point ToVector(LatLon const & ll); } // namespace ms diff --git a/libs/geometry/point3d.hpp b/libs/geometry/point3d.hpp index 1ffc95848..338725504 100644 --- a/libs/geometry/point3d.hpp +++ b/libs/geometry/point3d.hpp @@ -14,6 +14,12 @@ class Point public: Point() = default; constexpr Point(T x_, T y_, T z_) : x(x_), y(y_), z(z_) {} + Point operator+(Point const & obj) { return {x + obj.x, y + obj.y, z + obj.z}; } + Point operator-(Point const & obj) { return {x - obj.x, y - obj.y, z - obj.z}; } + + Point operator*(T const & obj) { return {x * obj, y * obj, z * obj}; } + + Point operator/(T const & obj) { return {x / obj, y / obj, z / obj}; } T Length() const { return std::sqrt(x * x + y * y + z * z); } diff --git a/libs/indexer/road_shields_parser.cpp b/libs/indexer/road_shields_parser.cpp index 1ae7e550e..e6e2ac1cd 100644 --- a/libs/indexer/road_shields_parser.cpp +++ b/libs/indexer/road_shields_parser.cpp @@ -578,11 +578,11 @@ public: {"LF.", RoadShieldType::Generic_White}, // Blue font. {"İK.", RoadShieldType::Generic_White}, // Blue font. // South Cyprus. - {"A", RoadShieldType::Generic_Green}, // Yellow font. Hexagon. - {"B", RoadShieldType::Generic_Blue}, // Yellow font. - {"E", RoadShieldType::Generic_Blue}, // Yellow font. - {"F", RoadShieldType::Generic_Blue}, // Yellow font. - {"U", RoadShieldType::Generic_Blue}}) // Yellow font. + {"A", RoadShieldType::Generic_Green}, // Yellow font. Hexagon. + {"B", RoadShieldType::Generic_Blue}, // Yellow font. + {"E", RoadShieldType::Generic_Blue}, // Yellow font. + {"F", RoadShieldType::Generic_Blue}, // Yellow font. + {"U", RoadShieldType::Generic_Blue}}) // Yellow font. {} }; diff --git a/libs/routing/CMakeLists.txt b/libs/routing/CMakeLists.txt index dfb9603e7..8194471db 100644 --- a/libs/routing/CMakeLists.txt +++ b/libs/routing/CMakeLists.txt @@ -113,6 +113,9 @@ set(SRC road_access.hpp road_access_serialization.cpp road_access_serialization.hpp + road_penalty.cpp + road_penalty.hpp + road_penalty_serialization.hpp road_graph.cpp road_graph.hpp road_index.cpp diff --git a/libs/routing/edge_estimator.cpp b/libs/routing/edge_estimator.cpp index 628485cd2..1fb3bf2db 100644 --- a/libs/routing/edge_estimator.cpp +++ b/libs/routing/edge_estimator.cpp @@ -1,5 +1,4 @@ #include "routing/edge_estimator.hpp" - #include "routing/geometry.hpp" #include "routing/latlon_with_altitude.hpp" #include "routing/routing_helpers.hpp" @@ -10,10 +9,11 @@ #include "geometry/distance_on_sphere.hpp" #include "geometry/point_with_altitude.hpp" -#include "base/assert.hpp" +#include "coding/csv_reader.hpp" -#include -#include +#include "base/assert.hpp" +#include "base/logging.hpp" +#include "platform/platform.hpp" namespace routing { @@ -166,10 +166,12 @@ double GetCarClimbPenalty(EdgeEstimator::Purpose, double, geometry::Altitude) } // EdgeEstimator ----------------------------------------------------------------------------------- -EdgeEstimator::EdgeEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH, +EdgeEstimator::EdgeEstimator(VehicleType vehicleType, double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH, DataSource * /*dataSourcePtr*/, std::shared_ptr /*numMwmIds*/) - : m_maxWeightSpeedMpS(KmphToMps(maxWeightSpeedKMpH)) + : m_vehicleType(vehicleType) + , m_maxWeightSpeedMpS(KmphToMps(maxWeightSpeedKMpH)) , m_offroadSpeedKMpH(offroadSpeedKMpH) + //, m_dataSourcePtr(dataSourcePtr) //, m_numMwmIds(numMwmIds) { @@ -179,6 +181,204 @@ EdgeEstimator::EdgeEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroa if (m_offroadSpeedKMpH.m_eta != kNotUsed) CHECK_GREATER_OR_EQUAL(m_maxWeightSpeedMpS, KmphToMps(m_offroadSpeedKMpH.m_eta), ()); + + LOG(LINFO, ("Loading turn penalties for vehicle type:", static_cast(vehicleType))); + + struct TurnPenaltyMatrix + { + int road; + VehicleType vehicleType; + double penalty; + }; + + struct TurnPenalty + { + HighwayType fromRoadType; + HighwayType toRoadType; + VehicleType vehicleType; + double penalty; + }; + +#define N 144 + + static auto constexpr kTurnPenaltyMatrix = [] + { + array constexpr kTable = {{ + {HighwayType::HighwayLivingStreet, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayPrimary, VehicleType::Car, 0.09}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayResidential, VehicleType::Car, 0.07}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwaySecondary, VehicleType::Car, 0.08}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayService, VehicleType::Car, 0.07}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayTrunk, VehicleType::Car, 0.09}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayLivingStreet, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07}, + {HighwayType::HighwayPrimary, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.11}, + {HighwayType::HighwayPrimary, HighwayType::HighwayPrimary, VehicleType::Car, 0.06}, + {HighwayType::HighwayPrimary, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.06}, + {HighwayType::HighwayPrimary, HighwayType::HighwayResidential, VehicleType::Car, 0.1}, + {HighwayType::HighwayPrimary, HighwayType::HighwaySecondary, VehicleType::Car, 0.07}, + {HighwayType::HighwayPrimary, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayPrimary, HighwayType::HighwayService, VehicleType::Car, 0.1}, + {HighwayType::HighwayPrimary, HighwayType::HighwayTertiary, VehicleType::Car, 0.08}, + {HighwayType::HighwayPrimary, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayPrimary, HighwayType::HighwayTrunk, VehicleType::Car, 0.04}, + {HighwayType::HighwayPrimary, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.04}, + {HighwayType::HighwayPrimary, HighwayType::HighwayUnclassified, VehicleType::Car, 0.1}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.1}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.06}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.06}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayResidential, VehicleType::Car, 0.1}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.06}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.06}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayService, VehicleType::Car, 0.09}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.08}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.07}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayPrimaryLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.1}, + {HighwayType::HighwayResidential, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07}, + {HighwayType::HighwayResidential, HighwayType::HighwayPrimary, VehicleType::Car, 0.09}, + {HighwayType::HighwayResidential, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayResidential, HighwayType::HighwayResidential, VehicleType::Car, 0.08}, + {HighwayType::HighwayResidential, HighwayType::HighwaySecondary, VehicleType::Car, 0.08}, + {HighwayType::HighwayResidential, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwayResidential, HighwayType::HighwayService, VehicleType::Car, 0.07}, + {HighwayType::HighwayResidential, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwayResidential, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayResidential, HighwayType::HighwayTrunk, VehicleType::Car, 0.09}, + {HighwayType::HighwayResidential, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayResidential, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondary, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.1}, + {HighwayType::HighwaySecondary, HighwayType::HighwayPrimary, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondary, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondary, HighwayType::HighwayResidential, VehicleType::Car, 0.1}, + {HighwayType::HighwaySecondary, HighwayType::HighwaySecondary, VehicleType::Car, 0.08}, + {HighwayType::HighwaySecondary, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwaySecondary, HighwayType::HighwayService, VehicleType::Car, 0.08}, + {HighwayType::HighwaySecondary, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondary, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondary, HighwayType::HighwayTrunk, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondary, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.06}, + {HighwayType::HighwaySecondary, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayResidential, VehicleType::Car, 0.07}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.05}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.05}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayService, VehicleType::Car, 0.06}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.05}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.05}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.08}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.08}, + {HighwayType::HighwaySecondaryLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.06}, + {HighwayType::HighwayService, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayPrimary, VehicleType::Car, 0.09}, + {HighwayType::HighwayService, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayService, HighwayType::HighwayResidential, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwaySecondary, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayService, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayTrunk, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayService, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.08}, + {HighwayType::HighwayTertiary, HighwayType::HighwayPrimary, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayResidential, VehicleType::Car, 0.08}, + {HighwayType::HighwayTertiary, HighwayType::HighwaySecondary, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayService, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayTrunk, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiary, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayResidential, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayService, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayTertiaryLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07}, + {HighwayType::HighwayTrunk, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.1}, + {HighwayType::HighwayTrunk, HighwayType::HighwayPrimary, VehicleType::Car, 0.05}, + {HighwayType::HighwayTrunk, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.03}, + {HighwayType::HighwayTrunk, HighwayType::HighwayResidential, VehicleType::Car, 0.09}, + {HighwayType::HighwayTrunk, HighwayType::HighwaySecondary, VehicleType::Car, 0.08}, + {HighwayType::HighwayTrunk, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwayTrunk, HighwayType::HighwayService, VehicleType::Car, 0.08}, + {HighwayType::HighwayTrunk, HighwayType::HighwayTertiary, VehicleType::Car, 0.08}, + {HighwayType::HighwayTrunk, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwayTrunk, HighwayType::HighwayTrunk, VehicleType::Car, 0.01}, + {HighwayType::HighwayTrunk, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.01}, + {HighwayType::HighwayTrunk, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.11}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.04}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.04}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayResidential, VehicleType::Car, 0.1}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.04}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.04}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayService, VehicleType::Car, 0.07}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.06}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.06}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.07}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.02}, + {HighwayType::HighwayTrunkLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.1}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayPrimary, VehicleType::Car, 0.09}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayResidential, VehicleType::Car, 0.07}, + {HighwayType::HighwayUnclassified, HighwayType::HighwaySecondary, VehicleType::Car, 0.08}, + {HighwayType::HighwayUnclassified, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayService, VehicleType::Car, 0.08}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayTertiary, VehicleType::Car, 0.07}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayTrunk, VehicleType::Car, 0.09}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.09}, + {HighwayType::HighwayUnclassified, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08}, + }}; + + array result{}; + + for (size_t i = 0; i < N; ++i) + result[i] = {static_cast(kTable[i].fromRoadType) * 65535 + static_cast(kTable[i].toRoadType), + kTable[i].vehicleType, kTable[i].penalty}; + + return result; + }(); + + double totalPenalty = 0; + + for (TurnPenaltyMatrix const & row : kTurnPenaltyMatrix) + { + if (row.vehicleType != vehicleType) + continue; + + m_turnPenaltyMap[row.road] += row.penalty; + totalPenalty += row.penalty; + } + + if (!m_turnPenaltyMap.empty()) + { + m_defaultPenalty = totalPenalty / m_turnPenaltyMap.size(); + LOG(LINFO, ("Loaded", m_turnPenaltyMap.size(), "turn penalties with default:", m_defaultPenalty)); + } + else + { + LOG(LWARNING, ("No turn penalties loaded for vehicle type:", static_cast(vehicleType))); + } } double EdgeEstimator::CalcHeuristic(ms::LatLon const & from, ms::LatLon const & to) const @@ -265,11 +465,18 @@ class PedestrianEstimator final : public EdgeEstimator { public: PedestrianEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH) - : EdgeEstimator(maxWeightSpeedKMpH, offroadSpeedKMpH) + : EdgeEstimator(VehicleType::Pedestrian, maxWeightSpeedKMpH, offroadSpeedKMpH) {} // EdgeEstimator overrides: double GetUTurnPenalty(Purpose /* purpose */) const override { return 0.0 /* seconds */; } + + double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road, + bool is_left_hand_traffic = false) const override + { + return 0; + } + double GetFerryLandingPenalty(Purpose purpose) const override { switch (purpose) @@ -293,11 +500,18 @@ class BicycleEstimator final : public EdgeEstimator { public: BicycleEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH) - : EdgeEstimator(maxWeightSpeedKMpH, offroadSpeedKMpH) + : EdgeEstimator(VehicleType::Bicycle, maxWeightSpeedKMpH, offroadSpeedKMpH) {} // EdgeEstimator overrides: double GetUTurnPenalty(Purpose /* purpose */) const override { return 20.0 /* seconds */; } + + double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road, + bool is_left_hand_traffic = false) const override + { + return 0; + } + double GetFerryLandingPenalty(Purpose purpose) const override { switch (purpose) @@ -349,20 +563,15 @@ class CarEstimator final : public EdgeEstimator public: CarEstimator(DataSource * dataSourcePtr, std::shared_ptr numMwmIds, shared_ptr trafficStash, double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH) - : EdgeEstimator(maxWeightSpeedKMpH, offroadSpeedKMpH, dataSourcePtr, numMwmIds) + : EdgeEstimator(VehicleType::Car, maxWeightSpeedKMpH, offroadSpeedKMpH, dataSourcePtr, numMwmIds) , m_trafficStash(std::move(trafficStash)) {} // EdgeEstimator overrides: double CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const override; - double GetUTurnPenalty(Purpose /* purpose */) const override - { - // Adds 2 minutes penalty for U-turn. The value is quite arbitrary - // and needs to be properly selected after a number of real-world - // experiments. - return 2 * 60; // seconds - } - + double GetUTurnPenalty(Purpose /* purpose */) const override; + double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road, + bool is_left_hand_traffic = false) const override; double GetFerryLandingPenalty(Purpose purpose) const override { switch (purpose) @@ -374,9 +583,47 @@ public: } private: - shared_ptr m_trafficStash; + std::shared_ptr m_trafficStash; }; +double CarEstimator::GetUTurnPenalty(Purpose /* purpose */) const +{ + // Adds 2 minutes penalty for U-turn. The value is quite arbitrary + // and needs to be properly selected after a number of real-world + // experiments. + return 2 * 60; // seconds +} + +double CarEstimator::GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, + RoadGeometry const & to_road, bool is_left_hand_traffic) const +{ + auto penalty = m_defaultPenalty; + + if (from_road.GetHighwayType().has_value() && to_road.GetHighwayType().has_value()) + { + int const from_road_idx = static_cast(from_road.GetHighwayType().value()); + int const to_road_idx = static_cast(to_road.GetHighwayType().value()); + auto const pen = m_turnPenaltyMap.find(from_road_idx * 65535 + to_road_idx); + if (pen != m_turnPenaltyMap.end()) + penalty = pen->second; + } + + // Determine if turn crosses traffic based on driving side + // @TODO We should really account for oneway roads etc. + + bool turn_crosses_traffic; + if (is_left_hand_traffic) + turn_crosses_traffic = angle < 0; + else + turn_crosses_traffic = angle > 0; + + // Twice as long to turn across traffic than not to + auto const extra_penalty = turn_crosses_traffic ? 2.0 : 1.0; + auto const result = fabs(angle) * penalty * extra_penalty; + + return result; +} + double CarEstimator::CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const { double result = road.GetDistance(segment.GetSegmentIdx()) / GetSpeedMpS(purpose, segment, road); diff --git a/libs/routing/edge_estimator.hpp b/libs/routing/edge_estimator.hpp index 4dedcd954..98a66169a 100644 --- a/libs/routing/edge_estimator.hpp +++ b/libs/routing/edge_estimator.hpp @@ -10,6 +10,7 @@ #include "geometry/point_with_altitude.hpp" #include +#include class DataSource; @@ -27,8 +28,8 @@ public: ETA }; - EdgeEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH, DataSource * dataSourcePtr = nullptr, - std::shared_ptr numMwmIds = nullptr); + EdgeEstimator(VehicleType vehicleType, double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH, + DataSource * dataSourcePtr = nullptr, std::shared_ptr numMwmIds = nullptr); virtual ~EdgeEstimator() = default; double CalcHeuristic(ms::LatLon const & from, ms::LatLon const & to) const; @@ -47,6 +48,8 @@ public: virtual double CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const = 0; virtual double GetUTurnPenalty(Purpose purpose) const = 0; + virtual double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, + RoadGeometry const & to_road, bool is_left_hand_traffic = false) const = 0; virtual double GetFerryLandingPenalty(Purpose purpose) const = 0; static std::shared_ptr Create(VehicleType vehicleType, double maxWeighSpeedKMpH, @@ -58,6 +61,11 @@ public: std::shared_ptr trafficStash, DataSource * dataSourcePtr, std::shared_ptr numMwmIds); +protected: + VehicleType const m_vehicleType; + double m_defaultPenalty = 0.0; + std::unordered_map m_turnPenaltyMap; + private: double const m_maxWeightSpeedMpS; SpeedKMpH const m_offroadSpeedKMpH; diff --git a/libs/routing/index_graph.cpp b/libs/routing/index_graph.cpp index eff85d3ba..a2de29691 100644 --- a/libs/routing/index_graph.cpp +++ b/libs/routing/index_graph.cpp @@ -11,9 +11,9 @@ #include "base/exception.hpp" #include "base/timer.hpp" +#include "geometry/distance_on_sphere.hpp" + #include -#include -#include #include #include @@ -34,13 +34,22 @@ bool IsBoarding(bool prevIsFerry, bool nextIsFerry) } IndexGraph::IndexGraph(shared_ptr geometry, shared_ptr estimator, - RoutingOptions routingOptions) + RoutingOptions routingOptions, feature::RegionData const * regionData) : m_geometry(std::move(geometry)) , m_estimator(std::move(estimator)) , m_avoidRoutingOptions(routingOptions) { CHECK(m_geometry, ()); CHECK(m_estimator, ()); + if (regionData) + { + auto const driving_side = regionData->Get(feature::RegionData::RD_DRIVING); + m_isLeftHandTraffic = (driving_side == "l"); + } + else + { + m_isLeftHandTraffic = false; + } } bool IndexGraph::IsJoint(RoadPoint const & roadPoint) const @@ -229,6 +238,11 @@ void IndexGraph::SetRoadAccess(RoadAccess && roadAccess) m_roadAccess.SetCurrentTimeGetter(m_currentTimeGetter); } +void IndexGraph::SetRoadPenalty(RoadPenalty && roadPenalty) +{ + m_roadPenalty = std::move(roadPenalty); +} + void IndexGraph::GetNeighboringEdges(astar::VertexData const & fromVertexData, RoadPoint const & rp, bool isOutgoing, bool useRoutingOptions, SegmentEdgeListT & edges, Parents const & parents, @@ -340,7 +354,7 @@ void IndexGraph::ReconstructJointSegment(astar::VertexDatam_timeSeconds; + switch (rpConfidence) { case RoadAccess::Confidence::Sure: @@ -478,7 +498,7 @@ RouteWeight IndexGraph::GetPenalties(EdgeEstimator::Purpose purpose, Segment con if (IsBoarding(fromPenaltyData.m_isFerry, toPenaltyData.m_isFerry)) weightPenalty += m_estimator->GetFerryLandingPenalty(purpose); - return {weightPenalty /* weight */, passThroughPenalty, accessPenalty, accessConditionalPenalties, + return {penaltyTime + weightPenalty /* weight */, passThroughPenalty, accessPenalty, accessConditionalPenalties, 0.0 /* transitTime */}; } @@ -516,12 +536,30 @@ RouteWeight IndexGraph::CalculateEdgeWeight(EdgeEstimator::Purpose purpose, bool Segment const & to, std::optional const & prevWeight) const { - auto const & segment = isOutgoing ? to : from; - auto const & road = GetRoadGeometry(segment.GetFeatureId()); - - auto const weight = RouteWeight(m_estimator->CalcSegmentWeight(segment, road, purpose)); + auto const & to_segment = isOutgoing ? to : from; + auto const & from_segment = isOutgoing ? from : to; + auto const & to_road = GetRoadGeometry(to_segment.GetFeatureId()); + auto const weight = RouteWeight(m_estimator->CalcSegmentWeight(to_segment, to_road, purpose)); auto const penalties = GetPenalties(purpose, isOutgoing ? from : to, isOutgoing ? to : from, prevWeight); + auto const turn_penalty = getTurnPenalty(purpose, from_segment, to_segment); + return weight + penalties + turn_penalty; +} - return weight + penalties; +RouteWeight IndexGraph::getTurnPenalty(EdgeEstimator::Purpose purpose, Segment const & from, Segment const & to) const +{ + if (from.GetFeatureId() == to.GetFeatureId()) + return {0, 0, 0, 0, 0}; + auto const & to_road = GetRoadGeometry(to.GetFeatureId()); + auto const & from_road = GetRoadGeometry(from.GetFeatureId()); + auto v1 = ms::ToVector(GetPoint(from, true)) - ms::ToVector(GetPoint(from, false)); + auto v2 = ms::ToVector(GetPoint(to, true)) - ms::ToVector(GetPoint(to, false)); + auto const dotLen = v1.Length() * v2.Length(); + if (dotLen == 0) + return {0, 0, 0, 0, 0}; + auto normal = ms::ToVector(GetPoint(from, true)) + ms::ToVector(GetPoint(to, false)); + normal = normal / normal.Length(); + auto const signed_angle = + math::RadToDeg(atan2(m3::DotProduct(m3::CrossProduct(v1, v2), normal), m3::DotProduct(v1, v2))); + return {m_estimator->GetTurnPenalty(purpose, signed_angle, from_road, to_road, m_isLeftHandTraffic), 0, 0, 0, 0}; } } // namespace routing diff --git a/libs/routing/index_graph.hpp b/libs/routing/index_graph.hpp index b065d9bcb..881956d63 100644 --- a/libs/routing/index_graph.hpp +++ b/libs/routing/index_graph.hpp @@ -11,10 +11,13 @@ #include "routing/restrictions_serialization.hpp" #include "routing/road_access.hpp" #include "routing/road_index.hpp" +#include "routing/road_penalty.hpp" #include "routing/road_point.hpp" #include "routing/routing_options.hpp" #include "routing/segment.hpp" +#include "indexer/feature_meta.hpp" + #include "geometry/point2d.hpp" #include @@ -49,7 +52,7 @@ public: IndexGraph() = default; IndexGraph(std::shared_ptr geometry, std::shared_ptr estimator, - RoutingOptions routingOptions = RoutingOptions()); + RoutingOptions routingOptions = RoutingOptions(), feature::RegionData const * regionData = nullptr); // Put outgoing (or ingoing) egdes for segment to the 'edges' vector. void GetEdgeList(astar::VertexData const & vertexData, bool isOutgoing, bool useRoutingOptions, @@ -89,6 +92,7 @@ public: void SetRestrictions(RestrictionVec && restrictions); void SetUTurnRestrictions(std::vector && noUTurnRestrictions); void SetRoadAccess(RoadAccess && roadAccess); + void SetRoadPenalty(RoadPenalty && roadPenalty); void PushFromSerializer(Joint::Id jointId, RoadPoint const & rp) { m_roadIndex.PushFromSerializer(jointId, rp); } @@ -186,10 +190,8 @@ private: std::shared_ptr m_estimator; RoadIndex m_roadIndex; JointIndex m_jointIndex; - Restrictions m_restrictionsForward; Restrictions m_restrictionsBackward; - // u_turn can be in both sides of feature. struct UTurnEnding { @@ -202,11 +204,13 @@ private: // If m_noUTurnRestrictions.count(featureId) == 0, that means, that there are no any // no_u_turn restriction at the feature with id = featureId. std::unordered_map m_noUTurnRestrictions; - RoadAccess m_roadAccess; + RoadPenalty m_roadPenalty; RoutingOptions m_avoidRoutingOptions; + bool m_isLeftHandTraffic; std::function m_currentTimeGetter = []() { return GetCurrentTimestamp(); }; + RouteWeight getTurnPenalty(EdgeEstimator::Purpose purpose, Segment const & from, Segment const & to) const; }; template diff --git a/libs/routing/index_graph_loader.cpp b/libs/routing/index_graph_loader.cpp index 87210cdf5..c342bae62 100644 --- a/libs/routing/index_graph_loader.cpp +++ b/libs/routing/index_graph_loader.cpp @@ -5,6 +5,8 @@ #include "routing/restriction_loader.hpp" #include "routing/road_access.hpp" #include "routing/road_access_serialization.hpp" +#include "routing/road_penalty.hpp" +#include "routing/road_penalty_serialization.hpp" #include "routing/route.hpp" #include "routing/speed_camera_ser_des.hpp" @@ -138,7 +140,7 @@ IndexGraphLoaderImpl::GraphPtrT IndexGraphLoaderImpl::CreateIndexGraph(NumMwmId geometry = make_shared(GeometryLoader::Create(handle, std::move(vehicleModel), m_loadAltitudes)); } - auto graph = make_unique(geometry, m_estimator, m_avoidRoutingOptions); + auto graph = make_unique(geometry, m_estimator, m_avoidRoutingOptions, &value->GetRegionData()); graph->SetCurrentTimeGetter(m_currentTimeGetter); DeserializeIndexGraph(*value, m_vehicleType, *graph); @@ -209,6 +211,40 @@ bool ReadRoadAccessFromMwm(MwmValue const & mwmValue, VehicleType vehicleType, R return false; } +bool ReadRoadPenaltyFromMwm(MwmValue const & mwmValue, VehicleType vehicleType, RoadPenalty & roadPenalty) +{ + try + { + auto const reader = mwmValue.m_cont.GetReader(ROAD_PENALTY_FILE_TAG); + ReaderSource src(reader); + + // Read number of vehicle types + uint32_t numVehicleTypes = ReadPrimitiveFromSource(src); + CHECK_EQUAL(numVehicleTypes, static_cast(VehicleType::Count), ()); + + // Skip to the correct vehicle type + for (uint32_t i = 0; i < static_cast(vehicleType); ++i) + { + RoadPenalty dummy; + RoadPenaltySerializer::Deserialize(src, dummy, static_cast(i)); + } + + // Read the penalty data for this vehicle type + RoadPenaltySerializer::Deserialize(src, roadPenalty, vehicleType); + return true; + } + catch (Reader::OpenException const &) + { + // This is expected for older mwm files - not an error + LOG(LDEBUG, (ROAD_PENALTY_FILE_TAG, "section not found - using legacy penalty system")); + } + catch (Reader::Exception const & e) + { + LOG(LERROR, ("Error while reading", ROAD_PENALTY_FILE_TAG, "section.", e.Msg())); + } + return false; +} + // static unique_ptr IndexGraphLoader::Create(VehicleType vehicleType, bool loadAltitudes, shared_ptr vehicleModelFactory, @@ -244,6 +280,10 @@ void DeserializeIndexGraph(MwmValue const & mwmValue, VehicleType vehicleType, I RoadAccess roadAccess; if (ReadRoadAccessFromMwm(mwmValue, vehicleType, roadAccess)) graph.SetRoadAccess(std::move(roadAccess)); + + RoadPenalty roadPenalty; + if (ReadRoadPenaltyFromMwm(mwmValue, vehicleType, roadPenalty)) + graph.SetRoadPenalty(std::move(roadPenalty)); } uint32_t DeserializeIndexGraphNumRoads(MwmValue const & mwmValue, VehicleType vehicleType) diff --git a/libs/routing/road_penalty.cpp b/libs/routing/road_penalty.cpp new file mode 100644 index 000000000..e76a38177 --- /dev/null +++ b/libs/routing/road_penalty.cpp @@ -0,0 +1,82 @@ +#include "routing/road_penalty.hpp" + +#include "base/assert.hpp" + +#include + +namespace routing +{ +namespace penalty_impl +{ +// Array of strings must be in the same order as RoadPenalty::Type enum +std::string const kNames[] = {"None", "SmallCalming", "MediumCalming", "LargeCalming", + "Gate", "UncontrolledJunction", "ControlledJunction", "Count"}; +} // namespace penalty_impl + +std::optional RoadPenalty::GetPenalty(RoadPoint const & point) const +{ + auto const it = m_pointToPenalty.find(point); + if (it != m_pointToPenalty.end()) + return it->second; + return {}; +} + +bool RoadPenalty::operator==(RoadPenalty const & rhs) const +{ + return m_pointToPenalty == rhs.m_pointToPenalty; +} + +std::string ToString(RoadPenalty::Type type) +{ + if (type <= RoadPenalty::Type::Count) + return penalty_impl::kNames[static_cast(type)]; + ASSERT(false, ("Bad RoadPenalty::Type", static_cast(type))); + return "Bad RoadPenalty::Type"; +} + +void FromString(std::string_view s, RoadPenalty::Type & result) +{ + for (size_t i = 0; i <= static_cast(RoadPenalty::Type::Count); ++i) + { + if (s == penalty_impl::kNames[i]) + { + result = static_cast(i); + return; + } + } + ASSERT(false, ("Could not read RoadPenalty::Type from string", s)); +} + +std::string DebugPrint(RoadPenalty::Type type) +{ + return ToString(type); +} + +std::string DebugPrint(RoadPenalty::Penalty const & penalty) +{ + std::ostringstream oss; + oss << "Penalty(" << DebugPrint(penalty.m_type) << ", " << penalty.m_timeSeconds << "s)"; + return oss.str(); +} + +std::string DebugPrint(RoadPenalty const & roadPenalty) +{ + size_t constexpr kMaxKV = 10; + std::ostringstream oss; + oss << "RoadPenalty { PointToPenalty: ["; + size_t i = 0; + for (auto const & kv : roadPenalty.GetPointToPenalty()) + { + if (i > 0) + oss << ", "; + oss << "(" << DebugPrint(kv.first) << ", " << DebugPrint(kv.second) << ")"; + ++i; + if (i == kMaxKV) + break; + } + if (roadPenalty.GetPointToPenalty().size() > kMaxKV) + oss << ", ..."; + oss << "] }"; + return oss.str(); +} +} // namespace routing diff --git a/libs/routing/road_penalty.hpp b/libs/routing/road_penalty.hpp new file mode 100644 index 000000000..5c1d9c711 --- /dev/null +++ b/libs/routing/road_penalty.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include "routing/road_point.hpp" +#include "routing/vehicle_mask.hpp" + +#include +#include +#include + +#include "3party/skarupke/flat_hash_map.hpp" + +namespace routing +{ +// This class provides information about road penalties (time delays) +// that are separate from access restrictions. Examples include: +// - Speed bumps and traffic calming measures +// - Traffic signals +// - Gates that take time to open +// One instance of RoadPenalty holds information about one mwm. +// +// FILE FORMATS: +// +// 1. Intermediate file (from RoadPenaltyCollector::Save()): +// [uint64_t] Number of ways with penalties +// For each way: +// [uint64_t] Way ID +// [uint32_t] Number of nodes with penalties +// For each node: +// [uint32_t] Node index within way +// [uint32_t] Coordinate X +// [uint32_t] Coordinate Y +// [VehicleMask] Vehicle mask (uint32_t) +// [uint8_t] Penalty type +// +// 2. Final MWM file: +// [uint32_t] Number of vehicle types (4) +// For each vehicle type: +// [uint32_t] Number of point penalties +// For each penalty: +// [uint32_t] Feature ID +// [uint32_t] Point ID +// [uint8_t] Penalty type (time derived from type + vehicle) +class RoadPenalty final +{ +public: + // Types of penalties that can be applied to roads/nodes + enum class Type : uint8_t + { + None = 0, + + // Traffic calming devices with different severities + SmallCalming, // Traffic islands + MediumCalming, // Cushions, chicanes, tables, choker + LargeCalming, // Humps, bumps + + // Gates and barriers + Gate, // Emergency gates, barrier gates + + // Traffic control + UncontrolledJunction, + ControlledJunction, + + // The number of different penalty types + Count + }; + + struct Penalty + { + Type m_type = Type::None; + uint16_t m_timeSeconds = 0; // Time penalty in seconds (0-65535) + + Penalty() = default; + + // Constructor that derives time from type and vehicle + Penalty(Type type, VehicleType vehicleType) : m_type(type), m_timeSeconds(GetTimePenalty(type, vehicleType)) {} + + bool operator==(Penalty const & rhs) const { return m_type == rhs.m_type && m_timeSeconds == rhs.m_timeSeconds; } + + bool operator!=(Penalty const & rhs) const { return !(*this == rhs); } + + // Get default time penalty for a given type and vehicle + static uint16_t GetTimePenalty(Type type, VehicleType vehicleType) + { + // Penalty times lookup table [Type][VehicleType] + // Rows: penalty types (None, SmallCalming, MediumCalming, LargeCalming, Gate, Uncontrolled, Controlled) + // Columns: vehicle types (Pedestrian, Bicycle, Car, Transit) + static constexpr uint16_t kPenaltyTimes[7][4] = { + // Ped, Bic, Car, Tran + {0, 0, 0, 0}, // None + {0, 0, 3, 0}, // SmallCalming + {0, 0, 5, 0}, // MediumCalming + {0, 0, 7, 0}, // LargeCalming + {10, 10, 30, 0}, // Gate + {0, 10, 5, 0}, // Uncontrolled Junction + {0, 15, 10, 0}, // Controlled Junction + }; + + if (type >= Type::Count || vehicleType >= VehicleType::Count) + return 0; + + return kPenaltyTimes[static_cast(type)][static_cast(vehicleType)]; + } + }; + + using PointToPenalty = ska::flat_hash_map; + + RoadPenalty() = default; + + PointToPenalty const & GetPointToPenalty() const { return m_pointToPenalty; } + + std::optional GetPenalty(RoadPoint const & point) const; + + void SetPointPenalties(PointToPenalty && penalties) { m_pointToPenalty = std::move(penalties); } + + bool operator==(RoadPenalty const & rhs) const; + +private: + PointToPenalty m_pointToPenalty; +}; + +std::string ToString(RoadPenalty::Type type); +void FromString(std::string_view s, RoadPenalty::Type & result); + +std::string DebugPrint(RoadPenalty::Type type); +std::string DebugPrint(RoadPenalty::Penalty const & penalty); +std::string DebugPrint(RoadPenalty const & roadPenalty); +} // namespace routing diff --git a/libs/routing/road_penalty_serialization.hpp b/libs/routing/road_penalty_serialization.hpp new file mode 100644 index 000000000..b472fc1d7 --- /dev/null +++ b/libs/routing/road_penalty_serialization.hpp @@ -0,0 +1,110 @@ +#pragma once + +#include "routing/road_penalty.hpp" +#include "routing/road_point.hpp" + +#include "coding/reader.hpp" +#include "coding/write_to_sink.hpp" + +#include "base/assert.hpp" + +#include +#include + +namespace routing +{ +class RoadPenaltySerializer final +{ +public: + using PointToPenalty = RoadPenalty::PointToPenalty; + using Penalty = RoadPenalty::Penalty; + + RoadPenaltySerializer() = delete; + + template + static void Serialize(Sink & sink, RoadPenalty const & roadPenalty) + { + SerializePenalties(sink, roadPenalty.GetPointToPenalty()); + } + + template + static void Deserialize(Source & src, RoadPenalty & roadPenalty, VehicleType vehicleType) + { + PointToPenalty pointToPenalty; + DeserializePenalties(src, pointToPenalty, vehicleType); + + roadPenalty.SetPointPenalties(std::move(pointToPenalty)); + } + +private: + template + static void SerializePenalty(Sink & sink, Penalty const & penalty) + { + WriteToSink(sink, static_cast(penalty.m_type)); + // Time is derived from type + vehicle when loading + } + + template + static void DeserializePenalty(Source & src, Penalty & penalty, VehicleType vehicleType) + { + penalty.m_type = static_cast(ReadPrimitiveFromSource(src)); + penalty.m_timeSeconds = Penalty::GetTimePenalty(penalty.m_type, vehicleType); + } + + template + static void SerializePenalties(Sink & sink, PenaltyMap const & penalties) + { + WriteToSink(sink, static_cast(penalties.size())); + for (auto const & [key, penalty] : penalties) + { + SerializeKey(sink, key); + SerializePenalty(sink, penalty); + } + } + + template + static void DeserializePenalties(Source & src, PenaltyMap & penalties, VehicleType vehicleType) + { + uint32_t const size = ReadPrimitiveFromSource(src); + penalties.reserve(size); + + for (uint32_t i = 0; i < size; ++i) + { + typename PenaltyMap::key_type key; + DeserializeKey(src, key); + + Penalty penalty; + DeserializePenalty(src, penalty, vehicleType); + + penalties[key] = penalty; + } + } + + template + static void SerializeKey(Sink & sink, uint32_t key) + { + WriteToSink(sink, key); + } + + template + static void DeserializeKey(Source & src, uint32_t & key) + { + key = ReadPrimitiveFromSource(src); + } + + template + static void SerializeKey(Sink & sink, RoadPoint const & key) + { + WriteToSink(sink, key.GetFeatureId()); + WriteToSink(sink, key.GetPointId()); + } + + template + static void DeserializeKey(Source & src, RoadPoint & key) + { + uint32_t featureId = ReadPrimitiveFromSource(src); + uint32_t pointId = ReadPrimitiveFromSource(src); + key = RoadPoint(featureId, pointId); + } +}; +} // namespace routing diff --git a/libs/routing/routing_tests/CMakeLists.txt b/libs/routing/routing_tests/CMakeLists.txt index 3340b8777..898b34865 100644 --- a/libs/routing/routing_tests/CMakeLists.txt +++ b/libs/routing/routing_tests/CMakeLists.txt @@ -26,6 +26,7 @@ set(SRC position_accumulator_tests.cpp restriction_test.cpp road_access_test.cpp + road_penalty_test.cpp road_graph_builder.cpp road_graph_builder.hpp road_graph_nearest_edges_test.cpp diff --git a/libs/routing/routing_tests/index_graph_tools.cpp b/libs/routing/routing_tests/index_graph_tools.cpp index 0a75cd98e..c980285d5 100644 --- a/libs/routing/routing_tests/index_graph_tools.cpp +++ b/libs/routing/routing_tests/index_graph_tools.cpp @@ -150,6 +150,11 @@ double WeightedEdgeEstimator::GetUTurnPenalty(Purpose purpose) const { return 0.0; } +double WeightedEdgeEstimator::GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, + RoadGeometry const & to_road, bool is_left_hand_traffic) const +{ + return 0; +} double WeightedEdgeEstimator::GetFerryLandingPenalty(Purpose purpose) const { return 0.0; diff --git a/libs/routing/routing_tests/index_graph_tools.hpp b/libs/routing/routing_tests/index_graph_tools.hpp index e2b883d71..e10ff4457 100644 --- a/libs/routing/routing_tests/index_graph_tools.hpp +++ b/libs/routing/routing_tests/index_graph_tools.hpp @@ -163,7 +163,7 @@ public: // maxSpeedKMpH doesn't matter, but should be greater then any road speed in all tests. // offroadSpeedKMpH doesn't matter, but should be > 0 and <= maxSpeedKMpH. explicit WeightedEdgeEstimator(std::map const & segmentWeights) - : EdgeEstimator(1e10 /* maxSpeedKMpH */, 1.0 /* offroadSpeedKMpH */) + : EdgeEstimator(VehicleType::Count, 1e10 /* maxSpeedKMpH */, 1.0 /* offroadSpeedKMpH */) , m_segmentWeights(segmentWeights) {} @@ -174,6 +174,8 @@ public: EdgeEstimator::Purpose purpose) const override; double GetUTurnPenalty(Purpose purpose) const override; + double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road, + bool is_left_hand_traffic = false) const override; double GetFerryLandingPenalty(Purpose purpose) const override; private: diff --git a/libs/routing/routing_tests/road_penalty_test.cpp b/libs/routing/routing_tests/road_penalty_test.cpp new file mode 100644 index 000000000..22850449b --- /dev/null +++ b/libs/routing/routing_tests/road_penalty_test.cpp @@ -0,0 +1,169 @@ +#include "testing/testing.hpp" + +#include "routing/road_penalty.hpp" +#include "routing/road_point.hpp" +#include "routing/vehicle_mask.hpp" + +#include +#include + +namespace road_penalty_test +{ +using namespace routing; + +UNIT_TEST(RoadPenalty_Basic) +{ + RoadPenalty penalty; + + // Test empty penalty + TEST(!penalty.GetPenalty(RoadPoint(1, 0)).has_value(), ()); + TEST(!penalty.GetPenalty(RoadPoint(2, 1)).has_value(), ()); +} + +UNIT_TEST(RoadPenalty_PointPenalties) +{ + RoadPenalty penalty; + + // Test point penalties + RoadPenalty::PointToPenalty pointPenalties; + pointPenalties[RoadPoint(1, 0)] = RoadPenalty::Penalty(RoadPenalty::Type::MediumCalming, VehicleType::Car); + pointPenalties[RoadPoint(2, 3)] = RoadPenalty::Penalty(RoadPenalty::Type::Gate, VehicleType::Car); + penalty.SetPointPenalties(std::move(pointPenalties)); + + auto p1 = penalty.GetPenalty(RoadPoint(1, 0)); + TEST(p1.has_value(), ()); + TEST_EQUAL(p1->m_type, RoadPenalty::Type::MediumCalming, ()); + TEST_EQUAL(p1->m_timeSeconds, 5, ()); + + auto p2 = penalty.GetPenalty(RoadPoint(2, 3)); + TEST(p2.has_value(), ()); + TEST_EQUAL(p2->m_type, RoadPenalty::Type::Gate, ()); + TEST_EQUAL(p2->m_timeSeconds, 30, ()); + + TEST(!penalty.GetPenalty(RoadPoint(1, 1)).has_value(), ()); +} + +UNIT_TEST(RoadPenalty_TypeConversion) +{ + // Test ToString/FromString + TEST_EQUAL(ToString(RoadPenalty::Type::None), "None", ()); + TEST_EQUAL(ToString(RoadPenalty::Type::SmallCalming), "SmallCalming", ()); + TEST_EQUAL(ToString(RoadPenalty::Type::MediumCalming), "MediumCalming", ()); + TEST_EQUAL(ToString(RoadPenalty::Type::LargeCalming), "LargeCalming", ()); + TEST_EQUAL(ToString(RoadPenalty::Type::Gate), "Gate", ()); + TEST_EQUAL(ToString(RoadPenalty::Type::UncontrolledJunction), "UncontrolledJunction", ()); + TEST_EQUAL(ToString(RoadPenalty::Type::ControlledJunction), "ControlledJunction", ()); + + RoadPenalty::Type type; + FromString("SmallCalming", type); + TEST_EQUAL(type, RoadPenalty::Type::SmallCalming, ()); + + FromString("Gate", type); + TEST_EQUAL(type, RoadPenalty::Type::Gate, ()); + + FromString("UncontrolledJunction", type); + TEST_EQUAL(type, RoadPenalty::Type::UncontrolledJunction, ()); + + FromString("ControlledJunction", type); + TEST_EQUAL(type, RoadPenalty::Type::ControlledJunction, ()); +} + +UNIT_TEST(RoadPenalty_Equality) +{ + RoadPenalty penalty1, penalty2; + TEST(penalty1 == penalty2, ()); + + RoadPenalty::PointToPenalty pointPenalties; + pointPenalties[RoadPoint(1, 0)] = RoadPenalty::Penalty(RoadPenalty::Type::SmallCalming, VehicleType::Car); + penalty1.SetPointPenalties(std::move(pointPenalties)); + + TEST(!(penalty1 == penalty2), ()); + + pointPenalties.clear(); + pointPenalties[RoadPoint(1, 0)] = RoadPenalty::Penalty(RoadPenalty::Type::SmallCalming, VehicleType::Car); + penalty2.SetPointPenalties(std::move(pointPenalties)); + + TEST(penalty1 == penalty2, ()); +} + +// Test vehicle mask functionality +UNIT_TEST(RoadPenalty_VehicleMask) +{ + // Test basic mask operations + VehicleMask mask = 0; + + // Set bit for Car (VehicleType::Car = 2) + mask |= (1 << 2); + TEST((mask & (1 << 2)) != 0, ()); + TEST((mask & (1 << 1)) == 0, ()); + + // Test mask with multiple vehicle types + VehicleMask multiMask = 0; + multiMask |= (1 << 0); // Pedestrian + multiMask |= (1 << 1); // Bicycle + multiMask |= (1 << 2); // Car + + // Verify multiple bits are set + TEST_EQUAL(__builtin_popcount(multiMask), 3, ()); +} + +// Test default time penalties +UNIT_TEST(RoadPenalty_DefaultTimes) +{ + // Test GetTimePenalty with Car as default + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::None, VehicleType::Car), 0, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::SmallCalming, VehicleType::Car), 3, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::MediumCalming, VehicleType::Car), 5, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::LargeCalming, VehicleType::Car), 7, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::Gate, VehicleType::Car), 30, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::UncontrolledJunction, VehicleType::Car), 15, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::ControlledJunction, VehicleType::Car), 30, ()); + + // Test constructor with vehicle type + RoadPenalty::Penalty smallCalming(RoadPenalty::Type::SmallCalming, VehicleType::Car); + TEST_EQUAL(smallCalming.m_type, RoadPenalty::Type::SmallCalming, ()); + TEST_EQUAL(smallCalming.m_timeSeconds, 3, ()); + + RoadPenalty::Penalty gate(RoadPenalty::Type::Gate, VehicleType::Car); + TEST_EQUAL(gate.m_type, RoadPenalty::Type::Gate, ()); + TEST_EQUAL(gate.m_timeSeconds, 30, ()); +} + +// Test vehicle-specific time penalties +UNIT_TEST(RoadPenalty_VehicleSpecificTimes) +{ + // Test GetTimePenalty with different vehicle types + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::Gate, VehicleType::Car), 30, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::Gate, VehicleType::Bicycle), 10, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::Gate, VehicleType::Pedestrian), 10, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::Gate, VehicleType::Transit), 5, ()); + + // Test constructor with vehicle type + RoadPenalty::Penalty carGate(RoadPenalty::Type::Gate, VehicleType::Car); + TEST_EQUAL(carGate.m_type, RoadPenalty::Type::Gate, ()); + TEST_EQUAL(carGate.m_timeSeconds, 30, ()); + + RoadPenalty::Penalty bicycleGate(RoadPenalty::Type::Gate, VehicleType::Bicycle); + TEST_EQUAL(bicycleGate.m_type, RoadPenalty::Type::Gate, ()); + TEST_EQUAL(bicycleGate.m_timeSeconds, 10, ()); + + // Test traffic calming with different vehicles + RoadPenalty::Penalty carCalming(RoadPenalty::Type::SmallCalming, VehicleType::Car); + TEST_EQUAL(carCalming.m_type, RoadPenalty::Type::SmallCalming, ()); + TEST_EQUAL(carCalming.m_timeSeconds, 3, ()); + + RoadPenalty::Penalty bicycleCalming(RoadPenalty::Type::SmallCalming, VehicleType::Bicycle); + TEST_EQUAL(bicycleCalming.m_type, RoadPenalty::Type::SmallCalming, ()); + TEST_EQUAL(bicycleCalming.m_timeSeconds, 0, ()); + + // Test junction penalties with different vehicles + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::UncontrolledJunction, VehicleType::Bicycle), 10, + ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::ControlledJunction, VehicleType::Bicycle), 30, ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::UncontrolledJunction, VehicleType::Pedestrian), 0, + ()); + TEST_EQUAL(RoadPenalty::Penalty::GetTimePenalty(RoadPenalty::Type::ControlledJunction, VehicleType::Pedestrian), 0, + ()); +} + +} // namespace road_penalty_test diff --git a/libs/routing_common/car_model_coefs.hpp b/libs/routing_common/car_model_coefs.hpp index 123ff71bd..6a466d946 100644 --- a/libs/routing_common/car_model_coefs.hpp +++ b/libs/routing_common/car_model_coefs.hpp @@ -18,7 +18,7 @@ HighwayBasedFactors const kHighwayBasedFactors = { // Tier 2: {HighwayType::HighwayPrimary, - InOutCityFactor(SpeedFactor{0.85 /* weight */, 0.80 /* eta */} /* in city */, 0.85 /* out city */)}, + InOutCityFactor(SpeedFactor{0.95 /* weight */, 0.90 /* eta */} /* in city */, 0.85 /* out city */)}, {HighwayType::HighwayPrimaryLink, InOutCityFactor(0.70 /* in city */, 0.75 /* out city */)}, // 0.10 less {HighwayType::HighwaySecondary, InOutCityFactor(0.80 /* in city */, 0.85 /* out city */)}, {HighwayType::HighwaySecondaryLink, InOutCityFactor(0.70 /* in city */, 0.75 /* out city */)}, // 0.10 less diff --git a/libs/routing_common/vehicle_model.cpp b/libs/routing_common/vehicle_model.cpp index af7966fd3..3c94d0613 100644 --- a/libs/routing_common/vehicle_model.cpp +++ b/libs/routing_common/vehicle_model.cpp @@ -7,6 +7,7 @@ #include #include +#include namespace routing { @@ -378,4 +379,41 @@ string DebugPrint(HighwayType type) UNREACHABLE(); } + +void FromString(std::string_view s, HighwayType & highwayType) +{ + // Build reverse lookup from DebugPrint function + static std::unordered_map const stringToEnum = []() + { + std::unordered_map map; + + // All possible HighwayType values + constexpr HighwayType allTypes[] = { + HighwayType::HighwayResidential, HighwayType::HighwayService, HighwayType::HighwayUnclassified, + HighwayType::HighwayFootway, HighwayType::HighwayTrack, HighwayType::HighwayTertiary, + HighwayType::HighwaySecondary, HighwayType::HighwayPath, HighwayType::HighwayPrimary, + HighwayType::HighwayRoad, HighwayType::HighwayCycleway, HighwayType::HighwayMotorwayLink, + HighwayType::HighwayLivingStreet, HighwayType::HighwayMotorway, HighwayType::HighwaySteps, + HighwayType::HighwayTrunk, HighwayType::HighwayPedestrian, HighwayType::HighwayTrunkLink, + HighwayType::HighwayPrimaryLink, HighwayType::ManMadePier, HighwayType::HighwayBridleway, + HighwayType::HighwaySecondaryLink, HighwayType::RouteFerry, HighwayType::HighwayTertiaryLink, + HighwayType::HighwayBusway, HighwayType::RouteShuttleTrain}; + + for (auto type : allTypes) + map[DebugPrint(type)] = type; + + return map; + }(); + + auto it = stringToEnum.find(std::string(s)); + if (it != stringToEnum.end()) + { + highwayType = it->second; + } + else + { + ASSERT(false, ("Could not read HighwayType from string", s)); + highwayType = HighwayType::HighwayResidential; // default fallback + } +} } // namespace routing diff --git a/libs/routing_common/vehicle_model.hpp b/libs/routing_common/vehicle_model.hpp index c60032344..3303854ba 100644 --- a/libs/routing_common/vehicle_model.hpp +++ b/libs/routing_common/vehicle_model.hpp @@ -366,4 +366,5 @@ std::string DebugPrint(SpeedFactor const & speedFactor); std::string DebugPrint(InOutCitySpeedKMpH const & speed); std::string DebugPrint(InOutCityFactor const & speedFactor); std::string DebugPrint(HighwayType type); +void FromString(std::string_view s, HighwayType & highwayType); } // namespace routing diff --git a/xcode/routing/routing.xcodeproj/project.pbxproj b/xcode/routing/routing.xcodeproj/project.pbxproj index a097ab6ac..e216877a9 100644 --- a/xcode/routing/routing.xcodeproj/project.pbxproj +++ b/xcode/routing/routing.xcodeproj/project.pbxproj @@ -212,6 +212,10 @@ 67C79BA21E2CEE1400C40034 /* restriction_loader.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 67C79BA01E2CEE1400C40034 /* restriction_loader.hpp */; }; 67C7D42D1B4EB48F00FE41AA /* turns_sound_settings.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 67C7D4251B4EB48F00FE41AA /* turns_sound_settings.cpp */; }; 67C7D42E1B4EB48F00FE41AA /* turns_sound_settings.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 67C7D4261B4EB48F00FE41AA /* turns_sound_settings.hpp */; }; + 833F71FA2E48DF27008F7617 /* road_penalty.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 833F71F82E48DF27008F7617 /* road_penalty.hpp */; }; + 833F71FB2E48DF27008F7617 /* road_penalty.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 833F71F92E48DF27008F7617 /* road_penalty.cpp */; }; + 833F71FD2E48E011008F7617 /* road_penalty_serialization.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 833F71FC2E48E010008F7617 /* road_penalty_serialization.hpp */; }; + 833F71FF2E48E02E008F7617 /* road_penalty_test.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 833F71FE2E48E02E008F7617 /* road_penalty_test.cpp */; }; A120B3531B4A7C1C002F3808 /* astar_algorithm.hpp in Headers */ = {isa = PBXBuildFile; fileRef = A120B3521B4A7C1C002F3808 /* astar_algorithm.hpp */; }; A1616E2B1B6B60AB003F078E /* router_delegate.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A1616E291B6B60AB003F078E /* router_delegate.cpp */; }; A1616E2C1B6B60AB003F078E /* router_delegate.hpp in Headers */ = {isa = PBXBuildFile; fileRef = A1616E2A1B6B60AB003F078E /* router_delegate.hpp */; }; @@ -498,6 +502,10 @@ 67C79BA01E2CEE1400C40034 /* restriction_loader.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = restriction_loader.hpp; sourceTree = ""; }; 67C7D4251B4EB48F00FE41AA /* turns_sound_settings.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; lineEnding = 0; path = turns_sound_settings.cpp; sourceTree = ""; xcLanguageSpecificationIdentifier = xcode.lang.cpp; }; 67C7D4261B4EB48F00FE41AA /* turns_sound_settings.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = turns_sound_settings.hpp; sourceTree = ""; }; + 833F71F82E48DF27008F7617 /* road_penalty.hpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.h; path = road_penalty.hpp; sourceTree = ""; }; + 833F71F92E48DF27008F7617 /* road_penalty.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = road_penalty.cpp; sourceTree = ""; }; + 833F71FC2E48E010008F7617 /* road_penalty_serialization.hpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.h; path = road_penalty_serialization.hpp; sourceTree = ""; }; + 833F71FE2E48E02E008F7617 /* road_penalty_test.cpp */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.cpp; path = road_penalty_test.cpp; sourceTree = ""; }; A120B3521B4A7C1C002F3808 /* astar_algorithm.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = astar_algorithm.hpp; sourceTree = ""; }; A1616E291B6B60AB003F078E /* router_delegate.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = router_delegate.cpp; sourceTree = ""; }; A1616E2A1B6B60AB003F078E /* router_delegate.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = router_delegate.hpp; sourceTree = ""; }; @@ -618,6 +626,7 @@ 6742ACAF1C68A0B1009CB89E /* road_graph_builder.cpp */, 6742ACB01C68A0B1009CB89E /* road_graph_builder.hpp */, 6742ACB11C68A0B1009CB89E /* road_graph_nearest_edges_test.cpp */, + 833F71FE2E48E02E008F7617 /* road_penalty_test.cpp */, 6742ACB21C68A0B1009CB89E /* route_tests.cpp */, 56290B85206A3231003892E0 /* routing_algorithm.cpp */, 56290B86206A3231003892E0 /* routing_algorithm.hpp */, @@ -694,6 +703,7 @@ 671F58BA1B874EA20032311E /* base */, 5694723F2418C8220013CD21 /* car_directions.hpp */, 569472402418C8220013CD21 /* car_directions.cpp */, + 5670595B1F3AF97F0062672D /* checkpoint_predictor.cpp */, 5670595C1F3AF97F0062672D /* checkpoint_predictor.hpp */, 5670595B1F3AF97F0062672D /* checkpoint_predictor.cpp */, 0C15B8011F02A61B0058E253 /* checkpoints.hpp */, @@ -792,6 +802,9 @@ 674F9BC41B0A580E00704FFA /* road_graph.cpp */, 0C5FEC671DDE193F0017688C /* road_index.hpp */, 0C5FEC661DDE193F0017688C /* road_index.cpp */, + 833F71F82E48DF27008F7617 /* road_penalty.hpp */, + 833F71F92E48DF27008F7617 /* road_penalty.cpp */, + 833F71FC2E48E010008F7617 /* road_penalty_serialization.hpp */, 0C5FEC681DDE193F0017688C /* road_point.hpp */, 6753440E1A3F644F00A0A8C3 /* route.hpp */, 6753440D1A3F644F00A0A8C3 /* route.cpp */, @@ -941,6 +954,7 @@ 44642D2D29CDA44700F8813A /* ruler_router.hpp in Headers */, 56CBED5522E9CE2600D51AF7 /* position_accumulator.hpp in Headers */, 5694CECB1EBA25F7004576D3 /* road_access_serialization.hpp in Headers */, + 833F71FA2E48DF27008F7617 /* road_penalty.hpp in Headers */, 0C08AA371DF8324D004195DD /* vehicle_mask.hpp in Headers */, 6753441D1A3F644F00A0A8C3 /* router.hpp in Headers */, A1616E2E1B6B60B3003F078E /* astar_progress.hpp in Headers */, @@ -972,6 +986,7 @@ 4443DC3822789793000C8E32 /* leaps_postprocessor.hpp in Headers */, 0C62BFE61E8ADC3100055A79 /* coding.hpp in Headers */, 674F9BD51B0A580E00704FFA /* road_graph.hpp in Headers */, + 833F71FD2E48E011008F7617 /* road_penalty_serialization.hpp in Headers */, 562BDE2020D14860008EFF6F /* routing_callbacks.hpp in Headers */, 567F81952154D6FF0093C25B /* city_roads.hpp in Headers */, D5C9497C2525C855001E45E2 /* directions_engine_helpers.hpp in Headers */, @@ -1179,6 +1194,7 @@ 6742AD371C68A9DF009CB89E /* turns_tts_text_tests.cpp in Sources */, 6742AD2C1C68A9DF009CB89E /* followed_polyline_test.cpp in Sources */, FAA838AC26BB4B3D002E54C6 /* coding_test.cpp in Sources */, + 833F71FF2E48E02E008F7617 /* road_penalty_test.cpp in Sources */, FAA8389D26BB4ADD002E54C6 /* guides_tests.cpp in Sources */, 56CBED5822E9CFB600D51AF7 /* position_accumulator_tests.cpp in Sources */, FAA838A826BB4B21002E54C6 /* routing_helpers_tests.cpp in Sources */, @@ -1233,6 +1249,7 @@ 6741AA9C1BF35331002C974C /* turns_notification_manager.cpp in Sources */, 0C0DF9211DE898B70055A22F /* index_graph_starter.cpp in Sources */, 56C439281E93BF8C00998E29 /* cross_mwm_graph.cpp in Sources */, + 833F71FB2E48DF27008F7617 /* road_penalty.cpp in Sources */, 670EE5731B664796001E8064 /* pedestrian_directions.cpp in Sources */, 567F81942154D6FF0093C25B /* city_roads.cpp in Sources */, 6753441B1A3F644F00A0A8C3 /* route.cpp in Sources */,