""" """ # 圆心点坐标字典 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)