diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp index aedbcf75add..3f1914371a2 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -334,7 +334,15 @@ void relaxOutgoingEdges(const DataFacade &facade, if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to) { const EdgeWeight to_weight = heapNode.weight + shortcut_weight; - BOOST_ASSERT(to_weight >= heapNode.weight); + if (to_weight < heapNode.weight || to_weight == INVALID_EDGE_WEIGHT) + { + ++destination; + if constexpr (IS_MAP_MATCHING) + { + ++distance; + } + continue; + } if constexpr (IS_MAP_MATCHING) { @@ -382,7 +390,15 @@ void relaxOutgoingEdges(const DataFacade &facade, if (shortcut_weight != INVALID_EDGE_WEIGHT && heapNode.node != to) { const EdgeWeight to_weight = heapNode.weight + shortcut_weight; - BOOST_ASSERT(to_weight >= heapNode.weight); + if (to_weight < heapNode.weight || to_weight == INVALID_EDGE_WEIGHT) + { + ++source; + if constexpr (IS_MAP_MATCHING) + { + ++distance; + } + continue; + } if constexpr (IS_MAP_MATCHING) { const EdgeDistance to_distance = heapNode.data.distance + *distance; diff --git a/include/updater/source.hpp b/include/updater/source.hpp index 6a5b56dcb52..0aa7dee827c 100644 --- a/include/updater/source.hpp +++ b/include/updater/source.hpp @@ -3,6 +3,7 @@ #include "util/typedefs.hpp" +#include #include #include #include @@ -19,8 +20,9 @@ template struct LookupTable std::lower_bound(lookup.begin(), lookup.end(), key, - [](const auto &lhs, const auto &rhs) { return rhs < lhs.first; }); - return it != std::end(lookup) && !(it->first < key) ? Result(it->second) : Result(); + [](const auto &lhs, const auto &rhs) { return lhs.first < rhs; }); + + return it != std::end(lookup) && it->first == key ? Result(it->second) : Result(); } std::vector> lookup; @@ -48,10 +50,18 @@ struct Segment final struct SpeedSource final { - SpeedSource() : speed(0.), rate() {} + enum Operation + { + ABSOLUTE, // Set absolute speed value + MULTIPLY, // Multiply current speed by factor + DIVIDE // Divide current speed by factor + }; + + SpeedSource() : speed(0.), rate(), operation(ABSOLUTE) {} double speed; std::optional rate; std::uint8_t source; + Operation operation; }; struct Turn final diff --git a/include/util/d_ary_heap.hpp b/include/util/d_ary_heap.hpp index ad7dbf82452..732029fc15b 100644 --- a/include/util/d_ary_heap.hpp +++ b/include/util/d_ary_heap.hpp @@ -34,7 +34,10 @@ template void decrease(HeapHandle handle, HeapData &&data, ReorderHandler &&reorderHandler) { - BOOST_ASSERT(handle < heap.size()); + if (handle >= heap.size()) + { + return; + } heap[handle] = std::forward(data); heapifyUp(handle, std::forward(reorderHandler)); diff --git a/include/util/query_heap.hpp b/include/util/query_heap.hpp index 72b9e4b8860..5b32fb83ed3 100644 --- a/include/util/query_heap.hpp +++ b/include/util/query_heap.hpp @@ -324,7 +324,10 @@ class QueryHeap void DecreaseKey(const HeapNode &heapNode) { - BOOST_ASSERT(!WasRemoved(heapNode.node)); + if (WasRemoved(heapNode.node)) + { + return; + } heap.decrease(heapNode.handle, HeapData{heapNode.weight, heap[heapNode.handle].index}, [this](const auto &heapData, auto new_handle) diff --git a/src/updater/csv_source.cpp b/src/updater/csv_source.cpp index ffcdc9ac50d..1eda50a72fd 100644 --- a/src/updater/csv_source.cpp +++ b/src/updater/csv_source.cpp @@ -1,10 +1,16 @@ #include "updater/csv_source.hpp" #include "updater/csv_file_parser.hpp" +#include "util/log.hpp" #include #include +#include +#include +#include +#include + // clang-format off BOOST_FUSION_ADAPT_STRUCT(osrm::updater::Segment, (decltype(osrm::updater::Segment::from), from) @@ -29,25 +35,106 @@ namespace osrm::updater::csv { SegmentLookupTable readSegmentValues(const std::vector &paths) { - static const auto value_if_blank = std::numeric_limits::quiet_NaN(); - const qi::real_parser> unsigned_double; - CSVFilesParser parser( - 1, - qi::ulong_long >> ',' >> qi::ulong_long, - unsigned_double >> -(',' >> (qi::double_ | qi::attr(value_if_blank)))); - - // Check consistency of keys in the result lookup table - auto result = parser(paths); - const auto found_inconsistency = - std::find_if(std::begin(result.lookup), - std::end(result.lookup), - [](const auto &entry) { return entry.first.from == entry.first.to; }); - if (found_inconsistency != std::end(result.lookup)) + SegmentLookupTable result; + + for (const auto &path : paths) { - util::Log(logWARNING) << "Empty segment in CSV with node " + - std::to_string(found_inconsistency->first.from); - } + std::ifstream file(path); + if (!file.is_open()) + { + util::Log(logWARNING) << "Could not open CSV file: " << path; + continue; + } + + std::string line; + std::size_t line_number = 0; + while (std::getline(file, line)) + { + line_number++; + + // Skip empty lines and comments + if (line.empty() || line[0] == '#') + continue; + + std::istringstream iss(line); + std::string from_str, to_str, speed_str; + + if (!std::getline(iss, from_str, ',') || + !std::getline(iss, to_str, ',') || + !std::getline(iss, speed_str, ',')) // Speed can be the last field or followed by rate + { + // Try without trailing comma for speed field + iss.clear(); + iss.str(line); + if (!std::getline(iss, from_str, ',') || + !std::getline(iss, to_str, ',') || + !std::getline(iss, speed_str)) + { + util::Log(logWARNING) << "Invalid CSV format at line " << line_number + << " in file " << path; + continue; + } + } + + try + { + Segment segment; + segment.from = std::stoull(from_str); + segment.to = std::stoull(to_str); + + if (segment.from == segment.to) + { + util::Log(logWARNING) << "Empty segment in CSV with node " << segment.from + << " at line " << line_number; + continue; + } + + SpeedSource speed_source; + speed_source.source = 1; // CSV source + + // Check for operation prefix + if (!speed_str.empty()) + { + if (speed_str[0] == '*') + { + speed_source.operation = SpeedSource::MULTIPLY; + speed_source.speed = std::stod(speed_str.substr(1)); + } + else if (speed_str[0] == '/') + { + speed_source.operation = SpeedSource::DIVIDE; + speed_source.speed = std::stod(speed_str.substr(1)); + } + else + { + speed_source.operation = SpeedSource::ABSOLUTE; + speed_source.speed = std::stod(speed_str); + } + } + + // Optional rate field + std::string rate_str; + if (std::getline(iss, rate_str, ',') && !rate_str.empty()) + { + speed_source.rate = std::stod(rate_str); + } + + result.lookup.push_back({segment, speed_source}); + } + catch (const std::exception &e) + { + util::Log(logWARNING) << "Error parsing CSV line " << line_number + << " in file " << path << ": " << e.what(); + } + } + + } + + // Sort the lookup table for binary search + std::sort(result.lookup.begin(), result.lookup.end(), + [](const auto &a, const auto &b) { return a.first < b.first; }); + return result; } diff --git a/src/updater/updater.cpp b/src/updater/updater.cpp index 3a08db577e3..c75fe0d79c8 100644 --- a/src/updater/updater.cpp +++ b/src/updater/updater.cpp @@ -1,6 +1,8 @@ #include "updater/updater.hpp" #include "updater/csv_source.hpp" +#include + #include "extractor/compressed_edge_container.hpp" #include "extractor/edge_based_graph_factory.hpp" #include "extractor/files.hpp" @@ -219,21 +221,63 @@ updateSegmentData(const UpdaterConfig &config, for (const auto segment_offset : util::irange(0, fwd_weights_range.size())) { - auto u = osm_node_ids[nodes_range[segment_offset]]; - auto v = osm_node_ids[nodes_range[segment_offset + 1]]; + auto node_u = nodes_range[segment_offset]; + auto node_v = nodes_range[segment_offset + 1]; + auto u = osm_node_ids[node_u]; + auto v = osm_node_ids[node_v]; // Self-loops are artifical segments (e.g. traffic light nodes), do not // waste time updating them with traffic data if (u == v) continue; - if (auto value = segment_speed_lookup({u, v})) + // Debug: Log first few segment lookups + static std::atomic debug_count{0}; + if (debug_count < 20) + { + debug_count++; + } + + // Convert OSMNodeID to uint64_t for lookup + std::uint64_t u_id = static_cast(u); + std::uint64_t v_id = static_cast(v); + auto lookup_result = segment_speed_lookup({u_id, v_id}); + if (lookup_result) { + auto value = lookup_result; auto segment_length = segment_lengths[segment_offset]; - auto new_duration = convertToDuration(value->speed, segment_length); + + // Get old values first + SegmentWeight old_weight = fwd_weights_range[segment_offset]; + SegmentDuration old_duration = fwd_durations_range[segment_offset]; + // Duration is in deciseconds (0.1s), so multiply by 10 to get correct speed + double old_speed = (static_cast(old_duration) > 0) ? (segment_length / (static_cast(static_cast(old_duration)) / 10.0)) * 3.6 : 0.0; + + // Calculate effective speed based on operation type + double effective_speed = value->speed; + if (value->operation == SpeedSource::MULTIPLY) + { + effective_speed = old_speed * value->speed; + } + else if (value->operation == SpeedSource::DIVIDE) + { + effective_speed = (value->speed > 0) ? (old_speed / value->speed) : old_speed; + } + // Create a modified SpeedSource with the effective speed for conversion functions + SpeedSource modified_value = *value; + modified_value.speed = effective_speed; + + auto new_duration = convertToDuration(effective_speed, segment_length); auto new_weight = convertToWeight( - fwd_weights_range[segment_offset], *value, segment_length); + fwd_weights_range[segment_offset], modified_value, segment_length); fwd_was_updated = true; + const char* op_str = (value->operation == SpeedSource::MULTIPLY) ? "*" : + (value->operation == SpeedSource::DIVIDE) ? "/" : "="; + util::Log(logDEBUG) << "[FWD] Segment " << u << "->" << v + << " (offset " << segment_offset << ", length " << segment_length << "m)" + << " | Old: weight=" << old_weight << ", duration=" << old_duration << ", speed=" << old_speed << " km/h" + << " | New: weight=" << new_weight << ", duration=" << new_duration + << ", speed=" << effective_speed << " km/h (op: " << op_str << value->speed << "), source=" << static_cast(value->source); fwd_weights_range[segment_offset] = new_weight; fwd_durations_range[segment_offset] = new_duration; @@ -260,21 +304,57 @@ updateSegmentData(const UpdaterConfig &config, for (const auto segment_offset : util::irange(0, rev_weights_range.size())) { - auto u = osm_node_ids[nodes_range[segment_offset]]; - auto v = osm_node_ids[nodes_range[segment_offset + 1]]; + auto node_u = nodes_range[segment_offset]; + auto node_v = nodes_range[segment_offset + 1]; + auto u = osm_node_ids[node_u]; + auto v = osm_node_ids[node_v]; // Self-loops are artifical segments (e.g. traffic light nodes), do not // waste time updating them with traffic data if (u == v) continue; - if (auto value = segment_speed_lookup({v, u})) + // Convert OSMNodeID to uint64_t for lookup + std::uint64_t u_id = static_cast(u); + std::uint64_t v_id = static_cast(v); + auto lookup_result = segment_speed_lookup({v_id, u_id}); + if (lookup_result) { + auto value = lookup_result; auto segment_length = segment_lengths[segment_offset]; - auto new_duration = convertToDuration(value->speed, segment_length); + + // Get old values first + SegmentWeight old_weight = rev_weights_range[segment_offset]; + SegmentDuration old_duration = rev_durations_range[segment_offset]; + // Duration is in deciseconds (0.1s), so multiply by 10 to get correct speed + double old_speed = (static_cast(old_duration) > 0) ? (segment_length / (static_cast(static_cast(old_duration)) / 10.0)) * 3.6 : 0.0; + + // Calculate effective speed based on operation type + double effective_speed = value->speed; + if (value->operation == SpeedSource::MULTIPLY) + { + effective_speed = old_speed * value->speed; + } + else if (value->operation == SpeedSource::DIVIDE) + { + effective_speed = (value->speed > 0) ? (old_speed / value->speed) : old_speed; + } + + // Create a modified SpeedSource with the effective speed for conversion functions + SpeedSource modified_value = *value; + modified_value.speed = effective_speed; + + auto new_duration = convertToDuration(effective_speed, segment_length); auto new_weight = convertToWeight( - rev_weights_range[segment_offset], *value, segment_length); + rev_weights_range[segment_offset], modified_value, segment_length); rev_was_updated = true; + const char* op_str = (value->operation == SpeedSource::MULTIPLY) ? "*" : + (value->operation == SpeedSource::DIVIDE) ? "/" : "="; + util::Log(logDEBUG) << "[REV] Segment " << v << "->" << u + << " (offset " << segment_offset << ", length " << segment_length << "m)" + << " | Old: weight=" << old_weight << ", duration=" << old_duration << ", speed=" << old_speed << " km/h" + << " | New: weight=" << new_weight << ", duration=" << new_duration + << ", speed=" << effective_speed << " km/h (op: " << op_str << value->speed << "), source=" << static_cast(value->source); rev_weights_range[segment_offset] = new_weight; rev_durations_range[segment_offset] = new_duration;