Stratégie de développement d’un plugin hardware pour Navigation RF-ID Pepper (partie 2)

(Partie 1)

Maintenant que le module de positionnement par compas est développé, le robot peut se déplacer par rapport au Nord. On est donc capable de définir un angle d’orientation pour le déplacement dans un système cartésien 3D x,y .  Il faut maintenant être capable de positionner le robot dans l’espace InDoor. Les solutions utilisées par la plupart des développeurs et chercheurs reposent sur l’analyse de l’image ou des positions de codes barres collés sur les murs.  Notre idée et de faire appel à une solution hardware qui pourra, de plus, être utilisée par d’autres robots.  Il s’agit de mettre en place une grille de puces RF ID 125 Khz sur le sol , sous le parquet et de disposer un lecteur dédié sous le robot Pepper.

Nous nous sommes orientés vers un lecteur simple Rhydolabz simple à configurer en I2C sur un Raspberry PiZero ou PI3. Tout le concept de cette solution hardware réside dans le positionnement et la détection de Tag RFID disposés en grille avec espacement de 10 cm entre les tags. Un serveur est en écoute permanente des signaux sous le robot et récupère la position X et Y de chaque tag. Le logiciel serveur connait la position de destination. Aussi, à chaque détection d’un Tag RFID nouveau, il est capable de recalculer la position, la direction et le vecteur vers la destination. Il peut aussi corriger une éventuelle dérive du robot (patinage des roues par exemple, ou robot poussé par un humain)

A ce stade du développement, nous nous focalisons sur des déplacements simples dans des couloirs rectangulaires. Dans les développements suivants, nous nous attacherons à gérer les bords de couloirs, les angles et les obstacles pour rendre les déplacement les plus intelligents et rationnels possibles

Développement d’un code source de génération de grille pour parquet

Avant de commencer la programmation, nous développons un petit code source python pour générer une grille de puce RFID pour un couloir de 2 m de large sur 20m de long avec des puces RFID espacées de chacune 10 cm. Ceci devrait donner une résolution et précision d’au moins 10 cm. Par le suite, en développant 2 ou 3 modules de détection, placés chacun à plus de 10 cm de distance l’un de l’autre et fixés sur le robot, nous pourrions avoir une précision au centimètre près par triangulation des 3 mesures. Pour le moment, restons simples.

Le code source en python est le suivant

#!/usr/bin/python
# Generateur de grilles de TAG RF ID
# 1 tag tous les 10 cms.
# Piece parallelepipedique
# Coordonnees X = 0 en bas a gauche, Y=0 en bas a gauche

starttag=5000

def my_range(start, end, step):
    while start <= end:
        yield start
        start += step

for x in my_range(0, 200, 10):
    # detection des bords x
    if x==0 or x==200:
        border=1
    else: border=0

    for y in my_range(0, 2000, 10):
        #Detection des bords y
        if y == 0 or y == 2000:
            border = 1
        else:
            border = 0

        print '%d,%d,%d,%d' % (starttag,x, y,border)
        starttag +=1

Il produit une sortie de base assez simple que nous décrivons ici

Numéro de la puce RFID, position X en cm, position Y cm et un 1 ou un zero pour indiquer si le tag est un TAG de bord ou non.

Ce bout de code Python permettra de générer toutes les positions des tags. Il faudra ensuite demander à l’artisan qui pose le parquet de les positionner exactement sur des lignes croisées. Les numéro des tags devront être parfaitement référencés pour éviter les erreurs. Le code Python peut être redirigé vers un fichier texte qui constituera la base de données. Le fichier text pourra éventuellement être chargé en mémoire dans des @array pour associer rapidement le numéro du TAG à ses coordonnées X,Y.

Développement du serveur de lecture des Tag RFID

Le serveur tourne sous python. Nous avons découpé le module en 1 client qui va être interfacé avec le hardware Raspberry et lecteur de carte RFID 125 Khz. Le code va récupérer la position du tag RFID dans le plan cartésien , calculer la distance et l’angle entre le TAG détecté dans l’espace 2D sur le sol et le TAG de destination.  Le code disponible ici demandera encore un peu de modifications pour définir la destination dynamiquement depuis la mémoire de Pepper ou via un paramétrage via une boite choregraphe (nous n’avons pas de Pepper sous la main pour le moment pour adapter le code – c’est le week end). Enfin, le logiciel serveur devra être adapté pour intégrer la calibration de l’angle du robot par rapport au nord magnétique pour faire coller l’orientation du plan 2D avec le Nord mesuré magnétiquement. Le delta-alpha sera paramétré en dur dans le code ou via les paramètres de la box python chorégraphe.

Nous allons jouer, dans cet exemple de code serveur, avec les formules mathématiques ArcTangeante2 pour calculer l’angle par rapport à l’axe des abscisses. Le nord magnétique ne sera pas encore inclus dans le code, mais il faudra le faire pour bien positionner le robot inDoor.

Le calcul de l’angle est assez simple

 

#!/usr/bin/python

# Serveur de navigation pour Robot Pepper
# Chargement en memoire des tags RFID de positionnement
# Mise en ecoute du hardware lecteur de carte  RFID connecte au RaspBerry PiZero
# A chaque detection de puce RFID, detection de la position inDoor, calcul du vecteurs vers le point de destination
# Si TAG de bordure, mise en route de la procedure pour longer le mur

import socket
import threading
import math
from math import atan2,degrees

# Definition de la position de navigation -  A changer dans le code definitif pour un changement dynamique
destinationx=10
destinationy=3000

class ThreadedServer(object):
    def __init__(self, host, port):
        self.host = host
        self.port = port
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.sock.bind((self.host, self.port))

    def listen(self):
        self.sock.listen(5)
        while True:
            client, address = self.sock.accept()
            client.settimeout(60)
            threading.Thread(target = self.listenToClient,args = (client,address)).start()

    def listenToClient(self, client, address):
        size = 1024
        while True:
            try:
                data = client.recv(size)
                if data:
                    # Set the response to echo back the recieved data
                    # Recuperation de la valeur du TAG
                    tagX,tagY = data.split(",")
                    print 'Lecture Tag RFID --> TagX:'+tagX+' - TagY:'+tagY

                    #response = "I got "+data
                    distance = math.hypot(destinationx - int(tagX), destinationy - int(tagY))

                    # Calcul de l'angle de la droite entre les 2 points
                    p2x=destinationx
                    p2y=destinationy
                    p1x=(int(tagX))
                    p1y=(int(tagY))
                    xDiff = p2x - p1x
                    yDiff = p2y - p1y
                    zeangle=degrees(atan2(yDiff, xDiff))

                    # Affichage de la distance et de l'angle dans le serveur
                    print 'Distance entre les 2 points:' + str(distance)
                    print 'Angle de trajectoire:'+str(zeangle)+' Deg'
                    # Renvoi des donnees pour exploitation dans une Box Chregraphe
                    response=str(distance)+':'+str(zeangle)
                    client.send(response)
                else:
                    raise error('Client disconnected')
            except:
                client.close()
                return False

if __name__ == "__main__":
    port_num = 1112
    print"Listening..."
    print "Target destination x:%d y:%d" % (destinationx,destinationy)
    ThreadedServer('',port_num).listen()

Le code source du Serveur peut donc récupérer sans problème les DATA et réagir en donnant une distance et un cap. Récupéré dans la box python choregraphe, le cap et l’angle peuvent être transmis aux boites de déplacement motion.

En attendant la fin de l’article qui développera la création du vrai client raspberry/lecteur RFID, vous trouverez ci-dessous le code d’un client simple qui permet de jouer avec le serveur.

 

#!/usr/bin/python

import socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
host = "127.0.0.1"
port = 1112
sock.connect((host,port))
data = "10,10" # Simulation de la lecture d'un TAG RFID X,y
sock.send(data)
print "response: ", sock.recv(1024)

Développement de la box python pour exploiter les données du serveur de navigation

Le projet est basé sur le lecteur RHYDOLABZ, carte lecteur de Tag 125 Khz. Elle sera connectée au Raspberry sur lequel tourne le serveur de géolocalisation et de calcul de cap. Le câblage est d’une simplicité déconcertante et est expliqué sur la photo suivante.

 

Le montage sur RaspberryPI Zero est le suivant. Nous avons fait une petite variante pour le GND car le GND suggéré par le fabricant était déjà monopolisé par le compas électronique. Nous vous laissons repérer les couleurs des fils 5v, GND et TX/RX et la numérotation des connecteurs du GPIO raspberry.


 

Le code source de base pour lire les TAG qui se présentent à la volée sous le capteur est également d’une simplicité à tomber par terre. Il est recopié de la page initiale du fabricant lisible ici.

 

import serial
import os, time

#Enable Serial Communication
port = serial.Serial("/dev/ttyAMA0", baudrate=9600, timeout=0.01)

# Find a character in a string
def find(str, ch):
    for i, ltr in enumerate(str):
        if ltr == ch:
            yield i

fd=''
while True:
    # Read the port
    rcv = port.read(10)
    if len(rcv) > 1:
        fd=fd+rcv
        ps=fd.find('\r')
        if ps >= 0:
            print fd[0:ps]
            fd=''

Le tout est en Python, ce qui va largement simplifier le codage global du client lecteur de tag. La stratégie de programmation et d’envoyer chaque valeur de TAG lue via socket sur le port du programme serveur.  Le programme client est bouclé pour rester en écoute permanente.

 

Encodage de puces autocollantes RFID 125 Khz

L’encodage des puces RFID ne pose pas de problème spécifique. Elles sont en générale livrées encodées avec un identifiant numérique unique. Pour des raisons de simplicité, il est peut être préférable de les encoder de façon rationnelle ou, une fois posées, de venir les encoder de façon incrémentale pour faciliter la programmation ou la lecture/débuggage. (Exemple: encoder les unités et dizaines en fonction de la position X de chaque tag, et encoder les centaines et milliers en fonction de la position Y du tag, les dizaines de milliers en fonction de la pièce ou de l’étage…  Exemple:  TAG 00523035 , pièce 5, Y=5230 cm, X=350 cm) . Cet encodage est possible avec des petits outils USB windows ou des outils alibaba  de type RFID ENCODER a 80 eur.

 

 

Partie 3 – Conception mécanique et armature

 

Publié dans Programmation, Python, Toutes les rubriques articles, Tutoriels Externes Français, Tutoriels texte