Add simple bag player examples

rosbag2_examples/rosbag2_examples_cpp/CMakeLists.txt

@@ -21,12 +21,13 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
 find_package(example_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
 
 add_executable(simple_bag_recorder src/simple_bag_recorder.cpp)
 target_link_libraries(simple_bag_recorder
   rclcpp::rclcpp
   rosbag2_cpp::rosbag2_cpp
-  ${example_interfaces_TARGETS}
+  ${std_msgs_TARGETS}
 )
 
 install(TARGETS
@@ -58,6 +59,18 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+add_executable(simple_bag_player src/simple_bag_player.cpp)
+target_link_libraries(simple_bag_player
+  rclcpp::rclcpp
+  rosbag2_cpp::rosbag2_cpp
+  ${std_msgs_TARGETS}
+)
+
+install(TARGETS
+  simple_bag_player
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

rosbag2_examples/rosbag2_examples_cpp/package.xml

@@ -14,6 +14,7 @@
   <depend>rclcpp</depend>
   <depend>rosbag2_cpp</depend>
   <depend>example_interfaces</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rosbag2_examples/rosbag2_examples_cpp/src/simple_bag_player.cpp

@@ -0,0 +1,94 @@
+// Copyright 2024 Open Source Robotics Foundation
+//
+// 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 <iostream>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/serialization.hpp"
+#include "rosbag2_cpp/reader.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+class SimpleBagPlayer : public rclcpp::Node
+{
+public:
+  explicit SimpleBagPlayer(const std::string & bag_filename)
+  : Node("simple_bag_player")
+  {
+    declare_parameter("edit", false);
+    message_needs_to_be_edit_before_send_ = get_parameter("edit").as_bool();
+
+    publisher_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);
+
+    // ignore timestamp and publish at a fixed rate (10 Hz).
+    timer_ = this->create_wall_timer(
+      100ms,
+      [this]() {this->timer_callback();}
+    );
+
+    reader_.open(bag_filename);
+  }
+
+private:
+  void timer_callback()
+  {
+    while (reader_.has_next()) {
+      rosbag2_storage::SerializedBagMessageSharedPtr msg = reader_.read_next();
+
+      if (msg->topic_name != "chatter") {
+        continue;
+      }
+
+      if (message_needs_to_be_edit_before_send_) {
+        rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
+        std_msgs::msg::String::SharedPtr ros_msg = std::make_shared<std_msgs::msg::String>();
+
+        serialization_.deserialize_message(&serialized_msg, ros_msg.get());
+        ros_msg->data += "[edited]";
+        publisher_->publish(*ros_msg);
+        std::cout << ros_msg->data << "\n";
+      } else {
+        publisher_->publish(*msg->serialized_data);
+      }
+
+      break;
+    }
+  }
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+
+  rclcpp::Serialization<std_msgs::msg::String> serialization_;
+  rosbag2_cpp::Reader reader_;
+
+  bool message_needs_to_be_edit_before_send_;
+};
+
+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<SimpleBagPlayer>(argv[1]));
+  rclcpp::shutdown();
+
+  return 0;
+}

rosbag2_examples/rosbag2_examples_cpp/src/simple_bag_recorder.cpp

@@ -14,7 +14,7 @@
 
 #include <memory>
 
-#include "example_interfaces/msg/string.hpp"
+#include "std_msgs/msg/string.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "rosbag2_cpp/writer.hpp"
@@ -27,23 +27,44 @@ class SimpleBagRecorder : public rclcpp::Node
   SimpleBagRecorder()
   : Node("simple_bag_recorder")
   {
+    declare_parameter("edit", false);
+    message_needs_to_be_edit_before_write_ = get_parameter("edit").as_bool();
+
     writer_ = std::make_unique<rosbag2_cpp::Writer>();
 
     writer_->open("my_bag");
 
-    subscription_ = create_subscription<example_interfaces::msg::String>(
-      "chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));
+    if (message_needs_to_be_edit_before_write_) {
+      subscription_ = create_subscription<std_msgs::msg::String>(
+        "chatter", 10, std::bind(&SimpleBagRecorder::topic_edit_callback, this, _1));
+    } else {
+      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", "example_interfaces/msg/String", time_stamp);
+    writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);
   }
 
-  rclcpp::Subscription<example_interfaces::msg::String>::SharedPtr subscription_;
+  void topic_edit_callback(std_msgs::msg::String::UniquePtr msg) const
+  {
+    rclcpp::Time time_stamp = this->now();
+    msg->data += "[edited]";
+    auto serialized_msg = std::make_shared<rclcpp::SerializedMessage>();
+    serialization_.serialize_message(msg.get(), serialized_msg.get());
+    writer_->write(serialized_msg, "chatter", "std_msgs/msg/String", time_stamp);
+  }
+
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  rclcpp::Serialization<std_msgs::msg::String> serialization_;
   std::unique_ptr<rosbag2_cpp::Writer> writer_;
+
+  bool message_needs_to_be_edit_before_write_;
 };
 
 int main(int argc, char * argv[])

rosbag2_examples/rosbag2_examples_py/rosbag2_examples_py/simple_bag_player.py

@@ -0,0 +1,63 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# 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.node import Node
+from rclpy.serialization import deserialize_message
+import rosbag2_py
+from std_msgs.msg import String
+
+
+class SimpleBagPlayer(Node):
+
+    def __init__(self, bag_filename):
+        super().__init__('simple_bag_player')
+        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)
+        # ignore timestamp and publish at a fixed rate (10 Hz).
+        self.timer = self.create_timer(0.1, self.timer_callback)
+        self.declare_parameter("edit", False)
+        self.message_needs_to_be_edit_before_send_ = self.get_parameter("edit").get_parameter_value().bool_value
+
+    def timer_callback(self):
+        while self.reader.has_next():
+            msg = self.reader.read_next()
+            if msg[0] != 'chatter':
+                continue
+
+            if self.message_needs_to_be_edit_before_send_:
+                ros_msg = deserialize_message(msg[1], String)
+                ros_msg.data += '[edited]'
+                self.publisher.publish(ros_msg)
+                self.get_logger().info(ros_msg.data)
+            else:
+                self.publisher.publish(msg[1])
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    sbr = SimpleBagPlayer(sys.argv[1])
+    rclpy.spin(sbr)
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    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_player = rosbag2_examples_py.simple_bag_player:main',
             'data_generator_node = rosbag2_examples_py.data_generator_node:main',
             'data_generator_executable = rosbag2_examples_py.data_generator_executable:main',
         ],