🤖 第05课：多楼层导航

📌 多楼层导航概述

多楼层导航让机器人从单层应用走向全楼覆盖。核心挑战：地图管理、楼层切换、跨层规划。

┌─────────────────────────────────┐
│     全局拓扑导航层              │
│  (楼层图、电梯节点、跨层路径)    │
├──────────┬──────────┬────────────┤
│ 1F地图   │ 3F地图   │ 5F地图     │
│ 大厅     │ 办公区   │ 行政区     │
├──────────┴──────────┴────────────┤
│     电梯交互层                   │
│  (呼叫→等待→进入→选层→离开)     │
└─────────────────────────────────┘

📌 多楼层地图管理与拓扑导航

import json
from collections import defaultdict, deque

class FloorMap:
    def __init__(self, floor, w, h):
        self.floor = floor; self.w = w; self.h = h
        self.rooms = {}; self.elevators = []; self.nav_points = {}

    def add_room(self, name, x1, y1, x2, y2):
        self.rooms[name] = (x1,y1,x2,y2)

    def add_elevator(self, x, y, eid):
        self.elevators.append((x,y,eid))

    def add_nav(self, name, x, y):
        self.nav_points[name] = (x,y)

class MultiFloorNav:
    def __init__(self):
        self.floors = {}; self.cross = {}

    def add_floor(self, fm):
        self.floors[fm.floor] = fm

    def connect(self, f1, p1, f2, p2, cost=30):
        self.cross.setdefault((f1,p1),[]).append((f2,p2,cost))
        self.cross.setdefault((f2,p2),[]).append((f1,p1,cost))

    def plan(self, sf, sp, gf, gp):
        if sf == gf: return [(sf,sp),(sf,gp)]
        q = deque([(sf,sp,[(sf,sp)])]); visited = {(sf,sp)}
        while q:
            cf, cp, path = q.popleft()
            if cf == gf and cp == gp: return path
            fm = self.floors.get(cf)
            if fm and cp in fm.nav_points:
                for np in fm.nav_points:
                    if (cf,np) not in visited:
                        visited.add((cf,np))
                        q.append((cf,np,path+[(cf,np)]))
            for tf, tp, c in self.cross.get((cf,cp),[]):
                if (tf,tp) not in visited:
                    visited.add((tf,tp))
                    q.append((tf,tp,path+[(tf,tp)]))
        return None

nav = MultiFloorNav()
f1 = FloorMap(1,50,40)
f1.add_room("大厅",0,0,50,30); f1.add_room("前台",5,25,15,35)
f1.add_elevator(45,10,"E1"); f1.add_elevator(45,15,"E2")
f1.add_nav("入口",5,5); f1.add_nav("前台",10,28)
f1.add_nav("电梯E1",45,10); f1.add_nav("电梯E2",45,15)
nav.add_floor(f1)

f3 = FloorMap(3,50,40)
f3.add_room("会议室A",5,12,18,22); f3.add_room("会议室B",22,12,35,22)
f3.add_elevator(45,5,"E1"); f3.add_elevator(45,8,"E2")
f3.add_nav("电梯E1",45,5); f3.add_nav("电梯E2",45,8)
f3.add_nav("会议室A",12,17); f3.add_nav("会议室B",28,17)
nav.add_floor(f3)

f5 = FloorMap(5,50,40)
f5.add_room("总裁办",5,12,18,22); f5.add_room("副总办",22,12,35,22)
f5.add_elevator(45,5,"E1")
f5.add_nav("电梯E1",45,5); f5.add_nav("总裁办",12,17)
nav.add_floor(f5)

nav.connect(1,"电梯E1",3,"电梯E1",30)
nav.connect(1,"电梯E2",3,"电梯E2",35)
nav.connect(3,"电梯E1",5,"电梯E1",25)

print("多楼层导航规划")
print("=" * 55)
for sp, sf, gp, gf in [("入口",1,"会议室A",3),("前台",1,"总裁办",5),("入口",1,"会议室B",3)]:
    path = nav.plan(sf,sp,gf,gp)
    if path:
        print(f"\n📋 {sf}楼{sp} → {gf}楼{gp}")
        for i,(fl,pt) in enumerate(path):
            print(f"  {'→' if i>0 else '📍'} {fl}楼: {pt}")

print("\n✅ 多楼层导航验证通过")

📌 楼层切换状态机

class FloorTransitionFSM:
    STATES = ["IDLE","NAV_ELEV","CALL","WAIT","ENTER","SELECT","IN_ELEV","EXIT","NAV_GOAL","DONE","ERROR"]

    def __init__(self):
        self.state = "IDLE"; self.log = []; self.retries = 0

    def start(self, from_f, to_f):
        self.from_f = from_f; self.to_f = to_f
        self.state = "NAV_ELEV"
        self._log(f"启动: {from_f}楼→{to_f}楼")

    def _log(self, msg):
        self.log.append(f"[{self.state}] {msg}")

    def step(self, event):
        transitions = {
            "NAV_ELEV": {"arrived":"CALL","failed":"ERROR"},
            "CALL": {"ok":"WAIT","fail":"CALL"},
            "WAIT": {"arrived":"ENTER","timeout":"CALL"},
            "ENTER": {"ok":"SELECT","closing":"WAIT"},
            "SELECT": {"ok":"IN_ELEV"},
            "IN_ELEV": {"arrived":"EXIT"},
            "EXIT": {"ok":"NAV_GOAL"},
            "NAV_GOAL": {"arrived":"DONE"},
        }
        if self.state in transitions and event in transitions[self.state]:
            old = self.state
            self.state = transitions[self.state][event]
            self._log(f"{event} → {self.state}")
        return self.state

fsm = FloorTransitionFSM()
fsm.start(1, 5)
events = ["arrived","ok","arrived","ok","ok","arrived","ok","arrived"]
print("楼层切换状态机 - 正常流程")
for e in events:
    s = fsm.step(e)
    print(f"  事件:{e:15s} → {s}")
for l in fsm.log: print(f"  {l}")

print("\n📋 异常场景")
fsm2 = FloorTransitionFSM()
fsm2.start(3, 1)
err_events = ["arrived","fail","fail","ok","timeout","ok","arrived","ok","ok","arrived","ok","arrived"]
for e in err_events:
    s = fsm2.step(e)
    print(f"  事件:{e:15s} → {s}")

print("\n✅ 状态机验证通过")

📌 多楼层任务调度

import random
from collections import defaultdict

class MultiFloorTaskScheduler:
    def __init__(self):
        self.tasks = []; self.travel_time = 3

    def add(self, tid, floor, loc, priority=0):
        self.tasks.append({"id":tid,"floor":floor,"loc":loc,"priority":priority})

    def est_transition(self, f1, f2):
        if f1 == f2: return 0
        return 30 + abs(f2-f1)*self.travel_time + 15

    def optimize(self, cur_floor):
        groups = defaultdict(list)
        for t in self.tasks: groups[t["floor"]].append(t)
        for f in groups: groups[f].sort(key=lambda t: -t["priority"])

        floors = list(groups.keys())
        visit = []
        remaining = list(floors)
        cur = cur_floor
        while remaining:
            nxt = min(remaining, key=lambda f: abs(f-cur))
            visit.append(nxt); remaining.remove(nxt); cur = nxt

        plan = []; total = 0; cf = cur_floor
        for floor in visit:
            if floor != cf:
                t = self.est_transition(cf, floor)
                plan.append({"action":"switch","from":cf,"to":floor,"time":t})
                total += t; cf = floor
            for task in groups[floor]:
                nt = random.uniform(20,60)
                plan.append({"action":"task","task":task,"time":nt})
                total += nt
        return plan, total

sched = MultiFloorTaskScheduler()
sched.add("T001",1,"前台",3); sched.add("T002",3,"会议室A",2)
sched.add("T003",5,"总裁办",5); sched.add("T004",3,"茶水间",1)
sched.add("T005",1,"休息区",2); sched.add("T006",5,"副总办",3)

plan, total = sched.optimize(1)
print("多楼层任务调度优化")
print("=" * 55)
for i, s in enumerate(plan):
    if s["action"] == "switch":
        print(f"\n🛗 [{i+1}] {s['from']}楼→{s['to']}楼 ({s['time']}秒)")
    else:
        t = s["task"]
        print(f"  📦 [{i+1}] {t['id']}: {t['loc']} (P{t['priority']}) ~{s['time']:.0f}s")
print(f"\n⏱️ 总计: {total:.0f}秒 ({total/60:.1f}分钟)")
print("✅ 调度验证通过")

📌 练习

📌 成就