Basi di robotica cooperativa in BrickOS (seconda parte)

Abstract
La robotica cooperativa è un ambito dell'intelligenza artificiale poco diffuso e ancora in fase embrionale. Le possibilità di ricerca sono ampie e molto promettenti: gruppi di robot verranno usati nelle prossime missioni spaziali per garantire maggiore affidabilità e un più ampio raggio d'azione. Ecco come abbiamo implementato un sistema a basso costo per iniziare qualche ricerca in piccolo...

Data di stesura: 25/07/2003
Data di pubblicazione: 15/10/2003
Ultima modifica: 04/04/2006

di Daniele Milan, Diego Valota

Contenuti

Integrazione con SWI-Prolog
Perchè GNU/Linux?
Introduzione
Compilare e caricare la libreria
Inizializzazione e chiusura del device di comunicazione
Invio e ricezione
coop_send()
coop_recv()
Sviluppi
Thread di comunicazione
Token: evitare problemi di trasmissione concorrente
Usare più torri
Esempi di utilizzo
Primo esempio
sender.c
runner1.c
runner2.c
Secondo esempio
robot.pl
robot1.c
Metodi costruttivi

Integrazione con SWI-Prolog

Perché GNU/Linux?

Abbiamo scelto di scrivere una libreria di integrazione per il sistema operativo GNU/Linux perchè non avendo i sorgenti del driver per Windows fornito dalla LEGO non abbiamo avuto modo di verificare che questo driver si adatti alla comunicazione cooperativa. Con Linux, sebbene il driver non sia perfettamente funzionante al momento, possiamo confidare sul fatto che, avendo i sorgenti a disposizione, qualcuno possa effettivamente adattarlo e renderlo perfettamente funzionante.

Per sviluppare e testare il sistema abbiamo usato una distribuzione Gentoo Linux 1.4 release candidate 1 con kernel 2.4.19 (non vanilla, il kernel è stato modificato con varie patch e alcune personalizzazioni) su un portatile Acer Travelmate 734TL.

Introduzione

SWI-Prolog offre un ottimo ventaglio di funzioni per gestire i thread e la relativa comunicazione, oltre ad offrire una comoda interfaccia per l'implementazione di estensioni in linguaggio C.

Per quanto riguarda lo sviluppo di estensioni C per l'interprete SWI-Prolog è consigliabile leggere il capitolo 7 (Foreign Language Interface) dello SWI-Prolog Reference Manual, disponibile al sito: www.swi-prolog.org

Sfruttando queste estensioni abbiamo implementato una serie di predicati per la comunicazione fra torre e RCX usando il protocollo cooperative. La possibilità di usarli come predicati nativi di Prolog permette una perfetta integrazione con i programmi classici. Ogni predicato di comunicazione cooperativa ha un prefisso coop_, che li rende facilmente identificabili dagli altri predicati.

La libreria di interfaccia sviluppata è una bozza. Non è stata testata a fondo, anche per problemi dovuti all'incompletezza del driver di comunicazione con la torre USB.

Le funzioni sono state implementate con particolare attenzione alla esecuzione concorrente, e sono tutte rientranti. È quindi possibile usare la stessa funzione in contemporanea da più thread, sebbene ciò non sia necessario nel caso in cui si scelga di adottare il thread di comunicazione descritto più avanti, che permette di semplificare notevolmente il codice da inserire nei thread pianificatori per gestire la comunicazione.

Compilare e caricare la libreria

La libreria è fornita come un file sorgente in linguaggio C (cooplib.c). Prima di poter essere utilizzata è necessario compilarla per ottenere il file cooplib.so che verrà poi caricato all'interno dell'interprete SWI-Prolog.

Per compilare la libreria è sufficiente digitare i seguenti comandi in una shell di linux (non sono necessari i privilegi di root):

% gcc -I/usr/local/lib/pl-2.2.1/include -fpic -c -g cooplib.c
% gcc -shared -o cooplib.so cooplib.o

Verificate che il percorso alla directory include, nell'esempio /usr/local/lib/pl-2.2.1/include, sia relativo alla locazione dove avete installato i file dell'interprete SWI-Prolog, altrimenti la compilazione non verrà portata a termine correttamente.

Se la compilazione ha avuto successo, dovreste avere un file cooplib.so: questo file contiene il codice binario delle funzioni che verranno poi caricate come predicati all'interno dell'interprete Prolog. Per caricare la libreria all'interno dell'interprete, è necessario richiamare il predicato load_foreign_library(cooplib).

Ora è possibile richiamare i predicati implementati nella libreria.

Inizializzazione e chiusura del device di comunicazione

Per poter comunicare da SWI-Prolog tramite la torre è necessario aprire il file di device associato al driver (in genere /dev/lego0). A questo scopo è stato creato un predicato, coop_open(), che richiede come argomenti il path al device file della torre (solitamente /dev/lego0), e una variabile a cui il predicato assegna il valore intero del descrittore di file usato dagli altri predicati di comunicazione per effettuare la lettura e la scrittura dei dati dal device.

Terminato l'uso del device, solitamente alla terminazione del programma Prolog, è bene chiudere il descrittore di file tramite l'uso della coop_close(), a cui va passato come unico argomento il descrittore di file restituito dalla coop_open().

Invio e ricezione

Una volta ottenuto il descrittore di file associato alla torre, è possibile inviare e ricevere messaggi, tramite l'uso di due funzioni:

coop_send(fd, string, robot, group);
coop_recv(fd, string, robot, group);

`coop_send()`

coop_send() accetta come argomenti il descrittore di file, una stringa contenente i dati da inviare, un intero indicante il robot e uno indicante il gruppo.

Nota: Il formato dei dati da inviare

il parsing dei dati è delegato all'handler utente definito nel programma caricato sull'RCX a cui si invia il messaggio. La definizione del formato dei dati è liberamente definibile dall'utente

La funzione è praticamente identica alla lnp_cooperative_write() implementata nel firmware di BrickOS: crea il pacchetto, calcola il checksum e lo invia alla torre.

`coop_recv()`

coop_recv() ha 4 argomenti: il primo, il file descriptor, viene passato dall'utente, mentre gli altri 3 dovrebbero essere termini non ground. Questi ultimi 3 termini vengono resi ground nel caso in cui venga ricevuto un pacchetto, assegnando ai termini i dati in esso contenuti. Quindi dopo la chiamata il termine passato come argomento string conterrà la stringa dei dati :ricevuti, il termine robot l'indirizzo del robot e il termine group l'indirizzo del gruppo. Questa funzione effettua una read() sul descrittore di file aperto in precedenza: se un pacchetto è presente, lo copia in un buffer e lo scarta nel caso in cui il checksum calcolato sia diverso da quello contenuto nel pacchetto.

Sviluppi

Non ci è stato possibile testare a dovere la libreria e implementarla in modo soddisfacente per problemi legati all'implementazione del driver linux per la torre USB: il processo di sviluppo di tale driver è lento in conseguenza al fatto che la LEGO non rilascia le specifiche di funzionamento della torre. Sono stati scritti vari driver per linux, di cui solo uno siamo riusciti ad usare con successo (la versione 0.5 scritta da Juergen Stuber). Il driver usato non implementa alcun tipo di chiamate ioctl() per poter variare le impostazioni della torre: la portata della trasmissione è fissata ed è di circa 20cm, sufficiente per il download del firmware e dei programmi, ma di scarsa utilità per una comunicazione efficace con i robot.

Il driver implementa una variabile che contiene il valore di default della portata della torre: l'autore del driver ha previsto un valore di 1 in un intervallo valido che va da 1 a 3, per cui la gittata standard è quella corta. Abbiamo provato a impostare il valore a 2 (gittata media), nella funzione tower_probe(), modificando il codice da cos&igrave:

dev->power_level = LEGO_USB_TOWER_POWER_LEVEL_LOW;

a così:

dev->power_level = LEGO_USB_TOWER_POWER_LEVEL_MEDIUM;

Funziona, ma l'aumento di gittata è scarso: si arriva a circa 55 centimetri, oltre l'RCX non riceve.
Cercando invece di impostare la gittata a 3 (usando la define LEGO_USB_TOWER_POWER_LEVEL_HIGH), nel momento in cui si cerca di aprire il device con la coop_open(), viene restituito un Protocol error di cui non siamo riusciti a determinare le cause nè il significato.

Abbiamo inoltre notato che il driver spesso genera un kernel oops, bloccando la macchina, durante la ricezione. Pur avendo controllato i sorgenti del driver, non siamo riusciti a determinare la causa del problema. Molto probabilmente ciò è dovuto a qualche puntatore nullo o ad un overflow del buffer di ricezione.
Il numero di oops aumenta considerevolmente usando kernel linux 2.4.20 o 2.4.21 (vanilla).

L'ideale sarebbe sistemare ciò che è già stato implementato e aggiungere al driver le funzionalità al momento mancanti. Sarebbe comodo implementare un sistema di trasmissione che abbia il terminale come master che controlli il canale e decida chi può trasmettere in un dato momento: si eviterebbero trasmissioni simultanee, che, visto l'uso di un'unica frequenza infrarossa per tutti i robot, sono fallimentari.

Thread di comunicazione

Poichè SWI-Prolog dalla versione 5.2.0 offre la possibilità di avere più thread che implementino pianificatori separati, è possibile rendere l'utilizzo delle funzioni di comunicazione cooperativa all'interno dei pianificatori molto semplice, delegando tutto il lavoro di trasmissione/ricezione ad un thread implementato ad-hoc. Questo thread dovrebbe incaricarsi di gestire la comunicazione fra i thread pianificatori e la torre: ogni thread pianificatore ha una message queue a cui il thread di comunicazione invia i pacchetti ricevuti dalla torre; i thread pianificatori possono a loro volta inviare messaggi alla message queue del thread di comunicazione, che si occuperà di inviarli ai robot. A questo punto ciascun thread pianificatore dovrà controllare periodicamente la presenza di messaggi nella coda e processarli se necessario.

Abbiamo steso una bozza del thread di comunicazione.

```
Comm() :- 
```
```
   repeat,( 
```
```
      ( thread_peek_message(X), 
```
```
        coop_send(X) ); 
```
```
      ( coop_recv(Msg, Name), 
```

        coop_associate(Name, ThreadId),

        thread_send_message(ThreadId, Msg))

```
   ). 
```

La coop_associate() associa al nome del robot l'identificatore del thread pianificatore ad esso associato: l'identificatore è necessario per poter inviare messaggi alla message queue del thread identificato, tramite l'uso della thread_send_message().

Token: evitare problemi di trasmissione concorrente

Sarebbe inoltre utile che il thread gestisse la comunicazione facendo circolare un token fra i robot in modo tale che solo uno possa trasmettere in un dato momento. Per far questo sarebbe sufficiente che il thread, avendo la lista di robot in uso, facesse una richiesta periodica a ciascuno di essi per eventuali messaggi da inviare alla torre.

Usare più torri

Abbiamo verificato che è possibile modificare la frequenza a cui le torri e gli RCX trasmettono. Poichè il driver è in grado di gestire fino a 16 torri contemporaneamente su un singolo PC (porte USB permettendo), si potrebbe assegnare a ciascuna torre una diversa frequenza, in modo tale che non vi siano interferenze: in questo modo si potrebbe gestire la comunicazione in contemporanea di 16 robot, compromettendo però la possibilità di inviare messaggi broadcast da parte dei robot (il terminale, essendo a conoscenza di tutte le frequenze di trasmissione, può comunque inviare lo stesso messaggio sulle diverse frequenze). Abbiamo notato che la luce solare se molto intensa può influire negativamente sulla trasmissione: non sappiamo in quale entità frequenze diverse da quella normalmente usata vengano influenzate.

Esempi di utilizzo

Per testare il protocollo di comunicazione tra robot e la libreria di integrazione con SWI-Prolog, abbiamo realizzato due piccoli sistemi, composti entrambi da 3 robot.

Il protocollo da noi implementato trasmette i pacchetti attraverso la porta infrarossi, quindi per la natura stessa del metodo trasmissivo i pacchetti sono di tipo broadcast. Nel primo esempio si può vedere come il pacchetto spedito da un robot arrivi a tutti gli altri, e questi, grazie all'indirizamento, decidano se interpretarlo o scartarlo.

Nel secondo esempio abbiamo cercato di testare il più possibile la libreria di interfaccia; si vedrà come realizzare programmi che da computer inviino comandi ai robot.
I problemi legati al driver della torre USB non ci hanno permesso di fare in modo che il terminale potesse ricevere messaggi inviati dai robot.

Durante la fase di testing del protocollo e della libreria, abbiamo trovato alcuni espedienti per facilitare la comunicazione infrarossi fra robot; l'ultima sezione tratterà nel dettaglio questi utili accorgimenti.

Primo esempio

Nel primo esempio abbiamo utilizzato tre robot. Il primo, denominato Sender, è composto semplicemente da un RCX e da una base per tenerlo in posizione verticale: si tratta quindi di un Imbot (vedere sezione 3.3).
Gli altri robot, Runner1 e Runner2, entrambi mobili, hanno una struttura identica che differisce solamente per l'aggiunta di una benna a Runner2.

Il programma di Sender utilizza la funzione lnp_cooperative_write() per spedire la stringa "AVANTI" a Runner1. I programmi caricati sui due Runner sono quasi identici; nella funzione main() del programma vengono impostati gli indirizzi dei robot usando la funzione lnp_set_hostadrr(), e viene assegnata come handler dei pacchetti cooperative la funzione vai(), definita nello stesso sorgente. L'handler vai() riceve il pacchetto, se lo riconosce attiva i motori del robot, altrimenti scrive "NOPE" sul display LCD del robot che ha scartato il pacchetto, e attiva la benna nel caso in cui il robot sia Runner2.

Il Sender è stato programmato per inviare pacchetti al robot con indirizzo uno, quindi, eseguendo i tre programmi sui robot, Runner1 comincierà a muoversi mentre Runner2 alzerà la benna.

Figura 1: Il primo esempio in funzione

`sender.c`

```
#include <lnp.h> 
```
```
#include <cooperative.h> 
```
```
#include <conio.h> 
```
```
#include <stdlib.h> 
```
```
#include <unistd.h> 
```
```
#include <dmotor.h> 
```
```
 
```
```
int main() 
```
```
{ 
```
```
   int r; 
```
```
   char *buffer = "AVANTI"; 
```
```
   unsigned char dest, src; 
```
```
 
```
```
   lnp_set_hostaddr(0); 
```
```
   src = lnp_hostaddr; 
```
```
   dest = (1 << 4) & 0xf0; 
```
```
 
```

   r = lnp_cooperative_write(buffer, strlen(buffer), dest, src, 0);

```
   if (r == 0) 
```
```
      cputs(buffer); 
```
```
 
```
```
   return 0; 
```
```
} 
```

`runner1.c`

```
#include <lnp.h> 
```
```
#include <cooperative.h> 
```
```
#include <conio.h> 
```
```
#include <dmotor.h> 
```
```
#include <string.h> 
```
```
 
```

int vai(unsigned char *buf, int len, unsigned char dest)

```
{ 
```
```
   int nlen; 
```
```
   char *ciccio = "AVANTI"; 
```
```
   int r; 
```

   unsigned char me = lnp_hostaddr & 0xf0;

```
 
```
```
   nlen = strlen(ciccio); 
```
```
   if (len < nlen) nlen = len; 
```
```
 
```

   if ((r = mystrcmp((char *)buf, ciccio, nlen)) == 0 && dest == me)

```
   { 
```
```
      motor_a_speed(100); 
```
```
      motor_b_speed(100); 
```
```
      motor_a_dir(fwd); 
```
```
      motor_b_dir(fwd); 
```
```
      sleep(1); 
```
```
   } else { 
```
```
      cputs("NOPE"); 
```
```
   } 
```
```
 
```
```
   return 0; 
```
```
} 
```
```
 
```
```
int main() 
```
```
{ 
```
```
   lnp_set_hostaddr(1); 
```
```
   set_cooperative_user_handler(vai); 
```
```
 
```
```
   return 0; 
```
```
} 
```

`runner2.c`

```
#include <lnp.h> 
```
```
#include <cooperative.h> 
```
```
#include <conio.h> 
```
```
#include <dmotor.h> 
```
```
#include <string.h> 
```
```
 
```

int vai(unsigned char *buf, int len, unsigned char dest)

```
{ 
```
```
   int nlen; 
```
```
   char *ciccio = "AVANTI"; 
```
```
   int r; 
```

   unsigned char me = lnp_hostaddr & 0xf0;

```
 
```
```
   nlen = strlen(ciccio); 
```
```
   if (len < nlen) nlen = len; 
```
```
 
```

   if ((r = mystrcmp((char *)buf, ciccio, nlen)) == 0 && dest == me)

```
   { 
```
```
      motor_a_speed(100); 
```
```
      motor_b_speed(100); 
```
```
      motor_a_dir(fwd); 
```
```
      motor_b_dir(fwd); 
```
```
      sleep(1); 
```
```
   } else { 
```
```
      cputs("NOPE"); 
```
```
      motor_c_speed(255); 
```
```
      motor_c_dir(fwd); 
```
```
      sleep(1); 
```
```
      motor_c_dir(brake); 
```
```
   } 
```
```
 
```
```
   return 0; 
```
```
} 
```
```
 
```
```
int main() 
```
```
{ 
```
```
   lnp_set_hostaddr(2); 
```
```
   set_cooperative_user_handler(vai); 
```
```
 
```
```
   return 0; 
```
```
} 
```

Secondo esempio

In questo esempio abbiamo realizzato una piccola gara di velocità usando tre robot: il primo che arriva su di una striscia bianca vince e invia un segnale di stop gli altri. Abbiamo usato tre robot, uno dei quali costruito dal Gruppo 5 (Cattinelli, Mulloni, Valletta).

Sui tre robot è stato caricato lo stesso programma, che una volta mandato in esecuzione assegna all'handler dei pacchetti cooperative la funzione handler_utente(), dopodichè attiva il sensore ottico, ed entra in un ciclo di controllo che rileva il raggiungimento della striscia bianca.

Dall'interprete SWI-prolog viene caricata la libreria d'interfaccia: utilizzando il predicato coop_open('/dev/lego0', X), inizializziamo la torre USB.

Con il predicato coop_send(X, 'AVANTÌ, 1, 0) viene inviato il messaggio: i robot ricevono il pacchetto e l'handler riconoscendo la stringa "AVANTI" aziona i motori.

Quando uno dei robot arriva sulla striscia bianca spedisce un pacchetto con la stringa "STOP", e l'handler degli altri robot riconoscendo la stringa ferma i motori, ponendo termine alla gara. Il robot vincitore invece gira su se stesso per manifestare la propria gioia!

`robot.pl`

```
vai :- load_foreign_library(cooplib), 
```
```
       coop_open('/dev/lego0', X), 
```
```
       coop_send(X, 'AVANTI', 1, 0). 
```

`robot1.c`

```
#include <lnp.h> 
```
```
#include <cooperative.h> 
```
```
#include <conio.h> 
```
```
#include <dmotor.h> 
```
```
#include <string.h> 
```
```
#include <dsensor.h> 
```
```
 
```
```
#define ROBOT_NAME 1 
```
```
 
```

int handler_utente(unsigned char *buf, int len, unsigned char dest)

```
{ 
```
```
   int nlen; 
```
```
   char *command = "AVANTI"; 
```
```
   char *command2 = "STOP"; 
```
```
   int r; 
```
```
 
```
```
   nlen = strlen(command); 
```
```
   if (len < nlen) nlen = len; 
```
```
 
```

   if ((r = mystrcmp((char *)buf, command, nlen)) == 0)

```
   { 
```
```
      motor_a_speed(255); 
```
```
      motor_b_speed(255); 
```
```
      motor_a_dir(fwd); 
```
```
      motor_b_dir(fwd); 
```
```
      sleep(1); 
```
```
   } else { 
```
```
 
```
```
      nlen = strlen(command2); 
```
```
      if (len < nlen) nlen = len; 
```
```
 
```

      if ((r = mystrcmp((char *)buf, command2, nlen)) == 0) {

```
         motor_a_dir(brake); 
```
```
         motor_b_dir(brake); 
```
```
         sleep(1); 
```
```
      } 
```
```
   } 
```
```
   return 0; 
```
```
} 
```
```
 
```
```
int main() 
```
```
{ 
```
```
   int done=1; 
```
```
 
```
```
   lnp_set_hostaddr(ROBOT_NAME); 
```

   set_cooperative_user_handler(handler_utente);

```
 
```
```
   ds_active(&SENSOR_1); 
```
```
 
```
```
   msleep(100); 
```
```
 
```
```
   while (done) { 
```
```
      lcd_int(LIGHT_1); 
```
```
      if (LIGHT_1 > 44) { 
```

         lnp_cooperative_write("STOP", strlen("STOP"), COOP_ADDR(1,0), 0);

```
         motor_b_speed(255); 
```
```
         motor_b_dir(rev); 
```
```
         done=0; 
```
```
      } 
```
```
      msleep(50); 
```
```
   } 
```
```
 
```
```
   sleep(2); 
```
```
 
```
```
   return 0; 
```
```
} 
```

Metodi costruttivi

La comunicazione fra robot e tra robot e torre avviene per mezzo della porta infrarossi: la situazione per una comunicazione ottimale sarebbe quella di avere le porte infrarossi l'una di fronte all'altra. Avendo a che fare con robot mobili ciò non è sempre possibile, e spesso ci si trova in situazioni in cui i robot sono 'schienà a 'schienà.

Durante il testing abbiamo notato che le superfici bianche o argentate riflettono molto bene i segnali infrarossi, quindi per avere una ricezione omnidirezionale abbiamo montato delle piccole antenne ai lati dei robot, ogni antenna dotata di un disco bianco di forma parabolica (in dotazione nella scatole Lego Mindstorms). Facendo in modo che le antenne siano orientabili a piacimento è possibile creare una struttura che ridiriga e riceva pacchetti in qualsiasi direzione.

Creando dei robot con l'unità RCX montata in verticale è più facile ottenere antenne omnidirezionali.

Questi piccoli accorgimenti risolvono il problema dell'orientamento dei robot, ma riducono di circa la metà la lunghezza di trasmissione, probabilmente a causa della riflessione del segnale infrarossi che viene così ridotto di intensità.

Informazioni sugli autori

Daniele Milan, laureando in Informatica all'Università degli Studi di Milano, si interessa di robotica, os programming e sicurezza. Convinto sostenitore dell'Open Source, usa prevalentemente Linux e il linguaggio C, interessandosi al contempo a sistemi meno conosciuti.

È possibile consultare l'elenco degli articoli scritti da Daniele Milan.

Diego Valota, studente di Informatica all'Università degli Studi di Milano.

È possibile consultare l'elenco degli articoli scritti da Diego Valota.

Altri articoli sul tema Robotica / LEGO Mindstorms.

Risorse

Parte 1
http://www.siforge.org/articles/2003/09/15-rcoop_brickos-1.html
Milano-Messina Action Group. Qui troverete tutti i sorgenti aggiornati del progetto.
http://mag.usr.dsi.unimi.it
SWI-Prolog
http://www.swi-prolog.org
BrickOS
http://brickos.sourceforge.net
RCX Internals
http://graphics.stanford.edu/~kekoa/rcx/
Lego Mindstorms
http://www.legomindstorms.com