feat: add SDK logging capabilities to tools and utilities

orbbec_camera/src/utils.cpp

@@ -15,13 +15,74 @@
  *******************************************************************************/
 
 #include <algorithm>
+#include <chrono>
+#include <cstdlib>
+#include <filesystem>
+#include <iomanip>
 #include <mutex>
 #include <regex>
+#include <sstream>
 #include <vector>
 #include "orbbec_camera/utils.h"
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 #include "orbbec_camera/constants.h"
 namespace orbbec_camera {
+OBLogSeverity obLogSeverityFromString(const std::string_view &log_level) {
+  if (log_level == "debug") {
+    return OBLogSeverity::OB_LOG_SEVERITY_DEBUG;
+  }
+  if (log_level == "info") {
+    return OBLogSeverity::OB_LOG_SEVERITY_INFO;
+  }
+  if (log_level == "warn" || log_level == "warning") {
+    return OBLogSeverity::OB_LOG_SEVERITY_WARN;
+  }
+  if (log_level == "error") {
+    return OBLogSeverity::OB_LOG_SEVERITY_ERROR;
+  }
+  if (log_level == "fatal") {
+    return OBLogSeverity::OB_LOG_SEVERITY_FATAL;
+  }
+  return OBLogSeverity::OB_LOG_SEVERITY_OFF;
+}
+
+std::string getRosLogDirectory() {
+  const char *home = std::getenv("HOME");
+  const std::filesystem::path home_dir = home != nullptr ? home : "";
+  return (home_dir / ".ros" / "Log").string();
+}
+
+std::string configureObSdkLoggerForTool(const std::string &tool_name,
+                                        const std::string &log_level) {
+  const auto severity = obLogSeverityFromString(log_level);
+  ob::Context::setLoggerSeverity(severity);
+  ob::Context::setLoggerToConsole(OBLogSeverity::OB_LOG_SEVERITY_OFF);
+
+  if (severity == OBLogSeverity::OB_LOG_SEVERITY_OFF) {
+    ob::Context::setLoggerToFile(OBLogSeverity::OB_LOG_SEVERITY_OFF, "");
+    return "";
+  }
+
+  const std::filesystem::path log_dir(getRosLogDirectory());
+  std::filesystem::create_directories(log_dir);
+
+  const auto now = std::chrono::system_clock::now();
+  const std::time_t now_time = std::chrono::system_clock::to_time_t(now);
+  std::tm tm{};
+#if defined(_WIN32)
+  localtime_s(&tm, &now_time);
+#else
+  localtime_r(&now_time, &tm);
+#endif
+
+  std::ostringstream file_name;
+  file_name << tool_name << "_sdk_" << std::put_time(&tm, "%Y%m%d_%H%M%S") << ".log";
+
+  ob::Context::setLoggerFileName(file_name.str());
+  ob::Context::setLoggerToFile(severity, log_dir.string().c_str());
+  return (log_dir / file_name.str()).string();
+}
+
 sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
                                                  OBCameraDistortion distortion, int width) {
   (void)width;