Server-cpp/project-fastapi-hs/unit/Scheduler_d1.py

"""
tips:
    推算半径：13.7
    倒车与正开的偏移量是5米；倒车中心点=正开中心点+5
"""

# 坐标数据文件
data_file_dir = '../data'
data_file_path = '../data/渣罐坐标数据.json'
# data_file_path = '/home/ubuntu/repositories/repositories/casperz.py-project/project-fastapi-hs/data/渣罐坐标数据.json'
# data_file_path = r"E:\casper\repositories\repositories\casperz.py-project\project-fastapi-hs\data\渣罐坐标数据.json"

# 道路名称列表
path_name_list = [
    'MN',
    'KL',
    'IJ',
    'GH',
    'EF',
    'CD',
    'AB',
]

# 圆心点坐标字典
d1 = {
    'LM.head': {'center_x': 11, 'center_y': 22},
    'JK.head': {'center_x': 11, 'center_y': 22},
    'HI.head': {'center_x': 11, 'center_y': 22},
    'FG.head': {'center_x': 11, 'center_y': 22},
    'DE.head': {'center_x': 11, 'center_y': 22},
    'BC.head': {'center_x': 11, 'center_y': 22},
    'A.head': {'center_x': 11, 'center_y': 22},

    'LM.tail': {'center_x': 11, 'center_y': 22},
    'JK.tail': {'center_x': 11, 'center_y': 22},
    'HI.tail': {'center_x': 11, 'center_y': 22},
    'FG.tail': {'center_x': 11, 'center_y': 22},
    'DE.tail': {'center_x': 11, 'center_y': 22},
    'BC.tail': {'center_x': 11, 'center_y': 22},
    'A.tail': {'center_x': 11, 'center_y': 22},

}

# 渣罐坐标字典
d4 = {

    # 停车区坐标
    'pack.1': {'point': (11, 22)},
    'pack.2': {'point': (11, 22)},

    # 接渣口坐标
    'load.1': {'point': (11, 22)},
    'load.2': {'point': (11, 22)},
    'load.3': {'point': (11, 22)},

    # 倒渣口坐标
    'dump.MN': {'point': (11, 205.75)},  # 1号倒渣口
    'dump.KL': {'point': (11, 177.75)},  # 2号倒渣口
    'dump.IJ': {'point': (11, 149.75)},  # 3号倒渣口
    'dump.GH': {'point': (11, 121.75)},  # 4号倒渣口
    'dump.EF': {'point': (11, 93.75)},  # 5号倒渣口
    'dump.CD': {'point': (11, 65.75)},  # 6号倒渣口
    'dump.AB': {'point': (11, 37.75)},  # 7号倒渣口

    # 渣罐位坐标
    'M.31': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.30': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.29': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.28': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.27': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.26': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.25': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.24': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.23': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.22': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.21': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.20': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.19': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.18': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.17': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.16': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.15': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.14': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.13': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.12': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.11': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.10': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.9': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.8': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.7': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.6': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.5': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.4': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.3': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.2': {'pot': (22, 33), 'road_center': (44, 55)},
    'M.1': {'pot': (22, 33), 'road_center': (44, 55)},

    'F.31': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.30': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.29': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.28': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.27': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.26': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.25': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.24': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.23': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.22': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.21': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.20': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.19': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.18': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.17': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.16': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.15': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.14': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.13': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.12': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.11': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.10': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.9': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.8': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.7': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.6': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.5': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.4': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.3': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.2': {'pot': (22, 33), 'road_center': (44, 55)},
    'F.1': {'pot': (22, 33), 'road_center': (44, 55)},

}


def get_nearest_point_name_from_pot_point(current_x, current_y):
    """
    获取当前位置距离最忌的罐体位置名称
    """
    current_x = float(current_x)
    current_y = float(current_y)
    min_distance = float('inf')  # 初始值无穷大
    nearest_point_name = str()  # 最近点名称

    # --- check road center ---
    for k, v in d4.items():
        if 'pack' in k or 'load' in k or 'dump' in k:
            continue
        point_x = float(v.get('road_center')[0])
        point_y = float(v.get('road_center')[1])
        distance = ((point_x - current_x) ** 2 + (point_y - current_y) ** 2) ** (1 / 2)
        if distance < min_distance:
            min_distance = distance
            nearest_point_name = k

    # --- check dump port ---
    for k, v in d4.items():
        if 'dump' not in k:
            continue
        point_x = float(v.get('point')[0])
        point_y = float(v.get('point')[1])
        distance = ((point_x - current_x) ** 2 + (point_y - current_y) ** 2) ** (1 / 2)
        if distance < min_distance:
            min_distance = distance
            nearest_point_name = k

    return nearest_point_name


def get_nearest_point_from_road_center_point(x, y):
    """
    获取当前所在位置最接近的坐标位置
    """
    x = float(x)
    y = float(y)
    min_distance = float('inf')  # 初始值无穷大
    for k, v in d4.items():
        center_x = float(v.get('road_center')[0])
        center_y = float(v.get('road_center')[1])
        distance = ((center_x - x) ** 2 + (center_y - y) ** 2) ** (1 / 2)

        if distance < min_distance:
            min_distance = distance
            nearest_point = center_x, center_y

    return nearest_point


def get_path_name_by_pot_name(pot_name='M.25'):
    """根据渣罐名获取道路名"""
    s1, s2 = pot_name.split('.')
    for s in path_name_list:
        if s1 in s:
            return s


def get_path_name_by_dump_name(dump_name='dump.MN'):
    """根据渣罐名获取道路名"""
    _, s2 = dump_name.split('.')
    return s2


def get_center_point(w1='N', w2='head'):
    """
    获取圆心点坐标
    w1: 关键字
    w2: 关键字
    """
    for k, v in d1.items():
        k_s1, k_s2 = k.split('.')
        if w1 not in k_s1:
            continue
        if w2 not in k_s2:
            continue
        return dict(**v, **{'name': k})


import json
import numpy


def test(current_direction_type=3, start_x=11, start_y=22, target_point_name='M.1'):
    """
    current_direction_type: 起始方向  3 围栏方向 9 渣场方向 12 维修间方向 6 维修间正对方向 0 未知（默认值）
    start_x: 起始位置
    start_y: 起始位置
    target_point_name: 目标名称
    """

    # --- update d1 ---
    data = json.load(open(f"{data_file_dir}/圆心坐标数据.json", 'rb'))
    for item in data.get(
            'name|center_x|center_y|center_3_x|center_3_y|center_9_x|center_9_y|center_12_x|center_12_y|center_6_x|center_6_y'):
        name, center_x, center_y, center_3_x, center_3_y, center_9_x, center_9_y, center_12_x, center_12_y, center_6_x, center_6_y = item.split(
            '|')
        d1[name] = {
            'center_x': float(center_x),
            'center_y': float(center_y),
            'center_3_x': float(center_3_x),
            'center_3_y': float(center_3_y),
            'center_9_x': float(center_9_x),
            'center_9_y': float(center_9_y),
            'center_12_x': float(center_12_x),
            'center_12_y': float(center_12_y),
            'center_6_x': float(center_6_x),
            'center_6_y': float(center_6_y),
        }

    # --- update d4 ---
    data = json.load(open(data_file_path, 'rb'))
    for item in data.get('pot_name|pot_x|pot_y|road_center_x|road_center_y'):
        pot_name, pot_x, pot_y, road_center_x, road_center_y = item.split('|')
        d4[pot_name] = {
            'pot': (float(pot_x), float(pot_y)),
            'road_center': (float(road_center_x), float(road_center_y)),
        }

    # --- update d4 ---
    data = json.load(open(f"{data_file_dir}/倒渣口坐标数据.json", 'rb'))
    for item in data.get('name|x|y'):
        name, x, y = item.split('|')
        d4[name] = {
            'point': (float(x), float(y)),
        }

    # --- check ---
    if not start_x and not start_y:
        return "Reason: 参数缺失"

    # --- check ---
    if current_direction_type not in [3, 9]:
        return "Reason: 暂不支持"

    # --- define ---
    current_point_name = get_nearest_point_name_from_pot_point(start_x, start_y)
    s_point_item = dict(**d4.get(current_point_name), **{'name': current_point_name})
    e_point_item = dict(**d4.get(target_point_name), **{'name': target_point_name})

    # --- define ---
    radius = 14  # 转弯半径
    s_s1, s_s2 = current_point_name.split('.')
    e_s1, e_s2 = target_point_name.split('.')
    print(f"current_point_name: {current_point_name}")
    print(f"target_point_name: {target_point_name}")

    # --- check ---
    if 'dump' not in current_point_name and 'dump' not in target_point_name:
        if current_direction_type == 9 and s_s1 == e_s1 and int(s_s2) > int(e_s2):
            """
            情况1：判断是否是在正前方的情况1
            """
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        {
                            # 直行轨迹坐标参数
                            'start_point_x': s_point_item.get('road_center')[0],
                            'start_point_y': s_point_item.get('road_center')[1],
                            'end_point_x': e_point_item.get('road_center')[0],
                            'end_point_y': e_point_item.get('road_center')[1],
                        },
                    ],
                }
            ]
        elif current_direction_type == 3 and s_s1 == e_s1 and int(s_s2) < int(e_s2):
            """
            情况2：判断是否是在正前方的情况2
            """
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': s_point_item.get('road_center')[0],
                            'start_point_y': s_point_item.get('road_center')[1],
                            'end_point_x': e_point_item.get('road_center')[0],
                            'end_point_y': e_point_item.get('road_center')[1],
                        },
                    ],
                }
            ]
        elif current_direction_type == 9 and s_s1 > e_s1:
            """
            情况3：从起始渣罐位向9点6点3点方向行驶到目标渣罐位
            """
            path_name = get_path_name_by_pot_name(current_point_name)
            corner_name = path_name[:1]  # 拐弯点 9点向6点 取首位
            center_point_1 = get_center_point(w1=corner_name, w2='head')
            path_name = get_path_name_by_pot_name(target_point_name)
            corner_name = path_name[-1:]  # 拐弯点 6点向3点 取末位
            center_point_2 = get_center_point(w1=corner_name, w2='head')
            # print(f"debug.294: center_point_1: {center_point_1}")
            # print(f"debug.294: center_point_2: {center_point_2}")
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': s_point_item.get('road_center')[0],  # 起始点坐标
                            'start_point_y': s_point_item.get('road_center')[1],  # 起始点坐标
                            'end_point_x': center_point_1.get('center_12_x'),  # 入弯坐标
                            'end_point_y': center_point_1.get('center_12_y'),  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_point_1.get('center_12_x'),  # 入弯坐标
                            "start_point_y": center_point_1.get('center_12_y'),  # 入弯坐标
                            "center_point_x": center_point_1.get('center_x'),  # 圆心坐标
                            "center_point_y": center_point_1.get('center_y'),  # 圆心坐标
                            "end_point_x": center_point_1.get('center_9_x'),  # 出弯坐标
                            "end_point_y": center_point_1.get('center_9_y'),  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_point_1.get('center_9_x'),  # 出弯坐标
                            'start_point_y': center_point_1.get('center_9_y'),  # 出弯坐标
                            'end_point_x': center_point_2.get('center_9_x'),  # 入弯坐标
                            'end_point_y': center_point_2.get('center_9_y'),  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_point_2.get('center_9_x'),  # 入弯坐标
                            "start_point_y": center_point_2.get('center_9_y'),  # 入弯坐标
                            "center_point_x": center_point_2.get('center_x'),  # 圆心坐标
                            "center_point_y": center_point_2.get('center_y'),  # 圆心坐标
                            "end_point_x": center_point_2.get('center_6_x'),  # 出弯坐标
                            "end_point_y": center_point_2.get('center_6_y'),  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_point_2.get('center_6_x'),  # 出弯坐标
                            'start_point_y': center_point_2.get('center_6_y'),  # 出弯坐标
                            'end_point_x': e_point_item.get('road_center')[0],  # 目标点坐标
                            'end_point_y': e_point_item.get('road_center')[1],  # 目标点坐标
                        },
                    ],
                }
            ]
        elif current_direction_type == 9 and s_s1 == e_s1 and int(s_s2) < int(e_s2):
            """
            情况5：判断是否是在正后方的情况
            """
            path_name = get_path_name_by_pot_name(current_point_name)
            corner_name = path_name[:1]  # 拐弯点 9点6点3点 取首位
            center_point_1 = get_center_point(w1=corner_name, w2='head')
            center_point_2 = get_center_point(w1=corner_name, w2='tail')
            print(f"debug.403: center_point_1: {center_point_1}")
            print(f"debug.403: center_point_2: {center_point_2}")
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': s_point_item.get('road_center')[0],  # 起始点坐标
                            'start_point_y': s_point_item.get('road_center')[1],  # 起始点坐标
                            'end_point_x': center_point_1.get('center_12_x'),  # 入弯坐标
                            'end_point_y': center_point_1.get('center_12_y'),  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_point_1.get('center_12_x'),  # 入弯坐标
                            "start_point_y": center_point_1.get('center_12_y'),  # 入弯坐标
                            "center_point_x": center_point_1.get('center_x'),  # 圆心坐标
                            "center_point_y": center_point_1.get('center_y'),  # 圆心坐标
                            "end_point_x": center_point_1.get('center_6_x'),  # 出弯坐标
                            "end_point_y": center_point_1.get('center_6_y'),  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_point_1.get('center_6_x'),  # 出弯坐标
                            'start_point_y': center_point_1.get('center_6_x'),  # 出弯坐标
                            'end_point_x': center_point_2.get('center_6_x'),  # 入弯坐标
                            'end_point_y': center_point_2.get('center_6_y'),  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_point_2.get('center_6_x'),  # 入弯坐标
                            "start_point_y": center_point_2.get('center_6_y'),  # 入弯坐标
                            "center_point_x": center_point_2.get('center_x'),  # 圆心坐标
                            "center_point_y": center_point_2.get('center_y'),  # 圆心坐标
                            "end_point_x": center_point_2.get('center_12_x'),  # 出弯坐标
                            "end_point_y": center_point_2.get('center_12_y'),  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_point_2.get('center_12_x'),  # 出弯坐标
                            'start_point_y': center_point_2.get('center_12_y'),  # 出弯坐标
                            'end_point_x': e_point_item.get('road_center')[0],  # 目标点坐标
                            'end_point_y': e_point_item.get('road_center')[1],  # 目标点坐标
                        },
                    ]
                }
            ]
        else:
            return "Result: 暂不支持"

    elif 'dump' not in current_point_name and 'dump' in target_point_name:
        if current_direction_type == 9:
            """
            情况4：渣罐位，车头9点方向，6点拐弯，3点拐弯，倒车倒入指定倒渣口
            """
            # --- get ---
            path_name = get_path_name_by_pot_name(current_point_name)
            corner_name = path_name[:1]  # 拐弯点 9点向6点 取首位
            center_point_1 = get_center_point(w1=corner_name, w2='head')
            path_name = get_path_name_by_dump_name(target_point_name)
            corner_name = path_name[-1:]  # 拐弯点 6点向3点 取末位
            center_point_2 = get_center_point(w1=corner_name, w2='head')
            print(f"debug.465: center_point_1: {center_point_1}")
            print(f"debug.465: center_point_2: {center_point_2}")
            # --- set ---
            degrees = 1.3
            short = 30 * numpy.sin(numpy.radians(degrees))
            long = 30 * numpy.cos(numpy.radians(degrees))
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': s_point_item.get('road_center')[0],  # 起始点坐标
                            'start_point_y': s_point_item.get('road_center')[1],  # 起始点坐标
                            'end_point_x': center_point_1.get('center_12_x'),  # 入弯坐标
                            'end_point_y': center_point_1.get('center_12_y'),  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_point_1.get('center_12_x'),  # 入弯坐标
                            "start_point_y": center_point_1.get('center_12_y'),  # 入弯坐标
                            "center_point_x": center_point_1.get('center_x'),  # 圆心坐标
                            "center_point_y": center_point_1.get('center_y'),  # 圆心坐标
                            "end_point_x": center_point_1.get('center_9_x'),  # 出弯坐标
                            "end_point_y": center_point_1.get('center_9_x'),  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_point_1.get('center_9_x'),  # 出弯坐标
                            'start_point_y': center_point_1.get('center_9_x'),  # 出弯坐标
                            'end_point_x': center_point_2.get('center_9_x'),  # 入弯坐标
                            'end_point_y': center_point_2.get('center_9_y'),  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_point_2.get('center_9_x'),  # 入弯坐标
                            "start_point_y": center_point_2.get('center_9_y'),  # 入弯坐标
                            "center_point_x": center_point_2.get('center_x'),  # 圆心坐标
                            "center_point_y": center_point_2.get('center_x'),  # 圆心坐标
                            "end_point_x": center_point_2.get('center_6_x'),  # 出弯坐标
                            "end_point_y": center_point_2.get('center_6_y'),  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_point_2.get('center_6_x'),  # 出弯坐标
                            'start_point_y': center_point_2.get('center_6_y'),  # 出弯坐标
                            'end_point_x': center_point_2.get('center_6_x') + long,  # 前进30米，摆正车身，为倒车准备
                            'end_point_y': center_point_2.get('center_6_y') - short,  # 前进30米，摆正车身，为倒车准备
                        },
                    ]
                },
                {
                    'type': 'backward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': center_point_2.get('center_6_x') + long,  # 前进30米，摆正车身，为倒车准备
                            'start_point_y': center_point_2.get('center_6_y') - short,  # 前进30米，摆正车身，为倒车准备
                            'end_point_x': e_point_item.get('point')[0],  # 倒渣口坐标
                            'end_point_y': e_point_item.get('point')[1],  # 倒渣口坐标
                        },
                    ]
                }
            ]
        else:
            return "Result: 暂不支持"
    elif 'dump' in current_point_name and 'dump' not in target_point_name and current_direction_type == 3:
        if e_s1 in s_s2:
            """
            情况6：倒渣位，车头3点方向，目标点正对目标巷道情况
            """
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': s_point_item.get('point')[0],  # 起始点坐标
                            'start_point_y': s_point_item.get('point')[1],  # 起始点坐标
                            'end_point_x': e_point_item.get('road_center')[0],  # 目标点坐标
                            'end_point_y': e_point_item.get('road_center')[1],  # 目标点坐标
                        },
                    ]
                }
            ]
        elif e_s1 > s_s2[1]:
            """
            情况7：倒渣位，车头3点方向，目标点位于巷道上侧
            """

            path_name = get_path_name_by_dump_name(current_point_name)
            corner_name = path_name[-1:]  # 拐弯点 3点12点 取末位
            center_point_1 = get_center_point(w1=corner_name, w2='tail')

            path_name = get_path_name_by_pot_name(target_point_name)
            corner_name = path_name[:1]  # 拐弯点 12点9点 取首位
            center_point_2 = get_center_point(w1=corner_name, w2='tail')

            nav_coordinates_1 = [
                # 直线行驶
                {
                    'start_point_x': s_point_item.get('point')[0],  # 起始点坐标
                    'start_point_y': s_point_item.get('point')[1],  # 起始点坐标
                    'end_point_x': center_point_1.get('center_6_x'),  # 入弯坐标
                    'end_point_y': center_point_1.get('center_6_y'),  # 入弯坐标
                },
                # 转弯行驶
                {
                    "start_point_x": center_point_1.get('center_6_x'),  # 入弯坐标
                    "start_point_y": center_point_1.get('center_6_y'),  # 入弯坐标
                    "center_point_x": center_point_1.get('center_x'),  # 圆心坐标
                    "center_point_y": center_point_1.get('center_y'),  # 圆心坐标
                    "end_point_x": center_point_1.get('center_3_x'),  # 出弯坐标
                    "end_point_y": center_point_1.get('center_3_y'),  # 出弯坐标
                },

            ]
            nav_coordinates_2 = [
                # 直线行驶
                {
                    'start_point_x': center_point_1.get('center_3_x'),  # 出弯坐标
                    'start_point_y': center_point_1.get('center_3_y'),  # 出弯坐标
                    'end_point_x': center_point_2.get('center_3_x'),  # 入弯坐标
                    'end_point_y': center_point_2.get('center_3_x'),  # 入弯坐标
                }
            ]

            # --- check ---
            if nav_coordinates_2[0].get('start_point_x') == nav_coordinates_2[0].get('end_point_x'):
                nav_coordinates_2 = []
            if nav_coordinates_2[0].get('start_point_y') == nav_coordinates_2[0].get('end_point_y'):
                nav_coordinates_2 = []

            nav_coordinates_3 = [
                # 转弯行驶
                {
                    "start_point_x": center_point_2.get('center_3_x'),  # 入弯坐标
                    "start_point_y": center_point_2.get('center_3_x'),  # 入弯坐标
                    "center_point_x": center_point_2.get('center_x'),  # 圆心坐标
                    "center_point_y": center_point_2.get('center_y'),  # 圆心坐标
                    "end_point_x": center_point_2.get('center_12_x'),  # 出弯坐标
                    "end_point_y": center_point_2.get('center_12_y'),  # 出弯坐标
                },
                # 直线行驶
                {
                    'start_point_x': center_point_2.get('center_12_x'),  # 出弯坐标
                    'start_point_y': center_point_2.get('center_12_y'),  # 出弯坐标
                    'end_point_x': e_point_item.get('road_center')[0],  # 目标点坐标
                    'end_point_y': e_point_item.get('road_center')[1],  # 目标点坐标
                }
            ]
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': nav_coordinates_1 + nav_coordinates_2 + nav_coordinates_3
                }
            ]
        elif e_s1 < s_s2[0]:
            """
            情况8：倒渣位，车头3点方向，目标点位于巷道下侧
            """
            return "Result: 暂不支持 222"
        else:
            return "Result: 暂不支持"
        return "Result: 暂不支持"
    else:
        return "Result: 暂不支持"


if __name__ == '__main__':
    # --- 现场实际测试数据 ---
    offset = 6.4
    M01 = 14.450 + (offset * 0), 199.64
    M31 = 184.93 - (offset * 6), 195.7
    M03 = 35.5, 205.75
    M12 = 85, 205.75
    dumpCD = 15.49 - 10, 63.06
    # 情况1：判断是否是在正前方的情况1 M.10 -> M.2
    # out = test(current_direction_type=9, start_x=M31[0], start_y=M31[1], target_point_name='M.1')
    # 情况2：判断是否是在正前方的情况2 M.12 -> M.20
    # out = test(current_direction_type=3, start_x=M12[0], start_y=M12[1], target_point_name='M.20')
    # 情况3：从起始渣罐位向9点6点3点方向行驶到目标渣罐位 | M.12 -> F.20
    # out = test(current_direction_type=9, start_x=M12[0], start_y=M12[1], target_point_name='F.20')
    # 情况4：渣罐位，车头9点方向，6点拐弯，3点拐弯，倒车倒入指定倒渣口
    # out = test(current_direction_type=9, start_x=M31[0], start_y=M31[1], target_point_name='dump.CD')
    # out = test(current_direction_type=9, start_x=M03[0], start_y=M03[1], target_point_name='dump.CD')
    # 情况5：判断是否是在正后方的情况 M.3 -> M.10（共2个弯）
    # out = test(current_direction_type=9, start_x=M01[0], start_y=M01[1], target_point_name='M.10')
    # out = test(current_direction_type=9, start_x=M03[0], start_y=M03[1], target_point_name='M.10')
    # 情况6：倒渣位，车头3点方向，目标点正对目标巷道情况
    # out = test(current_direction_type=3, start_x=dumpCD[0], start_y=dumpCD[1], target_point_name='D.20')
    # 情况7：倒渣位，车头3点方向，目标点位于巷道上侧
    out = test(current_direction_type=3, start_x=dumpCD[0], start_y=dumpCD[1], target_point_name='M.1')
    # 情况8：倒渣位，车头3点方向，目标点位于巷道下侧  todo 存在一上来就转弯，有个圆心怎么算呢
    # out = test(current_direction_type=3, start_x=dumpCD[0], start_y=dumpCD[1], target_point_name='A.30')
    print(out)
    # --- test ---
    # out = get_nearest_point_name_from_pot_point(dumpCD[0], dumpCD[1])
    # print(out)