Feature/qt128 (#1)

* add qt 128 support * add qt128 decoder * fix correction csv
MapIV · Oct 26, 2022 · a16e091 · a16e091
1 parent 429f3c6
commit a16e091
Show file tree

Hide file tree

Showing 10 changed files with 1,095 additions and 11 deletions.
diff --git 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
@@ -2,7 +2,7 @@
 <package format="2">
   <name>pandar_msgs</name>
   <version>0.0.0</version>
-  <description> ROS message definition for the Heisai PandarQT/Pandar64/Pandar40P/Pandar20A/Pandar20B/Pandar40M LiDAR sensor.
+  <description> ROS message definition for the Heisai Pandar128/PandarQT128/PandarQT/Pandar64/Pandar40P/Pandar20A/Pandar20B/Pandar40M LiDAR sensor.
   </description>
   <maintainer email="[email protected]">hirakawa</maintainer>
   <license>Apache 2</license>

diff --git 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
@@ -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
@@ -0,0 +1,90 @@
+#pragma once
+
+/**
+ * Pandar QT128
+ */
+
+#include <cstdint>
+
+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
@@ -0,0 +1,92 @@
+#pragma once
+
+#include <array>
+#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<std::array<float, UNIT_NUM>, BLOCK_NUM> firing_offset_;
+  std::array<float, BLOCK_NUM> block_offset_single_;
+  std::array<float, BLOCK_NUM> 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<float, UNIT_NUM> 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<float, UNIT_NUM> 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
@@ -0,0 +1,43 @@
+<launch>
+  <arg name="launch_driver" default="false" />
+
+  <arg name="pcap"  default=""/>
+  <arg name="device_ip" default="192.168.1.201"/>
+  <arg name="lidar_port"  default="2368"/>
+  <arg name="gps_port"  default="10110"/>
+  <arg name="scan_phase"  default="0"/>
+  <arg name="return_mode" default="Dual"/>
+  <arg name="dual_return_distance_threshold" default="0.1"/>
+  <arg name="model" default="PandarQT128"/>
+  <arg name="frame_id" default="pandar"/>
+  <arg name="calibration"  default="$(find pandar_pointcloud)/config/qt128.csv"/>
+  <arg name="manager" default="pandar_nodelet_manager"/>
+
+  <!-- nodelet manager -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
+
+  <!-- pandar driver -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_driver"
+        args="load pandar_driver/DriverNodelet $(arg manager)" if="$(arg launch_driver)" output="screen">
+    <param name="pcap" type="string" value="$(arg pcap)"/>
+    <param name="device_ip" type="string" value="$(arg device_ip)"/>
+    <param name="lidar_port"  type="int" value="$(arg lidar_port)"/>
+    <param name="gps_port"  type="int" value="$(arg gps_port)"/>
+    <param name="scan_phase"  type="double" value="$(arg scan_phase)"/>
+    <param name="model"  type="string" value="$(arg model)"/>
+    <param name="frame_id"  type="string" value="$(arg frame_id)"/>
+  </node>
+
+  <!-- pandar cloud -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_cloud"
+        args="load pandar_pointcloud/CloudNodelet $(arg manager)" output="screen">
+    <remap from="pandar_points" to="pointcloud_raw" />
+    <remap from="pandar_points_ex" to="pointcloud_raw_ex" />
+    <param name="scan_phase" type="double" value="$(arg scan_phase)"/>
+    <param name="return_mode"  type="string" value="$(arg return_mode)"/>
+    <param name="dual_return_distance_threshold"  type="double" value="$(arg dual_return_distance_threshold)"/>
+    <param name="model" type="string" value="$(arg model)"/>
+    <param name="device_ip" type="string" value="$(arg device_ip)"/>
+    <param name="calibration" type="string" value="$(arg calibration)"/>
+  </node>
+</launch>