diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..88623d36d --- /dev/null +++ b/.clang-format @@ -0,0 +1,44 @@ +--- +# Modified from https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeCategories: + # C++ system headers + - Regex: "<[a-z_]*>" + Priority: 6 + CaseSensitive: true + # C system headers + - Regex: '<.*\.h>' + Priority: 5 + CaseSensitive: true + # Boost headers + - Regex: "boost/.*" + Priority: 4 + CaseSensitive: true + # Message headers + - Regex: ".*_msgs/.*" + Priority: 3 + CaseSensitive: true + # Other Package headers + - Regex: "<.*>" + Priority: 2 + CaseSensitive: true + # Local package headers + - Regex: '".*"' + Priority: 1 + CaseSensitive: true diff --git a/README.md b/README.md index 14ef6cd16..86353a70d 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,5 @@ -# ros2-project-template -Project template for ROS2 +# Nebula Lidar Driver + +![DriverOrganization](doc/diagram.png) + + diff --git a/build_depends.repos b/build_depends.repos new file mode 100644 index 000000000..56f46b6f7 --- /dev/null +++ b/build_depends.repos @@ -0,0 +1 @@ +repositories: diff --git a/doc/diagram.png b/doc/diagram.png new file mode 100644 index 000000000..607ae8b07 Binary files /dev/null and b/doc/diagram.png differ diff --git a/messages/hesai_msgs/CMakeLists.txt b/messages/hesai_msgs/CMakeLists.txt new file mode 100644 index 000000000..3a80c32d0 --- /dev/null +++ b/messages/hesai_msgs/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.5) +project(pandar_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/PandarPacket.msg" + "msg/PandarScan.msg" + "msg/PandarJumboPacket.msg" + DEPENDENCIES + std_msgs + ) + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif () + +ament_package() diff --git a/messages/hesai_msgs/msg/PandarJumboPacket.msg b/messages/hesai_msgs/msg/PandarJumboPacket.msg new file mode 100644 index 000000000..f3a984859 --- /dev/null +++ b/messages/hesai_msgs/msg/PandarJumboPacket.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp +uint8[] data +uint32 size \ No newline at end of file diff --git a/messages/hesai_msgs/msg/PandarPacket.msg b/messages/hesai_msgs/msg/PandarPacket.msg new file mode 100644 index 000000000..86c9d070e --- /dev/null +++ b/messages/hesai_msgs/msg/PandarPacket.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp +uint8[1500] data +uint32 size \ No newline at end of file diff --git a/messages/hesai_msgs/msg/PandarScan.msg b/messages/hesai_msgs/msg/PandarScan.msg new file mode 100644 index 000000000..e506478cb --- /dev/null +++ b/messages/hesai_msgs/msg/PandarScan.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +PandarPacket[] packets \ No newline at end of file diff --git a/messages/hesai_msgs/package.xml b/messages/hesai_msgs/package.xml new file mode 100644 index 000000000..d67e97ddc --- /dev/null +++ b/messages/hesai_msgs/package.xml @@ -0,0 +1,25 @@ + + + pandar_msgs + 0.0.0 + ROS message definition for the Heisai PandarQT/Pandar64/Pandar40P/Pandar20A/Pandar20B/Pandar40M LiDAR + sensor. + + hirakawa + Apache 2 + + ament_cmake + rosidl_default_generators + + std_msgs + rosidl_default_runti)me + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + \ No newline at end of file diff --git a/messages/livox_msgs/CMakeLists.txt b/messages/livox_msgs/CMakeLists.txt new file mode 100644 index 000000000..dd86d4b15 --- /dev/null +++ b/messages/livox_msgs/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(livox_msgs) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/LidarPacket.msg" + "msg/LidarScan.msg" + DEPENDENCIES std_msgs + DEPENDENCIES builtin_interfaces + #ADD_LINTER_TESTS +) + +#if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# ament_lint_auto_find_test_dependencies() +#endif() + +ament_package() diff --git a/messages/livox_msgs/msg/LidarPacket.msg b/messages/livox_msgs/msg/LidarPacket.msg new file mode 100644 index 000000000..4a6666885 --- /dev/null +++ b/messages/livox_msgs/msg/LidarPacket.msg @@ -0,0 +1,8 @@ +# Raw LIDAR packet. + +#time stamp +builtin_interfaces/Time stamp +uint8[] data # support for Jumbo frames +uint32 size # data[] size +uint64 time_stamp +int32 point_num diff --git a/messages/livox_msgs/msg/LidarScan.msg b/messages/livox_msgs/msg/LidarScan.msg new file mode 100644 index 000000000..0f4622237 --- /dev/null +++ b/messages/livox_msgs/msg/LidarScan.msg @@ -0,0 +1,4 @@ +# Raw LIDAR scan. + +std_msgs/Header header +LidarPacket[] packets diff --git a/messages/livox_msgs/package.xml b/messages/livox_msgs/package.xml new file mode 100644 index 000000000..503415262 --- /dev/null +++ b/messages/livox_msgs/package.xml @@ -0,0 +1,20 @@ + + + + livox_msgs + 0.1.0 + ROS message definitions for Livox Horizon. + sample.test + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_drivers/CMakeLists.txt b/nebula_drivers/CMakeLists.txt new file mode 100644 index 000000000..9ff655664 --- /dev/null +++ b/nebula_drivers/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) +project(nebula_lidar_driver) + +# Default to C++14 +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function) +endif () + +find_package(ament_cmake_auto REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(PCL REQUIRED) +find_package(pcl_conversions REQUIRED) + +ament_auto_find_build_dependencies() + +include_directories( + include + ${PCL_INCLUDE_DIRS} + ${PCL_COMMON_INCLUDE_DIRS} +) + +# HwInterfaces Lib +ament_auto_add_library(hesai_hw_interface SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_hw_interface.cpp + ) +# RosHWInterface +ament_auto_add_library(hesai_hw_ros_wrapper SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_hw_interface_ros_wrapper.cpp + ) +ament_auto_add_executable(hesai_hw_ros_wrapper_node + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_hw_interface_ros_main.cpp + ) + +# RosDriver Lib +ament_auto_add_library(hesai_driver SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_driver.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/decoders/pandar_40_decoder.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/decoders/pandar_64_decoder.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/decoders/pandar_qt_decoder.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/decoders/pandar_xt_decoder.cpp + ) + +# RosDriverNode +ament_auto_add_library(hesai_driver_ros_wrapper SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_driver_ros_wrapper.cpp + ) +ament_auto_add_executable(hesai_driver_ros_wrapper_node + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_driver_ros_main.cpp + ) + +ament_auto_package( + INSTALL_TO_SHARE + launch + calibration + config +) + diff --git a/nebula_drivers/calibration/hesai/Pandar40P.csv b/nebula_drivers/calibration/hesai/Pandar40P.csv new file mode 100644 index 000000000..6f79c18ef --- /dev/null +++ b/nebula_drivers/calibration/hesai/Pandar40P.csv @@ -0,0 +1,41 @@ +Laser id,Elevation,Azimuth +1,14.794,-1.042 +2,10.944,-1.042 +3,7.971,-1.042 +4,4.969,-1.042 +5,2.952,-1.042 +6,1.94,-1.042 +7,1.6,3.125 +8,1.263,-5.208 +9,0.925,-1.042 +10,0.587,3.125 +11,0.249,-5.208 +12,-0.088,-1.042 +13,-0.425,3.125 +14,-0.763,-5.208 +15,-1.101,-1.042 +16,-1.439,3.125 +17,-1.776,-5.208 +18,-2.116,-1.042 +19,-2.453,3.125 +20,-2.788,-5.208 +21,-3.128,-1.042 +22,-3.463,3.125 +23,-3.8,-5.208 +24,-4.138,-1.042 +25,-4.473,3.125 +26,-4.808,-5.208 +27,-5.145,-1.042 +28,-5.479,3.125 +29,-5.814,-5.208 +30,-6.149,-1.042 +31,-7.151,-1.042 +32,-8.147,-1.042 +33,-9.148,-1.042 +34,-10.133,-1.042 +35,-11.12,-1.042 +36,-12.094,-1.042 +37,-13.062,-1.042 +38,-14.018,-1.042 +39,-18.977,-1.042 +40,-24.985,-1.042 diff --git a/nebula_drivers/calibration/hesai/Pandar64.csv b/nebula_drivers/calibration/hesai/Pandar64.csv new file mode 100644 index 000000000..30f488aea --- /dev/null +++ b/nebula_drivers/calibration/hesai/Pandar64.csv @@ -0,0 +1,65 @@ +Laser id,Elevation,Azimuth +1,14.9,-1.042 +2,11.05,-1.042 +3,8.077,-1.042 +4,5.075,-1.042 +5,3.058,-1.042 +6,2.046,-1.042 +7,1.878,1.042 +8,1.706,3.125 +9,1.54,5.208 +10,1.369,-5.208 +11,1.202,-3.125 +12,1.031,-1.042 +13,0.864,1.042 +14,0.693,3.125 +15,0.526,5.208 +16,0.355,-5.208 +17,0.187,-3.125 +18,0.018,-1.042 +19,-0.151,1.042 +20,-0.319,3.125 +21,-0.49,5.208 +22,-0.657,-5.208 +23,-0.827,-3.125 +24,-0.995,-1.042 +25,-1.166,1.042 +26,-1.333,3.125 +27,-1.504,5.208 +28,-1.67,-5.208 +29,-1.842,-3.125 +30,-2.01,-1.042 +31,-2.18,1.042 +32,-2.347,3.125 +33,-2.518,5.208 +34,-2.682,-5.208 +35,-2.855,-3.125 +36,-3.022,-1.042 +37,-3.192,1.042 +38,-3.357,3.125 +39,-3.53,5.208 +40,-3.694,-5.208 +41,-3.866,-3.125 +42,-4.032,-1.042 +43,-4.203,1.042 +44,-4.367,3.125 +45,-4.54,5.208 +46,-4.702,-5.208 +47,-4.874,-3.125 +48,-5.039,-1.042 +49,-5.211,1.042 +50,-5.373,3.125 +51,-5.547,5.208 +52,-5.708,-5.208 +53,-5.88,-3.125 +54,-6.043,-1.042 +55,-7.045,-1.042 +56,-8.041,-1.042 +57,-9.042,-1.042 +58,-10.027,-1.042 +59,-11.014,-1.042 +60,-11.988,-1.042 +61,-12.956,-1.042 +62,-13.912,-1.042 +63,-18.871,-1.042 +64,-24.879,-1.042 diff --git a/nebula_drivers/calibration/hesai/PandarQT64.csv b/nebula_drivers/calibration/hesai/PandarQT64.csv new file mode 100644 index 000000000..0992156fe --- /dev/null +++ b/nebula_drivers/calibration/hesai/PandarQT64.csv @@ -0,0 +1,65 @@ +Laser id,Elevation,Azimuth +1,-52.121,8.736 +2,-49.785,8.314 +3,-47.577,7.964 +4,-45.477,7.669 +5,-43.465,7.417 +6,-41.528,7.198 +7,-39.653,7.007 +8,-37.831,6.838 +9,-36.055,6.688 +10,-34.320,6.554 +11,-32.619,6.434 +12,-30.950,6.326 +13,-29.308,6.228 +14,-27.690,6.140 +15,-26.094,6.059 +16,-24.517,5.987 +17,-22.964,-5.270 +18,-21.420,-5.216 +19,-19.889,-5.167 +20,-18.372,-5.123 +21,-16.865,-5.083 +22,-15.368,-5.047 +23,-13.880,-5.016 +24,-12.399,-4.988 +25,-10.925,-4.963 +26,-9.457,-4.942 +27,-7.994,-4.924 +28,-6.535,-4.910 +29,-5.079,-4.898 +30,-3.626,-4.889 +31,-2.175,-4.884 +32,-0.725,-4.881 +33,0.725,5.493 +34,2.175,5.496 +35,3.626,5.502 +36,5.079,5.512 +37,6.534,5.525 +38,7.993,5.541 +39,9.456,5.561 +40,10.923,5.584 +41,12.397,5.611 +42,13.877,5.642 +43,15.365,5.676 +44,16.861,5.716 +45,18.368,5.759 +46,19.885,5.808 +47,21.415,5.862 +48,22.959,5.921 +49,24.524,-5.330 +50,26.101,-5.396 +51,27.697,-5.469 +52,29.315,-5.550 +53,30.957,-5.640 +54,32.627,-5.740 +55,34.328,-5.850 +56,36.064,-5.974 +57,37.840,-6.113 +58,39.662,-6.269 +59,41.537,-6.447 +60,43.475,-6.651 +61,45.487,-6.887 +62,47.587,-7.163 +63,49.795,-7.493 +64,52.133,-7.892 diff --git a/nebula_drivers/calibration/hesai/PandarXT32.csv b/nebula_drivers/calibration/hesai/PandarXT32.csv new file mode 100644 index 000000000..ec31d4b53 --- /dev/null +++ b/nebula_drivers/calibration/hesai/PandarXT32.csv @@ -0,0 +1,33 @@ +Channel,Elevation,Azimuth +1,14.972363,0.082385 +2,13.952372,0.076529 +3,12.938632,0.078240 +4,11.931141,0.076267 +5,10.926151,0.064337 +6,9.922410,0.059942 +7,8.917419,0.049319 +8,7.917429,0.044992 +9,6.919938,0.034446 +10,5.919947,0.027658 +11,4.922456,0.020896 +12,3.924965,0.014147 +13,2.929974,0.008668 +14,1.936233,-0.000548 +15,0.944992,-0.007248 +16,-0.047500,-0.015201 +17,-1.039991,-0.025654 +18,-2.036232,-0.031126 +19,-3.028723,-0.037837 +20,-4.024964,-0.045822 +21,-5.019955,-0.061312 +22,-6.016196,-0.065570 +23,-7.013687,-0.068599 +24,-8.013678,-0.076658 +25,-9.011169,-0.088476 +26,-10.011159,-0.091580 +27,-11.006150,-0.098438 +28,-12.003641,-0.092837 +29,-12.996131,-0.102247 +30,-13.997372,-0.104231 +31,-15.004862,-0.118783 +32,-16.023602,-0.129679 \ No newline at end of file diff --git a/nebula_drivers/config/pandar64.yaml b/nebula_drivers/config/pandar64.yaml new file mode 100644 index 000000000..40990e290 --- /dev/null +++ b/nebula_drivers/config/pandar64.yaml @@ -0,0 +1,9 @@ +nebula_hesai_hw_interface: + ros__parameters: + sensor_model: "Pandar64" + device_ip: "192.168.1.201" + host_ip: "255.255.255.255" + frame_id: "pandar" + data_port: 2368 + gnss_port: 2369 + online: True \ No newline at end of file diff --git a/nebula_drivers/include/common/nebula_common.hpp b/nebula_drivers/include/common/nebula_common.hpp new file mode 100644 index 000000000..e52494ea9 --- /dev/null +++ b/nebula_drivers/include/common/nebula_common.hpp @@ -0,0 +1,240 @@ +#ifndef NEBULA_COMMON_H +#define NEBULA_COMMON_H + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +// COMMON +enum class CoordinateMode { UNKNOWN = 0, CARTESIAN, SPHERICAL, CYLINDRICAL }; + +enum class ReturnMode { + UNKNOWN = 0, + SINGLE_FIRST, + SINGLE_STRONGEST, + SINGLE_LAST, + DUAL_ONLY, + DUAL_FIRST, + DUAL_LAST, + DUAL_WEAK_FIRST, + DUAL_WEAK_LAST, + DUAL_STRONGEST_LAST, + DUAL_STRONGEST_FIRST +}; + +inline uint8_t ReturnModeToInt(const ReturnMode & mode) +{ + switch (mode) { + case ReturnMode::SINGLE_STRONGEST: + return 1; + break; + case ReturnMode::SINGLE_LAST: + return 2; + break; + case ReturnMode::DUAL_FIRST: + return 3; + break; + case ReturnMode::DUAL_LAST: + return 4; + break; + case ReturnMode::DUAL_ONLY: + return 5; + break; + case ReturnMode::SINGLE_FIRST: + return 6; + break; + case ReturnMode::DUAL_STRONGEST_FIRST: + return 7; + break; + case ReturnMode::DUAL_STRONGEST_LAST: + return 8; + break; + case ReturnMode::DUAL_WEAK_FIRST: + return 9; + break; + case ReturnMode::DUAL_WEAK_LAST: + return 10; + break; + default: + case ReturnMode::UNKNOWN: + return 0; + break; + } +} + +inline std::ostream & operator<<(std::ostream & os, nebula::drivers::ReturnMode const & arg) +{ + switch (arg) { + case ReturnMode::SINGLE_FIRST: + os << "SingleFirst"; + break; + case ReturnMode::SINGLE_STRONGEST: + os << "SingleStrongest"; + break; + case ReturnMode::SINGLE_LAST: + os << "SingleLast"; + break; + case ReturnMode::DUAL_ONLY: + os << "Dual"; + break; + case ReturnMode::DUAL_FIRST: + os << "DualFirst"; + break; + case ReturnMode::DUAL_LAST: + os << "DualLast"; + break; + case ReturnMode::DUAL_WEAK_FIRST: + os << "WeakFirst"; + break; + case ReturnMode::DUAL_WEAK_LAST: + os << "WeakLast"; + break; + case ReturnMode::DUAL_STRONGEST_LAST: + os << "StrongLast"; + break; + case ReturnMode::DUAL_STRONGEST_FIRST: + os << "StrongFirst"; + break; + case ReturnMode::UNKNOWN: + os << "Unknown"; + break; + } + return os; +} + +// SENSOR_CONFIGURATION +enum class SensorModel { + UNKNOWN = 0, + HESAI_PANDAR64, + HESAI_PANDAR40P, + HESAI_PANDAR40M, + HESAI_PANDARQT64, + HESAI_PANDARQT128, + HESAI_PANDARXT32, + HESAI_PANDAR128_V13, + HESAI_PANDAR128_V14 +}; +inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel const & arg) +{ + switch (arg) { + case SensorModel::HESAI_PANDAR64: + os << "Pandar64"; + break; + case SensorModel::HESAI_PANDAR40P: + os << "Pandar40P"; + break; + case SensorModel::HESAI_PANDAR40M: + os << "Pandar40M"; + break; + case SensorModel::HESAI_PANDARQT64: + os << "PandarQT64"; + break; + case SensorModel::HESAI_PANDARQT128: + os << "PandarQT128"; + break; + case SensorModel::HESAI_PANDARXT32: + os << "PandarXT32"; + break; + case SensorModel::HESAI_PANDAR128_V13: + os << "Pandar128_1.3"; + break; + case SensorModel::HESAI_PANDAR128_V14: + os << "Pandar128_1.4"; + break; + case SensorModel::UNKNOWN: + os << "Sensor Unknown"; + break; + } + return os; +} + +struct SensorConfigurationBase +{ + SensorModel sensor_model; + ReturnMode return_mode; + std::string host_ip; + std::string sensor_ip; + std::string frame_id; + uint16_t data_port; + uint16_t frequency_ms; + bool sensor_online; + uint16_t packet_mtu_size; +}; +inline std::ostream & operator<<( + std::ostream & os, nebula::drivers::SensorConfigurationBase const & arg) +{ + os << "SensorModel: " << arg.sensor_model << ", ReturnMode: " << arg.return_mode + << ", HostIP: " << arg.host_ip << ", SensorIP: " << arg.sensor_ip + << ", FrameID: " << arg.frame_id << ", DataPort: " << arg.data_port + << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size; + return os; +} + +// CALIBRATION_CONFIGURATION +struct CalibrationConfigurationBase +{ + std::string calibration_file; +}; + +// CLOUD_CONFIGURATION +enum class datatype { + INT8 = 1, + UINT8 = 2, + INT16 = 3, + UINT16 = 4, + INT32 = 5, + UINT32 = 6, + FLOAT32 = 7, + FLOAT64 = 8 +}; + +struct PointField +{ + std::string name; + uint32_t offset; + uint8_t datatype; + uint32_t count; +}; + +struct CloudConfigurationBase +{ + CoordinateMode coordinate_mode; + ReturnMode return_mode; + double cloud_min_range; + double cloud_max_range; + bool remove_nans; /// todo: consider changing to only_finite + std::vector fields; +}; + +inline SensorModel SensorModelFromString(const std::string & sensor_model) +{ + if (sensor_model == "Pandar64") return SensorModel::HESAI_PANDAR64; + if (sensor_model == "Pandar40P") return SensorModel::HESAI_PANDAR40P; + if (sensor_model == "Pandar40M") return SensorModel::HESAI_PANDAR40M; + if (sensor_model == "PandarXT32") return SensorModel::HESAI_PANDARXT32; + if (sensor_model == "PandarQT64") return SensorModel::HESAI_PANDARQT64; + if (sensor_model == "PandarQT128") return SensorModel::HESAI_PANDARQT128; + if (sensor_model == "Pandar128") return SensorModel::HESAI_PANDAR128_V14; + + return SensorModel::UNKNOWN; +} + +inline ReturnMode ReturnModeFromString(const std::string & return_mode) +{ + if (return_mode == "SingleFirst") return ReturnMode::SINGLE_FIRST; + if (return_mode == "SingleStrongest") return ReturnMode::SINGLE_STRONGEST; + if (return_mode == "SingleLast") return ReturnMode::SINGLE_LAST; + if (return_mode == "Dual") return ReturnMode::DUAL_ONLY; + + return ReturnMode::UNKNOWN; +} + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_CONFIGURATION_BASE_H diff --git a/nebula_drivers/include/common/nebula_driver_base.hpp b/nebula_drivers/include/common/nebula_driver_base.hpp new file mode 100644 index 000000000..c946df2f3 --- /dev/null +++ b/nebula_drivers/include/common/nebula_driver_base.hpp @@ -0,0 +1,39 @@ +#ifndef NEBULA_DRIVER_BASE_H +#define NEBULA_DRIVER_BASE_H + +#include "common/nebula_common.hpp" +#include "common/nebula_status.hpp" + +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ +class NebulaDriverBase +{ +public: + NebulaDriverBase(NebulaDriverBase && c) = delete; + NebulaDriverBase & operator=(NebulaDriverBase && c) = delete; + NebulaDriverBase(const NebulaDriverBase & c) = delete; + NebulaDriverBase & operator=(const NebulaDriverBase & c) = delete; + + NebulaDriverBase() = default; + + virtual Status SetCalibrationConfiguration( + const CalibrationConfigurationBase & calibration_configuration) = 0; + + virtual Status SetCloudConfiguration(const CloudConfigurationBase & cloud_configuration) = 0; + + // Lidar specific conversion of Packet to Pointcloud2 Msg + // Header information should be filled in by the DriverBaseWrapper + // virtual std::shared_ptr ConvertScanToPointcloud( + // PACKET_MSG) = 0; +}; + +} // namespace drivers +} // namespace nebula +#endif // NEBULA_DRIVER_BASE_H diff --git a/nebula_drivers/include/common/nebula_driver_ros_wrapper_base.hpp b/nebula_drivers/include/common/nebula_driver_ros_wrapper_base.hpp new file mode 100644 index 000000000..c07303533 --- /dev/null +++ b/nebula_drivers/include/common/nebula_driver_ros_wrapper_base.hpp @@ -0,0 +1,43 @@ +#ifndef NEBULA_DRIVER_WRAPPER_BASE_H +#define NEBULA_DRIVER_WRAPPER_BASE_H + +#include "common/nebula_common.hpp" +#include "common/nebula_status.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class NebulaDriverRosWrapperBase +{ +public: + NebulaDriverRosWrapperBase() = default; + + NebulaDriverRosWrapperBase(NebulaDriverRosWrapperBase && c) = delete; + NebulaDriverRosWrapperBase & operator=(NebulaDriverRosWrapperBase && c) = delete; + NebulaDriverRosWrapperBase(const NebulaDriverRosWrapperBase & c) = delete; + NebulaDriverRosWrapperBase & operator=(const NebulaDriverRosWrapperBase & c) = delete; + +private: + virtual Status InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr cloud_configuration, + std::shared_ptr calibration_configuration) = 0; + + // status ReceiveScanMsgCallback(void * ScanMsg); // ROS message callback for individual packet + // type + rclcpp::Publisher::SharedPtr cloud_pub_; +}; + +} // namespace ros +} // namespace nebula +#endif // NEBULA_DRIVER_WRAPPER_BASE_H diff --git a/nebula_drivers/include/common/nebula_hw_interface_base.hpp b/nebula_drivers/include/common/nebula_hw_interface_base.hpp new file mode 100644 index 000000000..92338ba82 --- /dev/null +++ b/nebula_drivers/include/common/nebula_hw_interface_base.hpp @@ -0,0 +1,51 @@ +#ifndef NEBULA_HW_INTERFACE_BASE_H +#define NEBULA_HW_INTERFACE_BASE_H + +#include "common/nebula_common.hpp" +#include "common/nebula_status.hpp" +#include "udp_driver/udp_driver.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +class NebulaHwInterfaceBase +{ +protected: + /** + * Callback function to receive the Cloud Packet data from the UDP Driver + * @param buffer buffer containing the data received from the UDP socket + * @return Status::OK if no error occured. + */ + virtual void ReceiveCloudPacketCallback(const std::vector & buffer) = 0; + // virtual Status RegisterScanCallback( + // std::function>>)> scan_callback) = 0; + +public: + NebulaHwInterfaceBase(NebulaHwInterfaceBase && c) = delete; + NebulaHwInterfaceBase & operator=(NebulaHwInterfaceBase && c) = delete; + NebulaHwInterfaceBase(const NebulaHwInterfaceBase & c) = delete; + NebulaHwInterfaceBase & operator=(const NebulaHwInterfaceBase & c) = delete; + + NebulaHwInterfaceBase() = default; + + virtual Status CloudInterfaceStart() = 0; + virtual Status CloudInterfaceStop() = 0; + // You may want to also implement GpsInterfaceStart() and ReceiveGpsCallback, but that is sensor + // specific. + + virtual Status SetSensorConfiguration( + std::shared_ptr sensor_configuration) = 0; + virtual Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) = 0; + virtual Status GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) = 0; +}; + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_HW_INTERFACE_BASE_H diff --git a/nebula_drivers/include/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_drivers/include/common/nebula_hw_interface_ros_wrapper_base.hpp new file mode 100644 index 000000000..f187cb0f0 --- /dev/null +++ b/nebula_drivers/include/common/nebula_hw_interface_ros_wrapper_base.hpp @@ -0,0 +1,39 @@ +#ifndef NEBULA_HW_INTERFACE_WRAPPER_BASE_H +#define NEBULA_HW_INTERFACE_WRAPPER_BASE_H + +#include "common/nebula_common.hpp" +#include "common/nebula_status.hpp" + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class NebulaHwInterfaceWrapperBase +{ +public: + NebulaHwInterfaceWrapperBase() = default; + + NebulaHwInterfaceWrapperBase(NebulaHwInterfaceWrapperBase && c) = delete; + NebulaHwInterfaceWrapperBase & operator=(NebulaHwInterfaceWrapperBase && c) = delete; + NebulaHwInterfaceWrapperBase(const NebulaHwInterfaceWrapperBase & c) = delete; + NebulaHwInterfaceWrapperBase & operator=(const NebulaHwInterfaceWrapperBase & c) = delete; + + virtual Status StreamStart() = 0; + virtual Status StreamStop() = 0; + virtual Status Shutdown() = 0; + +protected: + virtual Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) = 0; + // void SendDataPacket(const std::vector &buffer); // Ideally this will be + // implemented as specific funtions, GetFanStatus, GetEchoMode +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_HW_INTERFACE_WRAPPER_BASE_H diff --git a/nebula_drivers/include/common/nebula_status.hpp b/nebula_drivers/include/common/nebula_status.hpp new file mode 100644 index 000000000..079c1c873 --- /dev/null +++ b/nebula_drivers/include/common/nebula_status.hpp @@ -0,0 +1,60 @@ +#ifndef NEBULA_STATUS_HPP +#define NEBULA_STATUS_HPP + +#include +#include + +namespace nebula +{ +enum class Status { + OK = 0, + UDP_CONNECTION_ERROR, + SENSOR_CONFIG_ERROR, + INVALID_SENSOR_MODEL, + INVALID_ECHO_MODE, + NOT_IMPLEMENTED, + NOT_INITIALIZED, + INVALID_CALIBRATION_FILE, + CANNOT_SAVE_FILE, + ERROR_1 +}; + +inline std::ostream& operator<<(std::ostream& os, nebula::Status const& arg) +{ + switch (arg) { + case Status::OK: + os << "OK"; + break; + case Status::UDP_CONNECTION_ERROR: + os << "Udp Connection Error"; + break; + case Status::SENSOR_CONFIG_ERROR: + os << "Could not set SensorConfiguration"; + break; + case Status::INVALID_SENSOR_MODEL: + os << "Invalid sensor model provided"; + break; + case Status::INVALID_ECHO_MODE: + os << "Invalid echo model provided"; + break; + case Status::NOT_IMPLEMENTED: + os << "Not Implemented"; + break; + case Status::NOT_INITIALIZED: + os << "Not Initialized"; + break; + case Status::INVALID_CALIBRATION_FILE: + os << "Invalid Calibration File"; + break; + case Status::CANNOT_SAVE_FILE: + os << "Cannot Save File"; + break; + case Status::ERROR_1: + default: + os << "Generic Error"; + } + return os; +} + +} // namespace nebula +#endif // NEBULA_STATUS_HPP diff --git a/nebula_drivers/include/hesai/decoders/hesai_scan_decoder.hpp b/nebula_drivers/include/hesai/decoders/hesai_scan_decoder.hpp new file mode 100644 index 000000000..25ed8785e --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/hesai_scan_decoder.hpp @@ -0,0 +1,52 @@ +#ifndef NEBULA_WS_HESAI_SCAN_DECODER_HPP +#define NEBULA_WS_HESAI_SCAN_DECODER_HPP + +#include "hesai/hesai_common.hpp" +#include "hesai/point_types.hpp" + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +namespace nebula +{ +namespace drivers +{ +class HesaiScanDecoder +{ +protected: + drivers::PointCloudXYZIRADTPtr scan_pc_; + drivers::PointCloudXYZIRADTPtr overflow_pc_; + + uint16_t scan_phase_{}; + int last_phase_{}; + bool has_scanned_{}; + double dual_return_distance_threshold_{}; + + std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_calibration_; + std::shared_ptr cloud_calibration_; + + static inline double deg2rad(double degrees) { return degrees * M_PI / 180.0; } + +public: + HesaiScanDecoder(HesaiScanDecoder && c) = delete; + HesaiScanDecoder & operator=(HesaiScanDecoder && c) = delete; + HesaiScanDecoder(const HesaiScanDecoder & c) = delete; + HesaiScanDecoder & operator=(const HesaiScanDecoder & c) = delete; + + virtual ~HesaiScanDecoder() = default; + HesaiScanDecoder() = default; + + virtual void unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + virtual bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + + virtual drivers::PointCloudXYZIRADTPtr convert(size_t block_id) = 0; + virtual drivers::PointCloudXYZIRADTPtr convert_dual(size_t block_id) = 0; + + virtual bool hasScanned() = 0; + + virtual drivers::PointCloudXYZIRADTPtr get_pointcloud() = 0; +}; +} // namespace drivers +} // namespace nebula +#endif // NEBULA_WS_HESAI_SCAN_DECODER_HPP diff --git a/nebula_drivers/include/hesai/decoders/pandar_40.hpp b/nebula_drivers/include/hesai/decoders/pandar_40.hpp new file mode 100644 index 000000000..5d2229ea9 --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_40.hpp @@ -0,0 +1,58 @@ +#pragma once +/** + * Pandar 40P + */ +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_40 +{ +constexpr size_t SOB_ANGLE_SIZE = 4; +constexpr size_t RAW_MEASURE_SIZE = 3; +constexpr size_t LASER_COUNT = 40; +constexpr size_t BLOCKS_PER_PACKET = 10; +constexpr size_t BLOCK_SIZE = RAW_MEASURE_SIZE * LASER_COUNT + SOB_ANGLE_SIZE; +constexpr size_t TIMESTAMP_SIZE = 4; +constexpr size_t FACTORY_INFO_SIZE = 1; +constexpr size_t RETURN_SIZE = 1; +constexpr size_t RESERVE_SIZE = 8; +constexpr size_t REVOLUTION_SIZE = 2; +constexpr size_t INFO_SIZE = + TIMESTAMP_SIZE + FACTORY_INFO_SIZE + RETURN_SIZE + RESERVE_SIZE + REVOLUTION_SIZE; +constexpr size_t UTC_TIME = 6; +constexpr size_t PACKET_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET + INFO_SIZE + UTC_TIME; +constexpr size_t SEQ_NUM_SIZE = 4; +constexpr double LASER_RETURN_TO_DISTANCE_RATE = 0.004; +constexpr uint32_t STRONGEST_RETURN = 0x37; +constexpr uint32_t LAST_RETURN = 0x38; +constexpr uint32_t DUAL_RETURN = 0x39; + +struct Unit +{ + uint8_t intensity; + float distance; +}; + +struct Block +{ + uint16_t azimuth; + uint16_t sob; + Unit units[LASER_COUNT]; +}; + +struct Packet +{ + Block blocks[BLOCKS_PER_PACKET]; + struct tm t; + uint32_t usec; + uint32_t return_mode; +}; + +} // namespace pandar40 +} +} \ No newline at end of file diff --git a/nebula_drivers/include/hesai/decoders/pandar_40_decoder.hpp b/nebula_drivers/include/hesai/decoders/pandar_40_decoder.hpp new file mode 100644 index 000000000..076df738d --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_40_decoder.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_40 +{ +class Pandar40Decoder : public HesaiScanDecoder +{ +public: + explicit Pandar40Decoder(const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration); + void unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override; + bool hasScanned() override; + drivers::PointCloudXYZIRADTPtr get_pointcloud() override; + +private: + bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) override; + drivers::PointXYZIRADT build_point(size_t block_id, size_t unit_id, ReturnMode return_type); + drivers::PointCloudXYZIRADTPtr convert(size_t block_id) override; + drivers::PointCloudXYZIRADTPtr convert_dual(size_t block_id) override; + + std::array elev_angle_{}; + std::array azimuth_offset_{}; + + std::array firing_offset_{}; + std::array firing_order_{}; + + std::array block_offset_single_{}; + std::array block_offset_dual_{}; + + Packet packet_{}; + +}; + +} // namespace pandar_40 +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/include/hesai/decoders/pandar_64.hpp b/nebula_drivers/include/hesai/decoders/pandar_64.hpp new file mode 100644 index 000000000..4bedb421d --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_64.hpp @@ -0,0 +1,77 @@ +#pragma once +/** + * Pandar 64 + */ +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_64 +{ +// Head +constexpr size_t HEAD_SIZE = 8; +// Body +constexpr size_t BLOCKS_PER_PACKET = 6; +constexpr size_t BLOCK_HEADER_AZIMUTH = 2; +constexpr size_t LASER_COUNT = 64; +constexpr size_t UNIT_SIZE = 3; +constexpr size_t BLOCK_SIZE = UNIT_SIZE * LASER_COUNT + BLOCK_HEADER_AZIMUTH; +constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET; +// Tail +constexpr size_t RESERVED_SIZE = 8; +constexpr size_t HIGH_TEMPERATURE = 1; +constexpr size_t ENGINE_VELOCITY = 2; +constexpr size_t TIMESTAMP_SIZE = 4; +constexpr size_t RETURN_SIZE = 1; // echo +constexpr size_t FACTORY_SIZE = 1; +constexpr size_t UTC_SIZE = 6; +constexpr size_t PACKET_TAIL_SIZE = 26; +constexpr size_t PACKET_TAIL_WITHOUT_UDPSEQ_SIZE = 22; + +// All +constexpr size_t PACKET_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_SIZE; +constexpr size_t PACKET_WITHOUT_UDPSEQ_SIZE = + HEAD_SIZE + BODY_SIZE + PACKET_TAIL_WITHOUT_UDPSEQ_SIZE; + +constexpr uint32_t STRONGEST_RETURN = 0x37; +constexpr uint32_t LAST_RETURN = 0x38; +constexpr uint32_t DUAL_RETURN = 0x39; + +struct Header +{ + uint16_t sob; // 0xEEFF 2bytes + int8_t chLaserNumber; // laser number 1byte + int8_t chBlockNumber; // block number 1byte + int8_t chReturnType; // return mode 1 byte when dual return 0-Single Return + // 1-The first block is the 1 st return. + // 2-The first block is the 2 nd return + int8_t chDisUnit; // Distance unit, 4mm +}; + +struct Unit +{ + float distance; + uint16_t intensity; +}; + +struct Block +{ + uint16_t azimuth; // packet angle,Azimuth = RealAzimuth * 100 + Unit units[LASER_COUNT]; +}; + +struct Packet +{ + Header header; + Block blocks[BLOCKS_PER_PACKET]; + uint32_t usec; // ms + uint32_t return_mode; + tm t; +}; +} // namespace pandar_64 +} +} diff --git a/nebula_drivers/include/hesai/decoders/pandar_64_decoder.hpp b/nebula_drivers/include/hesai/decoders/pandar_64_decoder.hpp new file mode 100644 index 000000000..2b9dba782 --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_64_decoder.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_64 +{ +class Pandar64Decoder : public HesaiScanDecoder +{ +public: + explicit Pandar64Decoder(const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration); + void unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override; + bool hasScanned() override; + drivers::PointCloudXYZIRADTPtr get_pointcloud() override; + +private: + bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) override; + drivers::PointXYZIRADT build_point(size_t block_id, size_t unit_id, ReturnMode return_type); + drivers::PointCloudXYZIRADTPtr convert(size_t block_id) override; + drivers::PointCloudXYZIRADTPtr convert_dual(size_t block_id) override; + + std::array elev_angle_{}; + std::array azimuth_offset_{}; + + std::array firing_offset_{}; + + std::array block_offset_single_{}; + std::array block_offset_dual_{}; + + Packet packet_{}; + +}; + +} // namespace pandar_64 +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/include/hesai/decoders/pandar_qt.hpp b/nebula_drivers/include/hesai/decoders/pandar_qt.hpp new file mode 100644 index 000000000..c147e54ef --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_qt.hpp @@ -0,0 +1,82 @@ +#pragma once +/** + * Pandar QT + */ +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_qt +{ +// Head +constexpr size_t HEAD_SIZE = 12; +constexpr size_t PRE_HEADER_SIZE = 6; +constexpr size_t HEADER_SIZE = 6; +// Body +constexpr size_t BLOCKS_PER_PACKET = 4; +constexpr size_t BLOCK_HEADER_AZIMUTH = 2; +constexpr size_t LASER_COUNT = 64; +constexpr size_t UNIT_SIZE = 4; +constexpr size_t BLOCK_SIZE = UNIT_SIZE * LASER_COUNT + BLOCK_HEADER_AZIMUTH; +constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET; +// Tail +constexpr size_t RESERVED_SIZE = 10; +constexpr size_t ENGINE_VELOCITY = 2; +constexpr size_t TIMESTAMP_SIZE = 4; +constexpr size_t RETURN_SIZE = 1; +constexpr size_t FACTORY_SIZE = 1; +constexpr size_t UTC_SIZE = 6; +constexpr size_t SEQUENCE_SIZE = 4; +constexpr size_t PACKET_TAIL_SIZE = 28; +constexpr size_t PACKET_TAIL_WITHOUT_UDPSEQ_SIZE = 24; + +// All +constexpr size_t PACKET_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_SIZE; +constexpr size_t PACKET_WITHOUT_UDPSEQ_SIZE = + HEAD_SIZE + BODY_SIZE + PACKET_TAIL_WITHOUT_UDPSEQ_SIZE; + +constexpr uint32_t FIRST_RETURN = 0x33; +constexpr uint32_t LAST_RETURN = 0x38; +constexpr uint32_t DUAL_RETURN = 0x3B; + +struct Header +{ + uint16_t sob; // 0xFFEE 2bytes + int8_t chProtocolMajor; // Protocol Version Major 1byte + int8_t chProtocolMinor; // Protocol Version Minor 1byte + int8_t chLaserNumber; // laser number 1byte + int8_t chBlockNumber; // block number 1byte + int8_t chReturnType; // return mode 1 byte when dual return 0-Single Return + // 1-The first block is the 1 st return. + // 2-The first block is the 2 nd return + int8_t chDisUnit; // Distance unit, 4mm +}; + +struct Unit +{ + float distance; + uint16_t intensity; + uint16_t confidence; +}; + +struct Block +{ + uint16_t azimuth; // packet angle,Azimuth = RealAzimuth * 100 + Unit units[LASER_COUNT]; +}; + +struct Packet +{ + Header header; + Block blocks[BLOCKS_PER_PACKET]; + uint32_t usec; // ms + uint32_t return_mode; + tm t; +}; +} // namespace pandar_qt +} +} diff --git a/nebula_drivers/include/hesai/decoders/pandar_qt_decoder.hpp b/nebula_drivers/include/hesai/decoders/pandar_qt_decoder.hpp new file mode 100644 index 000000000..4232629db --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_qt_decoder.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_qt +{ +class PandarQTDecoder : public HesaiScanDecoder +{ +public: + explicit PandarQTDecoder(const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration); + void unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override; + bool hasScanned() override; + drivers::PointCloudXYZIRADTPtr get_pointcloud() override; + +private: + bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) override; + drivers::PointXYZIRADT build_point( + size_t block_id, size_t unit_id, drivers::ReturnMode return_type); + drivers::PointCloudXYZIRADTPtr convert(size_t block_id) override; + drivers::PointCloudXYZIRADTPtr convert_dual(size_t block_id) override; + + std::array elev_angle_{}; + std::array azimuth_offset_{}; + + std::array firing_offset_{}; + + std::array block_offset_single_{}; + std::array block_offset_dual_{}; + + Packet packet_{}; +}; + +} // namespace pandar_qt +} // namespace drivers +} // namespace nebula diff --git a/nebula_drivers/include/hesai/decoders/pandar_xt.hpp b/nebula_drivers/include/hesai/decoders/pandar_xt.hpp new file mode 100644 index 000000000..c8fbdac73 --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_xt.hpp @@ -0,0 +1,84 @@ +#pragma once +/** + * Pandar XT-32 + */ +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_xt +{ +// Head +constexpr size_t HEAD_SIZE = 12; +constexpr size_t PRE_HEADER_SIZE = 6; +constexpr size_t HEADER_SIZE = 6; +// Body +constexpr size_t BLOCKS_PER_PACKET = 8; +constexpr size_t BLOCK_HEADER_AZIMUTH = 2; +constexpr size_t LASER_COUNT = 32; +constexpr size_t UNIT_SIZE = 4; +constexpr size_t BLOCK_SIZE = UNIT_SIZE * LASER_COUNT + BLOCK_HEADER_AZIMUTH; +constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET; +// Tail +constexpr size_t RESERVED_SIZE = 10; +constexpr size_t ENGINE_VELOCITY = 2; +constexpr size_t TIMESTAMP_SIZE = 4; +constexpr size_t RETURN_SIZE = 1; +constexpr size_t FACTORY_SIZE = 1; +constexpr size_t UTC_SIZE = 6; +constexpr size_t SEQUENCE_SIZE = 4; +constexpr size_t PACKET_TAIL_SIZE = 28; + +// All +constexpr size_t PACKET_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_SIZE; + +// 0x33 - First Return 0x39 - Dual Return (Last, Strongest) +// 0x37 - Strongest Return 0x3B - Dual Return (Last, First) +// 0x38 - Last Return 0x3C - Dual Return (First, Strongest) + +constexpr uint32_t FIRST_RETURN = 0x33; +constexpr uint32_t STRONGEST_RETURN = 0x37; +constexpr uint32_t LAST_RETURN = 0x38; +constexpr uint32_t DUAL_RETURN = 0x39; + +struct Header +{ + uint16_t sob; // 0xFFEE 2bytes + int8_t chProtocolMajor; // Protocol Version Major 1byte + int8_t chProtocolMinor; // Protocol Version Minor 1byte + int8_t chLaserNumber; // laser number 1byte + int8_t chBlockNumber; // block number 1byte + int8_t chReturnType; // return mode 1 byte when dual return 0-Single Return + // 1-The first block is the 1 st return. + // 2-The first block is the 2 nd return + int8_t chDisUnit; // Distance unit, 4mm +}; + +struct Unit +{ + float distance; + uint16_t intensity; + uint16_t confidence; +}; + +struct Block +{ + uint16_t azimuth; // packet angle,Azimuth = RealAzimuth * 100 + Unit units[LASER_COUNT]; +}; + +struct Packet +{ + Header header; + Block blocks[BLOCKS_PER_PACKET]; + uint32_t usec; // ms + uint32_t return_mode; + tm t; +}; +} // namespace pandar_qt +} +} diff --git a/nebula_drivers/include/hesai/decoders/pandar_xt_decoder.hpp b/nebula_drivers/include/hesai/decoders/pandar_xt_decoder.hpp new file mode 100644 index 000000000..8fc5be9c5 --- /dev/null +++ b/nebula_drivers/include/hesai/decoders/pandar_xt_decoder.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include "hesai/decoders/pandar_xt.hpp" +#include "hesai_scan_decoder.hpp" + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_xt +{ +class PandarXTDecoder : public HesaiScanDecoder +{ +public: + explicit PandarXTDecoder(const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration); + void unpack(const pandar_msgs::msg::PandarPacket & raw_packet) override; + bool hasScanned() override; + drivers::PointCloudXYZIRADTPtr get_pointcloud() override; + +private: + + bool parsePacket(const pandar_msgs::msg::PandarPacket& pandar_packet) override; + drivers::PointXYZIRADT build_point(int block_id, int unit_id, ReturnMode return_type); + drivers::PointCloudXYZIRADTPtr convert(size_t block_id) override; + drivers::PointCloudXYZIRADTPtr convert_dual(size_t block_id) override; + + std::array elev_angle_{}; + std::array azimuth_offset_{}; + + std::array firing_offset_{}; + + std::array block_offset_single_{}; + std::array block_offset_dual_{}; + + Packet packet_{}; + +}; + +} // namespace pandar_xt +} +} \ No newline at end of file diff --git a/nebula_drivers/include/hesai/hesai_common.hpp b/nebula_drivers/include/hesai/hesai_common.hpp new file mode 100644 index 000000000..1d9d5c5a6 --- /dev/null +++ b/nebula_drivers/include/hesai/hesai_common.hpp @@ -0,0 +1,100 @@ +#ifndef NEBULA_HESAI_COMMON_H +#define NEBULA_HESAI_COMMON_H + +#include "common/nebula_common.hpp" +#include "common/nebula_status.hpp" + +#include +#include +namespace nebula +{ +namespace drivers +{ +struct HesaiSensorConfiguration : SensorConfigurationBase +{ + uint16_t gnss_port{}; + double scan_phase{}; +}; +inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration const & arg) +{ + os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port + << ", ScanPhase:" << arg.scan_phase; + return os; +} + +struct HesaiCalibrationConfiguration : CalibrationConfigurationBase +{ + std::map elev_angle_map; + std::map azimuth_offset_map; + + inline nebula::Status LoadFromFile(const std::string & calibration_file) + { + std::ifstream ifs(calibration_file); + if (!ifs) { + return Status::INVALID_CALIBRATION_FILE; + } + + std::string header; + std::getline(ifs, header); + + char sep; + int laser_id; + float elevation; + float azimuth; + while (!ifs.eof()) { + ifs >> laser_id >> sep >> elevation >> sep >> azimuth; + elev_angle_map[laser_id - 1] = elevation; + azimuth_offset_map[laser_id - 1] = azimuth; + } + ifs.close(); + return Status::OK; + } + + inline nebula::Status LoadFromString(const std::string & calibration_content) + { + std::stringstream ss; + ss << calibration_content; + + std::string header; + std::getline(ss, header); + + char sep; + int laser_id; + float elevation; + float azimuth; + while (!ss.eof()) { + ss >> laser_id >> sep >> elevation >> sep >> azimuth; + elev_angle_map[laser_id - 1] = elevation; + azimuth_offset_map[laser_id - 1] = azimuth; + } + return Status::OK; + } + + inline nebula::Status SaveFile(const std::string & calibration_file) + { + std::ofstream ofs(calibration_file); + if (!ofs) { + return Status::CANNOT_SAVE_FILE; + } + ofs << "Laser id,Elevation,Azimuth" << std::endl; + for (const auto & pair : elev_angle_map) { + auto laser_id = pair.first + 1; + float elevation = pair.second; + float azimuth = azimuth_offset_map[pair.first]; + ofs << laser_id << "," << elevation << "," << azimuth << std::endl; + } + ofs.close(); + + return Status::OK; + } +}; + +struct HesaiCloudConfiguration : CloudConfigurationBase +{ + double dual_return_distance_threshold{}; +}; + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_HESAI_COMMON_H diff --git a/nebula_drivers/include/hesai/hesai_driver.hpp b/nebula_drivers/include/hesai/hesai_driver.hpp new file mode 100644 index 000000000..5b41606f2 --- /dev/null +++ b/nebula_drivers/include/hesai/hesai_driver.hpp @@ -0,0 +1,49 @@ +#ifndef NEBULA_HESAI_DRIVER_H +#define NEBULA_HESAI_DRIVER_H + +#include "common/nebula_common.hpp" +#include "common/nebula_driver_base.hpp" +#include "common/nebula_status.hpp" +#include "hesai/decoders/hesai_scan_decoder.hpp" +#include "hesai/hesai_common.hpp" +#include "hesai/point_types.hpp" + +#include "pandar_msgs/msg/pandar_jumbo_packet.hpp" +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +class HesaiDriver : NebulaDriverBase +{ +private: + Status driver_status_; + std::shared_ptr scan_decoder_; +public: + HesaiDriver() = delete; + HesaiDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration); + Status SetCalibrationConfiguration( + const CalibrationConfigurationBase & calibration_configuration) override; + Status SetCloudConfiguration(const CloudConfigurationBase & cloud_configuration) override; + + Status GetStatus(); + + PointCloudXYZIRADTPtr ConvertScanToPointcloud( + const std::shared_ptr& pandar_scan); +}; + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_HESAI_DRIVER_H diff --git a/nebula_drivers/include/hesai/hesai_driver_ros_wrapper.hpp b/nebula_drivers/include/hesai/hesai_driver_ros_wrapper.hpp new file mode 100644 index 000000000..3397bc87a --- /dev/null +++ b/nebula_drivers/include/hesai/hesai_driver_ros_wrapper.hpp @@ -0,0 +1,59 @@ +#ifndef NEBULA_HesaiDriverRosWrapper_H +#define NEBULA_HesaiDriverRosWrapper_H + +#include "common/nebula_common.hpp" +#include "common/nebula_driver_ros_wrapper_base.hpp" +#include "common/nebula_status.hpp" +#include "hesai/hesai_common.hpp" +#include "hesai/hesai_driver.hpp" + +#include +#include +#include + +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +namespace nebula +{ +namespace ros +{ +class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +{ + std::shared_ptr driver_ptr_; + Status wrapper_status_; + rclcpp::Subscription::SharedPtr pandar_scan_sub_; + rclcpp::Publisher::SharedPtr pandar_points_pub_; + + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr cloud_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; + + Status InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr cloud_configuration, + std::shared_ptr calibration_configuration) override; + + Status GetParameters( + drivers::HesaiSensorConfiguration & sensor_configuration, + drivers::HesaiCalibrationConfiguration & calibration_configuration, + drivers::HesaiCloudConfiguration & cloud_configuration); + + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + +public: + explicit HesaiDriverRosWrapper( + const rclcpp::NodeOptions & options, const std::string & node_name); + + void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); + Status GetStatus(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_HesaiDriverRosWrapper_H diff --git a/nebula_drivers/include/hesai/hesai_hw_interface.hpp b/nebula_drivers/include/hesai/hesai_hw_interface.hpp new file mode 100644 index 000000000..b23dfeab4 --- /dev/null +++ b/nebula_drivers/include/hesai/hesai_hw_interface.hpp @@ -0,0 +1,51 @@ +#ifndef NEBULA_HESAI_HW_INTERFACE_H +#define NEBULA_HESAI_HW_INTERFACE_H + +#include "common/nebula_hw_interface_base.hpp" +#include "hesai/hesai_common.hpp" +#include "udp_driver/udp_driver.hpp" + +#include "pandar_msgs/msg/pandar_jumbo_packet.hpp" +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +namespace nebula +{ +namespace drivers +{ +class HesaiHwInterface : NebulaHwInterfaceBase +{ +private: + std::unique_ptr cloud_io_context_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; + std::shared_ptr sensor_configuration_; + std::shared_ptr calibration_configuration_; + size_t azimuth_index_{}; + size_t mtu_size_{}; + std::unique_ptr scan_cloud_ptr_; + std::function + is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ + std::function buffer)> + scan_reception_callback_; /**This function pointer is called when the scan is complete*/ + + int prev_phase_{}; + +public: + HesaiHwInterface(); + + void ReceiveCloudPacketCallback(const std::vector & buffer) final; + Status CloudInterfaceStart() final; + Status CloudInterfaceStop() final; + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) final; + Status SetSensorConfiguration( + std::shared_ptr sensor_configuration) final; + Status RegisterScanCallback( + std::function)> scan_callback); +}; + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_HESAI_HW_INTERFACE_H diff --git a/nebula_drivers/include/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_drivers/include/hesai/hesai_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..a748f0285 --- /dev/null +++ b/nebula_drivers/include/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -0,0 +1,52 @@ +#ifndef NEBULA_HesaiHwInterfaceRosWrapper_H +#define NEBULA_HesaiHwInterfaceRosWrapper_H + +#include "common/nebula_common.hpp" +#include "common/nebula_hw_interface_ros_wrapper_base.hpp" +#include "hesai/hesai_common.hpp" +#include "hesai/hesai_hw_interface.hpp" + +#include +#include +#include + +#include "pandar_msgs/msg/pandar_jumbo_packet.hpp" +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +namespace nebula +{ +namespace ros +{ +class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase +{ + drivers::HesaiHwInterface hw_interface_; + Status interface_status_; + + drivers::HesaiSensorConfiguration sensor_configuration_; + drivers::HesaiCloudConfiguration cloud_configuration_; + drivers::HesaiCalibrationConfiguration calibration_configuration_; + + rclcpp::Publisher::SharedPtr pandar_scan_pub_; + + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + +public: + explicit HesaiHwInterfaceRosWrapper( + const rclcpp::NodeOptions & options, const std::string & node_name); + + Status StreamStart() override; + Status StreamStop() override; + Status Shutdown() override; + Status GetParameters( + drivers::HesaiSensorConfiguration & sensor_configuration, + drivers::HesaiCalibrationConfiguration & calibration_configuration, + drivers::HesaiCloudConfiguration & cloud_configuration); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_HesaiHwInterfaceRosWrapper_H \ No newline at end of file diff --git a/nebula_drivers/include/hesai/point_types.hpp b/nebula_drivers/include/hesai/point_types.hpp new file mode 100644 index 000000000..77f8361a5 --- /dev/null +++ b/nebula_drivers/include/hesai/point_types.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "hesai_common.hpp" + +#include +#include + +namespace nebula +{ +namespace drivers +{ +struct PointXYZIR +{ + PCL_ADD_POINT4D; + float intensity; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +struct PointXYZIRADT +{ + PCL_ADD_POINT4D; + float intensity; + uint16_t ring; + float azimuth; + float distance; + uint8_t return_type; + double time_stamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +using PointXYZIRADTPtr = std::shared_ptr; +using PointCloudXYZIRADT = pcl::PointCloud; +using PointCloudXYZIRADTPtr = pcl::PointCloud::Ptr; + +} // namespace drivers +} // namespace nebula + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::PointXYZIR, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::PointXYZIRADT, + (float, x, + x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(double, time_stamp, time_stamp)) \ No newline at end of file diff --git a/nebula_drivers/include/livox/.container b/nebula_drivers/include/livox/.container new file mode 100644 index 000000000..e69de29bb diff --git a/nebula_drivers/include/velodyne/.container b/nebula_drivers/include/velodyne/.container new file mode 100644 index 000000000..e69de29bb diff --git a/nebula_drivers/launch/hesai.xml b/nebula_drivers/launch/hesai.xml new file mode 100644 index 000000000..cf87bcc2e --- /dev/null +++ b/nebula_drivers/launch/hesai.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nebula_drivers/package.xml b/nebula_drivers/package.xml new file mode 100644 index 000000000..b9c454998 --- /dev/null +++ b/nebula_drivers/package.xml @@ -0,0 +1,29 @@ + + + + nebula_lidar_driver + 0.1.0 + + Nebula Lidar Drivers + + Perception Engine + + Apache 2 + Tier IV + + ament_cmake_auto + + rclcpp + rclcpp_components + sensor_msgs + udp_driver + pandar_msgs + livox_msgs + pcl_conversions + pcl_msgs + libpcl-all-dev + + + ament_cmake + + diff --git a/nebula_drivers/src/hesai/decoders/pandar_40_decoder.cpp b/nebula_drivers/src/hesai/decoders/pandar_40_decoder.cpp new file mode 100644 index 000000000..772d01179 --- /dev/null +++ b/nebula_drivers/src/hesai/decoders/pandar_40_decoder.cpp @@ -0,0 +1,282 @@ +#include "hesai/decoders/pandar_40_decoder.hpp" + +#include "hesai/decoders/pandar_40.hpp" + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace pandar_40 +{ +Pandar40Decoder::Pandar40Decoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration) +{ + sensor_configuration_ = sensor_configuration; + sensor_calibration_ = calibration_configuration; + cloud_calibration_ = cloud_configuration; + + firing_order_ = {7, 19, 14, 26, 6, 18, 4, 32, 36, 0, 10, 22, 17, 29, 9, 21, 5, 33, 37, 1, + 13, 25, 20, 30, 12, 8, 24, 34, 38, 2, 16, 28, 23, 31, 15, 11, 27, 35, 39, 3}; + + firing_offset_ = {42.22, 28.47, 16.04, 3.62, 45.49, 31.74, 47.46, 54.67, 20.62, 33.71, + 40.91, 8.19, 20.62, 27.16, 50.73, 8.19, 14.74, 36.98, 45.49, 52.7, + 23.89, 31.74, 38.95, 11.47, 18.65, 25.19, 48.76, 6.23, 12.77, 35.01, + 21.92, 9.5, 43.52, 29.77, 17.35, 4.92, 42.22, 28.47, 16.04, 3.62}; + + for (size_t block = 0; block < BLOCKS_PER_PACKET; ++block) { + block_offset_single_[block] = + 55.56f * static_cast(BLOCKS_PER_PACKET - block - 1) + 28.58f; + block_offset_dual_[block] = + 55.56f * (static_cast(BLOCKS_PER_PACKET - block - 1) / 2.f) + 28.58f; + } + + // TODO: add calibration data validation + // if(calibration.elev_angle_map.size() != num_lasers_){ + // // calibration data is not valid! + // } + for (size_t laser = 0; laser < LASER_COUNT; ++laser) { + elev_angle_[laser] = calibration_configuration->elev_angle_map[laser]; + azimuth_offset_[laser] = calibration_configuration->elev_angle_map[laser]; + } + + scan_phase_ = static_cast(sensor_configuration_->scan_phase * 100.0f); + dual_return_distance_threshold_ = cloud_calibration_->dual_return_distance_threshold; + + last_phase_ = 0; + has_scanned_ = false; + + scan_pc_.reset(new PointCloudXYZIRADT); + overflow_pc_.reset(new PointCloudXYZIRADT); +} + +bool Pandar40Decoder::hasScanned() { return has_scanned_; } + +drivers::PointCloudXYZIRADTPtr Pandar40Decoder::get_pointcloud() { return scan_pc_; } + +void Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (!parsePacket(pandar_packet)) { + return; + } + + if (has_scanned_) { + scan_pc_ = overflow_pc_; + overflow_pc_.reset(new PointCloudXYZIRADT); + has_scanned_ = false; + } + + bool dual_return = (packet_.return_mode == DUAL_RETURN); + auto step = dual_return ? 2 : 1; + + if (!dual_return) { + if ( + (packet_.return_mode == STRONGEST_RETURN && + sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_STRONGEST) || + (packet_.return_mode == LAST_RETURN && + sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_LAST)) { + // sensor config, driver mismatched + } + } + + for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); + int current_phase = + (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; + if (current_phase > last_phase_ && !has_scanned_) { + *scan_pc_ += *block_pc; + } else { + *overflow_pc_ += *block_pc; + has_scanned_ = true; + } + last_phase_ = current_phase; + } +} + +drivers::PointXYZIRADT Pandar40Decoder::build_point( + size_t block_id, size_t unit_id, ReturnMode return_type) +{ + const auto & block = packet_.blocks[block_id]; + const auto & unit = block.units[unit_id]; + auto unix_second = static_cast(timegm(&packet_.t)); + bool dual_return = (packet_.return_mode == DUAL_RETURN); + PointXYZIRADT point{}; + + double xyDistance = unit.distance * cosf(deg2rad(elev_angle_[unit_id])); + + point.x = static_cast( + xyDistance * + sinf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.y = static_cast( + xyDistance * + cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.z = static_cast(unit.distance * sinf(deg2rad(elev_angle_[unit_id]))); + + point.intensity = unit.intensity; + point.distance = unit.distance; + point.ring = unit_id; + point.azimuth = block.azimuth + std::round(azimuth_offset_[unit_id] * 100.0f); + point.return_type = drivers::ReturnModeToInt(return_type); + point.time_stamp = unix_second + (static_cast(packet_.usec)) / 1000000.0; + + point.time_stamp -= + dual_return + ? (static_cast(block_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000.0f) + : (static_cast(block_offset_single_[block_id] + firing_offset_[unit_id]) / + 1000000.0f); + + return point; +} + +drivers::PointCloudXYZIRADTPtr Pandar40Decoder::convert(size_t block_id) +{ + drivers::PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + for (auto unit_id : firing_order_) { + block_pc->points.emplace_back(build_point( + block_id, unit_id, + (packet_.return_mode == STRONGEST_RETURN) ? drivers::ReturnMode::SINGLE_STRONGEST + : drivers::ReturnMode::SINGLE_LAST)); + } + return block_pc; +} + +drivers::PointCloudXYZIRADTPtr Pandar40Decoder::convert_dual(size_t block_id) +{ + // Under the Dual Return mode, the measurements from each round of firing are stored in two + // adjacent blocks: + // · The even number block is the last return, and the odd number block is the strongest return + // · If the last and strongest returns coincide, the second strongest return will be placed in the + // odd number block · The Azimuth changes every two blocks · Important note: Hesai datasheet block + // numbering starts from 0, not 1, so odd/even are reversed here + drivers::PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + size_t even_block_id = block_id; + size_t odd_block_id = block_id + 1; + const auto & even_block = packet_.blocks[even_block_id]; + const auto & odd_block = packet_.blocks[odd_block_id]; + + for (auto unit_id : firing_order_) { + const auto & even_unit = even_block.units[unit_id]; + const auto & odd_unit = odd_block.units[unit_id]; + + bool even_usable = (even_unit.distance <= 0.1 || even_unit.distance > 200.0) ? 0 : 1; + bool odd_usable = (odd_unit.distance <= 0.1 || odd_unit.distance > 200.0) ? 0 : 1; + + if (sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_STRONGEST) { + // Strongest return is in even block when both returns coincide + if (even_unit.intensity >= odd_unit.intensity && even_usable) { + block_pc->push_back( + build_point(even_block_id, unit_id, drivers::ReturnMode::SINGLE_STRONGEST)); + } else if (even_unit.intensity < odd_unit.intensity && odd_usable) { + block_pc->push_back( + build_point(odd_block_id, unit_id, drivers::ReturnMode::SINGLE_STRONGEST)); + } + } else if ( + sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_LAST && even_usable) { + // Last return is always in even block + block_pc->push_back(build_point(even_block_id, unit_id, drivers::ReturnMode::SINGLE_LAST)); + } else if (sensor_configuration_->return_mode == drivers::ReturnMode::DUAL_ONLY) { + // If the two returns are too close, only return the last one + if ( + (abs(even_unit.distance - odd_unit.distance) < dual_return_distance_threshold_) && + even_usable) { + block_pc->push_back(build_point(even_block_id, unit_id, drivers::ReturnMode::DUAL_ONLY)); + } else if (even_unit.intensity >= odd_unit.intensity) { + // Strongest return is in even block when it is also the last + if (odd_usable) { + block_pc->push_back( + build_point(odd_block_id, unit_id, drivers::ReturnMode::DUAL_WEAK_FIRST)); + } + if (even_usable) { + block_pc->push_back( + build_point(even_block_id, unit_id, drivers::ReturnMode::DUAL_STRONGEST_LAST)); + } + } else { + // Normally, strongest return is in odd block and last return is in even block + if (odd_usable) { + block_pc->push_back( + build_point(odd_block_id, unit_id, drivers::ReturnMode::DUAL_STRONGEST_FIRST)); + } + if (even_usable) { + block_pc->push_back( + build_point(even_block_id, unit_id, drivers::ReturnMode::DUAL_WEAK_LAST)); + } + } + } + } + return block_pc; +} + +bool Pandar40Decoder::parsePacket(const pandar_msgs::msg::PandarPacket & raw_packet) +{ + if (raw_packet.size != PACKET_SIZE && raw_packet.size != PACKET_SIZE + SEQ_NUM_SIZE) { + // packet size mismatch ! + return false; + } + + // auto buf = raw_packet.data; + const uint8_t * buf = &raw_packet.data[0]; + + int index = 0; + for (size_t i = 0; i < BLOCKS_PER_PACKET; i++) { + Block & block = packet_.blocks[i]; + + block.sob = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + block.azimuth = (buf[index + 2] & 0xff) | ((buf[index + 3] & 0xff) << 8); + index += SOB_ANGLE_SIZE; + + for (size_t j = 0; j < LASER_COUNT; j++) { + Unit & unit = block.units[j]; + uint32_t range = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + + unit.distance = (static_cast(range)) * LASER_RETURN_TO_DISTANCE_RATE; + unit.intensity = (buf[index + 2] & 0xff); + + // if ((unit.distance == 0x010101 && unit.intensity == 0x0101) || + // unit.distance > (200 * 1000 / 2 /* 200m -> 2mm */)) { + // unit.distance = 0; + // unit.intensity = 0; + // } + + index += RAW_MEASURE_SIZE; + } + } + + index += RESERVE_SIZE; + + index += REVOLUTION_SIZE; + + packet_.usec = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | + ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); + packet_.usec %= 1000000; + + index += TIMESTAMP_SIZE; + packet_.return_mode = buf[index] & 0xff; + + index += FACTORY_INFO_SIZE + RETURN_SIZE; + + packet_.t.tm_year = (buf[index + 0] & 0xff) + 100; + + // in case of time error + if (packet_.t.tm_year >= 200) { + packet_.t.tm_year -= 100; + } + + packet_.t.tm_mon = (buf[index + 1] & 0xff) - 1; + packet_.t.tm_mday = buf[index + 2] & 0xff; + packet_.t.tm_hour = buf[index + 3] & 0xff; + packet_.t.tm_min = buf[index + 4] & 0xff; + packet_.t.tm_sec = buf[index + 5] & 0xff; + packet_.t.tm_isdst = 0; + + return true; +} + +} // namespace pandar_40 +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/src/hesai/decoders/pandar_64_decoder.cpp b/nebula_drivers/src/hesai/decoders/pandar_64_decoder.cpp new file mode 100644 index 000000000..b4085ab0c --- /dev/null +++ b/nebula_drivers/src/hesai/decoders/pandar_64_decoder.cpp @@ -0,0 +1,270 @@ +#include "hesai/decoders/pandar_64_decoder.hpp" + +#include + +#include "hesai/decoders/pandar_64.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace pandar_64 +{ +Pandar64Decoder::Pandar64Decoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration) +{ + sensor_configuration_ = sensor_configuration; + cloud_calibration_ = cloud_configuration; + sensor_calibration_ = calibration_configuration; + + firing_offset_ = {23.18, 21.876, 20.572, 19.268, 17.964, 16.66, 11.444, 46.796, 7.532, 36.956, + 50.732, 54.668, 40.892, 44.828, 31.052, 34.988, 48.764, 52.7, 38.924, 42.86, + 29.084, 33.02, 46.796, 25.148, 36.956, 50.732, 27.116, 40.892, 44.828, 31.052, + 34.988, 48.764, 25.148, 38.924, 42.86, 29.084, 33.02, 52.7, 6.228, 54.668, + 15.356, 27.116, 10.14, 23.18, 4.924, 21.876, 14.052, 17.964, 8.836, 19.268, + 3.62, 20.572, 12.748, 16.66, 7.532, 11.444, 6.228, 15.356, 10.14, 4.924, + 3.62, 14.052, 8.836, 12.748}; + + for (size_t block = 0; block < BLOCKS_PER_PACKET; ++block) { + block_offset_single_[block] = + 55.56f * static_cast(BLOCKS_PER_PACKET - block - 1) + 28.58f; + block_offset_dual_[block] = + 55.56f * (static_cast(BLOCKS_PER_PACKET - block - 1) / 2.f) + 28.58f; + } + + // TODO: add calibration data validation + // if(calibration.elev_angle_map.size() != num_lasers_){ + // // calibration data is not valid! + // } + for (size_t laser = 0; laser < LASER_COUNT; ++laser) { + elev_angle_[laser] = calibration_configuration->elev_angle_map[laser]; + azimuth_offset_[laser] = calibration_configuration->azimuth_offset_map[laser]; + } + scan_phase_ = static_cast(sensor_configuration_->scan_phase * 100.0f); + dual_return_distance_threshold_ = cloud_calibration_->dual_return_distance_threshold; + + last_phase_ = 0; + has_scanned_ = false; + scan_pc_.reset(new PointCloudXYZIRADT); + overflow_pc_.reset(new PointCloudXYZIRADT); +} + +bool Pandar64Decoder::hasScanned() { return has_scanned_; } + +drivers::PointCloudXYZIRADTPtr Pandar64Decoder::get_pointcloud() { return scan_pc_; } + +void Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (!parsePacket(pandar_packet)) { + return; + } + + if (has_scanned_) { + scan_pc_ = overflow_pc_; + overflow_pc_.reset(new PointCloudXYZIRADT); + has_scanned_ = false; + } + + bool dual_return = (packet_.return_mode == DUAL_RETURN); + auto step = dual_return ? 2 : 1; + + if (!dual_return) { + if ( + (packet_.return_mode == STRONGEST_RETURN && + sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_STRONGEST) || + (packet_.return_mode == LAST_RETURN && + sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_LAST)) { + // sensor config, driver mismatched + } + } + for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); + int current_phase = + (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; + if (current_phase > last_phase_ && !has_scanned_) { + *scan_pc_ += *block_pc; + } else { + *overflow_pc_ += *block_pc; + has_scanned_ = true; + } + last_phase_ = current_phase; + } +} + +drivers::PointXYZIRADT Pandar64Decoder::build_point( + size_t block_id, size_t unit_id, ReturnMode return_type) +{ + const auto & block = packet_.blocks[block_id]; + const auto & unit = block.units[unit_id]; + auto unix_second = static_cast(timegm(&packet_.t)); + bool dual_return = (packet_.return_mode == DUAL_RETURN); + PointXYZIRADT point{}; + + double xyDistance = unit.distance * cosf(deg2rad(elev_angle_[unit_id])); + + point.x = static_cast( + xyDistance * + sinf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.y = static_cast( + xyDistance * + cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.z = static_cast(unit.distance * sinf(deg2rad(elev_angle_[unit_id]))); + + point.intensity = unit.intensity; + point.distance = static_cast(unit.distance); + point.ring = unit_id; + point.azimuth = static_cast(block.azimuth) + std::round(azimuth_offset_[unit_id] * 100.0f); + point.return_type = drivers::ReturnModeToInt(return_type); + point.time_stamp = unix_second + (static_cast(packet_.usec)) / 1000000.0; + point.time_stamp += + dual_return + ? (static_cast(block_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000.0f) + : (static_cast(block_offset_single_[block_id] + firing_offset_[unit_id]) / + 1000000.0f); + + return point; +} + +drivers::PointCloudXYZIRADTPtr Pandar64Decoder::convert(size_t block_id) +{ + PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + const auto & block = packet_.blocks[block_id]; + for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) { + const auto & unit = block.units[unit_id]; + // skip invalid points + if (unit.distance <= 0.1 || unit.distance > 200.0) { + continue; + } + block_pc->points.emplace_back(build_point( + block_id, unit_id, + (packet_.return_mode == STRONGEST_RETURN) ? drivers::ReturnMode::SINGLE_STRONGEST + : drivers::ReturnMode::SINGLE_LAST)); + } + return block_pc; +} + +drivers::PointCloudXYZIRADTPtr Pandar64Decoder::convert_dual(size_t block_id) +{ + // Under the Dual Return mode, the ranging data from each firing is stored in two adjacent + // blocks: + // · The even number block is the first return + // · The odd number block is the last return + // · The Azimuth changes every two blocks + // · Important note: Hesai datasheet block numbering starts from 0, not 1, so odd/even are + // reversed here + PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + size_t even_block_id = block_id; + size_t odd_block_id = block_id + 1; + const auto & even_block = packet_.blocks[even_block_id]; + const auto & odd_block = packet_.blocks[odd_block_id]; + + for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) { + const auto & even_unit = even_block.units[unit_id]; + const auto & odd_unit = odd_block.units[unit_id]; + + bool even_usable = !(even_unit.distance <= 0.1 || even_unit.distance > 200.0); + bool odd_usable = !(odd_unit.distance <= 0.1 || odd_unit.distance > 200.0); + + if ( + sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_STRONGEST && even_usable) { + // First return is in even block + block_pc->points.emplace_back( + build_point(even_block_id, unit_id, drivers::ReturnMode::SINGLE_STRONGEST)); + } else if ( + sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_LAST && even_usable) { + // Last return is in odd block + block_pc->points.emplace_back( + build_point(odd_block_id, unit_id, drivers::ReturnMode::SINGLE_LAST)); + } else if (sensor_configuration_->return_mode == drivers::ReturnMode::DUAL_ONLY) { + // If the two returns are too close, only return the last one + if ( + (abs(even_unit.distance - odd_unit.distance) < dual_return_distance_threshold_) && + odd_usable) { + block_pc->points.emplace_back( + build_point(odd_block_id, unit_id, drivers::ReturnMode::DUAL_ONLY)); + } else { + if (even_usable) { + block_pc->points.emplace_back( + build_point(even_block_id, unit_id, drivers::ReturnMode::DUAL_FIRST)); + } + if (odd_usable) { + block_pc->points.emplace_back( + build_point(odd_block_id, unit_id, drivers::ReturnMode::DUAL_LAST)); + } + } + } + } + return block_pc; +} + +bool Pandar64Decoder::parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (pandar_packet.size != PACKET_SIZE && pandar_packet.size != PACKET_WITHOUT_UDPSEQ_SIZE) { + return false; + } + const uint8_t * buf = &pandar_packet.data[0]; + + size_t index = 0; + // Parse 12 Bytes Header + packet_.header.sob = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); + packet_.header.chLaserNumber = buf[index + 2] & 0xff; + packet_.header.chBlockNumber = buf[index + 3] & 0xff; + packet_.header.chReturnType = buf[index + 4] & 0xff; + packet_.header.chDisUnit = buf[index + 5] & 0xff; + index += HEAD_SIZE; + + if (packet_.header.sob != 0xEEFF) { + // Error Start of Packet! + return false; + } + + for (int8_t block = 0; block < packet_.header.chBlockNumber; block++) { + packet_.blocks[block].azimuth = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + index += BLOCK_HEADER_AZIMUTH; + + for (int unit = 0; unit < packet_.header.chLaserNumber; unit++) { + unsigned int unRange = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + + packet_.blocks[block].units[unit].distance = + (static_cast(unRange * packet_.header.chDisUnit)) / 1000.f; + packet_.blocks[block].units[unit].intensity = (buf[index + 2] & 0xff); + index += UNIT_SIZE; + } // end fot laser + } // end for block + + index += RESERVED_SIZE; // skip reserved bytes + index += ENGINE_VELOCITY; + + packet_.usec = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | + ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); + index += TIMESTAMP_SIZE; + + packet_.return_mode = buf[index] & 0xff; + + index += RETURN_SIZE; + index += FACTORY_SIZE; + + packet_.t.tm_year = (buf[index + 0] & 0xff) + 100; + // in case of time error + if (packet_.t.tm_year >= 200) { + packet_.t.tm_year -= 100; + } + + packet_.t.tm_mon = (buf[index + 1] & 0xff) - 1; + packet_.t.tm_mday = buf[index + 2] & 0xff; + packet_.t.tm_hour = buf[index + 3] & 0xff; + packet_.t.tm_min = buf[index + 4] & 0xff; + packet_.t.tm_sec = buf[index + 5] & 0xff; + packet_.t.tm_isdst = 0; + + index += UTC_SIZE; + + return true; +} // parsePacket +} // namespace pandar_64 +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/src/hesai/decoders/pandar_qt_decoder.cpp b/nebula_drivers/src/hesai/decoders/pandar_qt_decoder.cpp new file mode 100644 index 000000000..cf79c5028 --- /dev/null +++ b/nebula_drivers/src/hesai/decoders/pandar_qt_decoder.cpp @@ -0,0 +1,268 @@ +#include "hesai/decoders/pandar_qt_decoder.hpp" + +#include + +#include "hesai/decoders/pandar_qt.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace pandar_qt +{ +PandarQTDecoder::PandarQTDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration) +{ + sensor_configuration_ = sensor_configuration; + sensor_calibration_ = calibration_configuration; + cloud_calibration_ = cloud_configuration; + + firing_offset_ = { + 12.31, 14.37, 16.43, 18.49, 20.54, 22.6, 24.66, 26.71, 29.16, 31.22, 33.28, + 35.34, 37.39, 39.45, 41.5, 43.56, 46.61, 48.67, 50.73, 52.78, 54.84, 56.9, + 58.95, 61.01, 63.45, 65.52, 67.58, 69.63, 71.69, 73.74, 75.8, 77.86, 80.9, + 82.97, 85.02, 87.08, 89.14, 91.19, 93.25, 95.3, 97.75, 99.82, 101.87, 103.93, + 105.98, 108.04, 110.1, 112.15, 115.2, 117.26, 119.32, 121.38, 123.43, 125.49, 127.54, + 129.6, 132.05, 134.11, 136.17, 138.22, 140.28, 142.34, 144.39, 146.45, + }; + + for (size_t block = 0; block < BLOCKS_PER_PACKET; ++block) { + block_offset_single_[block] = 25.71f + 500.00f / 3.0f * static_cast(block); + block_offset_dual_[block] = 25.71f + 500.00f / 3.0f * (static_cast(block) / 2.f); + } + + // TODO: add calibration data validation + // if(calibration.elev_angle_map.size() != num_lasers_){ + // // calibration data is not valid! + // } + for (size_t laser = 0; laser < LASER_COUNT; ++laser) { + elev_angle_[laser] = calibration_configuration->elev_angle_map[laser]; + azimuth_offset_[laser] = calibration_configuration->azimuth_offset_map[laser]; + } + + scan_phase_ = static_cast(sensor_configuration_->scan_phase * 100.0f); + dual_return_distance_threshold_ = cloud_calibration_->dual_return_distance_threshold; + + last_phase_ = 0; + has_scanned_ = false; + + scan_pc_.reset(new PointCloudXYZIRADT); + overflow_pc_.reset(new PointCloudXYZIRADT); +} + +bool PandarQTDecoder::hasScanned() { return has_scanned_; } + +drivers::PointCloudXYZIRADTPtr PandarQTDecoder::get_pointcloud() { return scan_pc_; } + +void PandarQTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (!parsePacket(pandar_packet)) { + return; + } + + if (has_scanned_) { + scan_pc_ = overflow_pc_; + overflow_pc_.reset(new PointCloudXYZIRADT); + has_scanned_ = false; + } + + bool dual_return = (packet_.return_mode == DUAL_RETURN); + auto step = dual_return ? 2 : 1; + + if (!dual_return) { + if ( + (packet_.return_mode == FIRST_RETURN && sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_FIRST) || + (packet_.return_mode == LAST_RETURN && sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_LAST)) { + //sensor config, driver mismatched + } + } + + for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); + int current_phase = + (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; + if (current_phase > last_phase_ && !has_scanned_) { + *scan_pc_ += *block_pc; + } else { + *overflow_pc_ += *block_pc; + has_scanned_ = true; + } + last_phase_ = current_phase; + } +} + +drivers::PointXYZIRADT PandarQTDecoder::build_point( + size_t block_id, size_t unit_id, drivers::ReturnMode return_type) +{ + const auto & block = packet_.blocks[block_id]; + const auto & unit = block.units[unit_id]; + auto unix_second = static_cast(timegm(&packet_.t)); + bool dual_return = (packet_.return_mode == DUAL_RETURN); + PointXYZIRADT point{}; + + double xyDistance = unit.distance * cosf(deg2rad(elev_angle_[unit_id])); + + point.x = static_cast( + xyDistance * + sinf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.y = static_cast( + xyDistance * + cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.z = static_cast(unit.distance * sinf(deg2rad(elev_angle_[unit_id]))); + + point.intensity = unit.intensity; + point.distance = unit.distance; + point.ring = unit_id; + point.azimuth = block.azimuth + std::round(azimuth_offset_[unit_id] * 100.0f); + point.return_type = drivers::ReturnModeToInt(return_type);; + point.time_stamp = unix_second + (static_cast(packet_.usec)) / 1000000.0; + point.time_stamp += + dual_return + ? (static_cast(block_offset_dual_[block_id] + firing_offset_[unit_id]) / + 1000000.0f) : (static_cast(block_offset_single_[block_id] + + firing_offset_[unit_id]) / + 1000000.0f); + + return point; +} + +drivers::PointCloudXYZIRADTPtr PandarQTDecoder::convert(size_t block_id) +{ + PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + const auto & block = packet_.blocks[block_id]; + for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) { + const auto & unit = block.units[unit_id]; + // skip invalid points + if (unit.distance <= 0.1 || unit.distance > 200.0) { + continue; + } + + block_pc->points.emplace_back(build_point( + block_id, unit_id, + (packet_.return_mode == FIRST_RETURN) ? drivers::ReturnMode::SINGLE_FIRST + : drivers::ReturnMode::SINGLE_LAST)); + } + return block_pc; +} + +drivers::PointCloudXYZIRADTPtr PandarQTDecoder::convert_dual(size_t block_id) +{ + // Under the Dual Return mode, the ranging data from each firing is stored in two adjacent + // blocks: + // · The even number block is the first return + // · The odd number block is the last return + // · The Azimuth changes every two blocks + // · Important note: Hesai datasheet block numbering starts from 0, not 1, so odd/even are + // reversed here + PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + size_t even_block_id = block_id; + size_t odd_block_id = block_id + 1; + const auto & even_block = packet_.blocks[even_block_id]; + const auto & odd_block = packet_.blocks[odd_block_id]; + + for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) { + const auto & even_unit = even_block.units[unit_id]; + const auto & odd_unit = odd_block.units[unit_id]; + + bool even_usable = !(even_unit.distance <= 0.1 || even_unit.distance > 200.0); + bool odd_usable = !(odd_unit.distance <= 0.1 || odd_unit.distance > 200.0); + + if (sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_FIRST && even_usable) { + // First return is in even block + block_pc->push_back(build_point(even_block_id, unit_id, drivers::ReturnMode::SINGLE_FIRST)); + } else if (sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_LAST && even_usable) { + // Last return is in odd block + block_pc->push_back(build_point(odd_block_id, unit_id, drivers::ReturnMode::SINGLE_LAST)); + } else if (sensor_configuration_->return_mode == drivers::ReturnMode::DUAL_ONLY) { + // If the two returns are too close, only return the last one + if ( + (abs(even_unit.distance - odd_unit.distance) < dual_return_distance_threshold_) && + odd_usable) { + block_pc->push_back(build_point(odd_block_id, unit_id, drivers::ReturnMode::DUAL_ONLY)); + } else { + if (even_usable) { + block_pc->push_back(build_point(even_block_id, unit_id, drivers::ReturnMode::DUAL_FIRST)); + } + if (odd_usable) { + block_pc->push_back(build_point(odd_block_id, unit_id, drivers::ReturnMode::DUAL_LAST)); + } + } + } + } + return block_pc; +} + +bool PandarQTDecoder::parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (pandar_packet.size != PACKET_SIZE && pandar_packet.size != PACKET_WITHOUT_UDPSEQ_SIZE) { + return false; + } + const uint8_t * buf = &pandar_packet.data[0]; + + size_t index = 0; + // Parse 12 Bytes Header + packet_.header.sob = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); + packet_.header.chProtocolMajor = buf[index + 2] & 0xff; + packet_.header.chProtocolMinor = buf[index + 3] & 0xff; + packet_.header.chLaserNumber = buf[index + 6] & 0xff; + packet_.header.chBlockNumber = buf[index + 7] & 0xff; + packet_.header.chReturnType = buf[index + 8] & 0xff; + packet_.header.chDisUnit = buf[index + 9] & 0xff; + index += HEAD_SIZE; + + if (packet_.header.sob != 0xEEFF) { + // Error Start of Packet! + return false; + } + + for (size_t block = 0; block < static_cast(packet_.header.chBlockNumber); block++) { + packet_.blocks[block].azimuth = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + index += BLOCK_HEADER_AZIMUTH; + + for (int unit = 0; unit < packet_.header.chLaserNumber; unit++) { + unsigned int unRange = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + + packet_.blocks[block].units[unit].distance = + (static_cast(unRange * packet_.header.chDisUnit)) / 1000.f; + packet_.blocks[block].units[unit].intensity = (buf[index + 2] & 0xff); + packet_.blocks[block].units[unit].confidence = (buf[index + 3] & 0xff); + index += UNIT_SIZE; + } + } + + index += RESERVED_SIZE; // skip reserved bytes + index += ENGINE_VELOCITY; + + packet_.usec = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | + ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); + index += TIMESTAMP_SIZE; + + packet_.return_mode = buf[index] & 0xff; + + index += RETURN_SIZE; + index += FACTORY_SIZE; + + packet_.t.tm_year = (buf[index + 0] & 0xff) + 100; + packet_.t.tm_mon = (buf[index + 1] & 0xff) - 1; + packet_.t.tm_mday = buf[index + 2] & 0xff; + packet_.t.tm_hour = buf[index + 3] & 0xff; + packet_.t.tm_min = buf[index + 4] & 0xff; + packet_.t.tm_sec = buf[index + 5] & 0xff; + packet_.t.tm_isdst = 0; + + // in case of time error + if (packet_.t.tm_year >= 200) { + packet_.t.tm_year -= 100; + } + + index += UTC_SIZE; + + return true; +} +} // namespace pandar_qt +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/src/hesai/decoders/pandar_xt_decoder.cpp b/nebula_drivers/src/hesai/decoders/pandar_xt_decoder.cpp new file mode 100644 index 000000000..eef177ded --- /dev/null +++ b/nebula_drivers/src/hesai/decoders/pandar_xt_decoder.cpp @@ -0,0 +1,254 @@ +#include "hesai/decoders/pandar_xt_decoder.hpp" + +#include "hesai/decoders/pandar_xt.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace pandar_xt +{ +PandarXTDecoder::PandarXTDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration) +{ + sensor_configuration_ = sensor_configuration; + sensor_calibration_ = calibration_configuration; + cloud_calibration_ = cloud_configuration; + + for (size_t unit = 0; unit < LASER_COUNT; ++unit) { + firing_offset_[unit] = 1.512f * static_cast(unit) + 0.28f; + } + + for (size_t block = 0; block < BLOCKS_PER_PACKET; ++block) { + block_offset_single_[block] = + 3.28f - 50.00f * static_cast(BLOCKS_PER_PACKET - block - 1); + block_offset_dual_[block] = + 3.28f - 50.00f * (static_cast(BLOCKS_PER_PACKET - block - 1) / 2.f); + } + + // TODO: add calibration data validation + // if(calibration.elev_angle_map.size() != num_lasers_){ + // // calibration data is not valid! + // } + for (size_t laser = 0; laser < LASER_COUNT; ++laser) { + elev_angle_[laser] = calibration_configuration->elev_angle_map[laser]; + azimuth_offset_[laser] = calibration_configuration->azimuth_offset_map[laser]; + } + + scan_phase_ = static_cast(sensor_configuration_->scan_phase * 100.0f); + dual_return_distance_threshold_ = cloud_calibration_->dual_return_distance_threshold; + + last_phase_ = 0; + has_scanned_ = false; + + scan_pc_.reset(new PointCloudXYZIRADT); + overflow_pc_.reset(new PointCloudXYZIRADT); +} + +bool PandarXTDecoder::hasScanned() { return has_scanned_; } + +drivers::PointCloudXYZIRADTPtr PandarXTDecoder::get_pointcloud() { return scan_pc_; } + +void PandarXTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (!parsePacket(pandar_packet)) { + return; + } + + if (has_scanned_) { + scan_pc_ = overflow_pc_; + overflow_pc_.reset(new PointCloudXYZIRADT); + has_scanned_ = false; + } + + bool dual_return = + (packet_.return_mode != FIRST_RETURN && packet_.return_mode != STRONGEST_RETURN && + packet_.return_mode != LAST_RETURN); + auto step = dual_return ? 2 : 1; + + for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); + int current_phase = + (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; + if (current_phase > last_phase_ && !has_scanned_) { + *scan_pc_ += *block_pc; + } else { + *overflow_pc_ += *block_pc; + has_scanned_ = true; + } + last_phase_ = current_phase; + } +} + +drivers::PointXYZIRADT PandarXTDecoder::build_point( + int block_id, int unit_id, ReturnMode return_type) +{ + const auto & block = packet_.blocks[block_id]; + const auto & unit = block.units[unit_id]; + auto unix_second = static_cast(timegm(&packet_.t)); + bool dual_return = (packet_.return_mode == DUAL_RETURN); + PointXYZIRADT point{}; + + double xyDistance = unit.distance * cosf(deg2rad(elev_angle_[unit_id])); + + point.x = static_cast( + xyDistance * + sinf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.y = static_cast( + xyDistance * + cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.z = static_cast(unit.distance * sinf(deg2rad(elev_angle_[unit_id]))); + + point.intensity = unit.intensity; + point.distance = static_cast(unit.distance); + point.ring = unit_id; + point.azimuth = static_cast(block.azimuth) + std::round(azimuth_offset_[unit_id] * 100.0f); + point.return_type = drivers::ReturnModeToInt(return_type); + point.time_stamp = unix_second + (static_cast(packet_.usec)) / 1000000.0; + point.time_stamp += + dual_return + ? (static_cast(block_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000.0f) + : (static_cast(block_offset_single_[block_id] + firing_offset_[unit_id]) / + 1000000.0f); + + return point; +} + +drivers::PointCloudXYZIRADTPtr PandarXTDecoder::convert(size_t block_id) +{ + PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + auto unix_second = static_cast(timegm(&packet_.t)); + + const auto & block = packet_.blocks[block_id]; + for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) { + const auto & unit = block.units[unit_id]; + // skip invalid points + if (unit.distance <= 0.1 || unit.distance > 200.0) { + continue; + } + + block_pc->points.emplace_back(build_point( + block_id, unit_id, + (packet_.return_mode == STRONGEST_RETURN) ? drivers::ReturnMode::SINGLE_STRONGEST + : drivers::ReturnMode::SINGLE_LAST)); + } + return block_pc; +} + +drivers::PointCloudXYZIRADTPtr PandarXTDecoder::convert_dual(size_t block_id) +{ + PointCloudXYZIRADTPtr block_pc(new PointCloudXYZIRADT); + + auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) + + auto head = + block_id + ((sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_FIRST) ? 1 : 0); + auto tail = + block_id + ((sensor_configuration_->return_mode == drivers::ReturnMode::SINGLE_LAST) ? 1 : 2); + + for (size_t unit_id = 0; unit_id < LASER_COUNT; ++unit_id) { + for (size_t i = head; i < tail; ++i) { + PointXYZIRADT point{}; + const auto & block = packet_.blocks[i]; + const auto & unit = block.units[unit_id]; + // skip invalid points + if (unit.distance <= 0.1 || unit.distance > 200.0) { + continue; + } + double xyDistance = unit.distance * cosf(deg2rad(elev_angle_[unit_id])); + + point.x = static_cast( + xyDistance * + sinf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.y = static_cast( + xyDistance * + cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) / 100.0))); + point.z = static_cast(unit.distance * sinf(deg2rad(elev_angle_[unit_id]))); + + point.intensity = unit.intensity; + point.distance = unit.distance; + point.ring = unit_id; + point.azimuth = block.azimuth + std::round(azimuth_offset_[unit_id] * 100.0f); + + point.time_stamp = unix_second + (static_cast(packet_.usec)) / 1000000.0; + + point.time_stamp += + (static_cast(block_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000.0f); + + block_pc->points.emplace_back(point); + } + } + return block_pc; +} + +bool PandarXTDecoder::parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) +{ + if (pandar_packet.size != PACKET_SIZE) { + return false; + } + const uint8_t * buf = &pandar_packet.data[0]; + + size_t index = 0; + // Parse 12 Bytes Header + packet_.header.sob = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); + packet_.header.chProtocolMajor = buf[index + 2] & 0xff; + packet_.header.chProtocolMinor = buf[index + 3] & 0xff; + packet_.header.chLaserNumber = buf[index + 6] & 0xff; + packet_.header.chBlockNumber = buf[index + 7] & 0xff; + packet_.header.chReturnType = buf[index + 8] & 0xff; + packet_.header.chDisUnit = buf[index + 9] & 0xff; + index += HEAD_SIZE; + + if (packet_.header.sob != 0xEEFF) { + // Error Start of Packet! + return false; + } + + for (int8_t block = 0; block < packet_.header.chBlockNumber; block++) { + packet_.blocks[block].azimuth = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + index += BLOCK_HEADER_AZIMUTH; + + for (int unit = 0; unit < packet_.header.chLaserNumber; unit++) { + unsigned int unRange = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + + packet_.blocks[block].units[unit].distance = + (static_cast(unRange * packet_.header.chDisUnit)) / 1000.f; + packet_.blocks[block].units[unit].intensity = (buf[index + 2] & 0xff); + packet_.blocks[block].units[unit].confidence = (buf[index + 3] & 0xff); + index += UNIT_SIZE; + } + } + + index += RESERVED_SIZE; // skip reserved bytes + packet_.return_mode = buf[index] & 0xff; + + index += RETURN_SIZE; + index += ENGINE_VELOCITY; + + packet_.t.tm_year = (buf[index + 0] & 0xff) + 100; + packet_.t.tm_mon = (buf[index + 1] & 0xff) - 1; + packet_.t.tm_mday = buf[index + 2] & 0xff; + packet_.t.tm_hour = buf[index + 3] & 0xff; + packet_.t.tm_min = buf[index + 4] & 0xff; + packet_.t.tm_sec = buf[index + 5] & 0xff; + packet_.t.tm_isdst = 0; + // in case of time error + if (packet_.t.tm_year >= 200) { + packet_.t.tm_year -= 100; + } + + index += UTC_SIZE; + + packet_.usec = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 | + ((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24); + index += TIMESTAMP_SIZE; + index += FACTORY_SIZE; + + return true; +} +} // namespace pandar_xt +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/src/hesai/hesai_driver.cpp b/nebula_drivers/src/hesai/hesai_driver.cpp new file mode 100644 index 000000000..9d0ba8c3a --- /dev/null +++ b/nebula_drivers/src/hesai/hesai_driver.cpp @@ -0,0 +1,84 @@ +#include "hesai/hesai_driver.hpp" +#include "hesai/decoders/pandar_64_decoder.hpp" +#include "hesai/decoders/pandar_40_decoder.hpp" +#include "hesai/decoders/pandar_qt_decoder.hpp" +#include "hesai/decoders/pandar_xt_decoder.hpp" + +namespace nebula +{ +namespace drivers +{ + +HesaiDriver::HesaiDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & cloud_configuration, + const std::shared_ptr & calibration_configuration) +{ + // initialize proper parser from cloud config's model and echo mode + driver_status_ = nebula::Status::OK; + switch (sensor_configuration->sensor_model) { + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + break; + case SensorModel::HESAI_PANDAR64: + scan_decoder_.reset(new drivers::pandar_64::Pandar64Decoder(sensor_configuration, + cloud_configuration, + calibration_configuration)); + break; + case SensorModel::HESAI_PANDAR40P: + case SensorModel::HESAI_PANDAR40M: + scan_decoder_.reset(new drivers::pandar_40::Pandar40Decoder(sensor_configuration, + cloud_configuration, + calibration_configuration)); + break; + case SensorModel::HESAI_PANDARQT64: + scan_decoder_.reset(new drivers::pandar_qt::PandarQTDecoder(sensor_configuration, + cloud_configuration, + calibration_configuration)); + break; + case SensorModel::HESAI_PANDARXT32: + scan_decoder_.reset(new drivers::pandar_xt::PandarXTDecoder(sensor_configuration, + cloud_configuration, + calibration_configuration)); + break; + case SensorModel::HESAI_PANDARQT128: + case SensorModel::HESAI_PANDAR128_V13: + case SensorModel::HESAI_PANDAR128_V14: + driver_status_ = nebula::Status::NOT_INITIALIZED; + throw std::runtime_error("Driver not Implemented for selected sensor."); + break; + } +} + +Status HesaiDriver::SetCalibrationConfiguration( + const CalibrationConfigurationBase & calibration_configuration) +{ + throw std::runtime_error("SetCalibrationConfiguration. Not yet implemented"); +} + +Status HesaiDriver::SetCloudConfiguration( + const nebula::drivers::CloudConfigurationBase & cloud_configuration) +{ + throw std::runtime_error("SetCalibrationConfiguration. Not yet implemented"); +} + +PointCloudXYZIRADTPtr HesaiDriver::ConvertScanToPointcloud( + const std::shared_ptr& pandar_scan) +{ + PointCloudXYZIRADTPtr pointcloud; + if(driver_status_ == nebula::Status::OK) + { + for (auto& packet : pandar_scan->packets) { + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->get_pointcloud(); + } + } + } + + return pointcloud; +} +Status HesaiDriver::GetStatus() { return driver_status_; } + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/src/hesai/hesai_driver_ros_main.cpp b/nebula_drivers/src/hesai/hesai_driver_ros_main.cpp new file mode 100644 index 000000000..5d9c274b7 --- /dev/null +++ b/nebula_drivers/src/hesai/hesai_driver_ros_main.cpp @@ -0,0 +1,32 @@ +#include "hesai/hesai_driver_ros_wrapper.hpp" + +#include + +#include + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + std::string node_name = "nebula_hesai_driver"; + + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + auto hesai_driver = std::make_shared(options, node_name); + exec.add_node(hesai_driver->get_node_base_interface()); + + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); + nebula::Status driver_status = hesai_driver->GetStatus(); + if (driver_status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Hesai Driver Started"); + exec.spin(); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); + } + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); + rclcpp::shutdown(); + + return 0; +} diff --git a/nebula_drivers/src/hesai/hesai_driver_ros_wrapper.cpp b/nebula_drivers/src/hesai/hesai_driver_ros_wrapper.cpp new file mode 100644 index 000000000..77b02db8a --- /dev/null +++ b/nebula_drivers/src/hesai/hesai_driver_ros_wrapper.cpp @@ -0,0 +1,132 @@ +#include "hesai/hesai_driver_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiDriverRosWrapper::HesaiDriverRosWrapper( + const rclcpp::NodeOptions & options, const std::string & node_name) +: rclcpp::Node(node_name, options) +{ + drivers::HesaiCalibrationConfiguration calibration_configuration; + drivers::HesaiCloudConfiguration cloud_configuration; + drivers::HesaiSensorConfiguration sensor_configuration; + + wrapper_status_ = + GetParameters(sensor_configuration, calibration_configuration, cloud_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + calibration_cfg_ptr_ = + std::make_shared(calibration_configuration); + + cloud_cfg_ptr_ = std::make_shared(cloud_configuration); + + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast(sensor_cfg_ptr_), + std::const_pointer_cast(cloud_cfg_ptr_), + std::static_pointer_cast(calibration_cfg_ptr_)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + + pandar_scan_sub_ = create_subscription( + "pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); + pandar_points_pub_ = + this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); +} + +void HesaiDriverRosWrapper::ReceiveScanMsgCallback( + const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) +{ + // take packets out of scan msg + std::vector pkt_msgs = scan_msg->packets; + + nebula::drivers::PointCloudXYZIRADTPtr pointcloud = + driver_ptr_->ConvertScanToPointcloud(scan_msg); + + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + if (!pointcloud->points.empty()) { + double first_point_timestamp = pointcloud->points.front().time_stamp; + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(first_point_timestamp).count()); + } + ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id; + pandar_points_pub_->publish(std::move(ros_pc_msg_ptr)); +} + +Status HesaiDriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr cloud_configuration, + std::shared_ptr calibration_configuration) +{ + // driver should be initialized here with proper decoder + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(cloud_configuration), + std::static_pointer_cast(calibration_configuration)); + return driver_ptr_->GetStatus(); +} + +Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } + +Status HesaiDriverRosWrapper::GetParameters( + drivers::HesaiSensorConfiguration & sensor_configuration, + drivers::HesaiCalibrationConfiguration & calibration_configuration, + drivers::HesaiCloudConfiguration & cloud_configuration) +{ + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + this->declare_parameter("sensor_model", "")); + + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromString(this->declare_parameter("return_mode", "")); + + sensor_configuration.host_ip = this->declare_parameter("host_ip", "127.0.0.1"); + sensor_configuration.sensor_ip = + this->declare_parameter("sensor_ip", "192.168.1.201"); + sensor_configuration.frame_id = this->declare_parameter("frame_id", "pandar"); + sensor_configuration.data_port = this->declare_parameter("data_port", 2368); + sensor_configuration.gnss_port = this->declare_parameter("gnss_port", 2369); + sensor_configuration.scan_phase = this->declare_parameter("scan_phase", 0.); + sensor_configuration.frequency_ms = this->declare_parameter("frequency_ms", 100); + + calibration_configuration.calibration_file = + this->declare_parameter("calibration_file", ""); + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if ( + sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 365 || + sensor_configuration.frequency_ms == 0) { + return Status::SENSOR_CONFIG_ERROR; + } + if (calibration_configuration.calibration_file.empty()) { + return Status::INVALID_CALIBRATION_FILE; + } else { + auto cal_status = + calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); + return cal_status; + } + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_drivers/src/hesai/hesai_hw_interface.cpp b/nebula_drivers/src/hesai/hesai_hw_interface.cpp new file mode 100644 index 000000000..a546d60ad --- /dev/null +++ b/nebula_drivers/src/hesai/hesai_hw_interface.cpp @@ -0,0 +1,136 @@ +#include "hesai/hesai_hw_interface.hpp" + +#include +#include +#include + +#include + +namespace nebula +{ +namespace drivers +{ +HesaiHwInterface::HesaiHwInterface() +: cloud_io_context_{new IoContext(1)}, + cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, + scan_cloud_ptr_{std::make_unique()} +{ +} + +Status HesaiHwInterface::SetSensorConfiguration( + std::shared_ptr sensor_configuration) +{ + Status status = Status::OK; + mtu_size_ = 1500; + try { + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P) { + azimuth_index_ = 2; // 2 + 124 * [0-9] + is_valid_packet_ = [](size_t packet_size) { + return (packet_size == 1262 || packet_size == 1266); + }; + } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT64) { + azimuth_index_ = 12; // 12 + 258 * [0-3] + is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1072); }; + } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32) { + azimuth_index_ = 12; // 12 + 130 * [0-7] + is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1080); }; + } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { + azimuth_index_ = 8; // 8 + 192 * [0-5] + is_valid_packet_ = [](size_t packet_size) { + return (packet_size == 1194 || packet_size == 1198); + }; + } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_V14) { + azimuth_index_ = 12; // 12 + 386 * [0-1] + is_valid_packet_ = [](size_t packet_size) { return (packet_size == 893); }; // version 1.4 + mtu_size_ = 1800; + } else { + status = Status::INVALID_SENSOR_MODEL; + } + } catch (const std::exception & ex) { + status = Status::SENSOR_CONFIG_ERROR; + std::cerr << status << std::endl; + return status; + } + return status; +} + +Status HesaiHwInterface::CloudInterfaceStart() +{ + try { + std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl; + cloud_udp_driver_->init_receiver( + sensor_configuration_->host_ip, sensor_configuration_->data_port); + cloud_udp_driver_->receiver()->open(); + cloud_udp_driver_->receiver()->bind(); + + cloud_udp_driver_->receiver()->asyncReceive( + std::bind(&HesaiHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + } catch (const std::exception & ex) { + Status status = Status::UDP_CONNECTION_ERROR; + std::cerr << status << sensor_configuration_->sensor_ip << "," + << sensor_configuration_->data_port << std::endl; + return status; + } + return Status::OK; +} + +Status HesaiHwInterface::RegisterScanCallback( + std::function)> scan_callback) +{ + scan_reception_callback_ = std::move(scan_callback); + return Status::OK; +} + +void HesaiHwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) +{ + int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); + if (is_valid_packet_(buffer.size())) { + uint32_t buffer_size = buffer.size(); + std::array packet_data{}; + std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, packet_data.begin()); + pandar_msgs::msg::PandarPacket pandar_packet; + pandar_packet.data = packet_data; + pandar_packet.size = buffer_size; + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = std::chrono::duration_cast(now.time_since_epoch()).count(); + pandar_packet.stamp.sec = static_cast(now_secs); + pandar_packet.stamp.nanosec = static_cast((now_nanosecs/1000000000. - static_cast(now_secs))*1000000000); + scan_cloud_ptr_->packets.emplace_back(pandar_packet); + } + int current_phase = 0; + { + const auto & data = scan_cloud_ptr_->packets.back().data; + current_phase = (data[azimuth_index_] & 0xff) | ((data[azimuth_index_ + 1] & 0xff) << 8); + current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; + } + if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { + prev_phase_ = current_phase; + } else { // Scan complete + if (scan_reception_callback_) { + scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; + // Callback + scan_reception_callback_(std::move(scan_cloud_ptr_)); + scan_cloud_ptr_ = std::make_unique(); + } + } +} +Status HesaiHwInterface::CloudInterfaceStop() { return Status::ERROR_1; } + +Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +{ + return Status::ERROR_1; +} + +Status HesaiHwInterface::GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) +{ + return Status::ERROR_1; +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_drivers/src/hesai/hesai_hw_interface_ros_main.cpp b/nebula_drivers/src/hesai/hesai_hw_interface_ros_main.cpp new file mode 100644 index 000000000..a06efcd93 --- /dev/null +++ b/nebula_drivers/src/hesai/hesai_hw_interface_ros_main.cpp @@ -0,0 +1,29 @@ +#include "hesai/hesai_hw_interface_ros_wrapper.hpp" + +#include + +#include + +int main(int argc, char * argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + std::string node_name = "nebula_hesai_hw_interface"; + + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + auto hesai_hw_interface = + std::make_shared(options, node_name); + exec.add_node(hesai_hw_interface->get_node_base_interface()); + nebula::Status driver_status = hesai_hw_interface->StreamStart(); + if (driver_status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "UDP Driver Started"); + exec.spin(); + } + + rclcpp::shutdown(); + + return 0; +} diff --git a/nebula_drivers/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_drivers/src/hesai/hesai_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..90e350e90 --- /dev/null +++ b/nebula_drivers/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -0,0 +1,94 @@ +#include "hesai/hesai_hw_interface_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper( + const rclcpp::NodeOptions & options, const std::string & node_name) +: rclcpp::Node(node_name, options), hw_interface_() +{ + interface_status_ = GetParameters(sensor_configuration_, calibration_configuration_, cloud_configuration_); + if (Status::OK != interface_status_) + { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + // Initialize sensor_configuration + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + // register scan callback and publisher + hw_interface_.RegisterScanCallback( + std::bind(&HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); + pandar_scan_pub_ = + this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); +} + +Status HesaiHwInterfaceRosWrapper::StreamStart() +{ + if(Status::OK == interface_status_ ){ + interface_status_ = hw_interface_.CloudInterfaceStart(); + } + return interface_status_; +} + +Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } +Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } + +Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + return Status::OK; +} + +Status HesaiHwInterfaceRosWrapper::GetParameters( + drivers::HesaiSensorConfiguration & sensor_configuration, + drivers::HesaiCalibrationConfiguration & calibration_configuration, + drivers::HesaiCloudConfiguration & cloud_configuration) +{ + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + this->declare_parameter("sensor_model", "")); + + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromString(this->declare_parameter("return_mode", "")); + + sensor_configuration.host_ip = this->declare_parameter("host_ip", "255.255.255.255"); + sensor_configuration.sensor_ip = + this->declare_parameter("sensor_ip", "192.168.1.201"); + sensor_configuration.frame_id = this->declare_parameter("frame_id", "pandar"); + sensor_configuration.data_port = this->declare_parameter("data_port", 2368); + sensor_configuration.gnss_port = this->declare_parameter("gnss_port", 2369); + sensor_configuration.scan_phase = this->declare_parameter("scan_phase", 0.); + sensor_configuration.frequency_ms = this->declare_parameter("frequency_ms", 100); + sensor_configuration.packet_mtu_size = this->declare_parameter("packet_mtu_size", 1500); + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if ( + sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 365 || + sensor_configuration.frequency_ms == 0) { + return Status::SENSOR_CONFIG_ERROR; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback( + std::unique_ptr scan_buffer) +{ + // Publish + scan_buffer->header.frame_id = sensor_configuration_.frame_id; + scan_buffer->header.stamp = scan_buffer->packets.front().stamp; + pandar_scan_pub_->publish(*scan_buffer); +} + +} // namespace ros +} // namespace nebula