Record and play multiple topics (ros2#27)

* ros2GH-61 Read topic directly from message when playing and allow to play multiple topics

* ros2GH-61 Add test for SqliteStorage and update old ones

* ros2GH-62 Extend function to poll for any number of specified topics

* ros2GH-62 Allow subscription to several topics

* ros2GH-61 Obtain the topic name directly from the database

- Uses a JOIN instead of mapping the topic_id to the name in code

* ros2GH-61 Cache read row in result iterator

This allows repeated dereferencing on same row without quering the
database again.

* ros2GH-62 Change demo-record to allow specifying multiple topics

* ros2GH-62 Add test to write non-string topic + refactoring

* ros2GH-62 Add test for subscription to multiple topics

* ros2GH-62 Cleanup

* ros2GH-62 Simplify test setup

* ros2GH-61 Cleanup

* ros2GH-61 consolidate storage integration test

* ros2GH-62 Consolidate write integration tests

* ros2GH-61 enhance read integration test to check multiple topics

* ros2GH-62 Improve rosbag integration test

* ros2GH-62: Polish rosbag2_rosbag_node_test

* ros2GH-62 Fix cpplint

* ros2GH-62 Fix memory leak in rosbag helper

* ros2GH-62 Cleanup of subscriptions

* ros2GH-62 do not use flaky timers in rosbag2_write_integration_test

* ros2GH-62 Use rmw_serialize_message_t consistently in test helper classes

* ros2GH-73 Use test_msgs in read_integration_test

* ros2GH-26 Cleanup: fix alphabetic orderung

Author: anhosi

rosbag2/CMakeLists.txt

@@ -75,40 +75,43 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_lint_auto REQUIRED)
+  find_package(test_msgs REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
   ament_add_gmock(rosbag2_write_integration_test
     test/rosbag2/rosbag2_write_integration_test.cpp
-    test/rosbag2/rosbag2_test_fixture.hpp
-    test/rosbag2/test_helpers.hpp
-    src/rosbag2/typesupport_helpers.cpp
+    test/rosbag2/test_memory_management.cpp
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_write_integration_test)
     target_link_libraries(rosbag2_write_integration_test librosbag2)
   endif()
 
   ament_add_gmock(rosbag2_read_integration_test
     test/rosbag2/rosbag2_read_integration_test.cpp
-    test/rosbag2/rosbag2_test_fixture.hpp
-    test/rosbag2/test_helpers.hpp
-    src/rosbag2/typesupport_helpers.cpp
+    test/rosbag2/test_memory_management.cpp
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_read_integration_test)
     target_link_libraries(rosbag2_read_integration_test librosbag2)
+    ament_target_dependencies(rosbag2_read_integration_test
+      test_msgs
+    )
   endif()
 
   ament_add_gmock(rosbag2_typesupport_helpers_test
     test/rosbag2/rosbag2_typesupport_helpers_test.cpp
     src/rosbag2/typesupport_helpers.cpp
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_typesupport_helpers_test)
-    ament_target_dependencies(rosbag2_typesupport_helpers_test rcl Poco ament_index_cpp
-      rosidl_generator_cpp)
+    ament_target_dependencies(rosbag2_typesupport_helpers_test
+      ament_index_cpp
+      Poco
+      rcl
+      rosidl_generator_cpp
+    )
   endif()
 
   ament_add_gmock(rosbag2_integration_test
     test/rosbag2/rosbag2_integration_test.cpp
-    test/rosbag2/test_helpers.hpp
     src/rosbag2/typesupport_helpers.cpp
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_integration_test)
@@ -117,14 +120,19 @@ if(BUILD_TESTING)
 
   ament_add_gmock(rosbag2_rosbag_node_test
     test/rosbag2/rosbag2_rosbag_node_test.cpp
-    src/rosbag2/typesupport_helpers.cpp
-    src/rosbag2/generic_subscription.cpp
+    test/rosbag2/test_memory_management.cpp
     src/rosbag2/generic_publisher.cpp
+    src/rosbag2/generic_subscription.cpp
     src/rosbag2/rosbag2_node.cpp
-    test/rosbag2/test_helpers.hpp
+    src/rosbag2/typesupport_helpers.cpp
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_rosbag_node_test)
-    ament_target_dependencies(rosbag2_rosbag_node_test rclcpp Poco ament_index_cpp std_msgs)
+    ament_target_dependencies(rosbag2_rosbag_node_test
+      ament_index_cpp
+      Poco
+      rclcpp
+      std_msgs
+    )
   endif()
 endif()
 

rosbag2/include/rosbag2/rosbag2.hpp

@@ -16,37 +16,54 @@
 #define ROSBAG2__ROSBAG2_HPP_
 
 #include <functional>
+#include <map>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 
 #include "rosbag2_storage/storage_interfaces/read_only_interface.hpp"
+#include "rosbag2_storage/storage_interfaces/read_write_interface.hpp"
 #include "rosbag2/visibility_control.hpp"
 
 namespace rosbag2
 {
 
+class GenericPublisher;
+class GenericSubscription;
+class Rosbag2Node;
+
 class Rosbag2
 {
 public:
   ROSBAG2_PUBLIC
   void record(
     const std::string & file_name,
-    const std::string & topic_name,
-    std::function<void(void)> after_write_action = nullptr);
+    std::vector<std::string> topic_names,
+    std::function<void(std::string)> after_write_action = nullptr);
 
   ROSBAG2_PUBLIC
-  void play(const std::string & file_name, const std::string & topic_name);
+  void play(const std::string & file_name);
 
   ROSBAG2_PUBLIC
-  std::string get_topic_type(
-    const std::string & topic_name, const std::shared_ptr<rclcpp::Node> & node);
+  std::map<std::string, std::string> get_topics_with_types(
+    const std::vector<std::string> & topic_names, std::shared_ptr<rclcpp::Node> node);
 
-  ROSBAG2_PUBLIC
-  std::string get_topic_type(
-    std::shared_ptr<rosbag2_storage::storage_interfaces::ReadOnlyInterface> storage,
-    const std::string & topic);
+private:
+  void prepare_publishers(
+    std::shared_ptr<Rosbag2Node> node,
+    std::shared_ptr<rosbag2_storage::storage_interfaces::ReadOnlyInterface> storage);
+
+  std::shared_ptr<rosbag2::GenericSubscription>
+  create_subscription(
+    const std::function<void(std::string)> & after_write_action,
+    std::shared_ptr<rosbag2_storage::storage_interfaces::ReadWriteInterface> storage,
+    std::shared_ptr<Rosbag2Node> & node,
+    const std::string & topic_name, const std::string & topic_type) const;
+
+  std::vector<std::shared_ptr<GenericSubscription>> subscriptions_;
+  std::map<std::string, std::shared_ptr<GenericPublisher>> publishers_;
 };
 
 }  // namespace rosbag2

rosbag2/package.xml

@@ -21,6 +21,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rosbag2/src/rosbag2/demo_play.cpp

@@ -20,16 +20,10 @@
 
 int main(int argc, const char ** argv)
 {
-  if (argc < 2) {
-    std::cerr << "\nThe name of the topic to play to must be given as parameter!\n";
-    return 0;
-  }
-  std::string topic_name = argv[1];
-
   rclcpp::init(argc, argv);
 
   rosbag2::Rosbag2 rosbag2;
-  rosbag2.play("test.bag", topic_name);
+  rosbag2.play("test.bag");
 
   rclcpp::shutdown();
 

rosbag2/src/rosbag2/demo_record.cpp

@@ -14,6 +14,7 @@
 
 #include <cstdio>
 #include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 
@@ -22,10 +23,14 @@
 int main(int argc, const char ** argv)
 {
   if (argc < 2) {
-    std::cerr << "\nThe name of the topic to record must be given as parameter!\n";
+    std::cerr << "\nThe names of topics to record must be given as parameter!\n";
     return 0;
   }
-  std::string topic_name = argv[1];
+  std::vector<std::string> topics;
+
+  for (int i = 1; i < argc; i++) {
+    topics.emplace_back(argv[i]);
+  }
 
   // TODO(anhosi): allow output file to be specified by cli argument and do proper checking if
   // file already exists
@@ -35,7 +40,7 @@ int main(int argc, const char ** argv)
   rclcpp::init(argc, argv);
 
   rosbag2::Rosbag2 rosbag2;
-  rosbag2.record(filename, topic_name);
+  rosbag2.record(filename, topics);
 
   rclcpp::shutdown();
 

rosbag2/src/rosbag2/generic_subscription.cpp

@@ -48,7 +48,6 @@ std::shared_ptr<rmw_serialized_message_t> GenericSubscription::create_serialized
   return borrow_serialized_message(0);
 }
 
-
 void GenericSubscription::handle_message(
   std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
 {