Add rosbag2_examples_cpp/simple_bag_reader.cpp. (#1683)

* add rosbag2_examples_cpp/simple_bag_reader.cpp.

Signed-off-by: Tomoya Fujita <[email protected]>

* use rclcpp::GenericPublisher for SimpleBagReader example.

Signed-off-by: Tomoya Fujita <[email protected]>

* add rosbag2_examples_py/simple_bag_reader.py

Signed-off-by: Tomoya Fujita <[email protected]>

* add new line feed.

Signed-off-by: Tomoya Fujita <[email protected]>

* fix flake error.

Signed-off-by: Tomoya Fujita <[email protected]>

* address lint error.

Signed-off-by: Tomoya Fujita <[email protected]>

---------

Signed-off-by: Tomoya Fujita <[email protected]>

rosbag2_examples/rosbag2_examples_cpp/CMakeLists.txt

@@ -20,6 +20,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
+find_package(rosbag2_transport REQUIRED)
 find_package(example_interfaces REQUIRED)
 
 add_executable(simple_bag_recorder src/simple_bag_recorder.cpp)
@@ -34,6 +35,19 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+add_executable(simple_bag_reader src/simple_bag_reader.cpp)
+target_link_libraries(simple_bag_reader
+  rclcpp::rclcpp
+  rosbag2_cpp::rosbag2_cpp
+  rosbag2_transport::rosbag2_transport
+  ${example_interfaces_TARGETS}
+)
+
+install(TARGETS
+  simple_bag_reader
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 add_executable(data_generator_node src/data_generator_node.cpp)
 target_link_libraries(data_generator_node
   rclcpp::rclcpp

rosbag2_examples/rosbag2_examples_cpp/package.xml

@@ -13,6 +13,7 @@
 
   <depend>rclcpp</depend>
   <depend>rosbag2_cpp</depend>
+  <depend>rosbag2_transport</depend>
   <depend>example_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>

rosbag2_examples/rosbag2_examples_cpp/src/simple_bag_reader.cpp

@@ -0,0 +1,80 @@
+// Copyright 2024 Sony Group Corporation.
+//
+// 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 <chrono>
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include "example_interfaces/msg/string.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/serialization.hpp"
+#include "rosbag2_transport/reader_writer_factory.hpp"
+
+using namespace std::chrono_literals;
+
+class SimpleBagReader : public rclcpp::Node
+{
+public:
+  explicit SimpleBagReader(const std::string & bag_filename)
+  : Node("simple_bag_reader")
+  {
+    publisher_ = this->create_generic_publisher(
+      "chatter", "example_interfaces/msg/String", 10);
+
+    timer_ = this->create_wall_timer(
+      100ms,
+      [this]() {return this->timer_callback();}
+    );
+
+    rosbag2_storage::StorageOptions storage_options;
+    storage_options.uri = bag_filename;
+    reader_ = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options);
+    reader_->open(storage_options);
+  }
+
+private:
+  void timer_callback()
+  {
+    while (reader_->has_next()) {
+      rosbag2_storage::SerializedBagMessageSharedPtr msg = reader_->read_next();
+
+      if (msg->topic_name != "chatter") {
+        continue;
+      }
+      rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
+      std::cout << "Publish serialized data to " << msg->topic_name << ".\n";
+      publisher_->publish(serialized_msg);
+      break;
+    }
+  }
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::shared_ptr<rclcpp::GenericPublisher> publisher_;
+
+  std::unique_ptr<rosbag2_cpp::Reader> reader_;
+};
+
+int main(int argc, char ** argv)
+{
+  if (argc != 2) {
+    std::cerr << "Usage: " << argv[0] << " <bag>" << std::endl;
+    return 1;
+  }
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<SimpleBagReader>(argv[1]));
+  rclcpp::shutdown();
+  return 0;
+}

rosbag2_examples/rosbag2_examples_py/rosbag2_examples_py/simple_bag_reader.py

@@ -0,0 +1,58 @@
+# Copyright 2024 Sony Group Corporation.
+#
+# 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.
+import sys
+
+import rclpy
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+import rosbag2_py
+from std_msgs.msg import String
+
+
+class SimpleBagReader(Node):
+
+    def __init__(self, bag_filename):
+        super().__init__('simple_bag_reader')
+        self.reader = rosbag2_py.SequentialReader()
+        storage_options = rosbag2_py._storage.StorageOptions(
+            uri=bag_filename,
+            storage_id='sqlite3')
+        converter_options = rosbag2_py._storage.ConverterOptions('', '')
+        self.reader.open(storage_options, converter_options)
+
+        self.publisher = self.create_publisher(String, 'chatter', 10)
+        self.timer = self.create_timer(0.1, self.timer_callback)
+
+    def timer_callback(self):
+        while self.reader.has_next():
+            msg = self.reader.read_next()
+            if msg[0] != 'chatter':
+                continue
+            self.publisher.publish(msg[1])
+            self.get_logger().info('Publish serialized data to ' + msg[0])
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    try:
+        sbr = SimpleBagReader(sys.argv[1])
+        rclpy.spin(sbr)
+    except KeyboardInterrupt:
+        pass
+    except ExternalShutdownException:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rosbag2_examples/rosbag2_examples_py/rosbag2_examples_py/simple_bag_recorder.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 import rclpy
+from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
 from rclpy.serialization import serialize_message
 import rosbag2_py
@@ -53,9 +54,13 @@ def topic_callback(self, msg):
 
 def main(args=None):
     rclpy.init(args=args)
-    sbr = SimpleBagRecorder()
-    rclpy.spin(sbr)
-    rclpy.shutdown()
+    try:
+        sbr = SimpleBagRecorder()
+        rclpy.spin(sbr)
+    except KeyboardInterrupt:
+        pass
+    except ExternalShutdownException:
+        rclpy.shutdown()
 
 
 if __name__ == '__main__':

rosbag2_examples/rosbag2_examples_py/setup.py

@@ -21,6 +21,7 @@
     entry_points={
         'console_scripts': [
             'simple_bag_recorder = rosbag2_examples_py.simple_bag_recorder:main',
+            'simple_bag_reader = rosbag2_examples_py.simple_bag_reader:main',
             'data_generator_node = rosbag2_examples_py.data_generator_node:main',
             'data_generator_executable = rosbag2_examples_py.data_generator_executable:main',
         ],