Enregistrer un sac à partir d’un nœud (C++)

Objectif : Enregistrez les données de votre propre nœud C++ dans un sac.

Niveau du didacticiel : Avancé

Durée : 20 minutes

Arrière-plan

rosbag2 ne fournit pas seulement l’outil de ligne de commande ros2 bag. Il fournit également une API C++ pour lire et écrire dans un sac à partir de votre propre code source. Cela vous permet de vous abonner à un sujet et d’enregistrer les données reçues dans un sac en même temps que d’effectuer tout autre traitement de votre choix sur ces données.

Conditions préalables

Vous devriez avoir les packages rosbag2 installés dans le cadre de votre configuration habituelle de ROS 2.

Si vous avez installé des packages Debian sur Linux, il peut être installé par défaut. Si ce n’est pas le cas, vous pouvez l’installer à l’aide de cette commande.

sudo apt install ros-rolling-rosbag2

Ce tutoriel traite de l’utilisation des sacs ROS 2, y compris depuis le terminal. Vous devriez déjà avoir terminé le tutoriel de base sur le sac ROS 2.

Tâches

1 Créer un package

Ouvrez un nouveau terminal et sourcez votre installation ROS 2 pour que les commandes ros2 fonctionnent.

Naviguez dans le répertoire ros2_ws créé dans un tutoriel précédent. Naviguez dans le répertoire ros2_ws/src et créez un nouveau package :

ros2 pkg create --build-type ament_cmake bag_recorder_nodes --dependencies example_interfaces rclcpp rosbag2_cpp std_msgs

Votre terminal renverra un message vérifiant la création de votre package bag_recorder_nodes et tous ses fichiers et dossiers nécessaires. L’argument --dependencies ajoutera automatiquement les lignes de dépendance nécessaires à package.xml et CMakeLists.txt. Dans ce cas, le package utilisera le package rosbag2_cpp ainsi que le package rclcpp. Une dépendance au package example_interfaces est également requise pour les parties ultérieures de ce didacticiel.

1.1 Mettre à jour package.xml

Comme vous avez utilisé l’option --dependencies lors de la création du package, vous n’avez pas besoin d’ajouter manuellement des dépendances à package.xml ou CMakeLists.txt. Comme toujours, assurez-vous d’ajouter la description, l’adresse e-mail et le nom du responsable, ainsi que les informations de licence à package.xml.

<description>C++ bag writing tutorial</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

2 Écrivez le nœud C++

Dans le répertoire ros2_ws/src/bag_recorder_nodes/src, créez un nouveau fichier appelé simple_bag_recorder.cpp et collez-y le code suivant.

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

#include <rosbag2_cpp/writer.hpp>

using std::placeholders::_1;

class SimpleBagRecorder : public rclcpp::Node
{
public:
  SimpleBagRecorder()
  : Node("simple_bag_recorder")
  {
    writer_ = std::make_unique<rosbag2_cpp::Writer>();

    writer_->open("my_bag");

    subscription_ = create_subscription<std_msgs::msg::String>(
      "chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));
  }

private:
  void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
  {
    rclcpp::Time time_stamp = this->now();

    writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);
  }

  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
  std::unique_ptr<rosbag2_cpp::Writer> writer_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SimpleBagRecorder>());
  rclcpp::shutdown();
  return 0;
}

2.1 Examiner le code

Les déclarations #include en haut sont les dépendances du paquet. Notez l’inclusion des en-têtes du package rosbag2_cpp pour les fonctions et les structures nécessaires pour travailler avec les fichiers bag.

Dans le constructeur de classe, nous commençons par créer l’objet écrivain que nous utiliserons pour écrire dans le sac.

writer_ = std::make_unique<rosbag2_cpp::Writer>();

Maintenant que nous avons un objet écrivain, nous pouvons ouvrir le sac en l’utilisant. Nous spécifions uniquement l’URI du sac à créer, laissant les autres options à leurs valeurs par défaut. Les options de stockage par défaut sont utilisées, ce qui signifie qu’un sac au format sqlite3 sera créé. Les options de conversion par défaut sont également utilisées, ce qui n’effectuera aucune conversion, stockant à la place les messages dans le format de sérialisation dans lequel ils sont reçus.

writer_->open("my_bag");

Le rédacteur étant désormais configuré pour enregistrer les données que nous lui transmettons, nous créons un abonnement et lui spécifions un rappel. Nous écrirons des données dans le sac dans le rappel.

subscription_ = create_subscription<std_msgs::msg::String>(
  "chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));

Le rappel lui-même est différent d’un rappel typique. Plutôt que de recevoir une instance du type de données du sujet, nous recevons à la place un rclcpp::SerializedMessage. Nous le faisons pour deux raisons.

1. Les données du message devront être sérialisées par rosbag2 avant d’être écrites dans le sac, donc plutôt que de les désérialiser lors de la réception des données puis de les re-sérialiser, nous demandons à ROS de nous donner simplement le message sérialisé tel quel .

2. L’API du rédacteur peut accepter un message sérialisé.

void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
{

Dans le rappel d’abonnement, la première chose à faire est de déterminer l’horodatage à utiliser pour le message stocké. Il peut s’agir de tout élément approprié à vos données, mais deux valeurs courantes sont l’heure à laquelle les données ont été produites, si elles sont connues, et l’heure à laquelle elles sont reçues. La deuxième option, l’heure de réception, est utilisée ici.

rclcpp::Time time_stamp = this->now();

On peut alors écrire le message dans le sac. Étant donné que nous n’avons pas encore enregistré de sujets avec le sac, nous devons spécifier les informations complètes sur le sujet avec le message. C’est pourquoi nous transmettons le nom du sujet et le type de sujet.

writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);

La classe contient deux variables membres.

1. L’objet d’abonnement. Notez que le paramètre de modèle est le type du rappel, pas le type du sujet. Dans ce cas, le rappel reçoit un pointeur partagé rclcpp::SerializedMessage, c’est donc ce que doit être le paramètre de modèle.

2. Un pointeur géré vers l’objet écrivain utilisé pour écrire dans le sac. Notez que le type de rédacteur utilisé ici est rosbag2_cpp::Writer, l’interface de rédacteur générique. D’autres rédacteurs peuvent être disponibles avec des comportements différents.

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
std::unique_ptr<rosbag2_cpp::Writer> writer_;

Le fichier se termine par la fonction main utilisée pour créer une instance du nœud et démarrer le traitement par ROS.

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SimpleBagRecorder>());
  rclcpp::shutdown();
  return 0;
}

2.2 Ajouter un exécutable

Ouvrez maintenant le fichier CMakeLists.txt.

Vers le haut du fichier, changez CMAKE_CXX_STANDARD de 14 à 17.

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

Sous le bloc de dépendances, qui contient find_package(rosbag2_cpp REQUIRED), ajoutez les lignes de code suivantes.

add_executable(simple_bag_recorder src/simple_bag_recorder.cpp)
ament_target_dependencies(simple_bag_recorder rclcpp rosbag2_cpp std_msgs)

install(TARGETS
  simple_bag_recorder
  DESTINATION lib/${PROJECT_NAME}
)

3 Construire et exécuter

Revenez à la racine de votre espace de travail, ros2_ws, et créez votre nouveau package.

LinuxmacOSWindows

colcon build --packages-select bag_recorder_nodes

Ouvrez un nouveau terminal, accédez à ros2_ws et sourcez les fichiers d’installation.

LinuxmacOSWindows

source install/setup.bash

Exécutez maintenant le nœud :

ros2 run bag_recorder_nodes simple_bag_recorder

Ouvrez un deuxième terminal et exécutez le nœud d’exemple talker.

ros2 run demo_nodes_cpp talker

Cela commencera à publier des données sur le sujet chatter. Au fur et à mesure que le nœud d’écriture du sac reçoit ces données, il les écrira dans le sac my_bag.

Terminez les deux nœuds. Ensuite, dans un terminal, démarrez le nœud d’exemple listener.

ros2 run demo_nodes_cpp listener

Dans l’autre terminal, utilisez ros2 bag pour lire le sac enregistré par votre nœud.

ros2 bag play my_bag

Vous verrez les messages du sac reçus par le nœud listener.

Si vous souhaitez exécuter à nouveau le nœud d’écriture de sac, vous devrez d’abord supprimer le répertoire my_bag.

4 Enregistrer les données synthétiques d’un nœud

Toutes les données peuvent être enregistrées dans un sac, pas seulement les données reçues sur un sujet. Un cas d’utilisation courant pour écrire dans un sac à partir de votre propre nœud consiste à générer et à stocker des données synthétiques. Dans cette section, vous apprendrez à écrire un nœud qui génère des données et les stocke dans un sac. Nous allons démontrer deux approches pour ce faire. Le premier utilise un nœud avec une minuterie ; c’est l’approche que vous utiliseriez si votre génération de données est externe au nœud, comme la lecture de données directement à partir du matériel (par exemple, une caméra). La deuxième approche n’utilise pas de nœud ; c’est l’approche que vous pouvez utiliser lorsque vous n’avez pas besoin d’utiliser les fonctionnalités de l’infrastructure ROS.

4.1 Écrire un nœud C++

Dans le répertoire ros2_ws/src/bag_recorder_nodes/src, créez un nouveau fichier appelé data_generator_node.cpp et collez-y le code suivant.

#include <chrono>

#include <example_interfaces/msg/int32.hpp>
#include <rclcpp/rclcpp.hpp>

#include <rosbag2_cpp/writer.hpp>

using namespace std::chrono_literals;

class DataGenerator : public rclcpp::Node
{
public:
  DataGenerator()
  : Node("data_generator")
  {
    data_.data = 0;
    writer_ = std::make_unique<rosbag2_cpp::Writer>();

    writer_->open("timed_synthetic_bag");

    writer_->create_topic(
      {"synthetic",
       "example_interfaces/msg/Int32",
       rmw_get_serialization_format(),
       ""});

    timer_ = create_wall_timer(1s, std::bind(&DataGenerator::timer_callback, this));
  }

private:
  void timer_callback()
  {
    writer_->write(data_, "synthetic", now());

    ++data_.data;
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::unique_ptr<rosbag2_cpp::Writer> writer_;
  example_interfaces::msg::Int32 data_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DataGenerator>());
  rclcpp::shutdown();
  return 0;
}

4.2 Examiner le code

Une grande partie de ce code est identique au premier exemple. Les différences importantes sont décrites ici.

Tout d’abord, le nom du sac est changé.

writer_->open("timed_synthetic_bag");

Dans cet exemple, nous enregistrons le sujet avec le sac à l’avance. Ceci est facultatif dans la plupart des cas, mais cela doit être fait lors de la transmission d’un message sérialisé sans informations sur le sujet.

writer_->create_topic(
  {"synthetic",
   "example_interfaces/msg/Int32",
   rmw_get_serialization_format(),
   ""});

Plutôt qu’un abonnement à un sujet, ce nœud a une minuterie. Le minuteur se déclenche avec une période d’une seconde et appelle la fonction membre donnée lorsqu’il le fait.

timer_ = create_wall_timer(1s, std::bind(&DataGenerator::timer_callback, this));

Dans le rappel de la minuterie, nous générons (ou obtenons autrement, par exemple en lisant à partir d’un port série connecté à un matériel) les données que nous souhaitons stocker dans le sac. La différence importante entre cet échantillon et l’échantillon précédent est que les données ne sont pas encore sérialisées. Au lieu de cela, nous transmettons un type de données de message ROS à l’objet écrivain, dans ce cas une instance de example_interfaces/msg/Int32. L’écrivain sérialisera les données pour nous avant de les écrire dans le sac.

writer_->write(data_, "synthetic", now());

4.3 Ajouter un exécutable

Ouvrez le fichier CMakeLists.txt et ajoutez les lignes suivantes après les lignes ajoutées précédemment (en particulier, après l’appel de la macro install(TARGETS ...)).

add_executable(data_generator_node src/data_generator_node.cpp)
ament_target_dependencies(data_generator_node rclcpp rosbag2_cpp example_interfaces)

install(TARGETS
  data_generator_node
  DESTINATION lib/${PROJECT_NAME}
)

4.4 Construire et exécuter

Revenez à la racine de votre espace de travail, ros2_ws, et créez votre package.

LinuxmacOSWindows

colcon build --packages-select bag_recorder_nodes

Ouvrez un nouveau terminal, accédez à ros2_ws et sourcez les fichiers d’installation.

LinuxmacOSWindows

source install/setup.bash

(Si le répertoire timed_synthetic_bag existe déjà, vous devez d’abord le supprimer avant d’exécuter le nœud.)

Exécutez maintenant le nœud :

ros2 run bag_recorder_nodes data_generator_node

Attendez environ 30 secondes, puis terminez le nœud avec ctrl-c. Ensuite, lisez le sac créé.

ros2 bag play timed_synthetic_bag

Ouvrez un second terminal et renvoyez le sujet /synthetic.

ros2 topic echo /synthetic

Vous verrez les données générées et stockées dans le sac imprimées sur la console au rythme d’un message par seconde.

5 Enregistrer des données synthétiques à partir d’un exécutable

Maintenant que vous pouvez créer un sac qui stocke des données à partir d’une source autre qu’un sujet, vous allez apprendre à générer et à enregistrer des données synthétiques à partir d’un exécutable non nœud. L’avantage de cette approche est un code plus simple et la création rapide d’une grande quantité de données.

5.1 Écrire un exécutable C++

Dans le répertoire ros2_ws/src/bag_recorder_nodes/src, créez un nouveau fichier appelé data_generator_executable.cpp et collez-y le code suivant.

#include <chrono>

#include <rclcpp/rclcpp.hpp>  // For rclcpp::Clock, rclcpp::Duration and rclcpp::Time
#include <example_interfaces/msg/int32.hpp>

#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_cpp/writers/sequential_writer.hpp>
#include <rosbag2_storage/serialized_bag_message.hpp>

using namespace std::chrono_literals;

int main(int, char**)
{
  example_interfaces::msg::Int32 data;
  data.data = 0;
  std::unique_ptr<rosbag2_cpp::Writer> writer_ = std::make_unique<rosbag2_cpp::Writer>();

  writer_->open("big_synthetic_bag");

  writer_->create_topic(
    {"synthetic",
     "example_interfaces/msg/Int32",
     rmw_get_serialization_format(),
     ""});

  rclcpp::Clock clock;
  rclcpp::Time time_stamp = clock.now();
  for (int32_t ii = 0; ii < 100; ++ii) {
    writer_->write(data, "synthetic", time_stamp);
    ++data.data;
    time_stamp += rclcpp::Duration(1s);
  }

  return 0;
}

5.2 Examiner le code

Une comparaison de cet échantillon et de l’échantillon précédent révélera qu’ils ne sont pas si différents. La seule différence significative est l’utilisation d’une boucle for pour piloter la génération de données plutôt qu’un temporisateur.

Notez que nous générons également des horodatages pour les données plutôt que de nous fier à l’heure système actuelle pour chaque échantillon. L’horodatage peut être n’importe quelle valeur dont vous avez besoin. Les données seront lues à la vitesse donnée par ces horodatages, c’est donc un moyen utile de contrôler la vitesse de lecture par défaut des échantillons. Notez également que même si l’écart entre chaque échantillon est d’une seconde complète, cet exécutable n’a pas besoin d’attendre une seconde entre chaque échantillon. Cela nous permet de générer beaucoup de données couvrant une large période de temps en beaucoup moins de temps que la lecture ne prendra.

rclcpp::Clock clock;
rclcpp::Time time_stamp = clock.now();
for (int32_t ii = 0; ii < 100; ++ii) {
  writer_->write(data, "synthetic", time_stamp);
  ++data.data;
  time_stamp += rclcpp::Duration(1s);
}

5.3 Ajouter un exécutable

Ouvrez le fichier CMakeLists.txt et ajoutez les lignes suivantes après les lignes ajoutées précédemment.

add_executable(data_generator_executable src/data_generator_executable.cpp)
ament_target_dependencies(data_generator_executable rclcpp rosbag2_cpp example_interfaces)

install(TARGETS
  data_generator_executable
  DESTINATION lib/${PROJECT_NAME}
)

5.4 Construire et exécuter

Revenez à la racine de votre espace de travail, ros2_ws, et créez votre package.

LinuxmacOSWindows

colcon build --packages-select bag_recorder_nodes

Ouvrez un terminal, accédez à ros2_ws et sourcez les fichiers d’installation.

LinuxmacOSWindows

source install/setup.bash

(Si le répertoire big_synthetic_bag existe déjà, vous devez d’abord le supprimer avant de lancer l’exécutable.)

Lancez maintenant l’exécutable :

ros2 run bag_recorder_nodes data_generator_executable

Notez que l’exécutable s’exécute et se termine très rapidement.

Rejouez maintenant le sac créé.

ros2 bag play big_synthetic_bag

Ouvrez un second terminal et renvoyez le sujet /synthetic.

ros2 topic echo /synthetic

Vous verrez les données générées et stockées dans le sac imprimées sur la console au rythme d’un message par seconde. Même si le sac a été généré rapidement, il est toujours lu au rythme indiqué par les horodatages.

Résumé

Vous avez créé un nœud qui enregistre les données qu’il reçoit sur un sujet dans un sac. Vous avez testé l’enregistrement d’un sac à l’aide du nœud et vérifié que les données ont été enregistrées en lisant le sac. Vous avez ensuite créé un nœud et un exécutable pour générer des données synthétiques et les stocker dans un sac.