From 7584eee1f966df036fca20b2d524440c222b3078 Mon Sep 17 00:00:00 2001
From: feixyz10 <fei-china@139.com>
Date: Fri, 20 Nov 2020 20:07:41 +0800
Subject: [PATCH] fix bug in scanid computing

---
 .gitignore                       |  1 +
 main.cc                          |  6 ++++--
 src/extractor/extractor_VLP16.cc | 10 +---------
 3 files changed, 6 insertions(+), 11 deletions(-)

diff --git a/.gitignore b/.gitignore
index bbd868a..c453038 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 /.vscode
 /.log
+*.pcd
 /build
\ No newline at end of file
diff --git a/main.cc b/main.cc
index c07e46c..d535912 100644
--- a/main.cc
+++ b/main.cc
@@ -1,3 +1,4 @@
+#include <pcl/io/pcd_io.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -44,6 +45,7 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
                        OhMyLoam* const slam) {
   PointCloud cloud;
   pcl::fromROSMsg(*msg, cloud);
-  AINFO << "Timestamp = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
-  slam->Run(cloud, 0.0);
+  double timestamp = msg->header.stamp.toSec();
+  AINFO << "Timestamp = " << LOG_TIMESTAMP(timestamp);
+  slam->Run(cloud, timestamp);
 }
\ No newline at end of file
diff --git a/src/extractor/extractor_VLP16.cc b/src/extractor/extractor_VLP16.cc
index 74dfca8..3483219 100644
--- a/src/extractor/extractor_VLP16.cc
+++ b/src/extractor/extractor_VLP16.cc
@@ -3,15 +3,7 @@
 namespace oh_my_loam {
 
 int ExtractorVLP16::GetScanID(const Point& pt) const {
-  double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
-  // static int i = 0;
-  // if (i++ < 10) {
-  //   ADEBUG << "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;
-  // }
+  double omega = std::atan2(pt.z, std::hypot(pt.x, pt.y)) * 180 * M_1_PI + 15.0;
   return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
 };