diff --git a/common/macros.h b/common/macros.h index f1b5bc6..48b54d9 100644 --- a/common/macros.h +++ b/common/macros.h @@ -1,8 +1,14 @@ #pragma once +#include #include #include +// #define LOG_TIMESTAMP(timestamp, precision) +// std::fixed << std::precision(precision) << timestamp; +#define LOG_TIMESTAMP(timestamp) \ + std::fixed << std::setprecision(3) << timestamp; + #define DISALLOW_COPY_AND_ASSIGN(classname) \ classname(const classname &) = delete; \ classname &operator=(const classname &) = delete; diff --git a/main.cc b/main.cc index 769ec4e..b217754 100644 --- a/main.cc +++ b/main.cc @@ -28,11 +28,6 @@ int main(int argc, char* argv[]) { if (!slam.Init()) { AFATAL << "Failed to initilize slam system."; } - ADEBUG << "DEBUG"; - AINFO << "INFO"; - AWARN << "WARN"; - AERROR << "ERROR"; - AFATAL << "FATAL"; // ros ros::init(argc, argv, "oh_my_loam"); @@ -49,7 +44,7 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, OhMyLoam* const slam) { PointCloud cloud; pcl::fromROSMsg(*msg, cloud); - AINFO << "Point num = " << cloud.size() - << ", ts = " << msg->header.stamp.toSec(); + ADEBUG << "Point num = " << cloud.size() + << ", ts = " << LOG_TIMESTAMP(msg->header.stamp.toSec()); slam->Run(cloud, 0.0); } \ No newline at end of file diff --git a/src/feature_points_extractor/base_feature_points_extractor.cc b/src/feature_points_extractor/base_feature_points_extractor.cc index 4918f16..89f9358 100644 --- a/src/feature_points_extractor/base_feature_points_extractor.cc +++ b/src/feature_points_extractor/base_feature_points_extractor.cc @@ -14,7 +14,7 @@ const int kMinPtsNum = 100; bool FeaturePointsExtractor::Init(const YAML::Node& config) { config_ = config; is_vis_ = Config::Instance()->Get("vis") && config_["vis"].as(); - AINFO << "Feature points visualization: " << (is_vis_ ? "ON" : "OFF"); + AINFO << "Feature points extraction visualizer: " << (is_vis_ ? "ON" : "OFF"); if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer); return true; } @@ -22,10 +22,9 @@ bool FeaturePointsExtractor::Init(const YAML::Node& config) { void FeaturePointsExtractor::Extract(const PointCloud& cloud_in, FeaturePoints* const feature) { PointCloudPtr cloud(new PointCloud); - AINFO << "BEFORE REMOVE, num = " << cloud_in.size(); RemoveNaNPoint(cloud_in, cloud.get()); RemoveClosedPoints(*cloud, cloud.get(), kPointMinDist); - AINFO << "AFTER REMOVE, num = " << cloud->size(); + ADEBUG << "AFTER REMOVE, point num = " << cloud->size(); if (cloud->size() < kMinPtsNum) { return; } @@ -41,7 +40,7 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in, oss << scan.size() << " "; AssignPointType(&scan); } - AWARN << oss.str(); + ADEBUG << oss.str(); for (const auto& scan : scans) { *feature->feature_pts += scan; for (const auto& pt : scan.points) { diff --git a/src/feature_points_extractor/feature_points_extractor_VLP16.cc b/src/feature_points_extractor/feature_points_extractor_VLP16.cc index 2b85322..bdbf312 100644 --- a/src/feature_points_extractor/feature_points_extractor_VLP16.cc +++ b/src/feature_points_extractor/feature_points_extractor_VLP16.cc @@ -6,10 +6,10 @@ int FeaturePointsExtractorVLP16::GetScanID(const Point& pt) const { static int i = 0; double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0; if (i++ < 10) { - AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0 - << " id = " << static_cast(std::round(omega / 2.0) + 0.01) - << " z = " << pt.z << " " - << " d = " << Distance(pt) << std::endl; + ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0 + << " id = " << static_cast(std::round(omega / 2.0) + 0.01) + << " z = " << pt.z << " " + << " d = " << Distance(pt) << std::endl; } return static_cast(std::round(omega / 2.0) + 1.e-5); };