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

"""

"""

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

    'LM.tail': {'center': (11, 22)},
    'JK.tail': {'center': (11, 22)},
    'HI.tail': {'center': (11, 22)},
    'FG.tail': {'center': (11, 22)},
    'DE.tail': {'center': (11, 22)},
    'BC.tail': {'center': (11, 22)},
    'A.tail': {'center': (11, 22)},

}

# 倒渣口坐标字典
d2 = {
    '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号倒渣口
}

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

# 停车区坐标字典
d5 = {
    'pack.1': {'x': 77, 'y': 44},
    'pack.2': {'x': 77, 'y': 44},
}

# 渣罐坐标字典
d4 = {
    '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(x, y):
    """
    获取当前位置距离最忌的罐体位置名称
    """
    x = float(x)
    y = float(y)
    min_distance = float('inf')  # 初始值无穷大
    for k, v in d4.items():
        center_x = float(v.get('pot')[0])
        center_y = float(v.get('pot')[1])
        distance = ((center_x - x) ** 2 + (center_y - y) ** 2) ** (1 / 2)

        if distance < min_distance:
            min_distance = distance
            nearest_pot_name = k

    return nearest_pot_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_center_point_by_keyword(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})


def get_path_name_by_pot_name(pot_name='M.25'):
    """根据渣罐名获取道路名"""
    s1, s2 = pot_name.split('.')
    path_name_list = [
        'MN',
        'KL',
        'IJ',
        'GH',
        'EF',
        'CD',
        'AB',
    ]
    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/渣罐坐标数据.json', 'r'))
    # data = json.load(open('/home/ubuntu/repositories/repositories/casperz.py-project/project-fastapi-hs/data/渣罐坐标数据.json', 'r'))
    data = json.load(
        open(r'E:\casper\repositories\repositories\casperz.py-project\project-fastapi-hs\data\渣罐坐标数据.json', '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': (pot_x, pot_y),
            'road_center': (road_center_x, road_center_y),
        }

    # --- update d2 ---
    # data = json.load(open('../data/渣罐坐标数据.json', 'r'))
    # data = json.load(open('/home/ubuntu/repositories/repositories/casperz.py-project/project-fastapi-hs/data/倒渣口坐标数据.json', 'r'))
    data = json.load(
        open(r'E:\casper\repositories\repositories\casperz.py-project\project-fastapi-hs\data\倒渣口坐标数据.json',
             'rb'))
    for item in data.get('name|x|y'):
        name, x, y = item.split('|')
        d2[name] = {
            'point': (x, y),
        }

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

    # --- check ---
    if not start_x or not start_y:
        return 2  # 参数缺失

    # --- define ---
    current_point_name = get_nearest_point_name_from_pot_point(start_x, start_y)

    # --- define ---
    if 'dump' in current_point_name:
        s_point_item = dict(**d2.get(current_point_name), **{'name': current_point_name})
    else:
        s_point_item = dict(**d4.get(current_point_name), **{'name': current_point_name})

    # --- define ---
    if 'dump' in target_point_name:
        e_point_item = dict(**d2.get(target_point_name), **{'name': target_point_name})
    else:
        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': start_x,
                            'start_point_y': start_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 == 3 and s_s1 == e_s1 and int(s_s2) < int(e_s2):
            """
            情况2：判断是否是在正前方的情况2
            """
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': start_x,
                            'start_point_y': start_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:
            """
            情况3：车头9方向，向6点行驶，并拐弯情况（共2个弯，1个是出弯，1个是入弯）
            """
            w1 = get_path_name_by_pot_name(current_point_name)[:1]  # 首位
            center_1 = get_center_point_by_keyword(w1=w1, w2='head')
            w1 = get_path_name_by_pot_name(target_point_name)[-1:]  # 末位
            center_2 = get_center_point_by_keyword(w1=w1, w2='head')
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': start_x,  # 起始点坐标
                            'start_point_y': start_y,  # 起始点坐标
                            'end_point_x': center_1.get('center')[0],  # 入弯坐标
                            'end_point_y': center_1.get('center')[1] + radius,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_1.get('center')[0],  # 入弯坐标
                            "start_point_y": center_1.get('center')[1] + radius,  # 入弯坐标
                            "center_point_x": center_1.get('center')[0],  # 圆心坐标
                            "center_point_y": center_1.get('center')[1],  # 圆心坐标
                            "end_point_x": center_1.get('center')[0] - radius,  # 出弯坐标
                            "end_point_y": center_1.get('center')[1],  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_1.get('center')[0] - radius,  # 出弯坐标
                            'start_point_y': center_1.get('center')[1],  # 出弯坐标
                            'end_point_x': center_2.get('center')[0] - radius,  # 入弯坐标
                            'end_point_y': center_2.get('center')[1],  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_2.get('center')[0] - radius,  # 入弯坐标
                            "start_point_y": center_2.get('center')[1],  # 入弯坐标
                            "center_point_x": center_2.get('center')[0],  # 圆心坐标
                            "center_point_y": center_2.get('center')[1],  # 圆心坐标
                            "end_point_x": center_2.get('center')[0],  # 出弯坐标
                            "end_point_y": center_2.get('center')[1] - radius,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_2.get('center')[0],  # 出弯坐标
                            'start_point_y': center_2.get('center')[1] - radius,  # 出弯坐标
                            'end_point_x': e_point_item.get('road_center')[0],  # 目标点坐标
                            'end_point_y': e_point_item.get('road_center')[1],  # 目标点坐标
                        },
                    ],
                }
            ]

    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点方向转弯）
            center_1 = get_center_point_by_keyword(w1=corner_name, w2='head')
            # print(f"center_1: {center_1}")
            path_name = get_path_name_by_dump_name(target_point_name)
            corner_name = path_name[-1:]  # 拐弯点（6点到3点方向转弯）
            center_2 = get_center_point_by_keyword(w1=corner_name, w2='head')
            # print(f"center_2: {center_2}")
            return [
                {
                    'type': 'forward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': start_x,  # 起始点坐标
                            'start_point_y': start_y,  # 起始点坐标
                            'end_point_x': center_1.get('center')[0],  # 入弯坐标
                            'end_point_y': center_1.get('center')[1] + radius,  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_1.get('center')[0],  # 入弯坐标
                            "start_point_y": center_1.get('center')[1] + radius,  # 入弯坐标
                            "center_point_x": center_1.get('center')[0],  # 圆心坐标
                            "center_point_y": center_1.get('center')[1],  # 圆心坐标
                            "end_point_x": center_1.get('center')[0] - radius,  # 出弯坐标
                            "end_point_y": center_1.get('center')[1],  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_1.get('center')[0] - radius,  # 出弯坐标
                            'start_point_y': center_1.get('center')[1],  # 出弯坐标
                            'end_point_x': center_2.get('center')[0] - radius,  # 入弯坐标
                            'end_point_y': center_2.get('center')[1],  # 入弯坐标
                        },
                        # 转弯行驶
                        {
                            "start_point_x": center_2.get('center')[0] - radius,  # 入弯坐标
                            "start_point_y": center_2.get('center')[1],  # 入弯坐标
                            "center_point_x": center_2.get('center')[0],  # 圆心坐标
                            "center_point_y": center_2.get('center')[1],  # 圆心坐标
                            "end_point_x": center_2.get('center')[0],  # 出弯坐标
                            "end_point_y": center_2.get('center')[1] - radius,  # 出弯坐标
                        },
                        # 直线行驶
                        {
                            'start_point_x': center_2.get('center')[0],  # 出弯坐标
                            'start_point_y': center_2.get('center')[1] - radius,  # 出弯坐标
                            'end_point_x': center_2.get('center')[0] + 10,  # 3点方向移动10米，摆正车身，为倒车准备
                            'end_point_y': center_2.get('center')[1] - radius,  # 3点方向移动10米，摆正车身，为倒车准备
                        },
                    ]
                },
                {
                    'type': 'backward',
                    'nav_coordinates': [
                        # 直线行驶
                        {
                            'start_point_x': center_2.get('center')[0] + 10,  # 3点方向移动10米，摆正车身，为倒车准备
                            'start_point_y': center_2.get('center')[1] - radius,  # 3点方向移动10米，摆正车身，为倒车准备
                            'end_point_x': e_point_item.get('point')[0],  # 倒渣口坐标
                            'end_point_y': e_point_item.get('point')[1],  # 倒渣口坐标
                        },
                    ]
                }
            ]


if __name__ == '__main__':
    # --- test ---
    # 情况1：判断是否是在正前方的情况1 M.10 -> M.1
    # out = test(current_direction_type=9, start_x=74, start_y=205.75, target_point_name='M.1')
    # 情况2：判断是否是在正前方的情况2 M.12 -> M.20
    # out = test(current_direction_type=3, start_x=85, start_y=205.75, target_point_name='M.20')
    # 情况3：车头9方向，向6点行驶，并拐弯情况（共2个弯，1个是出弯，1个是入弯）
    # out = test(current_direction_type=9, start_x=85, start_y=205.75, target_point_name='F.20')
    # 情况4：渣罐位，车头9点方向，6点拐弯，3点拐弯，倒车倒入指定倒渣口
    out = test(current_direction_type=9, start_x=74, start_y=205.75, target_point_name='dump.CD')
    print(out)