Basi di robotica cooperativa in BrickOS (prima 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/09/2003
Ultima modifica: 04/04/2006

di Daniele Milan, Diego Valota

Contenuti

Introduzione
Specifiche del Protocollo
Dinamiche di ricezione
lnp_receive_packet()
Definire un handler per i pacchetti cooperative
Esempio di handler cooperative
Dinamiche di invio

Introduzione

Il lavoro da cui nascono questi due articoli ci è stato proposto come progetto d'esame per il corso di Intelligenza Artificiale, anno 2002/2003, al Dipartimento di Scienze dell'Informazione dell'Università degli Studi di Milano.
Il progetto era presentato come sperimentale, in quanto non vi erano precedenti esperienze su progetti di robotica cooperativa usando hardware del tipo da noi utilizzato: i mattoncini Mindstorms della Lego.
Ci è stato chiesto di far comunicare fra loro due o più robot e permettere la comunicazione di questi robot con un terminale; inoltre è stato necessario implementare una libreria di supporto che permettesse ad un interprete Prolog, nel nostro caso SWI-Prolog^[3], di comunicare con i robot.
Ci è stata data carta bianca sulla scelta del sistema operativo da usare: da convinti sostenitori dell'OpenSource abbiamo optato per BrickOS. La scelta si è rivelata buona: nel giro di tre giorni abbiamo fatto "parlare" i primi due robot. Abbiamo tratto molti spunti da questo progetto, alcuni dei quali troverete sparsi in questi due articoli. Il codice sorgente non lo rilasciamo qui ma potrete trovarlo con eventuali aggiornamenti sulla pagina del Milano-Messina Action Group^[2].

Specifiche del Protocollo

Il protocollo di cooperazione è una leggera variante del protocollo addressing già implementato in BrickOS, solitamente usato per il download dei programmi sull'RCX. Mentre l'addressing offre la possibilità di inviare messaggi unicast e gestirli tramite diversi handler indirizzando per porte, il protocollo da noi implementato permette l'indirizzamento per gruppi di robot, con un singolo handler definito dall'utente per ciascun RCX.

Abbiamo usato un campo di un byte, per l'indirizzamento, diviso in due sottocampi di 4 bit ciascuno (intervallo di valori: 0..15), usati uno per indicare il gruppo di appartenenza e l'altro per indirizzare il singolo robot nel gruppo. È quindi possibile indirizzare fino a 16 gruppi di 16 robot ciascuno. È previsto anche un campo flag di 1 byte, per ora inutilizzato (discuteremo in una sezione apposita gli usi possibili per questo campo).

Figura 1: formato del pacchetto

L'handler di primo livello, implementato nel firmware di BrickOS, non manipola in alcun modo il messaggio passandolo semplicemente all'handler di secondo livello (definito dall'utente) che deve gestire l'interpretazione del campo dati e l'eventuale indirizzamento (i pacchetti non desiderati devono venire scartati dall'handler utente, che altrimenti riceve TUTTI i pacchetti di tipo cooperativo trasmessi da ogni sorgente). Per assegnare l'handler per i pacchetti cooperative è necessario che l'utente effettui una chiamata alla funzione lnp_set_cooperative_handler(), passando l'indirizzo della funzione handler. A questa funzione viene passato l'indirizzo della stringa contenuta nel messaggio, un intero per la lunghezza e un intero per il destinatario. L'handler definito dall'utente può usare il campo destinazione per filtrare eventuali pacchetti non desiderati.

Dinamiche di ricezione

Nel momento in cui il ricevitore infrarosso dell'RCX rileva la presenza di un segnale, viene sollevato un interrupt che richiama la funzione rx_handler(): dopo aver controllato l'assenza di collisioni, questa richiama per ogni byte ricevuto la funzione lnp_integrity_byte() che controlla la corretta ricezione del pacchetto tramite il calcolo di un parity checksum. Ricevuto il pacchetto lnp_integrity_byte() richiama lnp_receive_packet() che sceglie l'handler relativo al tipo di pacchetto ricevuto (il tipo di pacchetto è determinato dal codice contenuto nell'header: 0xf0 per l'integrity layer, 0xf1 per l'addressing, 0xf2 per il cooperative).

Figura 2: network layer

`lnp_receive_packet()`

Vediamo le differenze fra i layer addressing e cooperative durante la selezione dell'handler:

```
case 0xf1: 
```
```
  if (length > 2) { 
```
```
    unsigned char dest =*(data++); 
```
```
 
```

    if(lnp_hostaddr == (dest & LNP_HOSTMASK)) {

      unsigned char port=dest & LNP_PORTMASK;

      addrh = lnp_addressing_handler[port];

```
      if(addrh) { 
```
```
        unsigned char src=*(data++); 
```
```
        addrh(data,length-2,src); 
```
```
      } 
```
```
    } 
```
```
  } 
```
```
  break; 
```

Nel caso del layer addressing l'handler viene richiamato solo se l'indirizzo di destinazione del pacchetto corrisponde con l'indirizzo del'RCX che l'ha ricevuto. È quindi possibile inviare solo pacchetti unicast.

```
case 0xf2: 
```
```
  { 
```
```
     unsigned char dest=*(data++); 
```
```
     unsigned char source=*(data++); 
```
```
     unsigned char flags=*(data++); 
```
```
     cooph = lnp_cooperative_handler; 
```
```
 
```
```
     if (cooph) { 
```

        lnp_cooperative_handler(data, length, dest);

```
     } 
```
```
  } 
```
```
  break; 
```

Usando i pacchetti cooperative la gestione dell'indirizzamento è invece delegata all'handler definito dall'utente, che può quindi gestire l'indirizzamento nel modo più opportuno per l'uso che se ne vuole fare.

Definire un handler per i pacchetti cooperative

```
int lnp_set_cooperative_handler( 
```

  int (*cuh)(unsigned char *, int, unsigned char));

Per assegnare un handler che venga richiamato dal firmware alla ricezione di pacchetti cooperative è sufficiente dichiarare una funzione con questo modello:

int funzione(unsigned char *, int, unsigned char);

Al primo argomento della funzione sarà assegnato il buffer contenente il campo DATA del pacchetto ricevuto; il secondo argomento è un intero che indica la lunghezza del campo DATA. L'ultimo argomento indica la destinazione del pacchetto: a differenza dell'addressing che passa come terzo argomento la sorgente e processa il campo destinazione prima di richiamare l'handler, il cooperative delega tutto all'utente.

Esempio di handler cooperative

Riportiamo di seguito un esempio che abbiamo usato per testare il funzionamento dell'handler:

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

```
{ 
```
```
   int nlen; 
```
```
   char *string = "VAI"; 
```

   unsigned char me = lnp_hostaddr & 0xf0;

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

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

```
   { 
```
```
      motor_b_speed(255); 
```
```
      motor_b_dir(fwd); 
```
```
      sleep(1); 
```
```
   } else { 
```
```
      cputs("NOPE"); 
```
```
   } 
```
```
 
```
```
   return 0; 
```
```
} 
```

Questo handler si aspetta una stringa "VAI", quindi ricevuto il pacchetto confronta il campo dati con la stringa attesa, verifica che il pacchetto sia destinato all'RCX su cui sta girando l'handler, dopodichè avvia il motore B se le condizioni sono entrambe vere, altrimenti scrive NOPE sull'lcd.

ATTENZIONE! Abbiamo notato che l'implementazione contenuta in BrickOS 2.6.10 della funzione strcmp() non funziona come previsto. È quindi consigliabile reimplementarla. Di seguito trovate l'implementazione che abbiamo usato per testare gli esempi:

int mystrcmp(char *a, char *b, int len)

```
{ 
```
```
   int i; 
```
```
 
```
```
   for (i = 0; i < len; i++) 
```
```
      if (*(a+i) != *(b+i)) { 
```
```
         return 1; 
```
```
      } 
```
```
   return 0; 
```
```
} 
```

Dinamiche di invio

Per inviare pacchetti sono previste funzioni apposite: lnp_integrity_write(), lnp_addressing_write() e la nostra lnp_cooperative_write(). Tutte allocano e riempiono coi corretti valori un buffer che viene verificato tramite calcolo del checksum (accodato al buffer) e passato alla lnp_logical_write() che invia il pacchetto sollevando l'interrupt TX (la documentazione sugli interrupt è contenuta nel Hitachi H8/3297 Series Hardware Manual, reperibile con qualche difficoltà su Internet).

La lnp_cooperative_write() è stata scritta in modo tale che prima di spedire un pacchetto la portata dell'infrarosso venga impostata al valore massimo, così che il messaggio possa raggiungere i robot entro una distanza di circa 3 metri (nelle migliori condizioni). Nel caso in cui la gittata prima non fosse impostata al valore massimo, il valore precedente viene ripristinato.

```
int lnp_cooperative_write( 
```

  const unsigned char *data, unsigned char length,

  unsigned char dest, unsigned char flags);

La lnp_cooperative_write() accetta come argomenti un puntatore al buffer contenente la stringa, un intero indicante la lunghezza della stringa, un intero per la destinazione (valore combinato di host e gruppo: usare la macro COOP_ADDR(host, group) per avere il valore corretto) e un intero per le eventuali flag (per ora inutilizzate).

```
int r; 
```
```
int far=lnp_logical_range_is_far(); 
```
```
char *buffer_ptr = malloc(length+6); 
```
```
 
```

unsigned char c = lnp_checksum_copy(buffer_ptr+5, data, length);

```
 
```

lnp_checksum_step(c, buffer_ptr[0]=0xf2);

lnp_checksum_step(c, buffer_ptr[1]=length+3);

lnp_checksum_step(c, buffer_ptr[2]=dest );

Viene copiato il buffer dati nel puntatore contenente il pacchetto da inviare, inseriti i valori nell'header e calcolato il checksum.

lnp_checksum_step(c, buffer_ptr[3]=(lnp_hostaddr | lnp_groupaddr));

Si noti che la funzione lnp_set_hostaddr() registra nella variabile lnp_hostaddr l'intero indicante l'indirizzo già shiftato di 4 bit a sinistra, per cui nel codice sopra riportato è sufficiente fare un or bit-a-bit con l'intero indicante il gruppo. Nel caso invece dell'indirizzo di destinazione è fondamentale shiftare l'indirizzo dell'host destinazione (usare la macro COOP_ADDR(host, group)).

lnp_checksum_step(c, buffer_ptr[4]=flags);

```
 
```
```
buffer_ptr[length+5] = c; 
```

Il checksum viene copiato nell'ultimo byte del pacchetto.

```
if (far) 
```

   r = lnp_logical_write(buffer_ptr, length+6);

```
else { 
```
```
   lnp_logical_range(1); 
```

   r = lnp_logical_write(buffer_ptr, length+6);

```
   lnp_logical_range(0); 
```
```
} 
```

Per la cooperazione è necessario avere la massima portata di trasmissione: per questo se la portata di default non è quella massima, viene temporaneamente incrementata e dopo la trasmissione reimpostata al default (così il consumo di batterie è ottimizzato).

```
free(buffer_ptr); 
```
```
 
```
```
return r; 
```

La memoria allocata per il buffer viene liberata e il numero di byte scritti assegnato al valore di ritorno della funzione.

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 2
http://www.siforge.org/articles/2003/10/15-rcoop_brickos-2.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