diff --git a/tools/demo/assign_parks.py b/tools/demo/assign_parks.py new file mode 100644 index 0000000..ffd2f14 --- /dev/null +++ b/tools/demo/assign_parks.py @@ -0,0 +1,87 @@ +# Brugerens 17 parkeringsbåse (N_mapMarker) -> "P " + "Connect P " (redskaber/vogne), køretøjer ind i båsene. +# Mønster fra savegame6: redskab ~4 m fra P mod udgangen, trækstang mod udgangen; Connect 9 m (korte) / 12 m (lange) +# ude ad båsen; traktorer forlæns ind med fronten ved P. Kør: python3 assign_parks.py +import sys, re, math +SG = sys.argv[1] +ad_path = SG + '/AutoDrive_config.xml' +raw = open(ad_path, encoding='utf-8-sig', newline='').read() +bom = open(ad_path, 'rb').read(3) == b'\xef\xbb\xbf' +nl = '\r\n' if '\r\n' in raw else '\n' +wpx = re.search(r'.*?', raw, re.S).group(0) +xs = [float(v) for v in re.search(r'(.*?)', wpx, re.S).group(1).split(',')] +zs = [float(v) for v in re.search(r'(.*?)', wpx, re.S).group(1).split(',')] +mms = re.findall(r'\s*([^<]*)\s*([^<]*)\s*([^<]*)\s*', raw) +markers = [[name, int(float(i)), grp] for _, i, name, grp in mms] +bays = sorted([m for m in markers if re.fullmatch(r'\d+_mapMarker', m[0])], key=lambda m: int(m[0].split('_')[0])) +assert len(bays) == 17, len(bays) + +# (filnavn-del, nummer blandt ens, navn, connect-afstand i m eller None for traktor) +PLAN = [("z18051", 1, "Tipper 1", 12), ("z18051", 2, "Tipper 2", 12), ("mks32", 1, "Milk Tank", 12), + ("supercis800", 1, "Slurry Tank", 12), ("cenio4000", 1, "Cultivator", 9), ("pov5xl", 1, "Plow", 9), + ("seeder1500", 1, "Seeder", 9), ("maxima3TIL", 1, "Planter", 9), ("k105", 1, "Spreader", 12), + ("crystal", 1, "Crystal", None), ("vario700", 1, "Vario 700", None), ("puma", 1, "Puma", None), + ("multiPurposeTruck", 1, "Pallet Truck", None), ("mt655", 1, "MT655", None), ("series6RLarge", 1, "6R", None), + ("t7", 1, "T7", None), ("series6C", 1, "6C", None)] + +def bay_waypoint(p_id, metres): + """vejpunkt i båsen 'metres' fra P mod udgangen (båsen løber mod nord, faldende z)""" + px, pz = xs[p_id-1], zs[p_id-1] + best = None + for i in range(p_id, p_id + 12): # båsens vejpunkter følger P-markøren i id-rækkefølge + if abs(xs[i-1] - px) > 0.6: continue + d = pz - zs[i-1] + if best is None or abs(d - metres) < abs(best[1] - metres): best = (i, d) + return best + +place = {} # (key,num) -> (cx, cz, yaw, parkIndex) +for (key, num, name, conn), bay in zip(PLAN, bays): + p_id = bay[1] + bay[0] = "P " + name + park_index = markers.index(bay) + 1 + px, pz = xs[p_id-1], zs[p_id-1] + if conn: + cid, dist = bay_waypoint(p_id, conn) + markers.append(["Connect P " + name, cid, "All"]) + place[(key, num)] = (px, pz - 4.0, 180.0, park_index) # redskab 4 m ude, trækstang mod nord (udgangen) + print(f"{name:12s} P wp{p_id} ({px:.1f},{pz:.1f}) Connect wp{cid} {dist:.1f} m ude") + else: + place[(key, num)] = (px, pz - 2.0, 0.0, park_index) # traktor forlæns ind (mod syd), front ved P + print(f"{name:12s} P wp{p_id} ({px:.1f},{pz:.1f})") + +mm = nl + "".join(" %s %d.000000%s %s%s %s%s %s" + % (k, nl, wid, nl, name, nl, grp, nl, k, nl) for k, (name, wid, grp) in enumerate(markers, 1)) + " " +raw = re.sub(r"().*?()", lambda m: m.group(1) + mm + m.group(2), raw, count=1, flags=re.S) +open(ad_path, 'w', encoding='utf-8-sig' if bom else 'utf-8', newline='').write(raw) + +# køretøjer +vpath = SG + '/vehicles.xml' +vraw = open(vpath, encoding='utf-8-sig', newline='').read() +vbom = open(vpath, 'rb').read(3) == b'\xef\xbb\xbf' +def yaw_of(rot): + rx, ry, rz = [float(v) for v in rot.split()] + return 180.0 - ry if abs(rx) > 90 else ry +def move(block, cx, cz, new_yaw): + comps = re.findall(r'', block) + x0, y0, z0 = [float(v) for v in comps[0][1].split()] + t0 = math.radians(yaw_of(comps[0][2])); tn = math.radians(new_yaw) + def mv(m): + px, py, pz = [float(v) for v in m.group(2).split()] + dx, dz = px - x0, pz - z0 + lx = dx*math.cos(t0) - dz*math.sin(t0); lz = dx*math.sin(t0) + dz*math.cos(t0) + nx = lx*math.cos(tn) + lz*math.sin(tn); nz = -lx*math.sin(tn) + lz*math.cos(tn) + return '' % (m.group(1), cx+nx, py, cz+nz, new_yaw) + return re.sub(r'', mv, block) +counts = {} +def fix(m): + block = m.group(0) + key = re.search(r'filename="([^"]*)"', block).group(1).split('/')[-1].replace('.xml', '') + counts[key] = counts.get(key, 0) + 1 + target = place.get((key, counts[key])) + if target is None: return block + cx, cz, yaw, park = target + block = re.sub(r'\s*]*/>', '', block) + block = move(block, cx, cz, yaw) + return block.replace('', ' \n ' % park) +vraw = re.sub(r'', fix, vraw, flags=re.S) +open(vpath, 'w', encoding='utf-8-sig' if vbom else 'utf-8', newline='').write(vraw) +print("markører nu:", len(markers), "— køretøjer placeret:", sum(1 for k in place if counts.get(k[0], 0) >= k[1]))