feat: add seyond lidar driver (#15468)

* [feat]: add seyond lidar driver

* [chore]: modify default conf

* [chore]: add seyond sdk depend

* [refactor]:inherit the apollo common base class

* [refactor]:fix code style

modules/drivers/lidar/cyberfile.xml

@@ -36,6 +36,7 @@
   <depend>livox-driver</depend>
   <depend>hesai2-driver</depend>
   <depend>vanjee-driver</depend>
+  <depend>seyond-driver</depend>
 
   <depend>bazel-extend-tools</depend>
   <depend expose="False">3rd-rules-python</depend>

modules/drivers/lidar/seyond/BUILD

@@ -0,0 +1,41 @@
+load("//tools:apollo_package.bzl", "apollo_package", "apollo_cc_library", "apollo_component")
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+SEYOND_COPTS = ['-DMODULE_NAME=\\"seyond\\"']
+
+filegroup(
+    name = "runtime_data",
+    srcs = glob([
+        "conf/*.txt",
+        "conf/*.conf",
+        "dag/*.dag",
+        "launch/*.launch",
+    ]),
+)
+
+apollo_component(
+    name = "libseyond_component.so",
+    srcs = [
+        "src/seyond_component.cpp",
+        "src/seyond_driver.cpp",
+    ],
+    hdrs = [
+        "src/seyond_component.h",
+        "src/seyond_driver.h",
+    ],
+    copts = SEYOND_COPTS + ['-std=c++17'],
+    deps = [
+        "//modules/common_msgs/sensor_msgs:pointcloud_cc_proto",
+        "//modules/drivers/lidar/seyond/proto:seyond_config_proto",
+        "//modules/drivers/lidar/seyond/proto:seyond_proto",
+        "//modules/drivers/lidar/common:lidar_common",
+    ],
+    linkopts = [
+        "-linnoclientsdk",
+    ]
+)
+
+apollo_package()
+cpplint()

modules/drivers/lidar/seyond/README_cn.md

@@ -0,0 +1,37 @@
+# **Seyond Lidar Driver**
+
+## 1 工程简介
+
+ **seyond** 图达通-Apollo平台-激光雷达驱动。 支持*Falcon/Robin*系列雷达。 
+
+## 2 运行
+
+**所有驱动均需要在Apollo Docker环境下运行**
+
+```sh
+cyber_launch start /apollo/modules/drivers/lidar/seyond/launch/seyond.launch
+```
+
+或
+
+```sh
+mainboard -d /apollo/modules/drivers/lidar/seyond/dag/seyond.dag
+```
+
+默认话题名：
+
+- 点云 -- /apollo/sensor/seyond/PointCloud2"
+- Scan -- /apollo/sensor/seyond/Scan
+
+## 3 参数介绍
+| 参数                | 默认值        | 描述  |
+| :--------:         | :---------:  | :---------:   |
+| device_ip          | 172.168.1.10 | 雷达 ip   |
+| port               | 8010         | tcp 端口   |
+| udp_port           | 8010         | udp端口，若<0, 使用tcp传输   |
+| reflectance_mode   | true         | false:强度模式 true:反射模式   |
+| multiple_return    | 1            | 回波模式   |
+| coordinate_mode    | 3            | 坐标转换模式, 0: 雷达默认配置, 3:WGS-84   |
+| max_range          | 2000.0       | 最大距离限制 (单位:m)   |
+| min_range          | 0.4          | 最小距离限制 (单位:m)   |
+| log_level          | "info"       | 日志等级限制 (info warn error)    |

modules/drivers/lidar/seyond/README_en.md

@@ -0,0 +1,37 @@
+# **Seyond Lidar Driver**
+
+## 1 Introduction
+
+ **seyond** Seyond-Apollo Lidar Driver. support *Falcon/Robin* lidar.
+
+## 2 Run
+
+**All the drivers need to be excuted in Apollo docker environment.**
+
+```sh
+cyber_launch start /apollo/modules/drivers/lidar/seyond/launch/seyond.launch
+```
+
+or
+
+```sh
+mainboard -d /apollo/modules/drivers/lidar/seyond/dag/seyond.dag
+```
+
+Default Topic：
+
+- PointCloud -- /apollo/sensor/seyond/PointCloud2"
+- Scan -- /apollo/sensor/seyond/Scan
+
+## 3 Parameters Intro
+| Parameter          | Default Value | description   |
+| :--------:         | :---------:   | :---------:   |
+| device_ip          | 172.168.1.10 | lidar ip   |
+| port               | 8010         | tcp port   |
+| udp_port           | 8010         | udp port, if < 0, use tcp for transmission   |
+| reflectance_mode   | true         | false:intensiy mode true:reflectance_mode mode   |
+| multiple_return    | 1            | lidar detection echo mode   |
+| coordinate_mode    | 3            | convert the xyz direction of a point cloud, 0: lidar-default, 3:WGS-84   |
+| max_range          | 2000.0       | point cloud display maximum distance (unit:m)   |
+| min_range          | 0.4          | point cloud display minimun distance (unit:m)   |
+| log_level          | "info"       | limit log from lidar, can choose from (info warn error)    |

modules/drivers/lidar/seyond/conf/seyond.pb.txt

@@ -0,0 +1,18 @@
+config_base {
+  scan_channel: "/apollo/sensor/seyond/Scan"
+  point_cloud_channel: "/apollo/sensor/seyond/PointCloud2"
+  frame_id: "seyond"
+
+  # sample ONLINE_LIDAR, RAW_PACKET
+  source_type: ONLINE_LIDAR
+}
+
+device_ip: "172.168.1.10"
+port: 8010
+udp_port: 8010
+reflectance_mode: true
+multiple_return: 1
+coordinate_mode: 3
+max_range: 2000.0
+min_range: 0.4
+log_level: "info"

modules/drivers/lidar/seyond/dag/seyond.dag

@@ -0,0 +1,10 @@
+module_config {
+    module_library : "modules/drivers/lidar/seyond/libseyond_component.so"
+    components {
+        class_name : "SeyondComponent"
+        config {
+          name : "Seyond_Driver"
+          config_file_path : "/apollo/modules/drivers/lidar/seyond/conf/seyond.pb.txt"
+        }
+    }
+}

modules/drivers/lidar/seyond/launch/seyond.launch

@@ -0,0 +1,7 @@
+<cyber>
+    <module>
+        <name>seyond</name>
+        <dag_conf>/apollo/modules/drivers/lidar/seyond/dag/seyond.dag</dag_conf>
+        <process_name>seyond</process_name>
+    </module>
+</cyber>

modules/drivers/lidar/seyond/proto/BUILD

@@ -0,0 +1,22 @@
+load("//tools:apollo_package.bzl", "apollo_package")
+load("//tools/proto:proto.bzl", "proto_library")
+
+package(default_visibility = ["//visibility:public"])
+
+proto_library(
+    name = "seyond_config_proto",
+    srcs = ["seyond_config.proto"],
+    deps = [
+        "//modules/drivers/lidar/common/proto:lidar_config_base_proto",
+    ],
+)
+
+proto_library(
+    name = "seyond_proto",
+    srcs = ["seyond.proto"],
+    deps = [
+        "//modules/common_msgs/basic_msgs:header_proto",
+    ],
+)
+
+apollo_package()

modules/drivers/lidar/seyond/proto/seyond.proto

@@ -0,0 +1,17 @@
+syntax = "proto2";
+
+package apollo.drivers.seyond;
+
+
+
+message SeyondPacket {
+  optional bool table_exist = 1;
+  optional bytes data = 2;
+  optional bytes table = 3;
+}
+
+message SeyondScan {
+  optional uint64 timestamp = 1;
+  optional double measurement_time = 2;
+  repeated SeyondPacket packets = 3;
+}

modules/drivers/lidar/seyond/proto/seyond_config.proto

@@ -0,0 +1,21 @@
+syntax = "proto2";
+
+package apollo.drivers.seyond;
+
+import "modules/drivers/lidar/common/proto/lidar_config_base.proto";
+
+message Config {
+
+  // lidar base
+  required apollo.drivers.lidar.LidarConfigBase config_base = 1;
+
+  optional string device_ip = 2 [default = "172.168.1.10"];
+  optional uint32 port = 3 [default = 8010];
+  optional int32 udp_port = 4 [default = 8010];
+  optional bool reflectance_mode = 5 [default = false];
+  optional uint32 multiple_return = 6 [default = 0];
+  optional uint32 coordinate_mode = 7 [default = 3];
+  optional double max_range = 8 [default = 2000.0];
+  optional double min_range = 9 [default = 0.4];
+  optional string log_level = 10 [default = "info"];
+}

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

@@ -0,0 +1,132 @@
+/******************************************************************************
+ * Copyright 2024 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/drivers/lidar/seyond/src/seyond_component.h"
+
+namespace apollo {
+namespace drivers {
+namespace lidar {
+
+bool SeyondComponent::Init() {
+  if (!GetProtoConfig(&conf_)) {
+    AERROR << "load config error, file:" << config_file_path_;
+    return false;
+  }
+
+  this->InitBase(conf_.config_base());
+
+  driver_ptr_ = std::make_shared<SeyondDriver>();
+
+  SeyondParam driver_param;
+  driver_param.device_ip = conf_.device_ip();
+  driver_param.port = conf_.port();
+  driver_param.udp_port = conf_.udp_port();
+  driver_param.reflectance_mode = conf_.reflectance_mode();
+  driver_param.multiple_return = conf_.multiple_return();
+  driver_param.coordinate_mode = conf_.coordinate_mode();
+  driver_param.max_range = conf_.max_range();
+  driver_param.min_range = conf_.min_range();
+  driver_param.log_level = conf_.log_level();
+
+  if (conf_.config_base().source_type() ==
+      LidarConfigBase_SourceType_RAW_PACKET) {
+    driver_param.raw_packets_mode = true;
+  }
+
+  driver_ptr_->register_log_callback(&SeyondComponent::SeyondLogCallback);
+
+  driver_ptr_->register_publish_packet_callback(
+        std::bind(&SeyondComponent::SeyondPacketCallback, this,
+                  std::placeholders::_1, std::placeholders::_2));
+
+  driver_ptr_->register_publish_point_callback(
+      std::bind(&SeyondComponent::SeyondCloudCallback, this,
+                std::placeholders::_1),
+      std::bind(&SeyondComponent::SeyondCloudAllocateCallback, this));
+
+  driver_param.print();
+
+  scan_packets_ptr_ = std::make_shared<seyond::SeyondScan>();
+
+  if (!driver_ptr_->init(driver_param)) {
+    AERROR << "seyond Driver init failed";
+    return false;
+  }
+
+  if (!driver_ptr_->start()) {
+    AERROR << "seyond Driver start failed";
+    return false;
+  }
+  AINFO << "seyond lidar init finished";
+  return true;
+}
+
+void SeyondComponent::ReadScanCallback(
+  const std::shared_ptr<seyond::SeyondScan>& scan_message) {
+  driver_ptr_->process_scan_packet_(scan_message);
+}
+
+void SeyondComponent::SeyondCloudCallback(std::shared_ptr<PointCloud> cloud) {
+  WritePointCloud(cloud);
+}
+
+std::shared_ptr<PointCloud> SeyondComponent::SeyondCloudAllocateCallback() {
+  return AllocatePointCloud();
+}
+
+void SeyondComponent::SeyondPacketCallback(const InnoDataPacket *pkt,
+                                           bool is_next_frame) {
+  if (is_next_frame) {
+    frame_count_++;
+    scan_packets_ptr_->set_timestamp(
+        (static_cast<uint64_t>(pkt->common.ts_start_us)) * 1000);
+    scan_packets_ptr_->set_measurement_time(pkt->common.ts_start_us * 1e-6);
+    WriteScan(scan_packets_ptr_);
+    scan_packets_ptr_ = std::make_shared<seyond::SeyondScan>();
+  }
+  seyond::SeyondPacket *scan_packet = scan_packets_ptr_->add_packets();
+  uint64_t pkt_len = sizeof(InnoDataPacket) + pkt->item_number * pkt->item_size;
+  scan_packet->mutable_data()->assign(reinterpret_cast<const char *>(pkt),
+                                      pkt_len);
+  scan_packet->set_table_exist(false);
+  if (driver_ptr_->anglehv_table_init_ && (frame_count_ == table_send_hz_)) {
+    frame_count_ = 0;
+    scan_packet->set_table_exist(true);
+    scan_packet->mutable_table()->assign(driver_ptr_->anglehv_table_.data(),
+                                         driver_ptr_->anglehv_table_.size());
+  }
+}
+
+void SeyondComponent::SeyondLogCallback(int32_t level, const char *header,
+                                        const char *msg) {
+  switch (level) {
+    case 0:  // INNO_LOG_LEVEL_FATAL
+    case 1:  // INNO_LOG_LEVEL_CRITICAL
+    case 2:  // INNO_LOG_LEVEL_ERROR
+    case 3:  // INNO_LOG_LEVEL_TEMP
+      AERROR << header << " " << msg;
+      break;
+    case 4:  // INNO_LOG_LEVEL_WARNING
+    case 5:  // INNO_LOG_LEVEL_DEBUG
+      AWARN << header << " " << msg;
+      break;
+    default:
+      AINFO << header << " " << msg;
+  }
+}
+
+}  // namespace lidar
+}  // namespace drivers
+}  // namespace apollo