tier4 · FelixHuang18 · Sep 27, 2023
diff --git a/README.md b/README.md
@@ -3,6 +3,7 @@
 Nebula is a sensor driver platform that is designed to provide a unified framework for as wide a variety of devices as possible.
 While it primarily targets Ethernet-based LiDAR sensors, it aims to be easily extendable to support new sensors and interfaces.
 Nebula provides the following features:
+
 - Support for Velodyne and Hesai sensors, with other LiDAR vendor support under development
 - ROS 2 interface implementations
 - TCP/IP and UDP communication implementations
@@ -14,17 +15,15 @@ Nebula provides the following features:
   - Receiving and interpretation of diagnostics information from the sensor
   - Support for multiple return modes and labelling of return types for each point
 
-
 With a rapidly increasing number of sensor types and models becoming available, and varying levels of vendor and third-party driver support, Nebula creates a centralized driver methodology. We hope that this project will be used to facilitate active collaboration and efficiency in development projects by providing a platform that reduces the need to re-implement and maintain many different sensor drivers. Contributions to extend the supported devices and features of Nebula are always welcome.
 
-
 ## How to build
 
 Nebula builds on ROS Galactic and Humble.
 
 > **Note**
 >
-> A [TCP enabled version of ROS' Transport Driver](https://github.com/MapIV/transport_drivers/tree/tcp) is required to use Nebula.
+> A [TCP enabled version of ROS&#39; Transport Driver](https://github.com/MapIV/transport_drivers/tree/tcp) is required to use Nebula.
 > It is installed automatically into your workspace using the below commands. However, if you already have ROS transport driver binaries installed, you will have to uninstall them to avoid conflicts (replace `humble` with your ROS distribution):
 > `sudo apt remove ros-humble-udp-driver ros-humble-io-context`
 
@@ -85,25 +84,31 @@ ros2 launch nebula_ros nebula_launch.py sensor_model:=Pandar64 config_file:=your
 
 Supported models, where sensor_model is the ROS param to be used at launch:
 
-| Manufacturer | Model         | sensor_model | Configuration file | Test status |
-| ------------ | ------------- | ------------ | ------------------ | ----------- |
-| HESAI        | Pandar 64     | Pandar64     | Pandar64.yaml      | :heavy_check_mark: |
-| HESAI        | Pandar 40P    | Pandar40P    | Pandar40P.yaml     | :heavy_check_mark: |
-| HESAI        | Pandar XT32   | PandarXT32   | PandarXT32.yaml    | :heavy_check_mark: |
-| HESAI        | Pandar XT32M  | PandarXT32M  | PandarXT32M.yaml   | :warning: |
-| HESAI        | Pandar QT64   | PandarQT64   | PandarQT64.yaml    | :heavy_check_mark: |
-| HESAI        | Pandar QT128  | PandarQT128  | PandarQT128.yaml   | :warning: |
-| HESAI        | Pandar AT128  | PandarAT128  | PandarAT128.yaml   | :heavy_check_mark: |
-| HESAI        | Pandar 128E4X | Pandar128E4X | Pandar128E4X.yaml  | :warning: |
-| Velodyne     | VLP-16        | VLP16        | VLP16.yaml         | :warning: |
-| Velodyne     | VLP-16-HiRes  | VLP16        |                    | :x: |
-| Velodyne     | VLP-32        | VLP32        | VLP32.yaml         | :warning: |
-| Velodyne     | VLS-128       | VLS128       | VLS128.yaml        | :warning: |
-
-Test status:\
-:heavy_check_mark:: complete\
-:warning:: some functionality yet to be tested\
-:x: : untested
+| Manufacturer | Model         | sensor_model       | Configuration file      | Test status |
+| ------------ | ------------- | ------------------ | ----------------------- | ----------- |
+| HESAI        | Pandar 64     | Pandar64           | Pandar64.yaml           | ✔️        |
+| HESAI        | Pandar 40P    | Pandar40P          | Pandar40P.yaml          | ✔️        |
+| HESAI        | Pandar XT32   | PandarXT32         | PandarXT32.yaml         | ✔️        |
+| HESAI        | Pandar XT32M  | PandarXT32M        | PandarXT32M.yaml        | ⚠️        |
+| HESAI        | Pandar QT64   | PandarQT64         | PandarQT64.yaml         | ✔️        |
+| HESAI        | Pandar QT128  | PandarQT128        | PandarQT128.yaml        | ⚠️        |
+| HESAI        | Pandar AT128  | PandarAT128        | PandarAT128.yaml        | ✔️        |
+| HESAI        | Pandar 128E4X | Pandar128E4X       | Pandar128E4X.yaml       | ⚠️        |
+| Velodyne     | VLP-16        | VLP16              | VLP16.yaml              | ⚠️        |
+| Velodyne     | VLP-16-HiRes  | VLP16              |                         | ❌          |
+| Velodyne     | VLP-32        | VLP32              | VLP32.yaml              | ⚠️        |
+| Velodyne     | VLS-128       | VLS128             | VLS128.yaml             | ⚠️        |
+| Robosense    | Helios 32     | RobosenseHelios    | RobosenseHelios.yaml    | ✔️        |
+| Robosense    | Helios 16     | RobosenseHelios16P | RobosenseHelios16P.yaml | ✔️        |
+| Robosense    | Bpearl        | RobosenseBpearl    | RobosenseBpearl.yaml    | ✔️        |
+| Robosense    | Ruby Plus     | RobosenseRubyPlus  | RobosenseRubyPlus.yaml  | ✔️        |
+| Robosense    | M1 Plus       | RobosenseM1Plus    | RobosenseM1Plus.yaml    | ✔️        |
+| Robosense    | E1            | RobosenseE1        | RobosenseE1.yaml        | ✔️        |
+
+Test status:
+✔️: complete
+⚠️: some functionality yet to be tested
+❌ : untested
 
 ## ROS parameters
 
@@ -213,20 +218,52 @@ Parameters shared by all supported models:
 
 #### Driver parameters
 
-| Parameter        | Type   | Default  | Accepted values      | Description                             |
-| ---------------- | ------ | -------- | -------------------- | --------------------------------------- |
-| frame_id         | string | velodyne |                      | ROS frame ID                            |
-| calibration_file | string |          |                      | LiDAR calibration file                  |
-| min_range        | double | 0.3      | meters, >= 0.3       | Minimum point range published           |
-| max_range        | double | 300.0    | meters, <= 300.0     | Maximum point range published           |
-| cloud_min_angle  | uint16 | 0        | degrees [0, 360]     | FoV start angle                         |
-| cloud_max_angle  | uint16 | 359      | degrees [0, 360]     | FoV end angle                           |
+| Parameter        | Type   | Default  | Accepted values  | Description                   |
+| ---------------- | ------ | -------- | ---------------- | ----------------------------- |
+| frame_id         | string | velodyne |                  | ROS frame ID                  |
+| calibration_file | string |          |                  | LiDAR calibration file        |
+| min_range        | double | 0.3      | meters, >= 0.3   | Minimum point range published |
+| max_range        | double | 300.0    | meters, <= 300.0 | Maximum point range published |
+| cloud_min_angle  | uint16 | 0        | degrees [0, 360] | FoV start angle               |
+| cloud_max_angle  | uint16 | 359      | degrees [0, 360] | FoV end angle                 |
+
+## Robosense specific parameters
+
+#### Supported return modes
+
+|  | return_mode     | Mode               |
+| - | --------------- | ------------------ |
+|  | SingleFirst     | Single (First)     |
+|  | SingleStrongest | Single (Strongest) |
+|  | SingleLast      | Single (Last)      |
+|  | Dual            | Dual               |
+
+#### Hardware interface parameters
+
+| Parameter       | Type   | Default       | Accepted values  | Description                             |
+| --------------- | ------ | ------------- | ---------------- | --------------------------------------- |
+| frame_id        | string | robosense     |                  | ROS frame ID                            |
+| sensor_ip       | string | 192.168.1.201 |                  | Sensor IP                               |
+| host_ip         | string | 192.168.1.102 |                  | Host IP                                 |
+| data_port       | uint16 | 6699          |                  | Sensor port                             |
+| difop_port      | uint16 | 7788          |                  | Device Information Output Protocol Port |
+| rotation_speed  | uint16 | 600           |                  | Rotation speed                          |
+| cloud_min_angle | uint16 | 0             | degrees [0, 360] | Fov start angle                         |
+| cloud_max_angle | uint16 | 360           | degrees [0, 360] | Fov end angle                           |
+|                 |        |               |                  |                                         |
+
+#### Driver parameters
+
+| Parameter        | Type   | Default   | Accepted values | Description            |
+| ---------------- | ------ | --------- | --------------- | ---------------------- |
+| frame_id         | string | robosense |                 | ROS frame ID           |
+| calibration_file | string |           |                 | LiDAR calibration file |
+| correction_file  | string |           |                 | LiDAR correction file  |
 
 ## Software design overview
 
 ![DriverOrganization](docs/diagram.png)
 
-
 ## How to evaluate performance
 
 You can evaluate Nebula performance on a given rosbag and sensor model using the below tools.
@@ -240,6 +277,7 @@ Run profiling for each version you want to compare:
 git checkout my_improved_branch
 ./scripts/profiling_runner.bash improved -m Pandar64 -b ~/my_rosbag -c 2 -t 20 -n 3
 ```
+
 Show results:
 
 ```bash

diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp
@@ -323,6 +323,12 @@ enum class SensorModel {
   VELODYNE_VLP32MR,
   VELODYNE_HDL32,
   VELODYNE_VLP16,
+  ROBOSENSE_HELIOS,
+  ROBOSENSE_HELIOS16P,
+  ROBOSENSE_BPEARL,
+  ROBOSENSE_RUBYPLUS,
+  ROBOSENSE_M1PLUS,
+  ROBOSENSE_E1
 };
 
 /// @brief not used?
@@ -401,6 +407,24 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel
     case SensorModel::VELODYNE_VLP16:
       os << "VLP16";
       break;
+    case SensorModel::ROBOSENSE_HELIOS:
+      os << "RobosenseHelios";
+      break;
+    case SensorModel::ROBOSENSE_HELIOS16P:
+      os << "RobosenseHelios16P";
+      break;
+    case SensorModel::ROBOSENSE_BPEARL:
+      os << "RobosenseBpearl";
+      break;
+    case SensorModel::ROBOSENSE_RUBYPLUS:
+      os << "RobosenseRubyPlus";
+      break;
+    case SensorModel::ROBOSENSE_M1PLUS:
+      os << "RobosenseM1Plus";
+      break;
+    case SensorModel::ROBOSENSE_E1:
+      os << "RobosenseE1";
+      break;
     case SensorModel::UNKNOWN:
       os << "Sensor Unknown";
       break;
@@ -468,6 +492,13 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model)
   if (sensor_model == "VLP32MR") return SensorModel::VELODYNE_VLP32MR;
   if (sensor_model == "HDL32") return SensorModel::VELODYNE_HDL32;
   if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16;
+  // Robosense
+  if (sensor_model == "RobosenseHelios") return SensorModel::ROBOSENSE_HELIOS;
+  if (sensor_model == "RobosenseHelios16P") return SensorModel::ROBOSENSE_HELIOS16P;
+  if (sensor_model == "RobosenseBpearl") return SensorModel::ROBOSENSE_BPEARL;
+  if (sensor_model == "RobosenseRubyPlus") return SensorModel::ROBOSENSE_RUBYPLUS;
+  if (sensor_model == "RobosenseM1Plus") return SensorModel::ROBOSENSE_M1PLUS;
+  if (sensor_model == "RobosenseE1") return SensorModel::ROBOSENSE_E1;
   return SensorModel::UNKNOWN;
 }
 
@@ -496,12 +527,18 @@ pcl::PointCloud<PointXYZIRADT>::Ptr convertPointXYZIRCAEDTToPointXYZIRADT(
 /// @brief Converts degrees to radians
 /// @param radians
 /// @return degrees
-static inline float deg2rad(double degrees) { return degrees * M_PI / 180.0; }
+static inline float deg2rad(double degrees)
+{
+  return degrees * M_PI / 180.0;
+}
 
 /// @brief Converts radians to degrees
 /// @param radians
 /// @return degrees
-static inline float rad2deg(double radians) { return radians * 180.0 / M_PI; }
+static inline float rad2deg(double radians)
+{
+  return radians * 180.0 / M_PI;
+}
 }  // namespace drivers
 }  // namespace nebula
 

diff --git a/nebula_common/include/nebula_common/robosense/error_code.hpp b/nebula_common/include/nebula_common/robosense/error_code.hpp
@@ -0,0 +1,164 @@
+/*********************************************************************************************************************
+Copyright (c) 2020 RoboSense
+All rights reserved
+
+By downloading, copying, installing or using the software you agree to this license. If you do not
+agree to this license, do not download, install, copy or use the software.
+
+License Agreement
+For RoboSense LiDAR SDK Library
+(3-clause BSD License)
+
+Redistribution and use in source and binary forms, with or without modification, are permitted
+provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions
+and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions
+and the following disclaimer in the documentation and/or other materials provided with the
+distribution.
+
+3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other
+contributors may be used to endorse or promote products derived from this software without specific
+prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
+IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
+IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
+THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************************************************************/
+
+#pragma once
+
+#include <ctime>
+#include <string>
+
+namespace nebular
+{
+namespace drivers
+{
+
+enum class ErrCodeType {
+  INFO_CODE,     // 0x00 ~ 0x3F
+  WARNING_CODE,  // 0x40 ~ 0x7F
+  ERROR_CODE     // 0x80 ~ 0xBF
+};
+
+enum ErrCode {
+  // info
+  ERRCODE_SUCCESS = 0x00,     ///< Normal Status
+  ERRCODE_PCAPREPEAT = 0x01,  ///< Reach file end, and play PCAP file again.
+  ERRCODE_PCAPEXIT = 0x02,    ///< Reach file end, and exit parsing PCAP file
+
+  // warning
+  ERRCODE_MSOPTIMEOUT = 0x40,  ///< Timeout (1s) of receiving MSOP Packets
+  ERRCODE_NODIFOPRECV = 0x41,  ///< Calibration data (in DIFOP packet in general) is not ready while
+                               ///< handling MOSP Packet
+  ERRCODE_WRONGMSOPLEN = 0x42,    ///< MSOP Packet length is wrong
+  ERRCODE_WRONGMSOPID = 0x43,     ///< MSOP Packet ID is wrong
+  ERRCODE_WRONGMSOPBLKID = 0x44,  ///< Block ID in MSOP Packet is wrong
+  ERRCODE_WRONGDIFOPLEN = 0x45,   ///< DIFOP Packet length is wrong
+  ERRCODE_WRONGDIFOPID = 0x46,    ///< DIFOP Packet ID is wrong
+  ERRCODE_ZEROPOINTS = 0x47,      ///< No points in PointCloud
+  ERRCODE_PKTBUFOVERFLOW = 0x48,  ///< Packet queue is overflow
+  ERRCODE_CLOUDOVERFLOW = 0x49,   ///< Point cloud buffer is overflow
+  ERRCODE_WRONGCRC32 = 0x4A,      ///< Wrong CRC32 value of MSOP Packet
+
+  // error
+  ERRCODE_STARTBEFOREINIT = 0x80,  ///< User calls start() before init()
+  ERRCODE_PCAPWRONGPATH = 0x81,    ///< Path of pcap file is wrong
+  ERRCODE_POINTCLOUDNULL = 0x82    ///< User provided PointCloud buffer is invalid
+};
+
+struct Error
+{
+  ErrCode error_code;
+  ErrCodeType error_code_type;
+
+  Error() : error_code(ErrCode::ERRCODE_SUCCESS) {}
+
+  explicit Error(const ErrCode & code) : error_code(code)
+  {
+    if (error_code < 0x40) {
+      error_code_type = ErrCodeType::INFO_CODE;
+    } else if (error_code < 0x80) {
+      error_code_type = ErrCodeType::WARNING_CODE;
+    } else {
+      error_code_type = ErrCodeType::ERROR_CODE;
+    }
+  }
+  std::string toString() const
+  {
+    switch (error_code) {
+      // info
+      case ERRCODE_PCAPREPEAT:
+        return "Info_PcapRepeat";
+      case ERRCODE_PCAPEXIT:
+        return "Info_PcapExit";
+
+      // warning
+      case ERRCODE_MSOPTIMEOUT:
+        return "ERRCODE_MSOPTIMEOUT";
+      case ERRCODE_NODIFOPRECV:
+        return "ERRCODE_NODIFOPRECV";
+      case ERRCODE_WRONGMSOPID:
+        return "ERRCODE_WRONGMSOPID";
+      case ERRCODE_WRONGMSOPLEN:
+        return "ERRCODE_WRONGMSOPLEN";
+      case ERRCODE_WRONGMSOPBLKID:
+        return "ERRCODE_WRONGMSOPBLKID";
+      case ERRCODE_WRONGDIFOPID:
+        return "ERRCODE_WRONGDIFOPID";
+      case ERRCODE_WRONGDIFOPLEN:
+        return "ERRCODE_WRONGDIFOPLEN";
+      case ERRCODE_ZEROPOINTS:
+        return "ERRCODE_ZEROPOINTS";
+      case ERRCODE_PKTBUFOVERFLOW:
+        return "ERRCODE_PKTBUFOVERFLOW";
+      case ERRCODE_CLOUDOVERFLOW:
+        return "ERRCODE_CLOUDOVERFLOW";
+      case ERRCODE_WRONGCRC32:
+        return "ERRCODE_WRONGCRC32";
+
+      // error
+      case ERRCODE_STARTBEFOREINIT:
+        return "ERRCODE_STARTBEFOREINIT";
+      case ERRCODE_PCAPWRONGPATH:
+        return "ERRCODE_PCAPWRONGPATH";
+      case ERRCODE_POINTCLOUDNULL:
+        return "ERRCODE_POINTCLOUDNULL";
+
+      // default
+      default:
+        return "ERRCODE_SUCCESS";
+    }
+  }
+};
+
+#define LIMIT_CALL(func, sec)       \
+  {                                 \
+    static time_t prev_tm = 0;      \
+    time_t cur_tm = time(NULL);     \
+    if ((cur_tm - prev_tm) > sec) { \
+      func;                         \
+      prev_tm = cur_tm;             \
+    }                               \
+  }
+
+#define DELAY_LIMIT_CALL(func, sec)     \
+  {                                     \
+    static time_t prev_tm = time(NULL); \
+    time_t cur_tm = time(NULL);         \
+    if ((cur_tm - prev_tm) > sec) {     \
+      func;                             \
+      prev_tm = cur_tm;                 \
+    }                                   \
+  }
+
+}  // namespace drivers
+}  // namespace nebular