地図画像データを読んで出発地点到着地点を設定し最短ルートを求める

from PIL import Image, ImageFilter, ImageDraw
import numpy as np
import networkx as nx
import cv2

def preprocess_image(image_path, road_colors, road_tolerance, exclude_colors, exclude_tolerance):
    image = cv2.imread(image_path)
    mask = np.zeros(image.shape[:2], dtype=np.uint8)
    
    # 道路と認識する色の範囲
    for road_color in road_colors:
        lower_bound = np.clip(road_color - road_tolerance, 0, 255)
        upper_bound = np.clip(road_color + road_tolerance, 0, 255)
        road_mask = cv2.inRange(image, lower_bound, upper_bound)
        mask = cv2.bitwise_or(mask, road_mask)
    
    # 除外する色の範囲
    for exclude_color in exclude_colors:
        exclude_lower = np.clip(exclude_color - exclude_tolerance, 0, 255)
        exclude_upper = np.clip(exclude_color + exclude_tolerance, 0, 255)
        exclude_mask = cv2.inRange(image, exclude_lower, exclude_upper)
        mask[exclude_mask > 0] = 0
    
    cv2.imwrite('masked_image.png', mask)  # マスク画像を保存
    return mask, image

def image_to_graph(image):
    height, width = image.shape
    graph = nx.grid_2d_graph(height, width)
    for y in range(height):
        for x in range(width):
            if image[y, x] > 0:  # 道路部分のみノードを追加
                graph.add_node((y, x))
            else:
                graph.remove_node((y, x))
    return graph

def find_shortest_path(graph, start, goal):
    try:
        path = nx.shortest_path(graph, source=start, target=goal)
    except nx.NetworkXNoPath:
        return None
    return path

def draw_path(original_image, path, thickness=5):
    for (y, x) in path:
        cv2.circle(original_image, (x, y), thickness, (0, 0, 255), -1)  # 赤色の太線で描画
    return original_image

def main():
    image_path = '2025-02-21_17h42_50.png'  # マップの画像データ
    road_colors = [
        np.array([210, 210, 210]),  # 一般的なグレーの道路色
        np.array([237, 242, 231]),  # 一般的なグレーの道路色
        np.array([200, 200, 200]),  # もう少し暗めのグレー
        np.array([220, 220, 220])   # 明るめのグレー
    ]
    road_tolerance = 20  # 道路とする色の許容範囲
    exclude_colors = [
        np.array([240, 229, 245]),  # #F5F0E5のBGR値
        np.array([247, 247, 248])   # #F8F7F7のBGR値
    ]
    exclude_tolerance = 15  # 除外する色の許容範囲
    start, goal = (47, 40), (453, 599)  # 開始位置とゴール位置を指定 座標はY X
    
    mask, original_image = preprocess_image(image_path, road_colors, road_tolerance, exclude_colors, exclude_tolerance)
    graph = image_to_graph(mask)
    
    if start not in graph or goal not in graph:
        print("Start or goal is not a valid point on the map.")
        return
    
    path = find_shortest_path(graph, start, goal)
    
    if path is None:
        print("No path found.")
        return
    
    result_image = draw_path(original_image, path, thickness=5)  # オリジナル画像に描画
    cv2.imwrite('shortest_path.png', result_image)
    print("Path saved as shortest_path.png")

if __name__ == "__main__":
    main()