TP 2 — Recevoir des ordres

Objectifs

s’abonner à un topic, et réagir aux messages reçus avec un exécuteur (executor) ;
publier une mesure périodique et horodatée, avec un timer ROS ;
survivre à une coupure de l’agent : détecter la perte de liaison, puis recréer le nœud.

Durée indicative : deux séances de 3 h. Prérequis : TP 1 — Premier nœud micro-ROS, dont on reprend le projet, et TP 7 — Le bus I²C : température, accéléromètre et gyroscope.

Ce qu’il faut savoir

L’exécuteur

Pour publier, il suffit d’appeler rcl_publish. Pour recevoir, il faut que quelqu’un regarde régulièrement si des messages sont arrivés, et appelle la bonne fonction : c’est le rôle de l”exécuteur. On lui confie des abonnements, des timers, des services, chacun avec sa fonction de rappel (callback) ; puis on l’appelle en boucle :

rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));

Cet appel attend au plus 10 ms qu’il se passe quelque chose, exécute les rappels concernés, et rend la main. Les rappels s’exécutent donc dans la tâche qui appelle l’exécuteur, jamais dans une interruption.

À sa création, l’exécuteur reçoit le nombre d’éléments qu’il gérera : un de plus que prévu, et rclc_executor_add_* échoue.

Les timers ROS

Un timer ROS appelle une fonction à intervalle régulier, depuis l’exécuteur. C’est la façon habituelle, dans ROS, de publier une mesure périodique.

L’heure

Un message de capteur porte un horodatage (header.stamp), pour qu’on sache quand la mesure a été faite, et la comparer aux autres. La carte n’a pas l’heure du PC : rmw_uros_sync_session la demande à l’agent, puis rmw_uros_epoch_nanos() donne l’heure courante, en nanosecondes, sur l’horloge du PC.

La liaison peut tomber

Si l’agent s’arrête, la session de la carte est perdue : même quand il revient, il ne connaît plus ses topics. Le programme doit le détecter, en interrogeant l’agent régulièrement (rmw_uros_ping_agent), et tout recréer. Le firmware du robot le fait, et se relance de lui-même quand le Raspberry Pi redémarre.

Étape 1 — Commander les LED

Ajoutez au nœud un abonné au topic leds, de type std_msgs/msg/UInt8 : le bit 0 commande LED_1, …, le bit 3 LED_4. (Pour ne pas mélanger, LED_1 ne sert plus à montrer l’attente de l’agent : utilisez LED_4.)

Créez un exécuteur à un élément, ajoutez-y l’abonnement avec sa fonction de rappel, et appelez rclc_executor_spin_some dans la boucle de la tâche :

static void leds_cb(const void *msgin)
{
    const std_msgs__msg__UInt8 *m = (const std_msgs__msg__UInt8 *)msgin;
    HAL_GPIO_WritePin(LED_1_GPIO_Port, LED_1_Pin, (m->data & 1u) ? GPIO_PIN_SET : GPIO_PIN_RESET);
    /* ... LED_2 a LED_4 ... */
}

rclc_executor_init(&executor, &support.context, 1, &allocator);
rclc_executor_add_subscription(&executor, &sub_leds, &msg_leds,
                               &leds_cb, ON_NEW_DATA);

Depuis le PC :

ros2 topic pub --once /tp/leds std_msgs/msg/UInt8 "{data: 5}"

Questions

Quelles LED s’allument avec data: 5 ? avec data: 10 ?
Retirez l’appel à rclc_executor_spin_some. Les messages arrivent-ils encore jusqu’à la carte ? Les LED changent-elles ?
Publiez sans --once, à 50 Hz (-r 50). La carte suit-elle ? Mesurez, et comparez à la cadence de la boucle de la tâche.

Étape 2 — Une mesure horodatée

Ajoutez I2C2 (PB10/PB11) au projet, et la lecture du TMP102 du TP 7 de la partie 1.
Après la création du nœud, synchronisez l’heure : rmw_uros_sync_session(1000).

Créez un éditeur temperature, de type sensor_msgs/msg/Temperature, et un timer ROS d’une seconde. Dans son rappel : lisez la température, remplissez le message, publiez-le.

int64_t ns = rmw_uros_epoch_nanos();
msg.header.stamp.sec     = (int32_t)(ns / 1000000000);
msg.header.stamp.nanosec = (uint32_t)(ns % 1000000000);
msg.temperature          = t_mC / 1000.0;
msg.variance             = 0.0;

Le champ header.frame_id est une chaîne : micro-ROS n’alloue rien, c’est à vous de fournir la mémoire.

static char frame[] = "ratbot_lab_board";
msg.header.frame_id.data     = frame;
msg.header.frame_id.size     = strlen(frame);
msg.header.frame_id.capacity = sizeof frame;

Le timer est un élément de plus pour l’exécuteur : deux éléments.

Côté PC : ros2 topic echo /tp/temperature, et comparez header.stamp avec date +%s.

Questions

L’horodatage est-il cohérent avec l’heure du PC ? Que vaudrait-il sans rmw_uros_sync_session ?
Le rappel du timer lit le TMP102 en I²C. Combien de temps bloque-t-il l’exécuteur ? Quelle conséquence pour les messages leds ?

Étape 3 — Survivre à une coupure

Organisez la tâche comme une machine à états :

Figure made with TikZ

La tâche micro-ROS comme machine à états : elle survit à la perte de l’agent.

ATTENTE_AGENT : ping toutes les 500 ms, LED_4 clignote ;
CREATION : support, nœud, éditeurs, abonné, timer, exécuteur ; en cas d’échec, tout détruire et revenir à l’attente ;
CONNECTE : l’exécuteur tourne ; une fois par seconde, un ping court (rmw_uros_ping_agent(100, 1)) vérifie que l’agent répond toujours ;
DESTRUCTION : détruire chaque entité (rcl_publisher_fini, rcl_subscription_fini, rcl_timer_fini, rclc_executor_fini, rcl_node_fini, rclc_support_fini), puis revenir à l’attente.

Avant de détruire, réglez à zéro le délai d’attente de l’agent, sans quoi chaque destruction attend une réponse qui ne viendra pas :

rmw_context_t *ctx = rcl_context_get_rmw_context(&support.context);
(void)rmw_uros_set_context_entity_destroy_session_timeout(ctx, 0);

Arrêtez l’agent, attendez, relancez-le : la carte doit revenir seule.

Questions

Combien de temps faut-il à la carte pour revenir après le retour de l’agent ? Qu’est-ce qui fixe ce délai ?
Pourquoi tout détruire, plutôt que de recréer seulement ce qui manque ?
Pendant la coupure, que doivent faire les LED commandées par /tp/leds ? Que fait votre programme ? (Le robot, lui, arrête ses moteurs une demi-seconde après la dernière commande reçue.)

Solution de l’étape 3

La machine à états de la tâche :

typedef enum { WAITING_AGENT, CREATING, CONNECTED, DESTROYING } link_state_t;

link_state_t state = WAITING_AGENT;
uint32_t     last_ping = 0u;

for (;;)
{
    switch (state)
    {
    case WAITING_AGENT:
        HAL_GPIO_TogglePin(LED_4_GPIO_Port, LED_4_Pin);
        if (rmw_uros_ping_agent(100, 1) == RMW_RET_OK) { state = CREATING; }
        else                                            { osDelay(500); }
        break;

    case CREATING:
        if (create_entities())                    /* tout ce qu'on créait avant */
        {
            HAL_GPIO_WritePin(LED_4_GPIO_Port, LED_4_Pin, GPIO_PIN_SET);
            last_ping = osKernelGetTickCount();
            state = CONNECTED;
        }
        else { state = DESTROYING; }              /* échec : tout défaire */
        break;

    case CONNECTED:
        (void)rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
        if (osKernelGetTickCount() - last_ping >= 1000u)
        {
            last_ping = osKernelGetTickCount();
            if (rmw_uros_ping_agent(100, 1) != RMW_RET_OK) { state = DESTROYING; }
        }
        break;

    case DESTROYING:
    default:
        destroy_entities();
        HAL_GPIO_WritePin(LED_4_GPIO_Port, LED_4_Pin, GPIO_PIN_RESET);
        state = WAITING_AGENT;
        break;
    }
}

et la destruction :

#define IGNORE_RET(call)  do { rcl_ret_t ignored_ = (call); (void)ignored_; } while (0)

static void destroy_entities(void)
{
    rmw_context_t *ctx = rcl_context_get_rmw_context(&support.context);

    if (ctx != NULL)        /* NULL si la création a échoué avant le support */
    {
        (void)rmw_uros_set_context_entity_destroy_session_timeout(ctx, 0);
    }
    IGNORE_RET(rcl_publisher_fini(&pub_temp, &node));
    IGNORE_RET(rcl_subscription_fini(&sub_leds, &node));
    IGNORE_RET(rcl_timer_fini(&timer_temp));
    IGNORE_RET(rclc_executor_fini(&executor));
    IGNORE_RET(rcl_node_fini(&node));
    IGNORE_RET(rclc_support_fini(&support));
}

Pourquoi une machine à états. La liaison a quatre situations bien distinctes, chacune avec son comportement : attendre, construire, travailler, démolir. Les écrire comme des états, avec des transitions explicites, rend le comportement lisible et complet : on voit tout de suite ce qui se passe en cas d’échec à chaque étape. C’est la structure du firmware du robot, qui se reconnecte seul quand le Raspberry Pi redémarre.

Le délai de destruction à zéro. Normalement, détruire une entité envoie une demande à l’agent et attend sa confirmation. L’agent étant parti, chaque destruction attendrait son délai complet : plusieurs secondes perdues. Le délai à zéro dit : « détruis localement, sans attendre de réponse ».

IGNORE_RET. Les fonctions de rcl sont marquées « ne pas ignorer le résultat » (warn_unused_result), et GCC refuse le simple (void). Ici, les erreurs sont attendues (la session est morte) : la macro rend l’intention explicite.

Exercices supplémentaires

Ces exercices n’ont pas de correction.

La couleur : un abonné rgb de type std_msgs/msg/ColorRGBA commande le voyant RGB (TP 2 de la partie 1), composantes entre 0 et 1.
La centrale : publiez sensor_msgs/msg/Imu à 50 Hz (gyroscope et accéléromètre, TP 10 de la partie 2), en best effort (rclc_publisher_init_best_effort), et affichez-la dans RViz.
Le chien de garde de commande : si aucun message leds n’est arrivé depuis une seconde, éteignez toutes les LED.

Après le TP

La flash externe contient votre programme : réinstallez celui du robot, voir Remettre la carte en état RatBot.