Utilisation des groupes de rappel

Lors de l’exécution d’un nœud dans un exécuteur multithread, ROS 2 propose des groupes de rappel comme outil pour contrôler l’exécution de différents rappels. Cette page est conçue comme un guide sur la façon d’utiliser efficacement les groupes de rappel. Il est supposé que le lecteur a une compréhension de base du concept de executors.

Principes de base des groupes de rappel

Lors de l’exécution d’un nœud dans un exécuteur multithread, ROS 2 propose deux types différents de groupes de rappel pour contrôler l’exécution des rappels :

  • Groupe de rappel mutuellement exclusif

  • Groupe de rappel réentrant

Ces groupes de rappel limitent l’exécution de leurs rappels de différentes manières. En bref:

  • Le groupe de rappel mutuellement exclusif empêche l’exécution de ses rappels en parallèle, ce qui fait essentiellement comme si les rappels du groupe étaient exécutés par un SingleThreadedExecutor.

  • Le groupe de rappel réentrant permet à l’exécuteur de programmer et d’exécuter les rappels du groupe de la manière qu’il juge appropriée, sans restrictions. Cela signifie qu’en plus des différents rappels exécutés parallèlement les uns aux autres, différentes instances du même rappel peuvent également être exécutées simultanément.

  • Les rappels appartenant à différents groupes de rappel (de tout type) peuvent toujours être exécutés parallèlement les uns aux autres.

Il est également important de garder à l’esprit que différentes entités ROS 2 relaient leur groupe de rappel à tous les rappels qu’elles génèrent. Par exemple, si l’on affecte un groupe de rappel à un client d’action, tous les rappels créés par le client seront affectés à ce groupe de rappel.

Les groupes de rappel peuvent être créés par la fonction create_callback_group d’un nœud dans rclcpp et en appelant le constructeur du groupe dans rclpy. Le groupe de rappel peut ensuite être passé en argument/option lors de la création d’un abonnement, d’un timer, etc.

my_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

rclcpp::SubscriptionOptions options;
options.callback_group = my_callback_group;

my_subscription = create_subscription<Int32>("/topic", rclcpp::SensorDataQoS(),
                                              callback, options);

Si l’utilisateur ne spécifie aucun groupe de rappel lors de la création d’un abonnement, d’une minuterie, etc., cette entité sera affectée au groupe de rappel par défaut du nœud. Le groupe de rappel par défaut est un groupe de rappel mutuellement exclusif et il peut être interrogé via NodeBaseInterface::get_default_callback_group() dans rclcpp et via Node.default_callback_group dans rclpy.

À propos des rappels

Dans le contexte de ROS 2 et des exécuteurs, un rappel signifie une fonction dont la planification et l’exécution sont gérées par un exécuteur. Des exemples de rappels dans ce contexte sont

  • rappels d’abonnement (réception et traitement des données d’un sujet),

  • rappels de minuterie,

  • les rappels de service (pour exécuter des demandes de service dans un serveur),

  • différents rappels dans les serveurs d’action et les clients,

  • done-rappels de Futures.

Vous trouverez ci-dessous quelques points importants concernant les rappels à garder à l’esprit lorsque vous travaillez avec des groupes de rappel.

  • Presque tout dans ROS 2 est un rappel ! Chaque fonction exécutée par un exécuteur est, par définition, un rappel. Les fonctions de non-rappel dans un système ROS 2 se trouvent principalement à la périphérie du système (entrées utilisateur et capteur, etc.).

  • Parfois, les rappels sont masqués et leur présence peut ne pas être évidente à partir de l’API utilisateur/développeur. C’est le cas notamment de tout type d’appel « synchrone » à un service ou à une action (en rclpy). Par exemple, l’appel synchrone Client.call(request) à un service ajoute un rappel terminé de Future qui doit être exécuté lors de l’exécution de l’appel de la fonction, mais ce rappel n’est pas directement visible par l’utilisateur.

Contrôler l’exécution

Afin de contrôler l’exécution avec des groupes de rappel, on peut considérer les directives suivantes.

  • Enregistrez les rappels qui ne doivent jamais être exécutés en parallèle sur le même groupe de rappel mutuellement exclusif. Un exemple de cas peut être que les rappels accèdent à des ressources partagées critiques et non thread-safe.

  • Si vous avez un rappel dont les instances d’exécution doivent pouvoir se chevaucher, enregistrez-le dans un groupe de rappel réentrant. Un exemple de cas pourrait être un serveur d’action qui doit être capable de traiter plusieurs appels d’action en parallèle les uns aux autres.

  • Si vous avez différents rappels qui doivent être potentiellement exécutés en parallèle, enregistrez-les dans

    • un groupe de rappel réentrant, ou

    • différents groupes de rappel mutuellement exclusifs (cette option est utile si vous souhaitez que les rappels ne se chevauchent pas ou si vous avez également besoin de la sécurité des threads par rapport à d’autres rappels) ou différents groupes de rappel de tout type (choisissez les types en fonction d’autres critères).

Notez que l’option dans la liste est un moyen valide d’autoriser l’exécution parallèle pour différents rappels, et peut même être plus souhaitable que de simplement tout enregistrer dans un groupe de rappel réentrant.

Éviter les blocages

La configuration incorrecte des groupes de rappel d’un nœud peut entraîner des blocages (ou d’autres comportements indésirables), en particulier si l’on souhaite utiliser des appels synchrones à des services ou des actions. En effet, même la documentation de l’API de ROS 2 mentionne que les appels synchrones à des actions ou à des services ne doivent pas se faire dans les rappels, car cela peut conduire à des blocages. Bien que l’utilisation d’appels asynchrones soit en effet plus sûre à cet égard, les appels synchrones peuvent également fonctionner. D’un autre côté, les appels synchrones ont aussi leurs avantages, comme rendre le code plus simple et plus facile à comprendre. Par conséquent, cette section fournit des directives sur la manière de configurer correctement les groupes de rappel d’un nœud afin d’éviter les blocages.

La première chose à noter ici est que le groupe de rappel par défaut de chaque nœud est un groupe de rappel mutuellement exclusif. Si l’utilisateur ne spécifie aucun autre groupe de rappel lors de la création d’un temporisateur, d’un abonnement, d’un client, etc., tous les rappels créés alors ou ultérieurement par ces entités utiliseront le groupe de rappel par défaut du nœud. De plus, si tout dans un nœud utilise le même groupe de rappel mutuellement exclusif, ce nœud agit essentiellement comme s’il était géré par un exécuteur monothread, même si un exécuteur multithread est spécifié ! Ainsi, chaque fois que l’on décide d’utiliser un exécuteur multithread, un ou plusieurs groupes de rappel doivent toujours être spécifiés pour que le choix de l’exécuteur ait un sens.

En gardant ce qui précède à l’esprit, voici quelques lignes directrices pour éviter les blocages :

  • Si vous effectuez un appel synchrone dans n’importe quel type de rappel, ce rappel et le client effectuant l’appel doivent appartenir à

    • différents groupes de rappel (de tout type), ou

    • un groupe de rappel réentrant.

  • Si la configuration ci-dessus n’est pas possible en raison d’autres exigences - telles que la sécurité des threads et/ou le blocage d’autres rappels en attendant le résultat (ou si vous voulez être absolument sûr qu’il n’y a jamais de possibilité de blocage), utilisez appels asynchrones.

L’échec du premier point entraînera toujours une impasse. Un exemple d’un tel cas serait de faire un appel de service synchrone dans un rappel de temporisateur (voir la section suivante pour un exemple).

Exemples

Examinons quelques exemples simples de différentes configurations de groupes de rappel. Le code de démonstration suivant considère l’appel d’un service de manière synchrone dans un rappel de minuterie.

Code démo

Nous avons deux nœuds - l’un fournissant un service simple :

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"

using namespace std::placeholders;

namespace cb_group_demo
{
class ServiceNode : public rclcpp::Node
{
public:
    ServiceNode() : Node("service_node")
    {
        service_ptr_ = this->create_service<std_srvs::srv::Empty>(
                "test_service",
                std::bind(&ServiceNode::service_callback, this, _1, _2, _3)
        );
    }

private:
    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service_ptr_;

    void service_callback(
            const std::shared_ptr<rmw_request_id_t> request_header,
            const std::shared_ptr<std_srvs::srv::Empty::Request> request,
            const std::shared_ptr<std_srvs::srv::Empty::Response> response)
    {
        (void)request_header;
        (void)request;
        (void)response;
        RCLCPP_INFO(this->get_logger(), "Received request, responding...");
    }
};  // class ServiceNode
}   // namespace cb_group_demo

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto service_node = std::make_shared<cb_group_demo::ServiceNode>();

    RCLCPP_INFO(service_node->get_logger(), "Starting server node, shut down with CTRL-C");
    rclcpp::spin(service_node);
    RCLCPP_INFO(service_node->get_logger(), "Keyboard interrupt, shutting down.\n");

    rclcpp::shutdown();
    return 0;
}

et un autre contenant un client au service avec une minuterie pour effectuer des appels de service :

Remarque : L’API du client de service dans rclcpp n’offre pas de méthode d’appel synchrone similaire à celle de rclpy, nous attendons donc que le futur objet simule l’effet d’un appel synchrone.

#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"

using namespace std::chrono_literals;

namespace cb_group_demo
{
class DemoNode : public rclcpp::Node
{
public:
    DemoNode() : Node("client_node")
    {
        client_cb_group_ = nullptr;
        timer_cb_group_ = nullptr;
        client_ptr_ = this->create_client<std_srvs::srv::Empty>("test_service", rmw_qos_profile_services_default,
                                                                client_cb_group_);
        timer_ptr_ = this->create_wall_timer(1s, std::bind(&DemoNode::timer_callback, this),
                                            timer_cb_group_);
    }

private:
    rclcpp::CallbackGroup::SharedPtr client_cb_group_;
    rclcpp::CallbackGroup::SharedPtr timer_cb_group_;
    rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client_ptr_;
    rclcpp::TimerBase::SharedPtr timer_ptr_;

    void timer_callback()
    {
        RCLCPP_INFO(this->get_logger(), "Sending request");
        auto request = std::make_shared<std_srvs::srv::Empty::Request>();
        auto result_future = client_ptr_->async_send_request(request);
        std::future_status status = result_future.wait_for(10s);  // timeout to guarantee a graceful finish
        if (status == std::future_status::ready) {
            RCLCPP_INFO(this->get_logger(), "Received response");
        }
    }
};  // class DemoNode
}   // namespace cb_group_demo

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto client_node = std::make_shared<cb_group_demo::DemoNode>();
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(client_node);

    RCLCPP_INFO(client_node->get_logger(), "Starting client node, shut down with CTRL-C");
    executor.spin();
    RCLCPP_INFO(client_node->get_logger(), "Keyboard interrupt, shutting down.\n");

    rclcpp::shutdown();
    return 0;
}

Le constructeur du nœud client contient des options pour définir les groupes de rappel du client de service et le temporisateur. Avec le paramètre par défaut ci-dessus (les deux étant nullptr / None), le temporisateur et le client utiliseront le groupe de rappel mutuellement exclusif par défaut du nœud.

Le problème

Étant donné que nous effectuons des appels de service avec une minuterie de 1 seconde, le résultat attendu est que le service est appelé une fois par seconde, le client reçoit toujours une réponse et imprime Réponse reçue. Si nous essayons d’exécuter les nœuds serveur et client dans les terminaux, nous obtenons les sorties suivantes.

[INFO] [1653034371.758739131] [client_node]: Starting client node, shut down with CTRL-C
[INFO] [1653034372.755865649] [client_node]: Sending request
^C[INFO] [1653034398.161674869] [client_node]: Keyboard interrupt, shutting down.

Ainsi, il s’avère qu’au lieu que le service soit appelé à plusieurs reprises, la réponse du premier appel n’est jamais reçue, après quoi le nœud client semble bloqué et ne passe plus d’appels. Autrement dit, l’exécution s’est arrêtée dans une impasse!

La raison en est que le rappel du minuteur et le client utilisent le même groupe de rappel mutuellement exclusif (la valeur par défaut du nœud). Lorsque l’appel de service est effectué, le client passe alors son groupe de rappel à l’objet Future (caché à l’intérieur de la méthode d’appel dans la version Python) dont le rappel terminé doit s’exécuter pour que le résultat de l’appel de service soit disponible. Mais comme ce rappel terminé et le rappel de minuterie se trouvent dans le même groupe mutuellement exclusif et que le rappel de minuterie est toujours en cours d’exécution (en attente du résultat de l’appel de service), le rappel terminé ne s’exécute jamais. Le rappel de la minuterie bloquée bloque également toutes les autres exécutions de lui-même, de sorte que la minuterie ne se déclenche pas une deuxième fois.

Solution

Nous pouvons résoudre ce problème facilement - par exemple - en affectant le minuteur et le client à différents groupes de rappel. Ainsi, changeons les deux premières lignes du constructeur du nœud client comme suit (tout le reste doit rester le même) :

client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
timer_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

Nous obtenons maintenant le résultat attendu, c’est-à-dire que la minuterie se déclenche à plusieurs reprises et chaque appel de service obtient le résultat comme il se doit :

[INFO] [1653067523.431731177] [client_node]: Starting client node, shut down with CTRL-C
[INFO] [1653067524.431912821] [client_node]: Sending request
[INFO] [1653067524.433230445] [client_node]: Received response
[INFO] [1653067525.431869330] [client_node]: Sending request
[INFO] [1653067525.432912803] [client_node]: Received response
[INFO] [1653067526.431844726] [client_node]: Sending request
[INFO] [1653067526.432893954] [client_node]: Received response
[INFO] [1653067527.431828287] [client_node]: Sending request
[INFO] [1653067527.432848369] [client_node]: Received response
^C[INFO] [1653067528.400052749] [client_node]: Keyboard interrupt, shutting down.

On pourrait se demander s’il suffit d’éviter le groupe de rappel par défaut du nœud. Ce n’est pas le cas : remplacer le groupe par défaut par un autre groupe mutuellement exclusif ne change rien. Ainsi, la configuration suivante conduit également au blocage découvert précédemment.

client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
timer_cb_group_ = client_cb_group_;

En fait, la condition exacte avec laquelle tout fonctionne dans ce cas est que la minuterie et le client ne doivent pas appartenir au même groupe mutuellement exclusif. Par conséquent, toutes les configurations suivantes (ainsi que certaines autres) produisent le résultat souhaité, dans lequel la minuterie se déclenche à plusieurs reprises et les appels de service sont terminés.

client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
timer_cb_group_ = client_cb_group_;

ou alors

client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
timer_cb_group_ = nullptr;

ou alors

client_cb_group_ = nullptr;
timer_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

ou alors

client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
timer_cb_group_ = nullptr;