Server-cpp/project-fastapi-hs/unit/Scheduler_c1.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',
]

# 渣罐坐标字典
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_corner_point(w1='N', w2='head'):
    """获取拐弯点"""
    if w2 == 'head':
        return d4.get(f"{w1}.{1}").get('road_center')
    elif w2 == 'tail':
        return d4.get(f"{w1}.{31}").get('road_center')


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


import json


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 未知（默认值）
    s_name: 起始位置
    e_name: 目标位置
    """

    # --- 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方向，向9点6点3点行驶（共2个弯，1个是出弯，1个是入弯）
            """
            path_name = get_path_name_by_pot_name(current_point_name)
            corner_name = path_name[:1]  # 拐弯点（9点到6点方向转弯）（取首位）
            corner_point_1 = get_corner_point(w1=corner_name, w2='head')
            corner_point_1_x = corner_point_1[0]
            corner_point_1_y = corner_point_1[1]
            path_name = get_path_name_by_pot_name(target_point_name)
            corner_name = path_name[-1:]  # 拐弯点（6点到3点方向转弯）（取末位）
            corner_point_2 = get_corner_point(w1=corner_name, w2='head')
            corner_point_2_x = corner_point_2[0]
            corner_point_2_y = corner_point_2[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': corner_point_1_x,  # 入弯坐标
                            'end_point_y': corner_point_1_y,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": corner_point_1_x,  # 入弯坐标
                            "start_point_y": corner_point_1_y,  # 入弯坐标
                            "center_point_x": corner_point_1_x,  # 圆心坐标
                            "center_point_y": corner_point_1_y - radius,  # 圆心坐标
                            "end_point_x": corner_point_1_x - radius,  # 出弯坐标
                            "end_point_y": corner_point_1_y - radius,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': corner_point_1_x - radius,  # 出弯坐标
                            'start_point_y': corner_point_1_y - radius,  # 出弯坐标
                            'end_point_x': corner_point_2_x - radius,  # 入弯坐标
                            'end_point_y': corner_point_2_y + radius,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": corner_point_2_x - radius,  # 入弯坐标
                            "start_point_y": corner_point_2_y + radius,  # 入弯坐标
                            "center_point_x": corner_point_2_x,  # 圆心坐标
                            "center_point_y": corner_point_2_y + radius,  # 圆心坐标
                            "end_point_x": corner_point_2_x,  # 出弯坐标
                            "end_point_y": corner_point_2_y,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': corner_point_2_x,  # 出弯坐标
                            'start_point_y': corner_point_2_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：判断是否是在正后方的情况1 M.3 -> M.10（共2个弯）
            """
            path_name = get_path_name_by_pot_name(current_point_name)
            corner_name = path_name[:1]  # 拐弯点（9点到6点方向转弯）（取首位）
            corner_point_1 = get_corner_point(w1=corner_name, w2='head')
            corner_point_1_x = corner_point_1[0]
            corner_point_1_y = corner_point_1[1]
            corner_point_2 = get_corner_point(w1=corner_name, w2='tail')
            corner_point_2_x = corner_point_2[0]
            corner_point_2_y = corner_point_2[0]
            # print(f"corner_point_1: {corner_point_1}")
            # print(f"corner_point_2: {corner_point_2}")
            # return "Result: 正在开发"
            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': corner_point_1_x,  # 入弯坐标
                            'end_point_y': corner_point_1_y,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": corner_point_1_x,  # 入弯坐标
                            "start_point_y": corner_point_1_y,  # 入弯坐标
                            "center_point_x": corner_point_1_x,  # 圆心坐标
                            "center_point_y": corner_point_1_y - radius,  # 圆心坐标
                            "end_point_x": corner_point_1_x,  # 出弯坐标
                            "end_point_y": corner_point_1_y - radius - radius,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': corner_point_1_x,  # 出弯坐标
                            'start_point_y': corner_point_1_y - radius - radius,  # 出弯坐标
                            'end_point_x': corner_point_2_x,  # 入弯坐标
                            'end_point_y': corner_point_2_y - radius - radius,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": corner_point_2_x,  # 入弯坐标
                            "start_point_y": corner_point_2_y - radius - radius,  # 入弯坐标
                            "center_point_x": corner_point_2_x,  # 圆心坐标
                            "center_point_y": corner_point_2_y - radius,  # 圆心坐标
                            "end_point_x": corner_point_2_x,  # 出弯坐标
                            "end_point_y": corner_point_2_y,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': corner_point_2_x,  # 出弯坐标
                            'start_point_y': corner_point_2_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点拐弯，倒车倒入指定倒渣口
            """
            path_name = get_path_name_by_pot_name(current_point_name)
            # print(f"path_name: {path_name}")
            corner_name = path_name[:1]  # 拐弯点（9点到6点方向转弯）（取首位）
            corner_point_1 = get_corner_point(w1=corner_name, w2='head')
            corner_point_1_x = corner_point_1[0]
            corner_point_1_y = corner_point_1[1]
            # print(f"corner_point_1: {corner_point_1}")
            path_name = get_path_name_by_dump_name(target_point_name)
            corner_name = path_name[-1:]  # 拐弯点（6点到3点方向转弯）（取末位）
            corner_point_2 = get_corner_point(w1=corner_name, w2='head')
            corner_point_2_x = corner_point_2[0]
            corner_point_2_y = corner_point_2[1]
            # print(f"corner_point_2: {corner_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': corner_point_1_x,  # 入弯坐标
                            'end_point_y': corner_point_1_y,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": corner_point_1_x,  # 入弯坐标
                            "start_point_y": corner_point_1_y,  # 入弯坐标
                            "center_point_x": corner_point_1_x,  # 圆心坐标
                            "center_point_y": corner_point_1_y - radius,  # 圆心坐标
                            "end_point_x": corner_point_1_x - radius,  # 出弯坐标
                            "end_point_y": corner_point_1_y - radius,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': corner_point_1_x - radius,  # 出弯坐标
                            'start_point_y': corner_point_1_y - radius,  # 出弯坐标
                            'end_point_x': corner_point_2_x - radius,  # 入弯坐标
                            'end_point_y': corner_point_2_y + radius,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": corner_point_2_x - radius,  # 入弯坐标
                            "start_point_y": corner_point_2_y + radius,  # 入弯坐标
                            "center_point_x": corner_point_2_x,  # 圆心坐标
                            "center_point_y": corner_point_2_y + radius,  # 圆心坐标
                            "end_point_x": corner_point_2_x,  # 出弯坐标
                            "end_point_y": corner_point_2_y,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': corner_point_2_x,  # 出弯坐标
                            'start_point_y': corner_point_2_y,  # 出弯坐标
                            'end_point_x': corner_point_2_x + 30,  # 前进30米，摆正车身，为倒车准备
                            'end_point_y': corner_point_2_y,  # 摆正车身，为倒车准备
                        },
                    ]
                },
                {
                    'type': 'backward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': corner_point_2_x + 30,  # 前进30米，摆正车身，为倒车准备
                            'start_point_y': corner_point_2_y,  # 摆正车身，为倒车准备
                            '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点）（取末位）
            corner_point_1_x, corner_point_1_y = get_corner_point(w1=corner_name, w2='tail')
            path_name = get_path_name_by_pot_name(target_point_name)
            corner_name = path_name[:1]  # 拐弯点（12点->9点）（取首位）
            corner_point_2_x, corner_point_2_y = get_corner_point(w1=corner_name, w2='tail')
            # print(f"corner_point_2_x: {corner_point_2_x}")
            # print(f"corner_point_2_y: {corner_point_2_y}")
            nav_coordinates_1 = [
                # 直线行驶
                {
                    'start_point_x': s_point_item.get('point')[0],  # 起始点坐标
                    'start_point_y': s_point_item.get('point')[1],  # 起始点坐标
                    'end_point_x': corner_point_1_x,  # 入弯坐标
                    'end_point_y': corner_point_1_y,  # 入弯坐标
                },
                # 转弯行驶
                {
                    "start_point_x": corner_point_1_x,  # 入弯坐标
                    "start_point_y": corner_point_1_y,  # 入弯坐标
                    "center_point_x": corner_point_1_x,  # 圆心坐标
                    "center_point_y": corner_point_1_y + radius,  # 圆心坐标
                    "end_point_x": corner_point_1_x + radius,  # 出弯坐标
                    "end_point_y": corner_point_1_y + radius,  # 出弯坐标
                },

            ]
            nav_coordinates_2 = [
                # 直线行驶
                {
                    'start_point_x': corner_point_1_x + radius,  # 出弯坐标
                    'start_point_y': corner_point_1_y + radius,  # 出弯坐标
                    'end_point_x': corner_point_2_x + radius,  # 入弯坐标
                    'end_point_y': corner_point_2_y - radius,  # 入弯坐标
                }
            ]
            # --- check ---
            if (corner_point_1_x + radius == corner_point_2_x + radius
                    or corner_point_1_y + radius == corner_point_2_y - radius):
                nav_coordinates_2 = []
            nav_coordinates_3 = [
                # 转弯行驶
                {
                    "start_point_x": corner_point_2_x + radius,  # 入弯坐标
                    "start_point_y": corner_point_2_y - radius,  # 入弯坐标
                    "center_point_x": corner_point_2_x,  # 圆心坐标
                    "center_point_y": corner_point_2_y - radius,  # 圆心坐标
                    "end_point_x": corner_point_2_x,  # 出弯坐标
                    "end_point_y": corner_point_2_y,  # 出弯坐标
                },
                # 直线行驶
                {
                    'start_point_x': corner_point_2_x,  # 出弯坐标
                    'start_point_y': corner_point_2_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
    # --- cad理论测试数据 ---
    dumpCD = 4.18, 65.75
    # 情况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点行驶，并拐弯情况（共2个弯，1个是出弯，1个是入弯）
    # 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：判断是否是在正后方的情况1 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)