137 feature fix seyond timestamp (#138)

* fix: Using system time when PTP is unavailable

* chore: disable gpsbin_folder

Author: daohu527

modules/drivers/gnss/conf/gnss_conf.pb.txt

@@ -48,7 +48,7 @@ tf {
 # If given, the driver will send velocity info into novatel one time per second
 wheel_parameters: "SETWHEELPARAMETERS 100 1 1\r\n"
 
-gpsbin_folder: "/apollo/data/gpsbin"
+#gpsbin_folder: "/apollo/data/gpsbin"
 
 #########################################################################
 # notice: only for debug, won't configure device through driver online!!!

modules/drivers/lidar/seyond/BUILD

@@ -33,10 +33,8 @@ cc_library(
         "src/seyond_driver.h",
     ],
     copts = SEYOND_COPTS + ["-std=c++17"],
-    # linkopts = [
-    #     "-linnoclientsdk",
-    # ],
     deps = [
+        "//cyber",
         "//modules/common_msgs/sensor_msgs:pointcloud_cc_proto",
         "//modules/drivers/lidar/common:lidar_common",
         "//modules/drivers/lidar/seyond/proto:seyond_cc_proto",

modules/drivers/lidar/seyond/src/seyond_driver.cpp

@@ -13,8 +13,11 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *****************************************************************************/
+
 #include "modules/drivers/lidar/seyond/src/seyond_driver.h"
 
+#include "cyber/time/time.h"
+
 static const uint32_t KBUF_SIZE = 1024 * 1024 * 10;
 static const double us_in_second_c = 1000000.0;
 static const double ten_us_in_second_c = 100000.0;
@@ -244,12 +247,28 @@ int32_t SeyondDriver::data_callback_(const InnoDataPacket *pkt) {
     is_next_frame = true;
   }
 
+  uint64_t lidar_raw_ns = static_cast<uint64_t>(pkt->common.ts_start_us) * 1000;
+  if (!param_.raw_packets_mode) {
+    if (lidar_raw_ns < 1577836800000000000ULL) {
+      if (ts_offset_ns_ == 0) {
+        ts_offset_ns_ =
+            apollo::cyber::Time::Now().ToNanosecond() - lidar_raw_ns;
+      }
+    } else {
+      ts_offset_ns_ = 0;
+    }
+  } else {
+    // There should be no software offset during playback of
+    // the recorded package.
+    ts_offset_ns_ = 0;
+  }
+
   packet_publish_cb_(pkt, is_next_frame);
 
   if (is_next_frame) {
-    point_cloud_ptr_->mutable_header()->set_lidar_timestamp(
-        (static_cast<uint64_t>(pkt->common.ts_start_us)) * 1000);
-    point_cloud_ptr_->set_measurement_time(pkt->common.ts_start_us * 1e-6);
+    uint64_t corrected_ns = lidar_raw_ns + ts_offset_ns_;
+    point_cloud_ptr_->mutable_header()->set_lidar_timestamp(corrected_ns);
+    point_cloud_ptr_->set_measurement_time(corrected_ns * 1e-9);
     point_cloud_ptr_->set_height(1);
     point_cloud_ptr_->set_width(frame_points_width_);
     // data publish
@@ -324,7 +343,8 @@ void SeyondDriver::convert_and_parse_(const InnoDataPacket *pkt) {
 
 int32_t SeyondDriver::process_data_packet_(const InnoDataPacket *pkt) {
   // calculate the point timestamp
-  current_ts_start_ = pkt->common.ts_start_us / us_in_second_c;
+  current_ts_start_ = (static_cast<double>(pkt->common.ts_start_us) * 1e-6) +
+                      (static_cast<double>(ts_offset_ns_) * 1e-9);
   // adapt different data structures form different lidar
   if (CHECK_EN_XYZ_POINTCLOUD_DATA(pkt->type)) {
     const InnoEnXyzPoint *pt = reinterpret_cast<const InnoEnXyzPoint *>(

modules/drivers/lidar/seyond/src/seyond_driver.h

@@ -160,6 +160,7 @@ class SeyondDriver {
   uint32_t packets_width_{0};
   int64_t current_frame_id_{-1};
   double current_ts_start_;
+  uint64_t ts_offset_ns_ = 0;
   uint64_t frame_points_width_;
   std::vector<uint8_t> convert_buffer_;
   static bool inno_logs_setted_;