rewrite bagfile_recorder from python to cpp

.gitlab-ci-local/builds/.docker

@@ -1 +0,0 @@
-Subproject commit 997acf2a9353b0015523577b1525579590dce3fb

rbs_utils/rbs_utils/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(rbs_utils_interfaces REQUIRED)
 find_package(dynmsg REQUIRED)
 find_package(rclpy REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
 
 set(deps
   rclcpp
@@ -29,6 +30,7 @@ set(deps
   sensor_msgs
   rbs_utils_interfaces
   dynmsg
+  rosbag2_cpp
 )
 
 add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp)
@@ -53,6 +55,9 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps})
 #   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
 #   "$<INSTALL_INTERFACE:include/>")
 
+add_executable(bagfile_recorder src/bagfile_recorder.cpp)
+ament_target_dependencies(bagfile_recorder ${deps})
+
 target_include_directories(${PROJECT_NAME}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -66,6 +71,11 @@ install(
   RUNTIME DESTINATION bin
 )
 
+install(
+  TARGETS bagfile_recorder
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
 

rbs_utils/rbs_utils/src/bagfile_recorder.cpp

@@ -0,0 +1,129 @@
+#include "nlohmann/json.hpp"
+#include <algorithm>
+#include <filesystem>
+#include <memory>
+#include <nlohmann/json_fwd.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <rosbag2_cpp/writer.hpp>
+#include <rosbag2_storage/storage_options.hpp>
+#include <rosbag2_storage/topic_metadata.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <string>
+#include <vector>
+
+using rclcpp_lifecycle::LifecycleNode;
+using CallbackReturn =
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+namespace fs = std::filesystem;
+
+class RecordingDemo : public LifecycleNode {
+public:
+  explicit RecordingDemo(const rclcpp::NodeOptions &options)
+      : LifecycleNode("lc_record_demo", options), output_path_("my_bag"),
+        writer_(std::make_shared<rosbag2_cpp::Writer>()) {
+    this->declare_parameter<std::string>("lc_record_demo_cfg", "");
+
+    auto parameters = this->get_parameters({"lc_record_demo_cfg"});
+    if (!parameters.empty()) {
+      std::basic_string<char> str = parameters.at(0).as_string();
+      // RCLCPP_INFO(this->get_logger(), "!!!!%s", str.c_str());
+      auto json_cfg = nlohmann::json::parse(str);
+      auto params_cfg = json_cfg["Settings"]["output"]["params"];
+      for (auto &param : params_cfg) {
+        if (param["name"] == "output_path")
+          output_path_ = param["value"];
+      }
+    }
+  }
+
+  std::vector<std::string> get_list_topics() {
+    std::vector<std::string> topic_list;
+    for (const auto &[topic_name, topic_types] :
+         this->get_topic_names_and_types()) {
+      if (std::any_of(
+              topic_types.begin(), topic_types.end(), [](const auto &type) {
+                return std::any_of(TOPIC_TYPES.begin(), TOPIC_TYPES.end(),
+                                   [&type](const auto &valid_type) {
+                                     return type == valid_type;
+                                   });
+              })) {
+        topic_list.push_back(topic_name);
+      }
+    }
+    return topic_list;
+  }
+
+protected:
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override {
+    if (fs::exists(output_path_)) {
+      int x = 1;
+      while (fs::exists(output_path_ + std::to_string(x))) {
+        ++x;
+      }
+      fs::rename(output_path_, output_path_ + std::to_string(x));
+    }
+
+    writer_->open({output_path_, "sqlite3"}, {"", ""});
+    topics_ = get_list_topics();
+
+    for (const auto &topic_name : topics_) {
+      writer_->create_topic({topic_name, "sensor_msgs/msg/Image", "cdr", ""});
+      RCLCPP_INFO(this->get_logger(), "Configuring topic: %s",
+                  topic_name.c_str());
+    }
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State &) override {
+    for (const auto &topic_name : topics_) {
+      auto sub = this->create_subscription<sensor_msgs::msg::Image>(
+          topic_name, 10,
+          [this, topic_name](sensor_msgs::msg::Image::UniquePtr msg) {
+            writer_->write(*msg, topic_name, this->get_clock()->now());
+          });
+      subs_.emplace_back(std::move(sub));
+    }
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override {
+    subs_.clear();
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override {
+    subs_.clear();
+    writer_->close();
+    RCLCPP_INFO(this->get_logger(), "Cleanup completed.");
+    return CallbackReturn::SUCCESS;
+  }
+
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override {
+    subs_.clear();
+    RCLCPP_INFO(this->get_logger(), "Shutdown completed.");
+    return CallbackReturn::SUCCESS;
+  }
+
+private:
+  std::string output_path_;
+  std::vector<std::string> topics_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> subs_;
+  std::shared_ptr<rosbag2_cpp::Writer> writer_;
+
+  inline static const std::array<const char *, 2> TOPIC_TYPES{
+      "sensor_msgs/msg/JointState", "sensor_msgs/msg/Image"};
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<RecordingDemo>(
+      rclcpp::NodeOptions().use_intra_process_comms(true));
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node->get_node_base_interface());
+  executor.spin();
+
+  rclcpp::shutdown();
+  return 0;
+}