czech english
Robotika.cz » Roboti » Ester » Řídící software

Řídící software

Autor: Zbyněk Winkler, 2004-07-16
jak naprogramovat robota a nezbláznit se u toho

Pro Ester jsme se letos rozhodli kompletně přebudovat software na základě zkušeností z minulých let tak, aby bylo umožněno snadnější modifikování a testování různých strategií. Řízení robota je do značné míry hlavně o detekování stavu robota a jeho okolí (např. jsem před brankou a mám míček) a aplikování příslušného řešení (vystřelit). Klasickým nástrojem pro programování tohoto druhu aplikací je FSM (Finite State Machine — konečný automat), které se nám ale jeví jako velmi těžko udržovatelné a vůbec tak nějak neohrabané. Jde to udělat lépe?

FSM — konečné automaty

Základní schéma, které jsme ve větší či menší míře používali ve všech našich robotech od Aleny dále vypadá následovně:
while (true)
{
    sendReceive(commandStruct, stateStruct);
    switch(state)
    {
        case INIT:               state = init(stateStruct); break;
        case GOING_TO_SUN:       state = goingToSun(stateStruct); break;
        case SUN_REACHED:        state = sunReached(stateStruct); break;
        case DEPLOYING_SUN_FLAG: state = deployingSunFlag(stateStruct); break;
        …
    }
}
Je to řešení jednoduché, přímočaré a velmi jednoduše se vysvětluje, jak funguje. Základem je nekonečná smyčka, ve které alternujeme komunikaci s robotem a zpracování příchozích dat.
Komunikace s robotem se po počátečních experimentech s asynchronním zasíláním různých typů zpráv po seriální lince ustálila na synchronní komunikaci pomocí dvou základních struktur. V nejjednodušším případě struktura commandStruct obsahuje stav, jaký si přejeme na výstupních pinech řídícího mikroprocesoru a stateStruct reprezentuje stav vstupních zařízení (A/D převodníky a digitální vstupy).
Zpracování příchozích dat odpovídá vnitřnímu stavu, v jakém se robot nachází. Například ve stavu INIT robot čeká na vytažení startovacího kabelu a v takovém případě začne měřit čas a přejde do stavu GOING_TO_SUN. V každém dalším stavu pak kontroluje, zda neuplynul celkový čas 90s a podmínky specifické pro daný stav, které by způsobily přechod do některého z jiných stavů.
Na první pohled jednoduché, přehledné, pochopitelné. V čem je tedy problém? Takto implementované FSM se hodí na systém, který lze zanalyzovat a přesně specifikovat potřebné stavy i přechody mezi nimi. Zkušenosti ukazují, že mobilní robot takovým systémem není. Je totiž mnoho činností, které jdou jakoby napříč stavy. Pokud se takové činnosti (funkce) snažíme postihnout ve výše navržené struktuře typicky nám exploduje počet stavů nad únosnou míru, přičemž mnoho stavů si je velmi podobných. Takto strukturovaný kód se rychle stává velmi nepřehledným a jen velmi těžko udržovatelným či rozšiřitelným. Hlavním důvodem je duplicita kódu (například v případě Aleny je jízda ke slunci skoro totožná s jízdou k další planetě apod.).
Jak je patrné z uvedených příkladů, dalším velmi důležitým problémem je vlastní definice jednotlivých stavů. Pokud existují stavy, do kterých vede více hran, je třeba zajistit, aby pro všechny hrany platily stejné, nebo alespoň podobné podmínky. Například ve stavu odpovídajícímu umisťování vlaječky na slunce je třeba, aby se robot nacházel u slunce (jinak daná akce nemá smysl). Díky typické rozsáhlosti kódu je to úkol téměř nadlidský. Struktura kódu tomuto totiž vůbec nenapomáhá, ba naopak. Teoreticky je možné přejít z každého stavu do každého (úplný graf) a je tedy pouze na programátorovi (a jeho disciplíně), aby tyto podmínky zaručil. Co se stane, má-li na kódu pracovat několik prográmátorů zároveň nebo má-li na práci jednoho navázat další, si asi dokážete představit sami.

Pokus o nápravu č.1

První pokus o znovuzískání kontroly nad naším kódem byla implementace vnořených FSM. Motivací bylo, že je typicky celkem jedno, jestli robot jede ke slunci, k puku nebo k brance, a že tedy jízda na cíl může být samostatná dovednost, které se bude moci použít v několika různých případech. Stejně tak některé jiné činnosti.
Hlavní smyčka potom může vypadat třeba takto:
while (true)
{
    sendReceive(commandStruct, stateStruct);
    switch(state)
    {
        case AWAITING_START:
            controler.stop();
            if (stateStruct.startCable)
            {
                timeLeft = 90.0;
                state = PLAYING;
            }
            break;

        case PLAYING:
            controler.execute();
            timeLeft -= stateStruct.dt;
            if (timeLeft <= 0)
            {
                state = GAME_OVER;
            }
            break;

        case GAME_OVER:
            controler.stop();
            if (currentSpeed == 0)
            {
                state = AWAITING_START;
                controller.reset();
            }
            break;
    }
}
Je vidět, že tímto způsobem se nám podařilo relativně hezky ošetřit start robota startovacím kabelem a jeho zastavení po 90s (po zastavení se řídící program zresetuje a připraví pro další zápas zejména pro účely testování). Kód pro tuto činnost je oddělen od zbytku řídícího systému, snadno se vizualizuje jeho činnost i jeho korektnost. Řídící systém je celý uschován v objektu controller.
Podobným způsobem můžeme oddělit třeba FSM implementující jízdu na cíl:
int goTo(goal)
{
    offset = calculateOffsetToGoal(goal);

    switch (action(offset))
    {
        case TURN:        action = &turn ;       return Command::IN_PROGRESS;
        case STRAIGHT:    action = &straightFW ; return Command::IN_PROGRESS;
        case STOP:        action = &stop ;       return Command::IN_PROGRESS;
        case IN_PROGRESS:                        return Command::IN_PROGRESS;
        case RESULT_OK:                          return Command::RESULT_OK;
    }
    return Command::IN_PROGRESS;
}
Tímto způsobem jsme dosáhli jednoho z našich cílů — podařilo se nám kód rozdělit na menší části, na kterých potom mohou pracovat různí lidé (anglicky bychom řekli, že jsme snížili coupling). Stále nám ale do jisté míry přetrvává problém s konzistencí jednotlivých stavů. Vnořené FSM také potřebují být v určitých případech (re)inicializovány a typicky očekávají, že se jejich tzv. akční procedura bude volat periodicky v každém kroku dokud samy neřeknou „dost”, a že mají úplnou kontrolu nad příslušnou částí robota (např. pro goTo(goal) to znamená kontrolu nad motory řídícími pohyb robota). Tyto požadavky stále zůstávají na bedrech programátorů, i když jsou částečně zjednodušeny modularizací kódu. Tímto způsobem byla naprogramována Dana.

Co na to konkurence?

Protože jsme stále ještě nebyli spokojeni se strukturou našeho kódu, zajímalo nás, jak tyto problémy řeší konkurence. Podařilo se nám najít následující projekty:

PlayerStage — robotický operační systém

Tento softwarový balík poskytuje řadu abstraktních interface pro nejrozšířenější hardwarové platformy mobilních robotů. Je vybudován na třech principech: (a) rozhraní (interface), (b) zařízení (device), (c) ovladač (driver). V zásadě odpovídá vrstvě, která dělá
základní postprocessing (analýzu) vstupních dat do nejaké hardwarově nezávislé podoby. Pro řízení robota poskytuje obdobnou funkcionalitu transformací obecných příkazů do hardwarově závislé podoby. Pro jednoduchost tento systém můžeme přirovnat k operačnímu systému.
K dispozici je několik aplikačních knihoven v několika různých jazycích, které ale nekladou nějaká zásadní omezení na strukturu klientského kódu. Nepodařilo se nám najít žádný rozsáhlejší příklad použití, přestože se kolem projektu pohybuje řada vědců (zejména amerických), kteří údajně využívají tento systém při implementaci svých algoritmů.

CARMEN — soubor modulů pro všechny aspekty řízení a navigace

Carnegie Mellon Robot Navigation Toolkit se skládá z několika modulů, které mezi sebou komunikují pomocí IPC. IPC je balík umožňující komunikaci mezi moduly pomocí TCP/IP sockets. Komunikace probíhá zasíláním zpráv a umožňuje publish/subscribe (modul generuje data, o která si může zažádat modul jiný bez jeho vědomí) i query/response (modul generuje dotaz a čeká na odpověd) architekturu. Hlavní vlastností omezující tvorbu aplikací je pravděpodobně potřeba konstruovat každý modul jako samostatný proces a s tím spojené obtíže se spouštěním a laděním takovéto aplikace. Za odměnu ale získáme aplikaci z níž každý modul může běžet na jiném počítači.

Tekkotsu — specifické pro AIBO

Tekktosu je velmi úzce specializovaný balík pro robotického psa AIBO. K dispozici máme vše, co je třeba na jeho ovládání, a předpokládané použití spočívá v rozšíření předdefinovaný tříd o specifické úkoly či algoritmy (tutorial, overview). Specifických rošíření dosáhneme předefinováním virtuálních funkcí v kombinaci s (obdobně jako v IPC) notifikacemi v podobě zpráv (registrací tzv. handler funkcí).

Robocode — simulace tankové bitvy v Javě

Robocode se od výše zmíněných programových balíků liší zejména tím, že se zabývá pouze simulovanými roboty. Základní API, které je k dispozici pro programování robotů, má synchronní charakter. Více asi napoví příklad:
// A basic robot that moves around in a square
public void run() {
    turnLeft(getHeading());
    while(true) {
        ahead(100);
        turnRight(90);
    }
}
Mimo tohoto synchronního API nabízí Robocode ještě takzvané AdvancedRobot API, které je více low-level a dává nahlédnout pod pokličku celému systému. API funkce ahead(int) by pomocí tohoto low-level API mohla být implementována následovně:
public void ahead(int dist) {
    setAhead(dist);
    while(dist– > 0)
        execute();
    setStop();
    execute();
  }
}
Metoda execute() zde slouží jako naše sendReceive(commandStruct, stateStruct) a setAhead(dist) si lze představit jako nastavení výkonu motorů. Synchronní API je pak vlastně naše původní FSM pouze otočená tak nějak naruby.

„Lessons learned”

Co jsme se tedy naučili od našich kolegů? Například že velmi důležitý je preprocessing příchozích i odchozích dat do hw nezávislé podoby. Přestože to nemusí být na první pohled patrné, je to celkem náročný úkol (Player v podstatě řeší pouze toto a i v Ester to nakonec byly asi dvě třetiny kódu). V důsledku ale zjednodušuje jak aplikační kód tak i rozhraní pro simulaci.
Dalším společným jmenovatelem všech projektů je snaha o modularizaci kódu se striktní separací a přesnou definicí komunikačního rozhraní, které bývá nečastěji implementováno různými metodami zasílání zpráv.
V neposlední řadě stojí za zmínku i synchnronní API Robocode, které nás zaujalo svojí jednoduchostí a přímočarostí. Jeho designu pravděpodobně značně napomohlo to, že bylo navrhováno pro simulované roboty a nezabývá se tedy takovými „detaily” jako jsou motory či tiky enkodérů, což v důsledku pomáha i snadnější simulaci takto ovládaného robota.

Pokus o nápravu č.2 — Ester

Pomocí vnořených FSM se nám podařilo dosáhnout jisté modularizace kódu tak, aby byl přehlednější a mohlo na něm zároveň pracovat více lidí. Stále se nám ale nepodařilo vyrovnat se s fundamentálním problémem FSM implementovaných pomocí konstrukce switch(state), kterým je fakt, že stále teoreticky můžeme libovolně přecházet mezi stavy a vizualizace těchto přechodů není jednoduchá. Také správná inicializace všech vnořených FSM stále leží na bedrech (a pozornosti) programátorů. To vše vede k obtížím při rozšiřování, ale i pouhém udržování kódu.
Při důkladnějším pohledu na switch-based FSM zjistíme, že se vlastně pouze velmi podivuhodným způsobem snažíme implementovat věc, která je nám programátorům běžná a vlastně se s ní setkáváme dnes a denně (pokud tedy neprogramujete pouze v prologu ). Příklad opět pomůže. Jak by asi vypadala implementace robota jezdícího do čtverce pomocí konstrukce switch(state)?
state = INIT;

while (true)
{
    sendReceive(commandStruct, stateStruct);
    switch(state)
    {
        case INIT:
            if (getAbsHeading() == 0)
            {
                state = STRAIGHT;
                dist = 100;
                break;
            }
            setLeft();
            break;

        case STRAIGHT:
            if (dist == 0)
            {
                state = TURN;
                angle = 90;
                break;
            }
            dist-@-;
            setAhead();
            break;

        case TURN:
            if (angle == 0)
            {
                state = STRAIGHT;
                dist = 100;
                break;
            }
            angle-@-;
            setLeft();
            break;
    }
}
Pro jednoduchost jsme vzdálenost i úhel definovali pouze počtem kroků. Slovy bychom činnost robota mohli popsat asi takto: na počátku se otoč tak, aby tvá absolutní orientace byla 0 a potom jeď vždy 100 tiků rovně a 90 tiků doleva. Už i na takto jednoduchém příkladě vidíme, že to, co lze slovy popsat velmi jednoduše, je trasformováno do tvaru, který původní zadání moc nepřipomíná. Oproti tomu při použití synchronního API Robocode je algoritmus zakódován přímočaře:
// A basic robot that moves around in a square
public void run() {
    turnLeft(getHeading());
    while(true) {
        ahead(100);
        turnRight(90);
    }
}
I tento kód je ve svém důsledku FSM, pouze nám ale vydatně pomáhá kompilátor, aby kód zůstal čitelný. Místo stavové proměné state zde máme IP (instruction pointer), který ukazuje na právě vykonávanou instrukci a zásobník pro volání funkcí (v důsledku implementující vnořené FSM).
Zásadní výhodou tohoto řešení ovšem je to, že většinu věcí, na které musel dříve dávat pozor programátor, za nás nyní obstará kompilátor. Například máme automaticky zajištěno, že se během pohybu robota nedostaneme znovu do inicializační části (odpovídá stavu INIT) i to, že při opětovném volání funkce run() bude tato bezpodmínečně vykonána.
Také stojí za to zmínit jednoduchost a přehlednost kódu. Ta se ještě výrazněji projeví v případě, kdy je třeba implementovat složitější algoritmus obsahující několik do sebe různě vnořených cyklů, podmínek a složitějších funkcí typu ahead() nebo turnRight().
Celý algoritmus řídící Ester na soutěži Eurobot 2004 lze potom popsat až takto jednoduchým kódem:
virtual int main()
{
    mapPalms();
    while (true)
    {
        try
        {
            BallDetect bd;
            EnemyDetect ed;
            while (true)
            {
                searchBall();
                randomMove();
            }
        }
        catch (BallDetect::done)  { shootBall();  }
        catch (EnemyDetect::done) { randomMove(); }
        catch (std::runtime_error){ randomMove(); }
    }
    return 0;
}
Ester nejprve najde pozice palem. To v současné době dělá přejetím hřiště ze startovní pozice po jeho delší straně a monitorováním vzdálenosti, kterou detekuje na stranu pohlížející infra range finder Sharp. Potom si vytvoří dva detektory, které se v systému zaregistrují, aby byly volány v každém kroku a vydá se hledat míčky. Pokud nenajde v blízkosti žádný míček, vybere si nový náhodný cíl na hřišti a pokračuje v hledání. Tyto dvě činnosti se alternují tak dlouho, dokud není nalezen míček a nebo dokud Ester nenarazí na soupeře. Nalezený míček vstřelí do branky, zatímco nalezenému nepříteli se vyhne. V obou případech pokračuje opět hledáním dalšího míčku.

Závěr

Řídící systém Ester by v případě implementace pomocí klasické FSM byl neúnosně složitý (v řádu až několika stovek stavů v několika desítkách FSM). Díky využití klasických postupů, na které je každý programátor zvyklý (konstrukce if, else, while nebo volání funkcí) pro řízení robota pomocího synchronního API jsme oproti minulým letům dosáhli výrazného zvýšení efektivity práce, což nám v důsledku umožnilo vypracovat spolehlivější řídící systém, který ve spojení se spolehlivým hardware zajistil Ester 4. místo.
Samozřejmě, že i aktuální systém má svá úskalí. Tak, jak jsme ho doposud popisovali, by například neumožňoval paralelní řízení pohybu robota a ovládání dalších periferií (v případě Ester válce pro manipulaci s míčky). To jsme vyřešili použitím několika vláken výpočtu, což samo o sobě přináší nemalé komplikace při přístupu ke sdíleným datům a synchronizaci vůbec.
Pro implementaci nastíněného API je třeba si připravit relativně hodně podpůrného kódu (zejména pro obsluhu více vláken). Ovšem po zvládnutí počátečních obtíží se podle našeho názoru taková investice rozhodně vyplatí.
Máte-li jakékoli dotazy či připomínky – kontaktujte nás. Rádi vám odpovíme.