Feature/closed set bag (#20)

* start on closed set bag executable

* finish first draft of executable

* make unspecified model file fail check

* make config output clearer

* use cli and fix issues

* fix substr bug

* use ianvs reader

* add missing deps and new utility func

* clean up rotation behavior and console logging

* add progress bar

Author: nathanhhughes

semantic_inference/include/semantic_inference/logging.h

@@ -86,15 +86,30 @@ class LogEntry {
   std::stringstream ss_;
 };
 
-struct CoutSink : logging::LogSink {
+/**
+ * @brief Log messages to cout/cerr as appropriate
+ */
+struct CoutSink : LogSink {
   CoutSink(Level level = Level::INFO);
   virtual ~CoutSink() = default;
 
-  void dispatch(const logging::LogEntry& entry) const override;
+  void dispatch(const LogEntry& entry) const override;
 
   Level level;
 };
 
+/**
+ * @brief Forward everything to cout without log-levels or optionally prefix
+ */
+struct SimpleSink : LogSink {
+  SimpleSink(Level level = Level::INFO, bool with_prefix = false);
+  virtual ~SimpleSink() = default;
+  void dispatch(const LogEntry& entry) const override;
+
+  const Level level;
+  const bool with_prefix;
+};
+
 void setConfigUtilitiesLogger();
 
 }  // namespace logging

semantic_inference/src/logging.cpp

@@ -117,6 +117,24 @@ void CoutSink::dispatch(const logging::LogEntry& entry) const {
   }
 }
 
+SimpleSink::SimpleSink(Level level, bool with_prefix)
+    : level(level), with_prefix(with_prefix) {}
+
+void SimpleSink::dispatch(const LogEntry& entry) const {
+  if (entry.level < level) {
+    // skip ignored entries
+    return;
+  }
+
+  std::stringstream ss;
+  if (with_prefix) {
+    ss << entry.prefix();
+  }
+
+  ss << entry.message();
+  std::cout << ss.str() << std::endl;
+}
+
 struct SlogLogger : config::internal::Logger {
   void logImpl(const config::internal::Severity severity,
                const std::string& message) override {

semantic_inference/src/model.cpp

@@ -212,7 +212,7 @@ Model::Model(const ModelConfig& config)
     engine_ = buildEngineFromOnnx(model, *runtime_);
     SLOG(INFO) << "Finished building engine";
   } else {
-    SLOG(INFO) << "Loaded engine file";
+    SLOG(DEBUG) << "Loaded engine file";
   }
 
   if (!engine_) {
@@ -226,19 +226,19 @@ Model::Model(const ModelConfig& config)
     throw std::runtime_error("failed to set up trt context");
   }
 
-  SLOG(INFO) << "Execution context started";
+  SLOG(DEBUG) << "Execution context started";
 
   if (cudaStreamCreate(&stream_) != cudaSuccess) {
     SLOG(ERROR) << "Creating cuda stream failed!";
     throw std::runtime_error("failed to set up cuda stream");
   } else {
-    SLOG(INFO) << "CUDA stream started";
+    SLOG(DEBUG) << "CUDA stream started";
   }
 
   initialized_ = true;
 
   info_ = ModelInfo(*engine_);
-  SLOG(INFO) << info_;
+  SLOG(DEBUG) << info_;
   if (!info_) {
     SLOG(ERROR) << "Invalid engine for segmentation!";
     throw std::runtime_error("invalid model");

semantic_inference/src/model_config.cpp

@@ -72,6 +72,7 @@ void declare_config(ModelConfig& config) {
   field(config.color, "color");
   field(config.depth, "depth");
   // checks
+  checkCondition(!config.model_file.empty(), "model_file required");
   check<Path::Exists>(config.model_path(), "model_file");
   checkIsOneOf(config.log_severity,
                {"INTERNAL_ERROR", "ERROR", "WARNING", "INFO", "VERBOSE"},

semantic_inference_ros/CMakeLists.txt

@@ -11,12 +11,15 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(CLI11 REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(ianvs REQUIRED)
 find_package(image_geometry REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(rosbag2_transport REQUIRED)
 find_package(semantic_inference REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -68,6 +71,12 @@ rclcpp_components_register_node(
   ${PROJECT_NAME} PLUGIN semantic_inference::RGBDSegmentationNode EXECUTABLE rgbd_closed_set_node
 )
 
+add_executable(closed_set_rosbag_writer app/closed_set_rosbag_writer.cpp)
+target_link_libraries(
+  closed_set_rosbag_writer PUBLIC ${PROJECT_NAME} cv_bridge::cv_bridge ianvs::ianvs_rosbag
+                                  rosbag2_transport::rosbag2_transport CLI11::CLI11
+)
+
 install(
   TARGETS ${PROJECT_NAME}
   EXPORT ${PROJECT_NAME}-targets
@@ -77,6 +86,7 @@ install(
 install(PROGRAMS app/image_embedding_node app/open_set_node app/text_embedding_node
         DESTINATION lib/${PROJECT_NAME}
 )
+install(TARGETS closed_set_rosbag_writer RUNTIME DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}/)
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})