Allow an arbitrary topic to be recorded (ros2#26)

* ros2GH-52 Extend db schema to include topic meta data

- Two table db layout (messages and topics)
- Messages table references topics table but without foreign key for
  improved write performance
- Create_topic must be called for every topic prior to storing a
  message of this topic.
- Sqlite_storage caches all known topics
- At least for now the type information is stored as a simple string.

* ros2GH-54 Make first rcl subscription prototype work

* ros2GH-54 find type name from topic

* ros2GH-54 Publish messages from database knowing only topic name and pass topic name by terminal

* ros2GH-54 Refactoring of typesupport helpers

* ros2GH-54 Use c++ typesupport

* ros2GH-54 Use cpp typesupport and rclcpp::Node for publisher

* ros2GH-54 Add raw subscription and use in rosbag_record

* ros2GH-54 Add Rosbag2Node and Rosbag2Publisher classes and use them in Rosbag2::play

* ros2GH-54 Rename Rosbag2Publisher to RawPublisher

* ros2GH-54 Minor refactoring of Rosbag2Node

* ros2GH-54 Extract and test waiting for topic into its own method

* ros2GH-54 Fix read integration tests and linters

* ros2GH-55 Refactor Rosbag2Node::create_raw_publisher()

* ros2GH-54 Add subscription method to rosbag node

* ros2GH-54 Keep subscription alive

* ros2GH-54: Extract subscription to correct class

* ros2GH-55 Change interface of raw_publisher to match subscriber

* ros2GH-54 Add test for rosbag node

* ros2GH-54 Unfriend rclcpp class

* ros2GH-54 Make test more robust

* ros2GH-54 Fix build

* ros2GH-54 Minor cleanup and documentation

* ros2GH-55 Minor refactoring + TODO comment

* ros2GH-54 Change dynamic library folder on Windows

* ros2GH-54 Fix build

* ros2GH-54 Add shutdown to test

* ros2GH-55 Add test helpers methods for usage in multiple tests

* ros2GH-55 Add new method to read all topics and types in BaseReadInterface and use it in Rosbag2::play

* ros2GH-55 Fix gcc and msvc

* ros2GH-54 Rename raw to generic in publisher/subscriber

* ros2GH-55 Check that topic and associated type in bag file are well defined before playing back messages

* ros2GH-54 Prevent unnecessary error message loading storage

* ros2GH-54 Fix memory leak

* ros2GH-54 stabilize node test

* ros2GH-55 Check if database exists when opening storage with READ_ONLY flag

* ros2GH-54 Minor cleanup of subscriber

* ros2GH-54 Wait a small amount of time to let node discover other nodes

* Add logging to false case

* ros2GH-54 Catch exceptions and exit cleanly

* Use rmw_serialized_message_t and rcutils_char_array_t consistently

* ros2GH-4 Refactoring for correctness

- pass a few strings as const reference
- throw error when no topics could be found

* Improve error messages when loading plugins

* alphabetical order

* type_id -> type

Author: Martin-Idel-SI

rosbag2/CMakeLists.txt

@@ -16,17 +16,36 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(poco_vendor)
+find_package(Poco COMPONENTS Foundation)
+find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcutils REQUIRED)
-find_package(std_msgs REQUIRED)
 find_package(rosbag2_storage REQUIRED)
+find_package(rosidl_generator_c REQUIRED)
+find_package(std_msgs REQUIRED)
 
 add_library(
   librosbag2
   SHARED
-  src/rosbag2/rosbag2.cpp)
+  src/rosbag2/rosbag2.cpp
+  src/rosbag2/typesupport_helpers.cpp
+  src/rosbag2/generic_publisher.cpp
+  src/rosbag2/generic_subscription.cpp
+  src/rosbag2/rosbag2_node.cpp)
 
-ament_target_dependencies(librosbag2 rclcpp rcutils std_msgs rosbag2_storage)
+ament_target_dependencies(
+  librosbag2
+  ament_index_cpp
+  Poco
+  rcl
+  rclcpp
+  rcutils
+  rosbag2_storage
+  rosidl_generator_c
+  std_msgs
+)
 target_include_directories(librosbag2
   PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -61,6 +80,8 @@ if(BUILD_TESTING)
   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
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_write_integration_test)
     target_link_libraries(rosbag2_write_integration_test librosbag2)
@@ -69,10 +90,42 @@ if(BUILD_TESTING)
   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
     WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
   if(TARGET rosbag2_read_integration_test)
     target_link_libraries(rosbag2_read_integration_test librosbag2)
   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)
+  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)
+    target_link_libraries(rosbag2_integration_test librosbag2)
+  endif()
+
+  ament_add_gmock(rosbag2_rosbag_node_test
+    test/rosbag2/rosbag2_rosbag_node_test.cpp
+    src/rosbag2/typesupport_helpers.cpp
+    src/rosbag2/generic_subscription.cpp
+    src/rosbag2/generic_publisher.cpp
+    src/rosbag2/rosbag2_node.cpp
+    test/rosbag2/test_helpers.hpp
+    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)
+  endif()
 endif()
 
 ament_package()

rosbag2/include/rosbag2/rosbag2.hpp

@@ -16,8 +16,12 @@
 #define ROSBAG2__ROSBAG2_HPP_
 
 #include <functional>
+#include <memory>
 #include <string>
 
+#include "rclcpp/rclcpp.hpp"
+
+#include "rosbag2_storage/storage_interfaces/read_only_interface.hpp"
 #include "rosbag2/visibility_control.hpp"
 
 namespace rosbag2
@@ -34,6 +38,15 @@ class Rosbag2
 
   ROSBAG2_PUBLIC
   void play(const std::string & file_name, const std::string & topic_name);
+
+  ROSBAG2_PUBLIC
+  std::string get_topic_type(
+    const std::string & topic_name, const 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);
 };
 
 }  // namespace rosbag2

rosbag2/package.xml

@@ -9,11 +9,15 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>rosbag2_storage</build_depend>
-  <build_depend>rcutils</build_depend>
-
-  <exec_depend>rosbag2_storage</exec_depend>
-  <exec_depend>rcutils</exec_depend>
+  <depend>ament_index_cpp</depend>
+  <depend>libpoco-dev</depend>
+  <depend>poco_vendor</depend>
+  <depend>rcl</depend>
+  <depend>rclcpp</depend>
+  <depend>rcutils</depend>
+  <depend>rosbag2_storage</depend>
+  <depend>rosidl_generator_c</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rosbag2/src/rosbag2/demo_play.cpp

@@ -12,16 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <string>
+
 #include "rclcpp/rclcpp.hpp"
 
 #include "rosbag2/rosbag2.hpp"
 
 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", "string_topic");
+  rosbag2.play("test.bag", topic_name);
 
   rclcpp::shutdown();
 

rosbag2/src/rosbag2/demo_record.cpp

@@ -21,6 +21,12 @@
 
 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";
+    return 0;
+  }
+  std::string topic_name = argv[1];
+
   // TODO(anhosi): allow output file to be specified by cli argument and do proper checking if
   // file already exists
   std::string filename("test.bag");
@@ -29,7 +35,7 @@ int main(int argc, const char ** argv)
   rclcpp::init(argc, argv);
 
   rosbag2::Rosbag2 rosbag2;
-  rosbag2.record(filename, "string_topic");
+  rosbag2.record(filename, topic_name);
 
   rclcpp::shutdown();
 

rosbag2/src/rosbag2/generic_publisher.cpp

@@ -0,0 +1,40 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "generic_publisher.hpp"
+
+#include <memory>
+#include <string>
+
+namespace rosbag2
+{
+
+GenericPublisher::GenericPublisher(
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
+  const std::string & topic,
+  const rosidl_message_type_support_t & type_support)
+: rclcpp::PublisherBase(node_base, topic, type_support, rcl_publisher_get_default_options())
+{}
+
+void GenericPublisher::publish(std::shared_ptr<rmw_serialized_message_t> message)
+{
+  auto return_code = rcl_publish_serialized_message(
+    get_publisher_handle(), message.get());
+
+  if (return_code != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message");
+  }
+}
+
+}  // namespace rosbag2

rosbag2/src/rosbag2/generic_publisher.hpp

@@ -0,0 +1,41 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROSBAG2__GENERIC_PUBLISHER_HPP_
+#define ROSBAG2__GENERIC_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace rosbag2
+{
+
+class GenericPublisher : public rclcpp::PublisherBase
+{
+public:
+  GenericPublisher(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rosidl_message_type_support_t & type_support);
+
+  ~GenericPublisher() override = default;
+
+  void publish(std::shared_ptr<rmw_serialized_message_t> message);
+};
+
+}  // namespace rosbag2
+
+#endif  // ROSBAG2__GENERIC_PUBLISHER_HPP_

rosbag2/src/rosbag2/generic_subscription.cpp

@@ -0,0 +1,111 @@
+// Copyright 2018, Bosch Software Innovations GmbH.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "generic_subscription.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/subscription.hpp"
+
+namespace rosbag2
+{
+
+GenericSubscription::GenericSubscription(
+  std::shared_ptr<rcl_node_t> node_handle,
+  const rosidl_message_type_support_t & ts,
+  const std::string & topic_name,
+  std::function<void(std::shared_ptr<rcutils_char_array_t>)> callback)
+: SubscriptionBase(
+    node_handle,
+    ts,
+    topic_name,
+    rcl_subscription_get_default_options(),
+    true),
+  default_allocator_(rcutils_get_default_allocator()),
+  callback_(callback)
+{}
+
+std::shared_ptr<void> GenericSubscription::create_message()
+{
+  return create_serialized_message();
+}
+
+std::shared_ptr<rmw_serialized_message_t> GenericSubscription::create_serialized_message()
+{
+  return borrow_serialized_message(0);
+}
+
+
+void GenericSubscription::handle_message(
+  std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
+{
+  (void) message_info;
+  auto typed_message = std::static_pointer_cast<rcutils_char_array_t>(message);
+  callback_(typed_message);
+}
+
+void GenericSubscription::return_message(std::shared_ptr<void> & message)
+{
+  auto typed_message = std::static_pointer_cast<rcutils_char_array_t>(message);
+  return_serialized_message(typed_message);
+}
+
+void GenericSubscription::return_serialized_message(
+  std::shared_ptr<rmw_serialized_message_t> & message)
+{
+  message.reset();
+}
+
+void GenericSubscription::handle_intra_process_message(
+  rcl_interfaces::msg::IntraProcessMessage & ipm,
+  const rmw_message_info_t & message_info)
+{
+  (void) ipm;
+  (void) message_info;
+  throw std::runtime_error("Intra process is not supported");
+}
+
+const std::shared_ptr<rcl_subscription_t>
+GenericSubscription::get_intra_process_subscription_handle() const
+{
+  return nullptr;
+}
+
+std::shared_ptr<rmw_serialized_message_t>
+GenericSubscription::borrow_serialized_message(size_t capacity)
+{
+  auto message = new rmw_serialized_message_t;
+  *message = rmw_get_zero_initialized_serialized_message();
+  auto init_return = rmw_serialized_message_init(message, capacity, &default_allocator_);
+  if (init_return != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(init_return);
+  }
+
+  auto serialized_msg = std::shared_ptr<rmw_serialized_message_t>(message,
+      [](rmw_serialized_message_t * msg) {
+        auto fini_return = rmw_serialized_message_fini(msg);
+        delete msg;
+        if (fini_return != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "rosbag2",
+            "failed to destroy serialized message: %s", rcl_get_error_string_safe());
+        }
+      });
+
+  return serialized_msg;
+}
+
+}  // namespace rosbag2