""" 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)