g3log OK

.gitignore

@@ -1,3 +1,4 @@
 /.vscode
+/.log
 /lib
 /build

CMakeLists.txt

@@ -7,6 +7,7 @@ set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 
 find_package(Ceres REQUIRED)
 find_package(PCL REQUIRED)
+find_package(g3log REQUIRED)
 
 find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
@@ -25,6 +26,7 @@ include_directories(
 	${catkin_INCLUDE_DIRS} 
   ${PCL_INCLUDE_DIRS}
   ${CERES_INCLUDE_DIRS}
+  ${G3LOG_INCLUDE_DIRS}
   src
   common
 )
@@ -35,6 +37,7 @@ catkin_package(
   INCLUDE_DIRS src common
 )
 
+
 add_subdirectory(common)
 add_subdirectory(src)
 
@@ -43,5 +46,7 @@ target_link_libraries(oh_my_loam
   ${catkin_LIBRARIES} 
   ${PCL_LIBRARIES}
   ${CERES_LIBRARIES}
+  ${G3LOG_INCLUDE_DIRS}
+  g3log
   OhMyLoam
 )

common/common.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include "filter.h"
+#include "log.h"
+#include "tic_toc.h"
+#include "types.h"
+#include "utils.h"

common/log.h

@@ -0,0 +1,88 @@
+#pragma once
+
+#include <chrono>
+#include <g3log/g3log.hpp>
+#include <g3log/logworker.hpp>
+
+#define ADEBUG LOG(DEBUG) << "[DEBUG] "
+#define AINFO LOG(INFO)
+#define AWARN LOG(WARNING)
+#define AFATAL LOG(FATAL)
+
+// LOG_IF
+#define AINFO_IF(cond) LOG_IF(INFO, cond)
+#define AWARN_IF(cond) LOG_IF(WARNING, cond)
+#define AFATAL_IF(cond) LOG_IF(FATAL, cond)
+#define ACHECK(cond) CHECK(cond)
+
+namespace g3 {
+class CustomSink {
+ public:
+  CustomSink() = default;
+  explicit CustomSink(const std::string& log_file_name)
+      : log_file_name_(log_file_name), log_to_file_(true) {}
+  ~CustomSink() = default;
+
+  void StdLogMessage(g3::LogMessageMover logEntry) {
+    std::clog << ColorFormatedMessage(logEntry.get()) << std::endl;
+  }
+
+  void FileLogMessage(g3::LogMessageMover logEntry) {
+    static std::ofstream ofs(log_file_name_);
+    ofs << FormatedMessage(logEntry.get()) << std::endl;
+  }
+
+ private:
+  std::string log_file_name_;
+  bool log_to_file_ = false;
+
+  std::string FormatedMessage(const g3::LogMessage& msg) const {
+    std::ostringstream oss;
+    oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " "
+        << msg.file() << ":" << msg.line() << "] " << msg.message();
+    return oss.str();
+  }
+
+  std::string ColorFormatedMessage(const g3::LogMessage& msg) const {
+    std::ostringstream oss;
+    oss << "\033[" << GetColor(msg._level) << "m" << FormatedMessage(msg)
+        << "\033[m";
+    return oss.str();
+  }
+
+  int GetColor(const LEVELS& level) const {
+    if (level.value == WARNING.value) {
+      return 33;  // yellow
+    }
+    if (level.value == DEBUG.value) {
+      return 32;  // green
+    }
+    if (g3::internal::wasFatal(level)) {
+      return 31;  // red
+    }
+    return 97;  // white
+  }
+};
+
+template <bool LogToFile>
+void InitG3Logging(const std::string& prefix, const std::string& path) {
+  static std::unique_ptr<g3::LogWorker> worker;
+  if (worker != nullptr) return;
+  worker = std::move(g3::LogWorker::createLogWorker());
+  worker->addSink(std::make_unique<g3::CustomSink>(),
+                  &g3::CustomSink::StdLogMessage);
+  if (LogToFile) {
+    std::ostringstream oss;
+    oss << path;
+    if (*path.rbegin() != '/') oss << '/';
+    oss << prefix << ".";
+    auto now = std::chrono::system_clock::now();
+    oss << g3::localtime_formatted(now, "%Y%m%d-%H%M%S");
+    oss << ".log";
+    worker->addSink(std::make_unique<g3::CustomSink>(oss.str()),
+                    &g3::CustomSink::FileLogMessage);
+  }
+  g3::initializeLogging(worker.get());
+}
+
+}  // namespace g3

main.cc

@@ -4,12 +4,15 @@
 
 #include <functional>
 
+#include "log.h"
 #include "oh_my_loam.h"
 
 void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        oh_my_loam::OhMyLoam* const slam);
 
 int main(int argc, char* argv[]) {
+  g3::InitG3Logging<true>("oh_my_loam", ".log");
+
   oh_my_loam::OhMyLoam slam;
   slam.Init();
 
@@ -27,7 +30,7 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        oh_my_loam::OhMyLoam* const slam) {
   oh_my_loam::PointCloud cloud;
   pcl::fromROSMsg(*msg, cloud);
-  ROS_INFO_STREAM("Point num = " << cloud.size()
-                                 << "ts = " << msg->header.stamp.toSec());
+  AINFO << "Point num = " << cloud.size()
+        << ", ts = " << msg->header.stamp.toSec();
   slam->Run(cloud, 0.0);
 }

src/feature_extractor_VLP16.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "feature_extractor_base.h"
-#include "utils.h"
 
 namespace oh_my_loam {
 
@@ -14,12 +13,12 @@ class FeatureExtractorVLP16 : public FeaturePointsExtractor {
   int GetScanID(const Point& pt) const override final {
     static int i = 0;
     double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
-    if (i++ < 10)
-      std::cout << "OMEGA: "
-                << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
-                << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
-                << " z = " << pt.z << " "
-                << " d = " << Distance(pt) << std::endl;
+    if (i++ < 10) {
+      AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
+            << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
+            << " z = " << pt.z << " "
+            << " d = " << Distance(pt) << std::endl;
+    }
     return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
   }
 };

src/feature_extractor_base.cc

@@ -6,18 +6,20 @@
 
 namespace oh_my_loam {
 
+namespace {
 const double kPointMinDist = 0.1;
 const int kScanSegNum = 6;
 const double kTwoPi = 2 * M_PI;
 const int kMinPtsNum = 100;
+}  // namespace
 
 void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
                                      FeaturePoints* const feature) const {
   PointCloudPtr cloud(new PointCloud);
-  std::cout << "BEFORE REMOVE, num = " << cloud_in.size() << std::endl;
+  AINFO << "BEFORE REMOVE, num = " << cloud_in.size();
   RemoveNaNPoint<Point>(cloud_in, cloud.get());
   RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
-  std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl;
+  AINFO << "AFTER REMOVE, num = " << cloud->size();
   if (cloud->size() < kMinPtsNum) {
     return;
   }

src/feature_extractor_base.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "utils.h"
+#include "common.h"
 
 namespace oh_my_loam {
 

src/oh_my_loam.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "common.h"
 #include "feature_extractor_VLP16.h"
 #include "visualizer_feature_points.h"
 

src/visualizer_base.h

@@ -9,7 +9,7 @@
 #include <string>
 #include <thread>
 
-#include "types.h"
+#include "common.h"
 
 namespace oh_my_loam {