Feature/multiple return (#2)

* Add return classification for Pandar40P Signed-off-by: davidw <[email protected]> * Remove redundant code for single return mode Signed-off-by: davidw <[email protected]> * Correct return type labelling for single return mode Signed-off-by: davidw <[email protected]> * Tidy comments for conversion in dual mode Signed-off-by: davidw <[email protected]> * Add PandarQT multiple echo support Signed-off-by: davidw <[email protected]> * Correct differences in timestamping of points between echo modes and devices Signed-off-by: davidw <[email protected]> * Add return distance threshold parameter Signed-off-by: davidw <[email protected]> * Change return types to make 0 invalid, all values positive integers Signed-off-by: davidw <[email protected]>
MapIV · May 18, 2021 · 5ac6004 · 5ac6004
1 parent 5dbe370
commit 5ac6004
Show file tree

Hide file tree

Showing 12 changed files with 247 additions and 127 deletions.
diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar40.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar40.hpp
@@ -15,17 +15,17 @@ 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 ECHO_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 + ECHO_SIZE + RESERVE_SIZE + REVOLUTION_SIZE;
+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_ECHO = 0x37;
-constexpr uint32_t LAST_ECHO = 0x38;
-constexpr uint32_t DUAL_ECHO = 0x39;
+constexpr uint32_t STRONGEST_RETURN = 0x37;
+constexpr uint32_t LAST_RETURN = 0x38;
+constexpr uint32_t DUAL_RETURN = 0x39;
 
 struct Unit
 {
@@ -45,7 +45,7 @@ struct Packet
   Block blocks[BLOCKS_PER_PACKET];
   struct tm t;
   uint32_t usec;
-  uint32_t echo;
+  uint32_t return_mode;
 };
 
 }  // namespace pandar40

diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar40_decoder.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar40_decoder.hpp
@@ -18,14 +18,26 @@ class Pandar40Decoder : public PacketDecoder
     STRONGEST,
     LAST,
   };
+  enum ReturnType : int8_t
+  {
+    INVALID = 0,
+    SINGLE_STRONGEST,
+    SINGLE_LAST,
+    DUAL_STRONGEST_FIRST,
+    DUAL_STRONGEST_LAST,
+    DUAL_WEAK_FIRST,
+    DUAL_WEAK_LAST,
+    DUAL_ONLY,
+  };
 
-  Pandar40Decoder(Calibration& calibration, float scan_phase = 0.0f, ReturnMode return_mode = ReturnMode::DUAL);
+  Pandar40Decoder(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;
   bool hasScanned() override;
   PointcloudXYZIRADT getPointcloud() override;
 
 private:
   bool parsePacket(const pandar_msgs::PandarPacket& raw_packet);
+  PointXYZIRADT build_point(int block_id, int unit_id, int8_t return_type);
   PointcloudXYZIRADT convert(const int block_id);
   PointcloudXYZIRADT convert_dual(const int block_id);
 
@@ -39,6 +51,7 @@ class Pandar40Decoder : public PacketDecoder
   std::array<size_t, LASER_COUNT> firing_order_;
 
   ReturnMode return_mode_;
+  double dual_return_distance_threshold_;
   Packet packet_;
 
   PointcloudXYZIRADT scan_pc_;

diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt.hpp
@@ -22,7 +22,7 @@ constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCK_NUM;
 constexpr size_t RESERVED_SIZE = 10;
 constexpr size_t ENGINE_VELOCITY = 2;
 constexpr size_t TIMESTAMP_SIZE = 4;
-constexpr size_t ECHO_SIZE = 1;
+constexpr size_t RETURN_SIZE = 1;
 constexpr size_t FACTORY_SIZE = 1;
 constexpr size_t UTC_SIZE = 6;
 constexpr size_t SEQUENCE_SIZE = 4;
@@ -33,9 +33,9 @@ constexpr size_t PACKET_TAIL_WITHOUT_UDPSEQ_SIZE = 24;
 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_ECHO = 0x33;
-constexpr uint32_t LAST_ECHO = 0x38;
-constexpr uint32_t DUAL_ECHO = 0x3B;
+constexpr uint32_t FIRST_RETURN = 0x33;
+constexpr uint32_t LAST_RETURN = 0x38;
+constexpr uint32_t DUAL_RETURN = 0x3B;
 
 struct Header
 {
@@ -68,7 +68,7 @@ struct Packet
   Header header;
   Block blocks[BLOCK_NUM];
   uint32_t usec;  // ms
-  uint32_t echo;
+  uint32_t return_mode;
   tm t;
 };
 }  // namespace pandar_qt

diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt_decoder.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_qt_decoder.hpp
@@ -18,9 +18,19 @@ class PandarQTDecoder : public PacketDecoder
     FIRST,
     LAST,
   };
+  enum ReturnType : int8_t
+  {
+    INVALID = 0,
+    SINGLE_FIRST,
+    SINGLE_LAST,
+    DUAL_FIRST,
+    DUAL_LAST,
+    DUAL_ONLY,
+  };
 
-  PandarQTDecoder(Calibration& calibration, float scan_phase = 0.0f, ReturnMode return_mode = ReturnMode::DUAL);
+  PandarQTDecoder(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, int8_t return_type);
   bool hasScanned() override;
   PointcloudXYZIRADT getPointcloud() override;
 
@@ -37,6 +47,7 @@ class PandarQTDecoder : public PacketDecoder
   std::array<float, BLOCK_NUM> block_offset_dual_;
 
   ReturnMode return_mode_;
+  double dual_return_distance_threshold_;
   Packet packet_;
 
   PointcloudXYZIRADT scan_pc_;

diff --git a/pandar_pointcloud/include/pandar_pointcloud/pandar_cloud.hpp b/pandar_pointcloud/include/pandar_pointcloud/pandar_cloud.hpp
@@ -23,8 +23,10 @@ class PandarCloud
   pcl::PointCloud<PointXYZIR>::Ptr convertPointcloud(const pcl::PointCloud<PointXYZIRADT>::ConstPtr& input_pointcloud);
 
   std::string model_;
+  std::string return_mode_;
   std::string device_ip_;
   std::string calibration_path_;
+  double dual_return_distance_threshold_;
   double scan_phase_;
 
   ros::Subscriber pandar_packet_sub_;

diff --git a/pandar_pointcloud/include/pandar_pointcloud/point_types.hpp b/pandar_pointcloud/include/pandar_pointcloud/point_types.hpp
@@ -20,6 +20,7 @@ struct PointXYZIRADT
   uint16_t ring;
   float azimuth;
   float distance;
+  int8_t return_type;
   double time_stamp;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 } EIGEN_ALIGN16;
@@ -35,5 +36,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZIR,
 POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_pointcloud::PointXYZIRADT,
                                   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
                                       std::uint16_t, ring, ring)(float, azimuth, azimuth)(float, distance,
-                                                                                          distance)(double, time_stamp,
-                                                                                                    time_stamp))
+                                                                                          distance)(int8_t, return_type,
+                                                                                          return_type)(double, time_stamp,
+                                                                                                        time_stamp))
diff --git a/pandar_pointcloud/launch/pandar40P.launch b/pandar_pointcloud/launch/pandar40P.launch
@@ -6,6 +6,8 @@
   <arg name="lidar_port"  default="2368"/>
   <arg name="gps_port"  default="10110"/>
   <arg name="scan_phase"  default="0"/>
+  <arg name="dual_return_distance_threshold" default="0.1"/>
+  <arg name="return_mode" default="Dual"/>
   <arg name="model" default="Pandar40P"/>
   <arg name="frame_id" default="pandar"/>
   <arg name="calibration"  default="$(find pandar_pointcloud)/config/40p.csv"/>
@@ -32,6 +34,8 @@
     <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)"/>

diff --git a/pandar_pointcloud/launch/pandarQT.launch b/pandar_pointcloud/launch/pandarQT.launch
@@ -6,6 +6,8 @@
   <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="PandarQT"/>
   <arg name="frame_id" default="pandar"/>
   <arg name="calibration"  default="$(find pandar_pointcloud)/config/qt.csv"/>
@@ -32,6 +34,8 @@
     <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)"/>

diff --git a/pandar_pointcloud/launch/pandar_cloud.launch b/pandar_pointcloud/launch/pandar_cloud.launch
@@ -1,5 +1,7 @@
 <launch>
   <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="Pandar40P"/>
   <arg name="device_ip" default="192.168.1.201"/>
   <arg name="calibration"  default="$(find pandar_pointcloud)/config/40p.csv"/>
@@ -18,6 +20,8 @@
         args="load pandar_pointcloud/CloudNodelet $(arg manager)">
     <param name="scan_phase"  type="double" value="$(arg scan_phase)"/>
     <param name="model"  type="string" value="$(arg model)"/>
+    <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="device_ip" type="string" value="$(arg device_ip)"/>
   </node>
 </launch>