Un premier exemple

EDIT 03/01/2015 : J’ai rédigé cet article alors que je faisais mes premiers pas avec ROS. Par conséquent, même si ce qui y est décrit ainsi que le code fourni fonctionnent correctement, tout n’est pas garanti fait dans les règles de l’art, ou au moins, dans les règles de bon usage de ROS.

Ayant depuis continué à étudier documentations, codes sources et contributions externes, et ayant donc mieux assimilé un certain nombre de points, une nouvelle version de ce qui est décrit ici a vu le jour. Tout cela est documenté dans cet article (en cours de rédaction) qui remplace celui-ci et que je vous invite à lire en priorité. La présente version est cependant conservée en ligne à titre de documentation.

A titre de test de bon fonctionnement de l’installation documentée dans un précédent article, j’ai implémenté un node pour interfacer une carte IOPi de ABElectronics.

Les fonctionnalités recherchées sont :

• services
  • lecture des I/O individuelles
  • modification des I/O individuelles
  • lecture globale des deux ports d’un expandeur
• signalisation
  • émission d’un message sur changement d’une entrée

Cela permettra de faire un tour assez complet du propriétaire puisqu’on tâtera du service et du message.

Quelques précisions sur le contexte technique

L’IOPi comporte deux expandeurs I2C accessibles chacun à son adresse. Chaque expandeur comporte deux ports de 8 bits chacun.

En date de rédaction [1], le module Python mis à disposition par AB Electronics fournit une classe qui modélise un expandeur (et non la carte complète, comme le nom aurait pu le laisser penser). Notre node sera calqué sur cette organisation. Deux nodes seront donc nécessaires pour interfacer la carte complète.

Création du package

Je suppose que vous avez déjà initialisé une workspace catkin, comme détaillé dans la documentation ROS, et nous partirons de cette situation.

Commençons par créer un package (abelec_iopi) à l’aide de la commande catkin_create_package. Avant d’aller plus loin, pensons à exécuter la commande source devel/setup.bash afin que les diverses variables d’environnement soient mises à jour pour tenir compte du nouveau venu.

Définitions des messages et services

Ceci étant fait, allons dans le sous-répertoire qui a été créé et ajoutons les sous-répertoires srv et msg, dans lesquels nous allons placer la spécifications des services et messages mentionnés précédemment.

Commençons par le service de lecture d’une entrée, que nous nommerons GetInput. Nous créons donc le fichier GetInput.srv dans le sous-répertoire srv, avec le contenu suivant :

uint16 io_num
---
bool state

Qu’est-ce que ça dit ?

• que ce service prend comme paramètre un message contenant comme unique champ un entier non signé nommé io_num (le numéro de l’I/O qu’on veut lire)
• qu’il retourne un message comportant comme unique champ un booléen nommé state (l’état de l’entrée lue)

Petits rappels :

• un service ROS renvoie toujours une réponse
• les paramètres (requête) et les résultats (réponse) des services sont packagés sous forme de messages, pouvant contenir un ou plusieurs champs, mais pouvant être vides également
• dans le formalisme des fichiers .srv, les deux messages sont séparés par une ligne composée de 3 tirets
  En résumé, un service consomme un message et en revoie un autre de manière synchrone.

Une fois ce fichier enregistré, on se rend dans la directory racine de la workspace et on entre la commande catkin_make. Son rôle est de générer les divers fichiers qui vont implémenter les classes proxies des messages et services définis, et ce pour les langages supportés par l’installation ROS (par défaut : C++, Python et Lisp) [2]. C’est grâce à cela que malgré le fait qu’on communique entre process distincts, et entre langages éventuellement différents, les interfaces restent typées et contrôlées au runtime.

Cette commande sera à exécuter comme décrit à chaque fois qu’une modification sera apportée aux interfaces externes du package. Pas besoin de le faire tant que les modifications ne portent que sur le code, sauf pour effectuer la compilation de pakages implémentés en C++.

On procède de même pour les autres interfaces :

GetIOs.srv

---
bool[16] state

Ici nous n’avons aucun paramètre en entrée, et donc un message vide défini au-dessus de la ligne séparatrice, et un unique champ dans le message de sortie, contenant un tableau de 16 booléens fournissant l’état des 16 I/Os de l’expandeur.

SetOutput.srv

uint16 io_num
bool state
---
bool success
string errmsg

Nous avons ici un exemple de message de retour à champs multiples.

Le message asynchrone de signalisation du changement d’état d’une des entrées utilise la même syntaxe, et se définit dans un fichier dont l’extension est .msg placé dans le sous-répertoire msg créé précédemment. Son contenu est le suivant :

InputChange.msg

uint16 io_num
bool state

Ici, pas de ligne de démarcation, puisque par définition nous n’avons qu’un seul message en jeu (et non un couple requête/réponse comme dans le cas des services).

Un petit passage par la case catkin_make une fois ces fichiers sauvegardés, et nous sommes prêts pour l’écriture du code qui va utiliser ces définitions.

Implémentation du serveur du node

Il s’agit du code qui va tourner en permanence lorsque le noeud sera lancé (par exemple avec la commande rosrun). Il a pour charge de gérer les requêtes de service, et de produire les notifications.

Nous allons l’implémenter dans la classe IOPiNode, dont la méthode run ci-après contient le coeur actif :

def run(self):
    rospy.init_node(name=self._name,
                    log_level=rospy.DEBUG if self._verbose else rospy.INFO)
    rospy.loginfo('starting node (board address : 0x%x)' % self._i2c_address)
    if self._verbose:
        rospy.logdebug("outputs: %s", self._outputs)
 
    prefix = "/gpio/"
    rospy.Service(prefix + 'get_input', GetInput, handler=self.handle_get_input)
    rospy.Service(prefix + 'get_ios', GetIOs, handler=self.handle_get_ios)
    rospy.Service(prefix + 'set_output', SetOutput, handler=self.handle_set_output)
 
    pub = rospy.Publisher(prefix + 'input_change', InputChange, queue_size=10)
 
    # initializes current ports state with a first read
    self._ports_states = self._get_inputs_states()
 
    # set the loop frequency to 10 Hz
    r = rospy.Rate(10)
 
    # monitor inputs changes and publish associated messages
    while not rospy.is_shutdown():
        # get current input states
        new_states = self._get_inputs_states()
        # is there some change ?
        if new_states != self._ports_states:
            if self._verbose:
                rospy.logdebug("inputs changed to 0x%x", new_states)
            # detect which IO(s) have changed their state
            change_mask = new_states ^ self._ports_states
            # publish a message for each of them
            for io_num_0 in [i for i in xrange(16) if change_mask & (1 << i)]:
                new = (new_states >> io_num_0) & 0x01
                msg = InputChange(io_num_0 + 1, new)
                pub.publish(msg)
            # remember new states
            self._ports_states = new_states
 
        # wait until it's time for next iteration
        r.sleep()
 
    rospy.loginfo('shutting down node')

On commence par créer le node via l’appel à rospy.init_node. Puis on publie les services, en créant les instances de rospy.Service correspondantes. Les paramètres d’initialisation sont :

• le nom sous lequel le service sera publié (ex : /gpio/get_input) et par lequel les autres nodes pourront y accéder
• la classe qui modélise son interface (ex : GetInput), précédemment générée par catkin_make à partir du fichier .srv correspondant
• le callback appelé pour traiter les requêtes. Ce callback reçoit comme paramètre une instance de la classe <service_name>Request, et doit retourner une instance de la classe <service_name>Response, ces deux classes ayant été générées par catkin_make à partir du fichier .srv également

Nous créons ensuite un Publisher pour émettre nos messages de notification. Ses paramètres principaux d’initialisation sont :

• le topic dans lequel les messages vont être émis, et auquel d’autres nodes pourront s’abonner
• la classe qui modélise les messages émis

Tout le reste est notre code métier, consistant à boucler jusqu’au signal de shutdown du node, en respectant une fréquence de récurrence de 10Hz (rospy.Rate(10)), la méthode Rate.sleep() se chargeant d’attendre le temps nécessaire à ce que l’ensemble de l’itération dure le temps demandé. Dans cette boucle nous lisons les états des entrées via la librairie support de la carte IOPi, et émettons un message InputChange si détection d’un changement.

Comme vous pouvez le constater, rien de très compliqué.

Comment ça se passe au niveau des handlers de services ? Illustrons-le par celui du service get_input.

    def handle_get_input(self, req):
        io_num = req.io_num
        self._check_io_num(io_num)

        if self._verbose:
            self.logdebug("returning state of input %d", io_num)

        io_state = self._iopi.readPin(io_num)
        self._io_states[io_num] = io_state
        return GetInputResponse(io_state)

La seule chose spécifique à ROS est la création du message retour en fin de méthode, utilisant l’une des classes générées par catkin_make et qui ont été mentionnées précédemment.

Et au fait : comment les classes proxies de nos messages et services sont-elles connues ?

Par les imports suivants :

from abelec_iopi.msg import InputChange
from abelec_iopi.srv import *

Les modules et packages référencés sont ceux générés par catkin_make (on reconnait au passage les sous-packages srv et msg de abelec_iopi, et vont se stocker dans le sous répertoire devel de la workspace, où se trouve entre autres l’équivalent d’un virtualenv Python. contenant nos packages et modules générés. Grâce au fait de sourcer devel/setup.bash, cet environnement vient s’ajouter au PYTHONPATH, ce qui permet d’en disposer lorsque nous allons exécuter tout cela.

Ca paraît compliqué la première fois qu’on se trouve face à tout cela, mais c’est très logique et bien structuré, ce qui fait qu’on prend rapidement ses marques.

Un code de test maintenant

Pour vérifier le bon fonctionnement de tout cela, on va développer un petit script qui créera un node dont le rôle sera :

• d’allumer une LED au lancement pour dire qu’il se réveille
• de se mettre à l’écoute des messages du topic /gpio/input_change et de les afficher à réception
• d’éteindre la LED lors du shutdown

Tout cela se résume à ce qui suit :

#!/usr/bin/env python
# -*- coding: utf-8 -*-

__author__ = 'Eric Pascual'

import rospy

from abelec_iopi.msg import InputChange
from abelec_iopi.srv import SetOutput


class Demo(object):
    LED = 8
    sub = None
    set_output = None

    def shutdown_hook(self):
        print('\n\nCleaning up before shutdown...')
        self.switch_led(False)

    def switch_led(self, state):
        print('turning LED %s...' % ('on' if state else 'off'))
        _resp = self.set_output(self.LED, 1 if state else 0)
        print(_resp)
        print('-'*10)

    @staticmethod
    def callback(msg):
        print("Input %d changed to %s" % (msg.io_num, msg.state))

    def run(self):
        print("""
abelec_iopi/iopi_ROS node demonstration
---------------------------------------

This node listens to input_change topic messages and print them when received.
Play with the IOPi board inputs and watch the program trace.

It also demonstrates how to use the GPIO control service published by the iopi node.
""")
        # initializes our node
        rospy.init_node('input_change_listener', anonymous=True, log_level=rospy.INFO)
        # get access to the outputs control service provided by the iopi node
        self.set_output = rospy.ServiceProxy('/gpio/set_output', SetOutput)

        self.switch_led(True)

        # subscribe to input_change messages emitted by the iopi node to reflect input
        # state changes
        self.sub = rospy.Subscriber('/gpio/input_change', InputChange, self.callback)

        # register the cleanup process. This is required if this process still needs ROS
        # related stuff, since once spin() is exited, the context is no more valid
        rospy.on_shutdown(self.shutdown_hook)

        # wait for messages until our node is shut down
        print('Listening to messages... [Ctrl-C to terminate]\n')
        rospy.spin()

        # need for any comment here ?
        print("\nThat's all folks !!")


if __name__ == '__main__':
    demo = Demo()
    demo.run()

Pas la peine d’ajouter des tonnes de commentaires : ceux du code suffisent amplement à décrire le fonctionnement.

Je vous avais bien dit que ce n’était pas si compliqué que cela.