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 : :doc:`tp01_premier_noeud`, dont on reprend le projet, et :doc:`../sans_os/tp07_i2c`. 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 : .. code-block:: c 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 : .. code-block:: c 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 : .. code-block:: bash ros2 topic pub --once /tp/leds std_msgs/msg/UInt8 "{data: 5}" .. Capture d'écran à insérer : le terminal avec ros2 topic pub, et une photo des LED allumées pour data: 5. Déposez _static/captures/ros/tp02-topic-pub.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp02-topic-pub.png .. :align: center .. :width: 90% .. admonition:: Questions 1. Quelles LED s'allument avec ``data: 5`` ? avec ``data: 10`` ? 2. Retirez l'appel à ``rclc_executor_spin_some``. Les messages arrivent-ils encore jusqu'à la carte ? Les LED changent-elles ? 3. Publiez sans ``--once``, à 50 Hz (``-r 50``). La carte suit-elle ? Mesurez, et comparez à la cadence de la boucle de la tâche. .. dropdown:: Solution de l'étape 1 :icon: code :color: secondary .. code-block:: c #include static rcl_subscription_t sub_leds; static std_msgs__msg__UInt8 msg_leds; /* rempli par l'exécuteur */ static rclc_executor_t executor; static void set_led(GPIO_TypeDef *port, uint16_t pin, uint32_t on) { HAL_GPIO_WritePin(port, pin, on ? GPIO_PIN_SET : GPIO_PIN_RESET); } static void leds_cb(const void *msgin) { const std_msgs__msg__UInt8 *m = (const std_msgs__msg__UInt8 *)msgin; set_led(LED_1_GPIO_Port, LED_1_Pin, m->data & 0x1u); set_led(LED_2_GPIO_Port, LED_2_Pin, m->data & 0x2u); set_led(LED_3_GPIO_Port, LED_3_Pin, m->data & 0x4u); /* LED_4 sert d'indicateur de liaison dans ce TP */ } après la création du nœud : .. code-block:: c rclc_subscription_init_default(&sub_leds, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt8), "leds"); executor = rclc_executor_get_zero_initialized_executor(); rclc_executor_init(&executor, &support.context, 1, &allocator); rclc_executor_add_subscription(&executor, &sub_leds, &msg_leds, &leds_cb, ON_NEW_DATA); et dans la boucle de la tâche : .. code-block:: c rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)); **Le message de réception est static.** L'exécuteur y copie chaque message reçu, puis appelle le rappel avec son adresse : le message doit exister aussi longtemps que l'abonnement. Une variable locale d'une fonction déjà terminée serait une erreur grave. **ON_NEW_DATA** : le rappel n'est appelé que lorsqu'un **nouveau** message est arrivé (l'autre mode, ``ALWAYS``, l'appelle à chaque tour, avec ou sans message). **Le rappel s'exécute dans la tâche**, celle qui appelle ``rclc_executor_spin_some`` : pas dans une interruption. On peut donc y appeler des fonctions de la HAL qui attendent, mais tout ce temps retarde les autres rappels. .. dropdown:: Réponses aux questions 1 à 3 :icon: light-bulb :color: info **1. Les LED.** ``data: 5`` = 0b0101 : bits 0 et 2, donc **LED_1 et LED_3**. ``data: 10`` = 0b1010 : bits 1 et 3, donc **LED_2**, et LED_4 si on l'applique (dans la solution, LED_4 est réservée à l'état de la liaison). Un octet porte ainsi l'état de huit sorties : c'est un **masque de bits**. **2. Sans spin_some.** Les octets **arrivent** quand même jusqu'à la carte : l'agent les envoie, le DMA les range dans le tampon du transport. Mais personne ne les **traite** : c'est ``rclc_executor_spin_some`` qui lit le transport, décode les messages et appelle les rappels. Sans lui, rien ne se passe, et le tampon finit par déborder. Un nœud micro-ROS qui reçoit doit faire tourner son exécuteur **régulièrement**. **3. À 50 Hz.** La carte suit sans difficulté : l'exécuteur tourne à chaque tour de boucle (au plus 10 ms d'attente), soit plus de 100 fois par seconde. On peut le vérifier en publiant un compteur dont on regarde les bits, ou en mesurant la cadence d'un écho. Les limites apparaissent plus haut : la liaison à 115 200 bit/s (chaque message fait quelques dizaines d'octets avec l'encapsulation XRCE, soit 3 à 5 ms), et l'**historique** du client (``RMW_UXRCE_MAX_HISTORY``, 4 messages par défaut). Au-delà, les messages s'accumulent, arrivent en retard, puis certains sont **perdus**, sans que l'émetteur en soit prévenu. É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. .. code-block:: c 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. .. code-block:: c 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``. .. admonition:: Questions 4. L'horodatage est-il cohérent avec l'heure du PC ? Que vaudrait-il sans ``rmw_uros_sync_session`` ? 5. 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`` ? .. dropdown:: Solution de l'étape 2 :icon: code :color: secondary .. code-block:: c #include #include static rcl_publisher_t pub_temp; static rcl_timer_t timer_temp; static sensor_msgs__msg__Temperature msg_temp; static char frame[] = "ratbot_lab_board"; static void temp_cb(rcl_timer_t *timer, int64_t last_call_time) { int32_t t_mC; int64_t ns; (void)last_call_time; if ((timer == NULL) || !tmp102_temp_mC(&t_mC)) { return; } ns = rmw_uros_epoch_nanos(); /* heure de l'agent */ msg_temp.header.stamp.sec = (int32_t)(ns / 1000000000); msg_temp.header.stamp.nanosec = (uint32_t)(ns % 1000000000); msg_temp.temperature = (double)t_mC / 1000.0; msg_temp.variance = 0.0; /* 0 : inconnue */ (void)rcl_publish(&pub_temp, &msg_temp, NULL); } après la création du nœud : .. code-block:: c (void)rmw_uros_sync_session(1000); /* demande l'heure */ rclc_publisher_init_default(&pub_temp, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Temperature), "temperature"); rclc_timer_init_default(&timer_temp, &support, RCL_MS_TO_NS(1000), temp_cb); rclc_executor_init(&executor, &support.context, 2, &allocator); /* 2 éléments */ rclc_executor_add_subscription(/* … comme à l'étape 1 … */); rclc_executor_add_timer(&executor, &timer_temp); et une fois, avant la boucle : .. code-block:: c msg_temp.header.frame_id.data = frame; msg_temp.header.frame_id.size = strlen(frame); msg_temp.header.frame_id.capacity = sizeof frame; **Les chaînes de micro-ROS.** Un champ ``string`` d'un message est une structure à trois membres : un pointeur vers les caractères, la longueur utile, et la **capacité** de la mémoire pointée. micro-ROS ne fait aucune allocation pour les messages : c'est au programme de fournir la mémoire, ici un tableau ``static``. Pour un message **reçu** contenant une chaîne, il faut de même préparer un tampon assez grand **avant** de le recevoir. **Le timer et l'exécuteur.** Le timer ROS n'a pas d'interruption à lui : à chaque ``spin_some``, l'exécuteur regarde si son échéance est passée, et appelle le rappel. Sa précision dépend donc de la cadence de la boucle (ici 10 ms au pire). **frame_id** nomme le **repère** de la mesure : pour une température, peu importe ; pour un capteur de distance ou une centrale, c'est ce qui permet à ROS de savoir où et dans quel sens il est monté sur le robot. .. dropdown:: Réponses aux questions 4 et 5 :icon: light-bulb :color: info **4. L'horodatage.** Il est cohérent avec ``date +%s`` à quelques millisecondes près. ``rmw_uros_sync_session`` échange des messages avec l'agent, mesure le temps aller-retour, et calcule l'**écart** entre l'horloge de la carte et celle du PC (comme le protocole NTP, en plus simple). ``rmw_uros_epoch_nanos`` renvoie ensuite l'heure locale corrigée de cet écart. Sans synchronisation, ``rmw_uros_epoch_nanos`` renvoie **0**, ou le temps depuis le démarrage de la carte : des dates en **1970**. Les messages sont alors **incomparables** avec ceux des autres machines : un filtre qui fusionne cette mesure avec d'autres la rejetterait, ou pire, l'utiliserait au mauvais moment. Le firmware du robot resynchronise régulièrement, car les deux horloges dérivent l'une par rapport à l'autre. **5. Le rappel bloque l'exécuteur.** Environ **0,4 ms** à 100 kHz pour lire le TMP102 : l'exécuteur ne fait rien d'autre pendant ce temps, et un message ``leds`` arrivé entre-temps attend. Ici, c'est négligeable. Mais un rappel lent (un capteur lent, un ``HAL_Delay``, un calcul) retarde **tous** les autres : les rappels d'un exécuteur s'exécutent **l'un après l'autre**. Pour un capteur lent, on fait la lecture dans une **autre tâche** FreeRTOS, et le rappel ne fait que publier la dernière valeur. Étape 3 — Survivre à une coupure -------------------------------- Organisez la tâche comme une **machine à états** : .. tikz:: La tâche micro-ROS comme machine à états : elle survit à la perte de l'agent. \tikzset{st/.style={draw, rounded corners, fill=blue!6, font=\small, minimum width=30mm, minimum height=9mm, align=center}, tr/.style={-{Stealth}, thick}, lb/.style={font=\scriptsize, fill=white, inner sep=1pt}} \node[st] (w) at (0,0) {ATTENTE\_AGENT\\[-2pt]\scriptsize ping / 500 ms, LED\_4 clignote}; \node[st] (c) at (6,0) {CRÉATION\\[-2pt]\scriptsize support, nœud, entités}; \node[st, fill=green!10] (k) at (6,-2.6) {CONNECTÉ\\[-2pt]\scriptsize exécuteur, ping / 1 s}; \node[st, fill=red!8] (d) at (0,-2.6) {DESTRUCTION\\[-2pt]\scriptsize tout détruire}; \draw[tr] (w) -- node[lb, above]{ping OK} (c); \draw[tr] (c) -- node[lb, right]{OK} (k); \draw[tr] (k) -- node[lb, below]{ping KO} (d); \draw[tr] (d) -- (w); \draw[tr] (c.south west) ++(0.3,0) -- node[lb, sloped]{échec} (d.north east); \draw[tr] (w.north west) ++(0.5,0) to[out=120, in=60, looseness=4] node[lb, above]{ping KO} ++(1.2,0); - **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 : .. code-block:: c 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. .. Capture d'écran à insérer : le terminal de l'agent qui redémarre, et la carte qui recrée sa session. Déposez _static/captures/ros/tp02-reconnexion.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp02-reconnexion.png .. :align: center .. :width: 90% .. admonition:: Questions 6. Combien de temps faut-il à la carte pour revenir après le retour de l'agent ? Qu'est-ce qui fixe ce délai ? 7. Pourquoi tout détruire, plutôt que de recréer seulement ce qui manque ? 8. 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.) .. dropdown:: Solution de l'étape 3 :icon: code :color: secondary La machine à états de la tâche : .. code-block:: c 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 : .. code-block:: c #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. .. dropdown:: Réponses aux questions 6 à 8 :icon: light-bulb :color: info **6. Le délai de retour.** Il se compose de l'attente du prochain ping (jusqu'à 0,5 s), puis de la **création des entités** : chacune demande un échange avec l'agent, qui crée l'entité DDS correspondante. À 115 200 bit/s, avec un nœud, deux éditeurs, un abonné, compter **une à deux secondes**. Le délai dépend de la cadence du ping d'attente et du nombre d'entités. La détection de la **perte**, elle, prend jusqu'à 1 s (la cadence du ping de surveillance). **7. Tout détruire.** Un agent relancé a **perdu toute la session** : il ne connaît plus aucune entité de la carte. Les structures locales (nœud, éditeurs) font toutes référence à cette session morte, et aucune n'est réutilisable. Recréer « seulement ce qui manque » supposerait de savoir ce que l'agent connaît encore : rien. Tout détruire et tout recréer est le seul état **sûr**, et c'est aussi le plus simple : une seule fonction de création, une seule de destruction. **8. Les LED pendant la coupure.** En bonne logique, elles devraient revenir à un **état sûr** (éteintes) : une commande **périmée** ne doit pas rester appliquée indéfiniment. Ce programme les laisse en l'état : c'est l'objet du troisième exercice. Pour une LED, c'est sans conséquence ; pour un moteur, c'est crucial : un robot dont la liaison tombe pendant qu'il avance doit s'**arrêter**, pas continuer tout droit. C'est pourquoi la carte du robot coupe ses moteurs une demi-seconde après la dernière commande reçue, quelle qu'en soit la raison. .. dropdown:: Tout le code de la correction :icon: download :color: secondary :download:`taches.c <../../corrections/ros/tp02/taches.c>` — compilé sans avertissement contre les en-têtes de micro-ROS Humble, la HAL et FreeRTOS du firmware RatBot (sans édition de liens ni essai sur carte). Le fichier contient les inclusions, les déclarations et la fonction de la tâche micro-ROS, qui remplace le squelette ``StartDefaultTask`` généré par CubeMX. La configuration attendue est rappelée en tête du fichier, et les réponses aux questions à la fin. 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 :ref:`etat-ratbot`.