blackopsrepl/vehicle-routing-python

1"""2Real-world routing service using OSMnx for road network data.3 4This module provides:5- OSMnxRoutingService: Downloads OSM network, caches locally, computes routes6- DistanceMatrix: Precomputes all pairwise routes with times and geometries7- Haversine fallback when OSMnx is unavailable8"""9 10from __future__ import annotations11 12import logging13import math14from dataclasses import dataclass, field15from pathlib import Path16from typing import TYPE_CHECKING, Optional17 18import polyline19 20if TYPE_CHECKING:21    from .domain import Location22 23logger = logging.getLogger(__name__)24 25# Cache directory for OSM network data26CACHE_DIR = Path(__file__).parent.parent.parent / ".osm_cache"27 28 29@dataclass30class RouteResult:31    """Result from a routing query."""32 33    duration_seconds: int34    distance_meters: int35    geometry: Optional[str] = None  # Encoded polyline36 37 38@dataclass39class DistanceMatrix:40    """41    Precomputed distance/time matrix for all location pairs.42 43    Stores RouteResult for each (origin, destination) pair,44    enabling O(1) lookup during solver execution.45    """46 47    _matrix: dict[tuple[tuple[float, float], tuple[float, float]], RouteResult] = field(48        default_factory=dict49    )50 51    def _key(52        self, origin: "Location", destination: "Location"53    ) -> tuple[tuple[float, float], tuple[float, float]]:54        """Create hashable key from two locations."""55        return (56            (origin.latitude, origin.longitude),57            (destination.latitude, destination.longitude),58        )59 60    def set_route(61        self, origin: "Location", destination: "Location", result: RouteResult62    ) -> None:63        """Store a route result in the matrix."""64        self._matrix[self._key(origin, destination)] = result65 66    def get_route(67        self, origin: "Location", destination: "Location"68    ) -> Optional[RouteResult]:69        """Get a route result from the matrix."""70        return self._matrix.get(self._key(origin, destination))71 72    def get_driving_time(self, origin: "Location", destination: "Location") -> int:73        """Get driving time in seconds between two locations."""74        result = self.get_route(origin, destination)75        if result is None:76            # Fallback to haversine if not in matrix77            return _haversine_driving_time(origin, destination)78        return result.duration_seconds79 80    def get_geometry(81        self, origin: "Location", destination: "Location"82    ) -> Optional[str]:83        """Get encoded polyline geometry for a route segment."""84        result = self.get_route(origin, destination)85        return result.geometry if result else None86 87 88def _haversine_driving_time(origin: "Location", destination: "Location") -> int:89    """90    Calculate driving time using haversine formula (fallback).91 92    Uses 50 km/h average speed assumption.93    """94    if (95        origin.latitude == destination.latitude96        and origin.longitude == destination.longitude97    ):98        return 099 100    EARTH_RADIUS_M = 6371000101    AVERAGE_SPEED_KMPH = 50102 103    lat1 = math.radians(origin.latitude)104    lon1 = math.radians(origin.longitude)105    lat2 = math.radians(destination.latitude)106    lon2 = math.radians(destination.longitude)107 108    # Haversine formula109    dlat = lat2 - lat1110    dlon = lon2 - lon1111    a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2112    c = 2 * math.asin(math.sqrt(a))113    distance_meters = EARTH_RADIUS_M * c114 115    # Convert to driving time116    return round(distance_meters / AVERAGE_SPEED_KMPH * 3.6)117 118 119class OSMnxRoutingService:120    """121    Routing service using OSMnx for real road network data.122 123    Downloads the OSM network for a given bounding box, caches it locally,124    and computes shortest paths using NetworkX.125    """126 127    def __init__(self, cache_dir: Path = CACHE_DIR):128        self.cache_dir = cache_dir129        self.cache_dir.mkdir(parents=True, exist_ok=True)130        self._graph = None131        self._graph_bbox = None132 133    def _get_cache_path(134        self, north: float, south: float, east: float, west: float135    ) -> Path:136        """Generate cache file path for a bounding box."""137        # Round to 2 decimal places for cache key138        key = f"osm_{north:.2f}_{south:.2f}_{east:.2f}_{west:.2f}.graphml"139        return self.cache_dir / key140 141    def load_network(142        self, north: float, south: float, east: float, west: float, padding: float = 0.01143    ) -> bool:144        """145        Load OSM road network for the given bounding box.146 147        Args:148            north, south, east, west: Bounding box coordinates149            padding: Extra padding around the bbox (in degrees)150 151        Returns:152            True if network loaded successfully, False otherwise153        """154        try:155            import osmnx as ox156 157            # Add padding to ensure we have roads outside the strict bbox158            north += padding159            south -= padding160            east += padding161            west -= padding162 163            cache_path = self._get_cache_path(north, south, east, west)164 165            if cache_path.exists() and cache_path.stat().st_size > 0:166                logger.info(f"Loading cached OSM network from {cache_path}")167                self._graph = ox.load_graphml(cache_path)168 169                # Check if the cached graph already has travel_time170                # (we now save enriched graphs)171                sample_edge = next(iter(self._graph.edges(data=True)), None)172                has_travel_time = sample_edge and "travel_time" in sample_edge[2]173 174                if not has_travel_time:175                    logger.info("Adding edge speeds and travel times to cached graph...")176                    self._graph = ox.add_edge_speeds(self._graph)177                    self._graph = ox.add_edge_travel_times(self._graph)178                    # Re-save with travel times included179                    ox.save_graphml(self._graph, cache_path)180                    logger.info("Updated cache with travel times")181            else:182                logger.info(183                    f"Downloading OSM network for bbox: N={north:.4f}, S={south:.4f}, E={east:.4f}, W={west:.4f}"184                )185                # OSMnx 2.x uses bbox as tuple: (left, bottom, right, top) = (west, south, east, north)186                bbox_tuple = (west, south, east, north)187                self._graph = ox.graph_from_bbox(188                    bbox=bbox_tuple,189                    network_type="drive",190                    simplify=True,191                )192 193                # Add edge speeds and travel times BEFORE caching194                logger.info("Computing edge speeds and travel times...")195                self._graph = ox.add_edge_speeds(self._graph)196                self._graph = ox.add_edge_travel_times(self._graph)197 198                # Save enriched graph to cache199                ox.save_graphml(self._graph, cache_path)200                logger.info(f"Saved enriched OSM network to cache: {cache_path}")201 202            self._graph_bbox = (north, south, east, west)203            logger.info(204                f"OSM network loaded: {self._graph.number_of_nodes()} nodes, "205                f"{self._graph.number_of_edges()} edges"206            )207            return True208 209        except ImportError:210            logger.warning("OSMnx not installed, falling back to haversine")211            return False212        except Exception as e:213            logger.warning(f"Failed to load OSM network: {e}, falling back to haversine")214            return False215 216    def get_nearest_node(self, location: "Location") -> Optional[int]:217        """Get the nearest graph node for a location."""218        if self._graph is None:219            return None220        try:221            import osmnx as ox222            return ox.nearest_nodes(self._graph, location.longitude, location.latitude)223        except Exception:224            return None225 226    def compute_all_routes(227        self,228        locations: list["Location"],229        progress_callback=None230    ) -> dict[tuple[int, int], RouteResult]:231        """232        Compute all pairwise routes efficiently using batch shortest paths.233 234        Returns a dict mapping (origin_idx, dest_idx) to RouteResult.235        """236        import networkx as nx237 238        if self._graph is None:239            return {}240 241        results = {}242        n = len(locations)243 244        # Map locations to nearest nodes (batch operation)245        if progress_callback:246            progress_callback("routes", "Finding nearest road nodes...", 30, f"{n} locations")247 248        nodes = []249        for loc in locations:250            node = self.get_nearest_node(loc)251            nodes.append(node)252 253        # Compute shortest paths from each origin to ALL destinations at once254        # This is MUCH faster than individual shortest_path calls255        total_origins = sum(1 for node in nodes if node is not None)256        processed = 0257 258        for i, origin_node in enumerate(nodes):259            if origin_node is None:260                continue261 262            # Compute shortest paths from this origin to all nodes at once263            # Using Dijkstra's algorithm with single-source264            try:265                lengths, paths = nx.single_source_dijkstra(266                    self._graph, origin_node, weight="travel_time"267                )268            except nx.NetworkXError:269                continue270 271            for j, dest_node in enumerate(nodes):272                if dest_node is None:273                    continue274 275                origin_loc = locations[i]276                dest_loc = locations[j]277 278                if i == j or origin_node == dest_node:279                    # Same location280                    results[(i, j)] = RouteResult(281                        duration_seconds=0,282                        distance_meters=0,283                        geometry=polyline.encode(284                            [(origin_loc.latitude, origin_loc.longitude)], precision=5285                        ),286                    )287                elif dest_node in paths:288                    path = paths[dest_node]289                    travel_time = lengths[dest_node]290 291                    # Calculate distance and extract geometry292                    total_distance = 0293                    coordinates = []294 295                    for k in range(len(path) - 1):296                        u, v = path[k], path[k + 1]297                        edge_data = self._graph.get_edge_data(u, v)298                        if edge_data:299                            edge = edge_data[0] if isinstance(edge_data, dict) else edge_data300                            total_distance += edge.get("length", 0)301 302                    for node in path:303                        node_data = self._graph.nodes[node]304                        coordinates.append((node_data["y"], node_data["x"]))305 306                    results[(i, j)] = RouteResult(307                        duration_seconds=round(travel_time),308                        distance_meters=round(total_distance),309                        geometry=polyline.encode(coordinates, precision=5),310                    )311 312            processed += 1313            if progress_callback and processed % max(1, total_origins // 10) == 0:314                percent = 30 + int((processed / total_origins) * 65)315                progress_callback(316                    "routes",317                    "Computing routes...",318                    percent,319                    f"{processed}/{total_origins} origins processed"320                )321 322        return results323 324    def get_route(325        self, origin: "Location", destination: "Location"326    ) -> Optional[RouteResult]:327        """328        Compute route between two locations.329 330        Returns:331            RouteResult with duration, distance, and geometry, or None if routing fails332        """333        if self._graph is None:334            return None335 336        try:337            import osmnx as ox338 339            # Find nearest nodes to origin and destination340            origin_node = ox.nearest_nodes(341                self._graph, origin.longitude, origin.latitude342            )343            dest_node = ox.nearest_nodes(344                self._graph, destination.longitude, destination.latitude345            )346 347            # Same node means same location (or very close)348            if origin_node == dest_node:349                return RouteResult(350                    duration_seconds=0,351                    distance_meters=0,352                    geometry=polyline.encode(353                        [(origin.latitude, origin.longitude)], precision=5354                    ),355                )356 357            # Compute shortest path by travel time358            route = ox.shortest_path(359                self._graph, origin_node, dest_node, weight="travel_time"360            )361 362            if route is None:363                logger.warning(364                    f"No route found between {origin} and {destination}"365                )366                return None367 368            # Extract route attributes369            total_time = 0370            total_distance = 0371            coordinates = []372 373            for i in range(len(route) - 1):374                u, v = route[i], route[i + 1]375                edge_data = self._graph.get_edge_data(u, v)376                if edge_data:377                    # Get the first edge if multiple exist378                    edge = edge_data[0] if isinstance(edge_data, dict) else edge_data379                    total_time += edge.get("travel_time", 0)380                    total_distance += edge.get("length", 0)381 382            # Get node coordinates for geometry383            for node in route:384                node_data = self._graph.nodes[node]385                coordinates.append((node_data["y"], node_data["x"]))386 387            # Encode geometry as polyline388            encoded_geometry = polyline.encode(coordinates, precision=5)389 390            return RouteResult(391                duration_seconds=round(total_time),392                distance_meters=round(total_distance),393                geometry=encoded_geometry,394            )395 396        except Exception as e:397            logger.warning(f"Routing failed: {e}")398            return None399 400 401def compute_distance_matrix(402    locations: list["Location"],403    routing_service: Optional[OSMnxRoutingService] = None,404    bbox: Optional[tuple[float, float, float, float]] = None,405) -> DistanceMatrix:406    """407    Compute distance matrix for all location pairs.408 409    Args:410        locations: List of Location objects411        routing_service: Optional pre-configured routing service412        bbox: Optional (north, south, east, west) tuple for network download413 414    Returns:415        DistanceMatrix with precomputed routes416    """417    return compute_distance_matrix_with_progress(418        locations, routing_service, bbox, use_osm=True, progress_callback=None419    )420 421 422def compute_distance_matrix_with_progress(423    locations: list["Location"],424    bbox: Optional[tuple[float, float, float, float]] = None,425    use_osm: bool = True,426    progress_callback=None,427    routing_service: Optional[OSMnxRoutingService] = None,428) -> DistanceMatrix:429    """430    Compute distance matrix for all location pairs with progress reporting.431 432    Args:433        locations: List of Location objects434        bbox: Optional (north, south, east, west) tuple for network download435        use_osm: If True, try to use OSMnx for real routing. If False, use haversine.436        progress_callback: Optional callback(phase, message, percent, detail) for progress updates437        routing_service: Optional pre-configured routing service438 439    Returns:440        DistanceMatrix with precomputed routes441    """442    matrix = DistanceMatrix()443 444    if not locations:445        return matrix446 447    def report_progress(phase: str, message: str, percent: int, detail: str = ""):448        if progress_callback:449            progress_callback(phase, message, percent, detail)450        logger.info(f"[{phase}] {message} ({percent}%) {detail}")451 452    # Compute bounding box from locations if not provided453    if bbox is None:454        lats = [loc.latitude for loc in locations]455        lons = [loc.longitude for loc in locations]456        bbox = (max(lats), min(lats), max(lons), min(lons))457 458    osm_loaded = False459 460    if use_osm:461        # Create routing service if not provided462        if routing_service is None:463            routing_service = OSMnxRoutingService()464 465        report_progress("network", "Checking for cached road network...", 5)466 467        # Check if cached468        north, south, east, west = bbox469        north += 0.01  # padding470        south -= 0.01471        east += 0.01472        west -= 0.01473 474        cache_path = routing_service._get_cache_path(north, south, east, west)475        is_cached = cache_path.exists()476 477        if is_cached:478            report_progress("network", "Loading cached road network...", 10, str(cache_path.name))479        else:480            report_progress(481                "network",482                "Downloading OpenStreetMap road network...",483                10,484                f"Area: {abs(north-south):.2f}° × {abs(east-west):.2f}°"485            )486 487        # Try to load OSM network488        osm_loaded = routing_service.load_network(489            north=bbox[0], south=bbox[1], east=bbox[2], west=bbox[3]490        )491 492        if osm_loaded:493            node_count = routing_service._graph.number_of_nodes()494            edge_count = routing_service._graph.number_of_edges()495            report_progress(496                "network",497                "Road network loaded",498                25,499                f"{node_count:,} nodes, {edge_count:,} edges"500            )501        else:502            report_progress("network", "OSMnx unavailable, using haversine", 25)503    else:504        report_progress("network", "Using fast haversine mode", 25)505 506    # Compute all pairwise routes507    total_pairs = len(locations) * len(locations)508 509    if osm_loaded and routing_service:510        # Use batch routing for OSMnx (MUCH faster than individual calls)511        report_progress(512            "routes",513            f"Computing {total_pairs:,} routes (batch mode)...",514            30,515            f"{len(locations)} locations"516        )517 518        batch_results = routing_service.compute_all_routes(519            locations,520            progress_callback=report_progress521        )522 523        # Transfer batch results to matrix, with haversine fallback for missing routes524        computed = 0525        for i, origin in enumerate(locations):526            for j, destination in enumerate(locations):527                if (i, j) in batch_results:528                    matrix.set_route(origin, destination, batch_results[(i, j)])529                else:530                    # Fallback to haversine for routes not found531                    matrix.set_route(532                        origin,533                        destination,534                        RouteResult(535                            duration_seconds=_haversine_driving_time(origin, destination),536                            distance_meters=_haversine_distance_meters(origin, destination),537                            geometry=_straight_line_geometry(origin, destination),538                        ),539                    )540                computed += 1541 542        report_progress("complete", "Distance matrix ready", 100, f"{computed:,} routes computed")543    else:544        # Use haversine fallback for all routes545        report_progress(546            "routes",547            f"Computing {total_pairs:,} route pairs...",548            30,549            f"{len(locations)} locations"550        )551 552        computed = 0553        for origin in locations:554            for destination in locations:555                if origin is destination:556                    matrix.set_route(557                        origin,558                        destination,559                        RouteResult(560                            duration_seconds=0,561                            distance_meters=0,562                            geometry=polyline.encode(563                                [(origin.latitude, origin.longitude)], precision=5564                            ),565                        ),566                    )567                else:568                    matrix.set_route(569                        origin,570                        destination,571                        RouteResult(572                            duration_seconds=_haversine_driving_time(origin, destination),573                            distance_meters=_haversine_distance_meters(origin, destination),574                            geometry=_straight_line_geometry(origin, destination),575                        ),576                    )577                computed += 1578 579                # Report progress every 5%580                if total_pairs > 0 and computed % max(1, total_pairs // 20) == 0:581                    percent_complete = int(30 + (computed / total_pairs) * 65)582                    report_progress(583                        "routes",584                        f"Computing routes...",585                        percent_complete,586                        f"{computed:,}/{total_pairs:,} pairs"587                    )588 589        report_progress("complete", "Distance matrix ready", 100, f"{computed:,} routes computed")590 591    return matrix592 593 594def _haversine_distance_meters(origin: "Location", destination: "Location") -> int:595    """Calculate haversine distance in meters."""596    if (597        origin.latitude == destination.latitude598        and origin.longitude == destination.longitude599    ):600        return 0601 602    EARTH_RADIUS_M = 6371000603 604    lat1 = math.radians(origin.latitude)605    lon1 = math.radians(origin.longitude)606    lat2 = math.radians(destination.latitude)607    lon2 = math.radians(destination.longitude)608 609    dlat = lat2 - lat1610    dlon = lon2 - lon1611    a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2612    c = 2 * math.asin(math.sqrt(a))613 614    return round(EARTH_RADIUS_M * c)615 616 617def _straight_line_geometry(origin: "Location", destination: "Location") -> str:618    """Generate a straight-line encoded polyline between two points."""619    return polyline.encode(620        [(origin.latitude, origin.longitude), (destination.latitude, destination.longitude)],621        precision=5,622    )623