From a16e09122f036afc95c9578e2ce400caed5894b7 Mon Sep 17 00:00:00 2001 From: urasakikeisuke <79469739+urasakikeisuke@users.noreply.github.com> Date: Wed, 26 Oct 2022 13:15:09 +0900 Subject: [PATCH] Feature/qt128 (#1) * add qt 128 support * add qt128 decoder * fix correction csv --- pandar_driver/src/driver/pandar_driver.cpp | 4 + pandar_msgs/package.xml | 2 +- pandar_pointcloud/CMakeLists.txt | 3 +- pandar_pointcloud/config/qt128.csv | 129 ++++ .../decoder/pandar_qt128.hpp | 90 +++ .../decoder/pandar_qt128_decoder.hpp | 92 +++ pandar_pointcloud/launch/pandarQT128.launch | 43 ++ pandar_pointcloud/launch/pandar_cloud.launch | 12 +- .../src/lib/decoder/pandar_qt128_decoder.cpp | 707 ++++++++++++++++++ pandar_pointcloud/src/pandar_cloud.cpp | 24 +- 10 files changed, 1095 insertions(+), 11 deletions(-) create mode 100644 pandar_pointcloud/config/qt128.csv create mode 100644 pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128.hpp create mode 100644 pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128_decoder.hpp create mode 100644 pandar_pointcloud/launch/pandarQT128.launch create mode 100644 pandar_pointcloud/src/lib/decoder/pandar_qt128_decoder.cpp diff --git a/pandar_driver/src/driver/pandar_driver.cpp b/pandar_driver/src/driver/pandar_driver.cpp index 54679c1..fb4963c 100644 --- a/pandar_driver/src/driver/pandar_driver.cpp +++ b/pandar_driver/src/driver/pandar_driver.cpp @@ -53,6 +53,10 @@ PandarDriver::PandarDriver(ros::NodeHandle node, ros::NodeHandle private_nh) azimuth_index_ = 12; // 12 + 130 * [0-7] is_valid_packet_ = [](size_t packet_size) { return (packet_size == 820); }; } + else if (model_ == "PandarQT128") { + azimuth_index_ = 12; // 12 + 512 * [0-1] + is_valid_packet_ = [](size_t packet_size) { return (packet_size == 1127); }; + } else { ROS_ERROR("Invalid model name"); } diff --git a/pandar_msgs/package.xml b/pandar_msgs/package.xml index 49c1d21..ea8dc1e 100644 --- a/pandar_msgs/package.xml +++ b/pandar_msgs/package.xml @@ -2,7 +2,7 @@ pandar_msgs 0.0.0 - ROS message definition for the Heisai PandarQT/Pandar64/Pandar40P/Pandar20A/Pandar20B/Pandar40M LiDAR sensor. + ROS message definition for the Heisai Pandar128/PandarQT128/PandarQT/Pandar64/Pandar40P/Pandar20A/Pandar20B/Pandar40M LiDAR sensor. hirakawa Apache 2 diff --git a/pandar_pointcloud/CMakeLists.txt b/pandar_pointcloud/CMakeLists.txt index b6aed7d..22f9f0f 100644 --- a/pandar_pointcloud/CMakeLists.txt +++ b/pandar_pointcloud/CMakeLists.txt @@ -39,6 +39,7 @@ add_library(pandar_cloud src/lib/decoder/pandar_xt_decoder.cpp src/lib/decoder/pandar_xtm_decoder.cpp src/lib/decoder/pandar64_decoder.cpp + src/lib/decoder/pandar_qt128_decoder ) target_link_libraries(pandar_cloud ${catkin_LIBRARIES} @@ -50,7 +51,7 @@ add_executable(pandar_cloud_node ) target_link_libraries(pandar_cloud_node - pandar_cloud + pandar_cloud ${catkin_LIBRARIES} ) diff --git a/pandar_pointcloud/config/qt128.csv b/pandar_pointcloud/config/qt128.csv new file mode 100644 index 0000000..1fc0b14 --- /dev/null +++ b/pandar_pointcloud/config/qt128.csv @@ -0,0 +1,129 @@ +Laser id,Elevation,Azimuth +1,-52.62676282,10.10830596 +2,-51.0280939,9.719503673 +3,-49.51495392,9.384265827 +4,-48.07394795,9.091433335 +5,-46.69466297,8.832899657 +6,-45.36882812,8.602605729 +7,-44.08974439,8.395920495 +8,-42.85190128,8.209210679 +9,-41.65069769,8.03961095 +10,-40.48225401,7.8848012 +11,-39.34326358,7.742887867 +12,-38.23088092,7.612311759 +13,-37.14263654,7.491771683 +14,-36.07637316,7.380164213 +15,-35.03019446,7.276562397 +16,-34.00242585,7.180171603 +17,-32.99157758,7.090302002 +18,-31.99631045,7.006369095 +19,-31.01543179,6.927848973 +20,-30.04786472,6.854295551 +21,-29.09262707,6.785306938 +22,-28.14883195,6.720536921 +23,-27.21566989,6.659666871 +24,-26.29240356,6.602428464 +25,-25.37835081,6.548569874 +26,-24.47288318,6.497872808 +27,-23.57542315,6.450129948 +28,-22.68544071,6.405178951 +29,-21.80243688,6.362840261 +30,-20.92594329,6.32298504 +31,-20.05553602,6.285467298 +32,-19.19081372,6.25017485 +33,-18.33139172,-6.21699538 +34,-17.47691901,-6.18583344 +35,-16.62705442,-6.156599148 +36,-15.78149317,-6.129208192 +37,-14.93993032,-6.103581848 +38,-14.10208035,-6.079663981 +39,-13.26767188,-6.057381402 +40,-12.43645574,-6.036683527 +41,-11.60818201,-6.017519726 +42,-10.78261371,-5.999833659 +43,-9.959523504,-5.983597281 +44,-9.138697037,-5.968771181 +45,-8.319929599,-5.955310246 +46,-7.503005408,-5.943192006 +47,-6.687807162,-5.926719867 +48,-5.873926875,-5.922876604 +49,-5.061393513,-5.914628713 +50,-4.249948322,-5.907639071 +51,-3.43941536,-5.901885097 +52,-2.629617595,-5.897355529 +53,-1.820378325,-5.894039091 +54,-1.011522452,-5.891935825 +55,-0.202883525,-5.89103442 +56,0.605717878,-5.89132922 +57,1.414444374,-5.892831558 +58,2.223469054,-5.895541414 +59,3.032962884,-5.899458754 +60,3.8431104,-5.904600534 +61,4.654074511,-5.910966684 +62,5.466034723,-5.918579795 +63,6.279180222,-5.927451101 +64,7.093696848,-5.937608821 +65,7.909760474,5.949064148 +66,8.727579858,5.961845258 +67,9.54735011,5.97598597 +68,10.36927364,5.99150874 +69,11.19356846,6.008464331 +70,12.0204614,6.026875139 +71,12.85016831,6.046797524 +72,13.68294669,6.068282136 +73,14.51903314,6.091373918 +74,15.35869922,6.116134758 +75,16.20222125,6.142626487 +76,17.04988688,6.170927865 +77,17.90200196,6.201111915 +78,18.75890049,6.233268566 +79,19.62091911,6.267487655 +80,20.48842178,6.303870236 +81,21.36180725,6.342539884 +82,22.24149217,6.383614369 +83,23.12791172,6.427233938 +84,24.02155359,6.473555624 +85,24.92293181,6.52274755 +86,25.83260224,6.574994555 +87,26.75116558,6.63050945 +88,27.67927338,6.689521655 +89,28.61763387,6.752299732 +90,29.56702472,6.819128697 +91,30.52828971,6.890343815 +92,31.50236238,6.966319159 +93,32.49026442,7.0474731 +94,33.49313666,7.134285032 +95,34.51223682,7.227334588 +96,35.54897914,7.327222327 +97,19.19081372,-6.25017485 +98,20.05553602,-6.285467298 +99,20.92594329,-6.32298504 +100,21.80243688,-6.362840261 +101,22.68544071,-6.405178951 +102,23.57542315,-6.450129948 +103,24.47288318,-6.497872808 +104,25.37835081,-6.548569874 +105,26.29240356,-6.602428464 +106,27.21566989,-6.659666871 +107,28.14883195,-6.720536921 +108,29.09262707,-6.785306938 +109,30.04786472,-6.854295551 +110,31.01543179,-6.927848973 +111,31.99631045,-7.006369095 +112,32.99157758,-7.090302002 +113,34.00242585,-7.180171603 +114,35.03019446,-7.276562397 +115,36.07637316,-7.380164213 +116,37.14263654,-7.491771683 +117,38.23088092,-7.612311759 +118,39.34326358,-7.742887867 +119,40.48225401,-7.8848012 +120,41.65069769,-8.03961095 +121,42.85190128,-8.209210679 +122,44.08974439,-8.395920495 +123,45.36882812,-8.602605729 +124,46.69466297,-8.832899657 +125,48.07394795,-9.091433335 +126,49.51495392,-9.384265827 +127,51.0280939,-9.719503673 +128,52.62676282,-10.10830596 diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128.hpp new file mode 100644 index 0000000..4780823 --- /dev/null +++ b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128.hpp @@ -0,0 +1,90 @@ +#pragma once + +/** + * Pandar QT128 + */ + +#include + +namespace pandar_pointcloud +{ +namespace pandar_qt128 +{ + +// Head +constexpr std::size_t HEAD_SIZE = 12; +constexpr std::size_t PRE_HEADER_SIZE = 6; +constexpr std::size_t HEADER_SIZE = 6; + +// Body +constexpr std::size_t BLOCK_NUM = 2; +constexpr std::size_t BLOCK_HEADER_AZIMUTH = 2; +constexpr std::size_t UNIT_NUM = 128; +constexpr std::size_t UNIT_SIZE = 4; +constexpr std::size_t CRC_1_SIZE = 4; +constexpr std::size_t BLOCK_SIZE = UNIT_SIZE * UNIT_NUM + BLOCK_HEADER_AZIMUTH; +constexpr std::size_t BODY_SIZE = BLOCK_SIZE * BLOCK_NUM + CRC_1_SIZE; + +// Functional Safety +constexpr std::size_t FUNCTIONAL_SAFETY_SIZE = 17; + +// Tail +constexpr std::size_t RESERVED_1_SIZE = 5; +constexpr std::size_t MODE_FLAG_SIZE = 1; +constexpr std::size_t RESERVED_2_SIZE = 6; +constexpr std::size_t RETURN_MODE_SIZE = 1; +constexpr std::size_t MOTOR_SPEED_SIZE = 2; +constexpr std::size_t DATE_TIME_SIZE = 6; +constexpr std::size_t TIMESTAMP_SIZE = 4; +constexpr std::size_t FACTORY_INFO_SIZE = 1; +constexpr std::size_t UDP_SEQUENCE_SIZE = 4; +constexpr std::size_t CRC_3_SIZE = 4; +constexpr std::size_t PACKET_TAIL_SIZE = 34; + +// Cyber Security +constexpr std::size_t CYBER_SECURITY_SIZE = 32; + +// All +constexpr std::size_t PACKET_SIZE = + HEAD_SIZE + BODY_SIZE + FUNCTIONAL_SAFETY_SIZE + PACKET_TAIL_SIZE + CYBER_SECURITY_SIZE; + +constexpr uint32_t FIRST_RETURN = 0x33; +constexpr uint32_t LAST_RETURN = 0x38; +constexpr uint32_t DUAL_RETURN = 0x3B; + +struct Header +{ + std::uint16_t u16Sob; // Start of block 0xFFEE 2bytes + std::uint8_t u8ProtocolMajor; // Protocol Version Major 1byte + std::uint8_t u8ProtocolMinor; // Protocol Version Minor 1byte + std::uint8_t u8LaserNum; // Laser Num 1byte (0x80 = 128 channels) + std::uint8_t u8BlockNum; // Block Num 1byte (0x02 = 2 blocks per packe) + std::uint8_t u8DistUnit; // Distance unit 1byte (0x04 = 4mm) + std::uint8_t u8EchoNum; // Number of returns that each channel generates. 1byte + std::uint8_t u8Flags; // Flags 1byte +}; + +struct Unit +{ + double distance; + std::uint16_t intensity; +}; + +struct Block +{ + std::uint16_t azimuth; + Unit units[UNIT_NUM]; +}; + +struct Packet +{ + Header header; + Block blocks[BLOCK_NUM]; + std::uint32_t timestamp; // ms + std::uint32_t mode_flag; + uint32_t usec; // ms + uint32_t return_mode; + tm t; +}; +} // namespace pandar_qt128 +} // namespace pandar_pointcloud diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128_decoder.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128_decoder.hpp new file mode 100644 index 0000000..a02e358 --- /dev/null +++ b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt128_decoder.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include +#include "pandar_pointcloud/calibration.hpp" +#include "packet_decoder.hpp" +#include "pandar_qt128.hpp" + +namespace pandar_pointcloud +{ +namespace pandar_qt128 +{ +class PandarQT128Decoder : public PacketDecoder +{ +public: + enum class ReturnMode : int8_t + { + DUAL, + FIRST, + LAST, + }; + enum ReturnType : uint8_t + { + INVALID = 0, + SINGLE_FIRST, + SINGLE_LAST, + DUAL_FIRST, + DUAL_LAST, + DUAL_ONLY, + }; + + PandarQT128Decoder(Calibration& calibration, float scan_phase = 0.0f, double dual_return_distance_threshold = 0.1, ReturnMode return_mode = ReturnMode::DUAL); + void unpack(const pandar_msgs::PandarPacket& raw_packet) override; + PointXYZIRADT build_point(int block_id, int unit_id, int seq_id, uint8_t return_type); + bool hasScanned() override; + PointcloudXYZIRADT getPointcloud() override; + +private: + bool parsePacket(const pandar_msgs::PandarPacket& raw_packet); + PointcloudXYZIRADT convert(const int block_id); + PointcloudXYZIRADT convert_dual(const int block_id); + + void initFiringOffset(); + + std::array, BLOCK_NUM> firing_offset_; + std::array block_offset_single_; + std::array block_offset_dual_; + + ReturnMode return_mode_; + double dual_return_distance_threshold_; + Packet packet_; + + PointcloudXYZIRADT scan_pc_; + PointcloudXYZIRADT overflow_pc_; + + uint16_t scan_phase_; + int last_phase_; + bool has_scanned_; + + std::array elev_angle_ = { + -52.6267f, -51.0280f, -49.5149f, -48.0739f, -46.6946f, -45.3688f, -44.0897f, -42.8519f, -41.6506f, -40.4822f, + -39.3432f, -38.2308f, -37.1426f, -36.0763f, -35.0301f, -34.0024f, -32.9915f, -31.9963f, -31.0154f, -30.0478f, + -29.0926f, -28.1488f, -27.2156f, -26.2924f, -25.3783f, -24.4728f, -23.5754f, -22.6854f, -21.8024f, -20.9259f, + -20.0555f, -19.1908f, -18.3313f, -17.4769f, -16.6270f, -15.7814f, -14.9399f, -14.1020f, -13.2676f, -12.4364f, + -11.6081f, -10.7826f, -9.9595f, -9.1386f, -8.3199f, -7.5030f, -6.6878f, -5.8739f, -5.0613f, -4.2499f, + -3.4394f, -2.6296f, -1.8203f, -1.0115f, -0.2028f, 0.6057f, 1.4144f, 2.2234f, 3.0329f, 3.8431f, + 4.6540f, 5.4660f, 6.2791f, 7.0936f, 7.9097f, 8.7275f, 9.5473f, 10.3692f, 11.1935f, 12.0204f, + 12.8501f, 13.6829f, 14.5190f, 15.3586f, 16.2022f, 17.0498f, 17.9020f, 18.7589f, 19.6209f, 20.4884f, + 21.3618f, 22.2414f, 23.1279f, 24.0215f, 24.9229f, 25.8326f, 26.7511f, 27.6792f, 28.6176f, 29.5670f, + 30.5282f, 31.5023f, 32.4902f, 33.4931f, 34.5122f, 35.5489f, 19.1908f, 20.0555f, 20.9259f, 21.8024f, + 22.6854f, 23.5754f, 24.4728f, 25.3783f, 26.2924f, 27.2156f, 28.1488f, 29.0926f, 30.0478f, 31.0154f, + 31.9963f, 32.9915f, 34.0024f, 35.0301f, 36.0763f, 37.1426f, 38.2308f, 39.3432f, 40.4822f, 41.6506f, + 42.8519f, 44.0897f, 45.3688f, 46.6946f, 48.0739f, 49.5149f, 51.0280f, 52.6267f +}; + +std::array azimuth_offset_ = { + 10.6267f, 9.0280f, 9.5149f, 9.0739f, 8.6946f, 8.3688f, 8.0897f, 8.8519f, 8.6506f, 7.4822f, 7.3432f, + 7.2308f, 7.1426f, 7.0763f, 7.0301f, 7.0024f, 7.9915f, 7.9963f, 6.0154f, 6.0478f, 6.0926f, 6.1488f, + 6.2156f, 6.2924f, 6.3783f, 6.4728f, 6.5754f, 6.6854f, 6.8024f, 6.9259f, 6.0555f, 6.1908f, -6.3313f, + -6.4769f, -6.6270f, -6.7814f, -6.9399f, -6.1020f, -6.2676f, -6.4364f, -6.6081f, -5.7826f, -5.9595f, -5.1386f, + -5.3199f, -5.5030f, -5.6878f, -5.8739f, -5.0613f, -5.2499f, -5.4394f, -5.6296f, -5.8203f, -5.0115f, -5.2028f, + -5.6057f, -5.4144f, -5.2234f, -5.0329f, -5.8431f, -5.6540f, -5.4660f, -5.2791f, -5.0936f, 5.9097f, 5.7275f, + 5.5473f, 5.3692f, 6.1935f, 6.0204f, 6.8501f, 6.6829f, 6.5190f, 6.3586f, 6.2022f, 6.0498f, 6.9020f, + 6.7589f, 6.6209f, 6.4884f, 6.3618f, 6.2414f, 6.1279f, 6.0215f, 6.9229f, 6.8326f, 6.7511f, 6.6792f, + 6.6176f, 6.5670f, 6.5282f, 6.5023f, 7.4902f, 7.4931f, 7.5122f, 7.5489f, -6.1908f, -6.0555f, -6.9259f, + -6.8024f, -6.6854f, -6.5754f, -6.4728f, -6.3783f, -6.2924f, -6.2156f, -6.1488f, -6.0926f, -6.0478f, -6.0154f, + -7.9963f, -7.9915f, -7.0024f, -7.0301f, -7.0763f, -7.1426f, -7.2308f, -7.3432f, -7.4822f, -8.6506f, -8.8519f, + -8.0897f, -8.3688f, -8.6946f, -9.0739f, -9.5149f, -9.0280f, -10.6267f +}; +}; + +} // namespace pandar_qt128 +} // namespace pandar_pointcloud diff --git a/pandar_pointcloud/launch/pandarQT128.launch b/pandar_pointcloud/launch/pandarQT128.launch new file mode 100644 index 0000000..81c2b11 --- /dev/null +++ b/pandar_pointcloud/launch/pandarQT128.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pandar_pointcloud/launch/pandar_cloud.launch b/pandar_pointcloud/launch/pandar_cloud.launch index 936180b..1c1ce83 100644 --- a/pandar_pointcloud/launch/pandar_cloud.launch +++ b/pandar_pointcloud/launch/pandar_cloud.launch @@ -2,15 +2,15 @@ - + - + - + + + + diff --git a/pandar_pointcloud/src/lib/decoder/pandar_qt128_decoder.cpp b/pandar_pointcloud/src/lib/decoder/pandar_qt128_decoder.cpp new file mode 100644 index 0000000..4761f66 --- /dev/null +++ b/pandar_pointcloud/src/lib/decoder/pandar_qt128_decoder.cpp @@ -0,0 +1,707 @@ +#include "pandar_pointcloud/decoder/pandar_qt128_decoder.hpp" +#include "pandar_pointcloud/decoder/pandar_qt128.hpp" + +namespace +{ +static inline double deg2rad(double degrees) +{ + return degrees * M_PI / 180.0; +} +} // namespace + +namespace pandar_pointcloud +{ +namespace pandar_qt128 +{ +PandarQT128Decoder::PandarQT128Decoder(Calibration& calibration, float scan_phase, + double dual_return_distance_threshold, ReturnMode return_mode) +{ + initFiringOffset(); + + block_offset_single_[1] = 7.0f * 1.0f + 111.11f; + block_offset_single_[0] = 7.0f * 0.0f + 111.11f; + + block_offset_dual_[1] = 7.0f * 0.0f + 111.11f; + block_offset_dual_[0] = 7.0f * 0.0f + 111.11f; + + // TODO: add calibration data validation + // if(calibration.elev_angle_map.size() != num_lasers_){ + // // calibration data is not valid! + // } + + scan_phase_ = static_cast(scan_phase * 100.0f); + return_mode_ = return_mode; + dual_return_distance_threshold_ = dual_return_distance_threshold; + + last_phase_ = 0; + has_scanned_ = false; + + scan_pc_.reset(new pcl::PointCloud); + overflow_pc_.reset(new pcl::PointCloud); +} + +bool PandarQT128Decoder::hasScanned() +{ + return has_scanned_; +} + +PointcloudXYZIRADT PandarQT128Decoder::getPointcloud() +{ + return scan_pc_; +} + +void PandarQT128Decoder::unpack(const pandar_msgs::PandarPacket& raw_packet) +{ + if (!parsePacket(raw_packet)) + { + return; + } + + if (has_scanned_) + { + scan_pc_ = overflow_pc_; + overflow_pc_.reset(new pcl::PointCloud); + 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 && return_mode_ != ReturnMode::FIRST) || + (packet_.return_mode == LAST_RETURN && return_mode_ != ReturnMode::LAST)) + { + ROS_WARN("Sensor return mode configuration does not match requested return mode"); + } + } + + for (int block_id = 0; block_id < BLOCK_NUM; 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; + } + return; +} + +PointXYZIRADT PandarQT128Decoder::build_point(int block_id, int unit_id, int seq_id, uint8_t return_type) +{ + const auto& block = packet_.blocks[block_id]; + const auto& unit = block.units[unit_id]; + double 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)) * 1e-02))); + point.y = static_cast(xyDistance * + cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast(block.azimuth)) * 1e-02))); + 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 + round(azimuth_offset_[unit_id] * 100.0f); + point.return_type = return_type; + point.time_stamp = unix_second + (static_cast(packet_.usec)) * 1e-06f; + point.time_stamp += + dual_return ? (static_cast(block_offset_dual_[block_id] + firing_offset_[seq_id][unit_id]) * 1e-06f) : + (static_cast(block_offset_single_[block_id] + firing_offset_[seq_id][unit_id]) * 1e-06f); + + return point; +} + +PointcloudXYZIRADT PandarQT128Decoder::convert(const int block_id) +{ + PointcloudXYZIRADT block_pc(new pcl::PointCloud); + + int seq_id = block_id; + + const auto& block = packet_.blocks[block_id]; + for (size_t unit_id = 0; unit_id < UNIT_NUM; ++unit_id) + { + PointXYZIRADT point; + const auto& unit = block.units[unit_id]; + // skip invalid points + if (unit.distance <= 0.1 || unit.distance > 200.0) + { + continue; + } + block_pc->push_back( + build_point(block_id, unit_id, seq_id, + (packet_.return_mode == FIRST_RETURN) ? ReturnType::SINGLE_FIRST : ReturnType::SINGLE_LAST)); + } + return block_pc; +} + +PointcloudXYZIRADT PandarQT128Decoder::convert_dual(const int 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 + PointcloudXYZIRADT block_pc(new pcl::PointCloud); + + int even_block_id = block_id; + int odd_block_id = block_id + 1; + const auto& even_block = packet_.blocks[even_block_id]; + const auto& odd_block = packet_.blocks[odd_block_id]; + + int seq_id; + if (return_mode_ == ReturnMode::DUAL) + { + if ((packet_.mode_flag >> 0) & 0x01) + { + if ((packet_.mode_flag >> 1) & 0x01) + seq_id = 0; + else + seq_id = block_id; + } + else + { + if ((packet_.mode_flag >> 1) & 0x01) + seq_id = 1 - block_id; + else + seq_id = 1; + } + } + else + { + seq_id = block_id; + } + + for (size_t unit_id = 0; unit_id < UNIT_NUM; ++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) ? 0 : 1; + bool odd_usable = (odd_unit.distance <= 0.1 || odd_unit.distance > 200.0) ? 0 : 1; + + if (return_mode_ == ReturnMode::FIRST && even_usable) + { + // First return is in even block + block_pc->push_back(build_point(even_block_id, unit_id, seq_id, ReturnType::SINGLE_FIRST)); + } + else if (return_mode_ == ReturnMode::LAST && even_usable) + { + // Last return is in odd block + block_pc->push_back(build_point(odd_block_id, unit_id, seq_id, ReturnType::SINGLE_LAST)); + } + else if (return_mode_ == ReturnMode::DUAL) + { + // 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, seq_id, ReturnType::DUAL_ONLY)); + } + else + { + if (even_usable) + { + block_pc->push_back(build_point(even_block_id, unit_id, seq_id, ReturnType::DUAL_FIRST)); + } + if (odd_usable) + { + block_pc->push_back(build_point(odd_block_id, unit_id, seq_id, ReturnType::DUAL_LAST)); + } + } + } + } + return block_pc; +} + +bool PandarQT128Decoder::parsePacket(const pandar_msgs::PandarPacket& raw_packet) +{ + if (raw_packet.size != PACKET_SIZE) + { + return false; + } + const uint8_t* buf = &raw_packet.data[0]; + + size_t index = 0; + // Parse 12 Bytes Header + packet_.header.u16Sob = ((buf[index] & 0xff) << 8) | ((buf[index + 1] & 0xff)); + packet_.header.u8ProtocolMajor = buf[index + 2] & 0xff; + packet_.header.u8ProtocolMinor = buf[index + 3] & 0xff; + /* <--- Reserved 2 bytes [index + 4], [index + 5] ---> */ + packet_.header.u8LaserNum = buf[index + 6] & 0xff; + packet_.header.u8BlockNum = buf[index + 7] & 0xff; + /* <--- Reserved 1 byte [index + 8] ---> */ + packet_.header.u8DistUnit = buf[index + 9] & 0xff; + packet_.header.u8EchoNum = buf[index + 10] & 0xff; + packet_.header.u8Flags = buf[index + 11] & 0xff; + index += HEAD_SIZE; + + if (packet_.header.u16Sob != 0xEEFF) + { + // Error Start of Packet! + return false; + } + + // Parse 1032 Bytes Body + for (size_t block = 0; block < packet_.header.u8BlockNum; block++) + { + packet_.blocks[block].azimuth = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + index += BLOCK_HEADER_AZIMUTH; + + for (int unit = 0; unit < packet_.header.u8LaserNum; unit++) + { + unsigned int unRange = (buf[index] & 0xff) | ((buf[index + 1] & 0xff) << 8); + + packet_.blocks[block].units[unit].distance = + (static_cast(unRange * packet_.header.u8DistUnit)) / (double)1000; + packet_.blocks[block].units[unit].intensity = (buf[index + 2] & 0xff); + index += UNIT_SIZE; + } + } + + index += CRC_1_SIZE; + + // Parse 17 Bytes Functional Safety + index += FUNCTIONAL_SAFETY_SIZE; + + // Parse 34 Bytes Tail + index += RESERVED_1_SIZE; + + packet_.mode_flag = buf[index] & 0xff; + index += MODE_FLAG_SIZE; + + index += RESERVED_2_SIZE; + + packet_.return_mode = buf[index] & 0xff; + index += RETURN_MODE_SIZE; + + index += MOTOR_SPEED_SIZE; + + packet_.t.tm_year = (buf[index + 0] & 0xff) + 100; // Year (Current year minus 1900) + packet_.t.tm_mon = (buf[index + 1] & 0xff) - 1; // Month + packet_.t.tm_mday = buf[index + 2] & 0xff; // Day + packet_.t.tm_hour = buf[index + 3] & 0xff; // Hour + packet_.t.tm_min = buf[index + 4] & 0xff; // Minute + packet_.t.tm_sec = buf[index + 5] & 0xff; // Second + packet_.t.tm_isdst = 0; + + // in case of time error + if (packet_.t.tm_year >= 200) + { + packet_.t.tm_year -= 100; + } + + index += DATE_TIME_SIZE; + + return true; +} + +void PandarQT128Decoder::initFiringOffset() +{ + /* Firing Order 1-10 @ p. 62 User Manual */ + /* Firing Order 1 */ + firing_offset_[0][99 - 1] = 0.6f; + firing_offset_[1][65 - 1] = 0.6f; + + /* Firing Order 2 */ + firing_offset_[0][65 - 1] = 1.456f; + firing_offset_[1][99 - 1] = 1.456f; + + /* Firing Order 3 */ + firing_offset_[0][35 - 1] = 2.312f; + firing_offset_[1][1 - 1] = 2.312f; + + /* Firing Order 4 */ + firing_offset_[0][102 - 1] = 3.768f; + firing_offset_[1][72 - 1] = 3.768f; + + /* Firing Order 5 */ + firing_offset_[0][72 - 1] = 4.624f; + firing_offset_[1][102 - 1] = 4.624f; + + /* Firing Order 6 */ + firing_offset_[0][38 - 1] = 5.48f; + firing_offset_[1][8 - 1] = 5.48f; + + /* Firing Order 7 */ + firing_offset_[0][107 - 1] = 6.936f; + firing_offset_[1][73 - 1] = 6.936f; + + /* Firing Order 8 */ + firing_offset_[0][73 - 1] = 7.792f; + firing_offset_[1][107 - 1] = 7.792f; + + /* Firing Order 9 */ + firing_offset_[0][43 - 1] = 8.648f; + firing_offset_[1][9 - 1] = 8.648f; + + /* Firing Order 10 */ + firing_offset_[0][110 - 1] = 10.104f; + firing_offset_[1][80 - 1] = 10.104f; + + /* Firing Order 11-20 @ p. 62 User Manual */ + /* Firing Order 11 */ + firing_offset_[0][80 - 1] = 10.96f; + firing_offset_[1][110 - 1] = 10.96f; + + /* Firing Order 12 */ + firing_offset_[0][46 - 1] = 11.816f; + firing_offset_[1][16 - 1] = 11.816f; + + /* Firing Order 13 */ + firing_offset_[0][115 - 1] = 13.272f; + firing_offset_[1][81 - 1] = 13.272f; + + /* Firing Order 14 */ + firing_offset_[0][81 - 1] = 14.128f; + firing_offset_[1][115 - 1] = 14.128f; + + /* Firing Order 15 */ + firing_offset_[0][51 - 1] = 14.984f; + firing_offset_[1][17 - 1] = 14.984f; + + /* Firing Order 16 */ + firing_offset_[0][118 - 1] = 16.44f; + firing_offset_[1][88 - 1] = 16.44f; + + /* Firing Order 17 */ + firing_offset_[0][88 - 1] = 17.296f; + firing_offset_[1][118 - 1] = 17.296f; + + /* Firing Order 18 */ + firing_offset_[0][54 - 1] = 18.152f; + firing_offset_[1][24 - 1] = 18.152f; + + /* Firing Order 19 */ + firing_offset_[0][123 - 1] = 19.608f; + firing_offset_[1][89 - 1] = 19.608f; + + /* Firing Order 20 */ + firing_offset_[0][89 - 1] = 20.464f; + firing_offset_[1][123 - 1] = 20.464f; + + /* Firing Order 21-30 @ p. 62 User Manual */ + /* Firing Order 21 */ + firing_offset_[0][59 - 1] = 21.32f; + firing_offset_[1][25 - 1] = 21.32f; + + /* Firing Order 22 */ + firing_offset_[0][126 - 1] = 22.776f; + firing_offset_[1][96 - 1] = 22.776f; + + /* Firing Order 23 */ + firing_offset_[0][96 - 1] = 23.632f; + firing_offset_[1][126 - 1] = 23.632f; + + /* Firing Order 24 */ + firing_offset_[0][62 - 1] = 24.488f; + firing_offset_[1][32 - 1] = 24.488f; + + /* Firing Order 25 */ + firing_offset_[0][97 - 1] = 25.944f; + firing_offset_[1][67 - 1] = 25.944f; + + /* Firing Order 26 */ + firing_offset_[0][67 - 1] = 26.8f; + firing_offset_[1][97 - 1] = 26.8f; + + /* Firing Order 27 */ + firing_offset_[0][33 - 1] = 27.656f; + firing_offset_[1][3 - 1] = 27.656f; + + /* Firing Order 28 */ + firing_offset_[0][104 - 1] = 29.112f; + firing_offset_[1][70 - 1] = 29.112f; + + /* Firing Order 29 */ + firing_offset_[0][70 - 1] = 29.968f; + firing_offset_[1][104 - 1] = 29.968f; + + /* Firing Order 30 */ + firing_offset_[0][40 - 1] = 30.824f; + firing_offset_[1][6 - 1] = 30.824f; + + /* Firing Order 31-40 @ p. 62 User Manual */ + /* Firing Order 31 */ + firing_offset_[0][105 - 1] = 32.28f; + firing_offset_[1][75 - 1] = 32.28f; + + /* Firing Order 32 */ + firing_offset_[0][75 - 1] = 33.136f; + firing_offset_[1][105 - 1] = 33.136f; + + /* Firing Order 33 */ + firing_offset_[0][41 - 1] = 33.992f; + firing_offset_[1][11 - 1] = 33.992f; + + /* Firing Order 34 */ + firing_offset_[0][112 - 1] = 35.448f; + firing_offset_[1][78 - 1] = 35.448f; + + /* Firing Order 35 */ + firing_offset_[0][78 - 1] = 36.304f; + firing_offset_[1][112 - 1] = 36.304f; + + /* Firing Order 36 */ + firing_offset_[0][48 - 1] = 37.16f; + firing_offset_[1][14 - 1] = 37.16f; + + /* Firing Order 37 */ + firing_offset_[0][113 - 1] = 38.616f; + firing_offset_[1][83 - 1] = 38.616f; + + /* Firing Order 38 */ + firing_offset_[0][83 - 1] = 39.472f; + firing_offset_[1][113 - 1] = 39.472f; + + /* Firing Order 39 */ + firing_offset_[0][49 - 1] = 40.328f; + firing_offset_[1][19 - 1] = 40.328f; + + /* Firing Order 40 */ + firing_offset_[0][120 - 1] = 41.784f; + firing_offset_[1][86 - 1] = 41.784f; + + /* Firing Order 41-50 @ p. 63 User Manual */ + /* Firing Order 41 */ + firing_offset_[0][86 - 1] = 42.64f; + firing_offset_[1][120 - 1] = 42.64f; + + /* Firing Order 42 */ + firing_offset_[0][56 - 1] = 43.496f; + firing_offset_[1][22 - 1] = 43.496f; + + /* Firing Order 43 */ + firing_offset_[0][121 - 1] = 44.952f; + firing_offset_[1][91 - 1] = 44.952f; + + /* Firing Order 44 */ + firing_offset_[0][91 - 1] = 45.808f; + firing_offset_[1][121 - 1] = 45.808f; + + /* Firing Order 45 */ + firing_offset_[0][57 - 1] = 46.664f; + firing_offset_[1][27 - 1] = 46.664f; + + /* Firing Order 46 */ + firing_offset_[0][128 - 1] = 48.12f; + firing_offset_[1][94 - 1] = 48.12f; + + /* Firing Order 47 */ + firing_offset_[0][94 - 1] = 48.976f; + firing_offset_[1][128 - 1] = 48.976f; + + /* Firing Order 48 */ + firing_offset_[0][64 - 1] = 49.832f; + firing_offset_[1][30 - 1] = 49.832f; + + /* Firing Order 49 */ + firing_offset_[0][98 - 1] = 51.288f; + firing_offset_[1][68 - 1] = 51.288f; + + /* Firing Order 50 */ + firing_offset_[0][68 - 1] = 52.144f; + firing_offset_[1][98 - 1] = 52.144f; + + /* Firing Order 51-60 @ p. 63 User Manual */ + /* Firing Order 51 */ + firing_offset_[0][34 - 1] = 53.0f; + firing_offset_[1][4 - 1] = 53.0f; + + /* Firing Order 52 */ + firing_offset_[0][103 - 1] = 54.456f; + firing_offset_[1][69 - 1] = 54.456f; + + /* Firing Order 53 */ + firing_offset_[0][69 - 1] = 55.312f; + firing_offset_[1][103 - 1] = 55.312f; + + /* Firing Order 54 */ + firing_offset_[0][39 - 1] = 56.168f; + firing_offset_[1][5 - 1] = 56.168f; + + /* Firing Order 55 */ + firing_offset_[0][106 - 1] = 57.624f; + firing_offset_[1][76 - 1] = 57.624f; + + /* Firing Order 56 */ + firing_offset_[0][76 - 1] = 58.48f; + firing_offset_[1][106 - 1] = 58.48f; + + /* Firing Order 57 */ + firing_offset_[0][42 - 1] = 59.336f; + firing_offset_[1][12 - 1] = 59.336f; + + /* Firing Order 58 */ + firing_offset_[0][111 - 1] = 60.792f; + firing_offset_[1][77 - 1] = 60.792f; + + /* Firing Order 59 */ + firing_offset_[0][77 - 1] = 61.648f; + firing_offset_[1][111 - 1] = 61.648f; + + /* Firing Order 60 */ + firing_offset_[0][47 - 1] = 62.504f; + firing_offset_[1][13 - 1] = 62.504f; + + /* Firing Order 61-70 @ p. 63 User Manual */ + /* Firing Order 61 */ + firing_offset_[0][114 - 1] = 63.96f; + firing_offset_[1][84 - 1] = 63.96f; + + /* Firing Order 62 */ + firing_offset_[0][84 - 1] = 64.816f; + firing_offset_[1][114 - 1] = 64.816f; + + /* Firing Order 63 */ + firing_offset_[0][50 - 1] = 65.672f; + firing_offset_[1][20 - 1] = 65.672f; + + /* Firing Order 64 */ + firing_offset_[0][119 - 1] = 67.128f; + firing_offset_[1][85 - 1] = 67.128f; + + /* Firing Order 65 */ + firing_offset_[0][85 - 1] = 67.984f; + firing_offset_[1][119 - 1] = 67.984f; + + /* Firing Order 66 */ + firing_offset_[0][55 - 1] = 68.84f; + firing_offset_[1][21 - 1] = 68.84f; + + /* Firing Order 67 */ + firing_offset_[0][122 - 1] = 70.296f; + firing_offset_[1][92 - 1] = 70.296f; + + /* Firing Order 68 */ + firing_offset_[0][92 - 1] = 71.152f; + firing_offset_[1][122 - 1] = 71.152f; + + /* Firing Order 69 */ + firing_offset_[0][58 - 1] = 72.008f; + firing_offset_[1][28 - 1] = 72.008f; + + /* Firing Order 70 */ + firing_offset_[0][127 - 1] = 73.464f; + firing_offset_[1][93 - 1] = 73.464f; + + /* Firing Order 71-80 @ p. 63 User Manual */ + /* Firing Order 71 */ + firing_offset_[0][93 - 1] = 74.32f; + firing_offset_[1][127 - 1] = 74.32f; + + /* Firing Order 72 */ + firing_offset_[0][63 - 1] = 75.176f; + firing_offset_[1][29 - 1] = 75.176f; + + /* Firing Order 73 */ + firing_offset_[0][100 - 1] = 76.632f; + firing_offset_[1][66 - 1] = 76.632f; + + /* Firing Order 74 */ + firing_offset_[0][66 - 1] = 77.488f; + firing_offset_[1][100 - 1] = 77.488f; + + /* Firing Order 75 */ + firing_offset_[0][36 - 1] = 78.344f; + firing_offset_[1][2 - 1] = 78.344f; + + /* Firing Order 76 */ + firing_offset_[0][101 - 1] = 79.8f; + firing_offset_[1][71 - 1] = 79.8f; + + /* Firing Order 77 */ + firing_offset_[0][71 - 1] = 80.656f; + firing_offset_[1][101 - 1] = 80.656f; + + /* Firing Order 78 */ + firing_offset_[0][37 - 1] = 81.512f; + firing_offset_[1][7 - 1] = 81.512f; + + /* Firing Order 79 */ + firing_offset_[0][108 - 1] = 82.968f; + firing_offset_[1][74 - 1] = 82.968f; + + /* Firing Order 80 */ + firing_offset_[0][74 - 1] = 83.824f; + firing_offset_[1][108 - 1] = 83.824f; + + /* Firing Order 81-90 @ p. 64 User Manual */ + /* Firing Order 81 */ + firing_offset_[0][44 - 1] = 84.68f; + firing_offset_[1][10 - 1] = 84.68f; + + /* Firing Order 82 */ + firing_offset_[0][109 - 1] = 86.136f; + firing_offset_[1][79 - 1] = 86.136f; + + /* Firing Order 83 */ + firing_offset_[0][79 - 1] = 86.992f; + firing_offset_[1][109 - 1] = 86.992f; + + /* Firing Order 84 */ + firing_offset_[0][45 - 1] = 87.848f; + firing_offset_[1][15 - 1] = 87.848f; + + /* Firing Order 85 */ + firing_offset_[0][116 - 1] = 89.304f; + firing_offset_[1][82 - 1] = 89.304f; + + /* Firing Order 86 */ + firing_offset_[0][82 - 1] = 90.16f; + firing_offset_[1][116 - 1] = 90.16f; + + /* Firing Order 87 */ + firing_offset_[0][52 - 1] = 91.016f; + firing_offset_[1][18 - 1] = 91.016f; + + /* Firing Order 88 */ + firing_offset_[0][117 - 1] = 92.472f; + firing_offset_[1][87 - 1] = 92.472f; + + /* Firing Order 89 */ + firing_offset_[0][87 - 1] = 93.328f; + firing_offset_[1][117 - 1] = 93.328f; + + /* Firing Order 90 */ + firing_offset_[0][53 - 1] = 94.184f; + firing_offset_[1][23 - 1] = 94.184f; + + /* Firing Order 91-96 @ p. 64 User Manual */ + /* Firing Order 91 */ + firing_offset_[0][124 - 1] = 95.64f; + firing_offset_[1][90 - 1] = 95.64f; + + /* Firing Order 92 */ + firing_offset_[0][90 - 1] = 96.496f; + firing_offset_[1][124 - 1] = 96.496f; + + /* Firing Order 93 */ + firing_offset_[0][60 - 1] = 97.352f; + firing_offset_[1][26 - 1] = 97.352f; + + /* Firing Order 94 */ + firing_offset_[0][125 - 1] = 98.808f; + firing_offset_[1][95 - 1] = 98.808f; + + /* Firing Order 95 */ + firing_offset_[0][95 - 1] = 99.664f; + firing_offset_[1][125 - 1] = 99.664f; + + /* Firing Order 96 */ + firing_offset_[0][61 - 1] = 100.52f; + firing_offset_[1][31 - 1] = 100.52f; +} + +} // namespace pandar_qt128 +} // namespace pandar_pointcloud diff --git a/pandar_pointcloud/src/pandar_cloud.cpp b/pandar_pointcloud/src/pandar_cloud.cpp index f3a2e9c..604adbd 100644 --- a/pandar_pointcloud/src/pandar_cloud.cpp +++ b/pandar_pointcloud/src/pandar_cloud.cpp @@ -6,6 +6,7 @@ #include "pandar_pointcloud/decoder/pandar_xt_decoder.hpp" #include "pandar_pointcloud/decoder/pandar_xtm_decoder.hpp" #include "pandar_pointcloud/decoder/pandar64_decoder.hpp" +#include "pandar_pointcloud/decoder/pandar_qt128_decoder.hpp" #include #include @@ -41,7 +42,7 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh) else if (return_mode_ == "Dual") selected_return_mode = pandar40::Pandar40Decoder::ReturnMode::DUAL; else { - ROS_ERROR("Invalid return mode, defaulting to strongest return mode"); + ROS_ERROR("Invalid return mode, defaulting to strongest return mode"); selected_return_mode = pandar40::Pandar40Decoder::ReturnMode::STRONGEST; } decoder_ = std::make_shared(calibration_, scan_phase_, @@ -57,7 +58,7 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh) else if (return_mode_ == "Dual") selected_return_mode = pandar_qt::PandarQTDecoder::ReturnMode::DUAL; else { - ROS_ERROR("Invalid return mode, defaulting to dual return mode"); + ROS_ERROR("Invalid return mode, defaulting to dual return mode"); selected_return_mode = pandar_qt::PandarQTDecoder::ReturnMode::DUAL; } decoder_ = std::make_shared(calibration_, scan_phase_, @@ -75,7 +76,7 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh) else if (return_mode_ == "Dual") selected_return_mode = pandar_xt::PandarXTDecoder::ReturnMode::DUAL; else { - ROS_ERROR("Invalid return mode, defaulting to dual return mode"); + ROS_ERROR("Invalid return mode, defaulting to dual return mode"); selected_return_mode = pandar_xt::PandarXTDecoder::ReturnMode::DUAL; } decoder_ = std::make_shared(calibration_, scan_phase_, @@ -123,6 +124,23 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh) dual_return_distance_threshold_, selected_return_mode); } + else if (model_ == "PandarQT128") { + std::cout << "Constructor" << std::endl; + pandar_qt128::PandarQT128Decoder::ReturnMode selected_return_mode; + if (return_mode_ == "First") + selected_return_mode = pandar_qt128::PandarQT128Decoder::ReturnMode::FIRST; + else if (return_mode_ == "Last") + selected_return_mode = pandar_qt128::PandarQT128Decoder::ReturnMode::LAST; + else if (return_mode_ == "Dual") + selected_return_mode = pandar_qt128::PandarQT128Decoder::ReturnMode::DUAL; + else { + ROS_ERROR("Invalid return mode, defaulting to dual return mode"); + selected_return_mode = pandar_qt128::PandarQT128Decoder::ReturnMode::DUAL; + } + decoder_ = std::make_shared(calibration_, scan_phase_, + dual_return_distance_threshold_, + selected_return_mode); + } else { // TODO : Add other models ROS_ERROR("Invalid model name");