Merge pull request #2 from Perception-Engine/xtm

Add support for Hesai Pandar XT32M

pandar_driver/package.xml

@@ -16,6 +16,7 @@
   <depend>pluginlib</depend>
   <depend>pandar_msgs</depend>
   <depend>pandar_api</depend>
+  <depend>libpcap</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelet_pandar_driver.xml"/>

pandar_driver/src/driver/pandar_driver.cpp

@@ -49,6 +49,10 @@ PandarDriver::PandarDriver(ros::NodeHandle node, ros::NodeHandle private_nh)
     azimuth_index_ = 12;  // 12 + 386 * [0-1]
     is_valid_packet_ = [](size_t packet_size) { return (packet_size == 812); };
   }
+  else if (model_ == "PandarXTM") {
+    azimuth_index_ = 12;  // 12 + 130 * [0-7]
+    is_valid_packet_ = [](size_t packet_size) { return (packet_size == 820); };
+  }
   else {
     ROS_ERROR("Invalid model name");
   }

pandar_pointcloud/CMakeLists.txt

@@ -37,6 +37,7 @@ add_library(pandar_cloud
   src/lib/decoder/pandar40_decoder.cpp
   src/lib/decoder/pandar_qt_decoder.cpp
   src/lib/decoder/pandar_xt_decoder.cpp
+  src/lib/decoder/pandar_xtm_decoder.cpp
   src/lib/decoder/pandar64_decoder.cpp
 )
 target_link_libraries(pandar_cloud

pandar_pointcloud/config/xtm.csv

@@ -0,0 +1,33 @@
+Channel,Elevation,Azimuth
+1,19.5,0
+2,18.2,0
+3,16.9,0
+4,15.6,0
+5,14.3,0
+6,13.0,0
+7,11.7,0
+8,10.4,0
+9,9.1,0
+10,7.8,0
+11,6.5,0
+12,5.2,0
+13,3.9,0
+14,2.6,0
+15,1.3,0
+16,0.0,0
+17,-1.3,0
+18,-2.6,0
+19,-3.9,0
+20,-5.2,0
+21,-6.5,0
+22,-7.8,0
+23,-9.1,0
+24,-10.4,0
+25,-11.7,0
+26,-13.0,0
+27,-14.3,0
+28,-15.6,0
+29,-16.9,0
+30,-18.2,0
+31,-19.5,0
+32,-20.8,0

pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm.hpp

@@ -0,0 +1,83 @@
+#pragma once
+/**
+ * Pandar XT-32
+ */
+#include <cstdint>
+namespace pandar_pointcloud
+{
+namespace pandar_xtm
+{
+
+
+// Head
+constexpr size_t HEAD_SIZE = 12;
+constexpr size_t PRE_HEADER_SIZE = 6;
+constexpr size_t HEADER_SIZE = 6;
+// Body
+constexpr size_t BLOCK_NUM = 8;
+constexpr size_t BLOCK_HEADER_AZIMUTH = 2;
+constexpr size_t UNIT_NUM = 32;
+constexpr size_t UNIT_SIZE = 4;
+constexpr size_t BLOCK_SIZE = UNIT_SIZE * UNIT_NUM + BLOCK_HEADER_AZIMUTH;
+constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCK_NUM;
+// 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 = 820;
+
+// 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;
+constexpr uint32_t DUAL_RETURN_B = 0x3b;
+constexpr uint32_t DUAL_RETURN_C = 0x3c;
+constexpr uint32_t TRIPLE_RETURN = 0x3d;
+
+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
+{
+  double distance;
+  uint16_t intensity;
+  uint16_t confidence;
+};
+
+struct Block
+{
+  uint16_t azimuth;  // packet angle,Azimuth = RealAzimuth * 100
+  Unit units[UNIT_NUM];
+};
+
+struct Packet
+{
+  Header header;
+  Block blocks[BLOCK_NUM];
+  uint32_t usec;  // ms
+  uint32_t return_mode;
+  tm t;
+};
+}  // namespace pandar_qt
+}  // namespace pandar_pointcloud

pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm_decoder.hpp

@@ -0,0 +1,150 @@
+#pragma once
+
+#include <array>
+#include "pandar_pointcloud/calibration.hpp"
+#include "packet_decoder.hpp"
+#include "pandar_xtm.hpp"
+
+namespace pandar_pointcloud
+{
+namespace pandar_xtm
+{
+
+  const float pandarXTM_elev_angle_map[] = {
+    19.5f, 18.2f, 16.9f, 15.6f, 14.3f, 13.0f, 11.7f, 10.4f, \
+  9.1f,  7.8f,  6.5f,  5.2f,  3.9f,  2.6f,  1.3f, 0.0f,  \
+  -1.3f, -2.6f, -3.9f, -5.2f, -6.5f, -7.8f, -9.1f, -10.4f, \
+  -11.7f, -13.0f, -14.3f, -15.6f, -16.9f, -18.2f, -19.5f, -20.8f
+  };
+
+  const float pandarXTM_horizontal_azimuth_offset_map[] = {
+    0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
+   0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
+   0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
+   0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
+  };
+
+  const float blockXTMOffsetTriple[] = {
+    5.632f - 50.0f * 1.0f,
+    5.632f - 50.0f * 1.0f,
+    5.632f - 50.0f * 1.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f
+  };
+
+  const float blockXTMOffsetDual[] = {
+    5.632f - 50.0f * 2.0f,
+    5.632f - 50.0f * 2.0f,
+    5.632f - 50.0f * 1.0f,
+    5.632f - 50.0f * 1.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f
+  };
+  const float blockXTMOffsetSingle[] = {
+    5.632f - 50.0f * 5.0f,
+    5.632f - 50.0f * 4.0f,
+    5.632f - 50.0f * 3.0f,
+    5.632f - 50.0f * 2.0f,
+    5.632f - 50.0f * 1.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f,
+    5.632f - 50.0f * 0.0f
+  };
+
+  const float laserXTMOffset[] = {
+    2.856f * 0.0f + 0.368f,
+    2.856f * 1.0f + 0.368f,
+    2.856f * 2.0f + 0.368f,
+    2.856f * 3.0f + 0.368f,
+    2.856f * 4.0f + 0.368f,
+    2.856f * 5.0f + 0.368f,
+    2.856f * 6.0f + 0.368f,
+    2.856f * 7.0f + 0.368f,
+
+    2.856f * 8.0f + 0.368f,
+    2.856f * 9.0f + 0.368f,
+    2.856f * 10.0f + 0.368f,
+    2.856f * 11.0f + 0.368f,
+    2.856f * 12.0f + 0.368f,
+    2.856f * 13.0f + 0.368f,
+    2.856f * 14.0f + 0.368f,
+    2.856f * 15.0f + 0.368f,
+
+    2.856f * 0.0f + 0.368f,
+    2.856f * 1.0f + 0.368f,
+    2.856f * 2.0f + 0.368f,
+    2.856f * 3.0f + 0.368f,
+    2.856f * 4.0f + 0.368f,
+    2.856f * 5.0f + 0.368f,
+    2.856f * 6.0f + 0.368f,
+    2.856f * 7.0f + 0.368f,
+
+    2.856f * 8.0f + 0.368f,
+    2.856f * 9.0f + 0.368f,
+    2.856f * 10.0f + 0.368f,
+    2.856f * 11.0f + 0.368f,
+    2.856f * 12.0f + 0.368f,
+    2.856f * 13.0f + 0.368f,
+    2.856f * 14.0f + 0.368f,
+    2.856f * 15.0f + 0.368f
+  };
+
+const uint16_t MAX_AZIMUTH_DEGREE_NUM=36000;
+
+class PandarXTMDecoder : public PacketDecoder
+{
+public:
+  enum class ReturnMode : int8_t
+  {
+    DUAL,
+    FIRST,
+    STRONGEST,
+    LAST,
+    TRIPLE
+  };
+
+  PandarXTMDecoder(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);
+  PointcloudXYZIRADT convert(const int block_id);
+
+  std::array<float, UNIT_NUM> elev_angle_;
+  std::array<float, UNIT_NUM> azimuth_offset_;
+
+  std::array<float, BLOCK_NUM> block_offset_single_;
+  std::array<float, BLOCK_NUM> block_offset_dual_;
+  std::array<float, BLOCK_NUM> block_offset_triple_;
+
+  std::vector<float> m_sin_elevation_map_;
+  std::vector<float> m_cos_elevation_map_;
+
+  std::vector<float> m_sin_azimuth_map_;
+  std::vector<float> m_cos_azimuth_map_;
+
+  ReturnMode return_mode_;
+  Packet packet_;
+
+  PointcloudXYZIRADT scan_pc_;
+  PointcloudXYZIRADT overflow_pc_;
+
+  uint16_t scan_phase_;
+  int last_phase_;
+  bool has_scanned_;
+  uint16_t last_azimuth_;
+  int start_angle_;
+  double last_timestamp_;
+
+  void CalcXTPointXYZIT(int blockid, char chLaserNumber, boost::shared_ptr<pcl::PointCloud<PointXYZIRADT>> cld);
+};
+
+}  // namespace pandar_xt
+}  // namespace pandar_pointcloud

pandar_pointcloud/launch/pandar.launch

@@ -0,0 +1,91 @@
+<launch>
+
+  <!-- Params -->
+  <arg name="launch_driver" default="true" />
+
+  <arg name="model" default="Pandar40P"/>
+  <arg name="device_ip" default="192.168.1.201" />
+  <arg name="lidar_port"  default="2368"/>
+  <arg name="gps_port"  default="10110"/>
+  <arg name="sensor_frame" default="pandar" />
+  <arg name="base_frame" default="base_link" />
+  <arg name="pcap" default="" />
+  <arg name="calibration" default=""/>
+  <arg name="scan_phase" default="0.0" />
+  <arg name="manager" default="pandar_nodelet_manager" />
+
+  <!-- nodelet manager -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" />
+
+
+  <!-- pandar driver -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_driver"
+        args="load pandar_driver/DriverNodelet $(arg manager)" if="$(arg launch_driver)">
+    <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 sensor_frame)"/>
+  </node>
+
+  <!-- pandar_packets to pointcloud -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_cloud" args="load pandar_pointcloud/CloudNodelet $(arg manager)">
+    <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="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>
+
+  <!-- crop self -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_crop_box_filter_self" args="load pointcloud_preprocessor/crop_box_filter_nodelet $(arg manager)" output="log">
+    <remap from="~input" to="pointcloud_raw_ex" />
+    <remap from="~output" to="self_cropped/pointcloud_ex" />
+    <remap from="~min_x" to="/vehicle_info/min_longitudinal_offset" />
+    <remap from="~max_x" to="/vehicle_info/max_longitudinal_offset" />
+    <remap from="~min_y" to="/vehicle_info/min_lateral_offset" />
+    <remap from="~max_y" to="/vehicle_info/max_lateral_offset" />
+    <remap from="~min_z" to="/vehicle_info/min_height_offset" />
+    <remap from="~max_z" to="/vehicle_info/max_height_offset" />
+
+    <param name="negative" value="True" />
+    <param name="input_frame" value="$(arg base_frame)" />
+    <param name="output_frame" value="$(arg base_frame)" />
+  </node>
+
+  <!-- crop mirror -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_crop_box_filter_mirror" args="load pointcloud_preprocessor/crop_box_filter_nodelet $(arg manager)" output="log">
+    <remap from="~input" to="self_cropped/pointcloud_ex" />
+    <remap from="~output" to="mirror_cropped/pointcloud_ex" />
+    <remap from="~min_x" to="/vehicle_info/mirror/min_longitudinal_offset" />
+    <remap from="~max_x" to="/vehicle_info/mirror/max_longitudinal_offset" />
+    <remap from="~min_y" to="/vehicle_info/mirror/min_lateral_offset" />
+    <remap from="~max_y" to="/vehicle_info/mirror/max_lateral_offset" />
+    <remap from="~min_z" to="/vehicle_info/mirror/min_height_offset" />
+    <remap from="~max_z" to="/vehicle_info/mirror/max_height_offset" />
+
+    <param name="negative" value="True" />
+    <param name="input_frame" value="$(arg base_frame)" />
+    <param name="output_frame" value="$(arg base_frame)" />
+  </node>
+
+  <!-- fix distortion -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_fix_distortion" args="load velodyne_pointcloud/InterpolateNodelet $(arg manager)">
+    <remap from="/vehicle/status/twist" to="/localization/eagleye/twist" />
+    <remap from="velodyne_points_ex" to="mirror_cropped/pointcloud_ex" />
+    <remap from="velodyne_points_interpolate" to="rectified/pointcloud" />
+    <remap from="velodyne_points_interpolate_ex" to="rectified/pointcloud_ex" />
+  </node>
+
+  <!-- PointCloud Outlier Filter -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_ring_outlier_filter" args="load pointcloud_preprocessor/ring_outlier_filter_nodelet $(arg manager)">
+    <!-- <remap from="~input" to="rectified/pointcloud_ex" /> -->
+    <remap from="~input" to="rectified/pointcloud_ex" />
+    <remap from="~output" to="outlier_filtered/pointcloud" />
+    <rosparam>
+    </rosparam>
+  </node>
+</launch>