Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,200 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <chrono>
+#include <algorithm>
+
+#include "lds.h"
+#include "comm/ldq.h"
+
+namespace livox_ros {
+
+CacheIndex Lds::cache_index_;
+
+/* Member function --------------------------------------------------------- */
+Lds::Lds(const double publish_freq, const uint8_t data_src)
+    : lidar_count_(kMaxSourceLidar),
+      pcd_semaphore_(0),
+      imu_semaphore_(0),
+      publish_freq_(publish_freq),
+      data_src_(data_src),
+      request_exit_(false) {
+  ResetLds(data_src_);
+}
+
+Lds::~Lds() {
+  lidar_count_ = 0;
+  ResetLds(0);
+  printf("lds destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
+}
+
+void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
+  //cache_index_.ResetIndex(lidar);
+  DeInitQueue(&lidar->data);
+  lidar->imu_data.Clear();
+
+  lidar->data_src = data_src;
+  lidar->connect_state = kConnectStateOff;
+}
+
+void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
+  lidar->data_src = data_src;
+}
+
+void Lds::ResetLds(uint8_t data_src) {
+  lidar_count_ = kMaxSourceLidar;
+  for (uint32_t i = 0; i < kMaxSourceLidar; i++) {
+    ResetLidar(&lidars_[i], data_src);
+  }
+}
+
+void Lds::RequestExit() {
+  request_exit_ = true;
+}
+
+bool Lds::IsAllQueueEmpty() {
+  for (int i = 0; i < lidar_count_; i++) {
+    if (!QueueIsEmpty(&lidars_[i].data)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool Lds::IsAllQueueReadStop() {
+  for (int i = 0; i < lidar_count_; i++) {
+    uint32_t data_size = QueueUsedSize(&lidars_[i].data);
+    if (data_size) {
+      return false;
+    }
+  }
+  return true;
+}
+
+void Lds::StorageImuData(ImuData* imu_data) {
+  uint32_t device_num = 0;
+  if (imu_data->lidar_type == kLivoxLidarType) {
+    device_num = imu_data->handle;
+  } else {
+    printf("Storage imu data failed, unknown lidar type:%u.\n", imu_data->lidar_type);
+    return;
+  }
+
+  uint8_t index = 0;
+  int ret = cache_index_.GetIndex(imu_data->lidar_type, device_num, index);
+  if (ret != 0) {
+    printf("Storage point data failed, can not get index, lidar type:%u, device_num:%u.\n", imu_data->lidar_type, device_num);
+    return;
+  }
+
+  LidarDevice *p_lidar = &lidars_[index];
+  LidarImuDataQueue* imu_queue = &p_lidar->imu_data;
+  imu_queue->Push(imu_data);
+  if (!imu_queue->Empty()) {
+    if (imu_semaphore_.GetCount() <= 0) {
+      imu_semaphore_.Signal();
+    }
+  }
+}
+
+void Lds::StorageLvxPointData(PointFrame* frame) {
+  if (frame == nullptr) {
+    return;
+  }
+
+  uint8_t lidar_number = frame->lidar_num;
+  for (uint i = 0; i < lidar_number; ++i) {
+    PointPacket& lidar_point = frame->lidar_point[i];
+
+    uint64_t base_time = frame->base_time[i];
+    uint8_t index = 0;
+    int8_t ret = cache_index_.LvxGetIndex(lidar_point.lidar_type, lidar_point.handle, index);
+    if (ret != 0) {
+      printf("Storage lvx point data failed, lidar type:%u, device num:%u.\n", lidar_point.lidar_type, lidar_point.handle);
+      continue;
+    }
+
+    lidars_[index].connect_state = kConnectStateSampling;
+
+    PushLidarData(&lidar_point, index, base_time);
+  }
+}
+
+void Lds::StoragePointData(PointFrame* frame) {
+  if (frame == nullptr) {
+    return;
+  }
+
+  uint8_t lidar_number = frame->lidar_num;
+  for (uint i = 0; i < lidar_number; ++i) {
+    PointPacket& lidar_point = frame->lidar_point[i];
+    //printf("StoragePointData, lidar_type:%u, point_num:%lu.\n", lidar_point.lidar_type, lidar_point.points_num);
+
+    uint64_t base_time = frame->base_time[i];
+
+    uint8_t index = 0;
+    int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index);
+    if (ret != 0) {
+      printf("Storage point data failed, lidar type:%u, handle:%u.\n", lidar_point.lidar_type, lidar_point.handle);
+      continue;
+    }
+    PushLidarData(&lidar_point, index, base_time);
+  }
+}
+
+void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time) {
+  if (lidar_data == nullptr) {
+    return;
+  }
+
+  LidarDevice *p_lidar = &lidars_[index];
+  LidarDataQueue *queue = &p_lidar->data;
+
+  if (nullptr == queue->storage_packet) {
+    uint32_t queue_size = CalculatePacketQueueSize(publish_freq_);
+    InitQueue(queue, queue_size);
+    printf("Lidar[%u] storage queue size: %u\n", index, queue_size);
+  }
+
+  if (!QueueIsFull(queue)) {
+    QueuePushAny(queue, (uint8_t *)lidar_data, base_time);
+    if (!QueueIsEmpty(queue)) {
+      if (pcd_semaphore_.GetCount() <= 0) {
+        pcd_semaphore_.Signal();
+      }
+    }
+  } else {
+    if (pcd_semaphore_.GetCount() <= 0) {
+        pcd_semaphore_.Signal();
+    }
+  }
+}
+
+void Lds::PrepareExit(void) {}
+
+}  // namespace livox_ros