diff --git a/pandar_driver/package.xml b/pandar_driver/package.xml
index 9d2f967..a17421c 100644
--- a/pandar_driver/package.xml
+++ b/pandar_driver/package.xml
@@ -16,6 +16,7 @@
pluginlib
pandar_msgs
pandar_api
+ libpcap
diff --git a/pandar_driver/src/driver/pandar_driver.cpp b/pandar_driver/src/driver/pandar_driver.cpp
index 624a462..54679c1 100644
--- a/pandar_driver/src/driver/pandar_driver.cpp
+++ b/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");
}
diff --git a/pandar_pointcloud/CMakeLists.txt b/pandar_pointcloud/CMakeLists.txt
index a9caf15..b6aed7d 100644
--- a/pandar_pointcloud/CMakeLists.txt
+++ b/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
diff --git a/pandar_pointcloud/config/xtm.csv b/pandar_pointcloud/config/xtm.csv
new file mode 100644
index 0000000..d251ef4
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm.hpp
new file mode 100644
index 0000000..ef5ac67
--- /dev/null
+++ b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm.hpp
@@ -0,0 +1,83 @@
+#pragma once
+/**
+ * Pandar XT-32
+ */
+#include
+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
diff --git a/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm_decoder.hpp b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm_decoder.hpp
new file mode 100644
index 0000000..7ea8dc2
--- /dev/null
+++ b/pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm_decoder.hpp
@@ -0,0 +1,150 @@
+#pragma once
+
+#include
+#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 elev_angle_;
+ std::array azimuth_offset_;
+
+ std::array block_offset_single_;
+ std::array block_offset_dual_;
+ std::array block_offset_triple_;
+
+ std::vector m_sin_elevation_map_;
+ std::vector m_cos_elevation_map_;
+
+ std::vector m_sin_azimuth_map_;
+ std::vector 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> cld);
+};
+
+} // namespace pandar_xt
+} // namespace pandar_pointcloud
diff --git a/pandar_pointcloud/launch/pandar.launch b/pandar_pointcloud/launch/pandar.launch
new file mode 100644
index 0000000..9ca4bef
--- /dev/null
+++ b/pandar_pointcloud/launch/pandar.launch
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/pandar_pointcloud/launch/pandarXTM.launch b/pandar_pointcloud/launch/pandarXTM.launch
new file mode 100644
index 0000000..c152717
--- /dev/null
+++ b/pandar_pointcloud/launch/pandarXTM.launch
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/pandar_pointcloud/src/lib/decoder/pandar_xtm_decoder.cpp b/pandar_pointcloud/src/lib/decoder/pandar_xtm_decoder.cpp
new file mode 100644
index 0000000..fadc7db
--- /dev/null
+++ b/pandar_pointcloud/src/lib/decoder/pandar_xtm_decoder.cpp
@@ -0,0 +1,210 @@
+#include "pandar_pointcloud/decoder/pandar_xtm_decoder.hpp"
+#include "pandar_pointcloud/decoder/pandar_xtm.hpp"
+
+namespace
+{
+static inline double deg2rad(double degrees)
+{
+ return degrees * M_PI / 180.0;
+}
+}
+
+namespace pandar_pointcloud
+{
+namespace pandar_xtm
+{
+PandarXTMDecoder::PandarXTMDecoder(Calibration& calibration, float scan_phase, double dual_return_distance_threshold, ReturnMode return_mode)
+{
+ m_sin_elevation_map_.resize(UNIT_NUM);
+ m_cos_elevation_map_.resize(UNIT_NUM);
+ for (size_t laser = 0; laser < UNIT_NUM; ++laser) {
+ m_sin_elevation_map_[laser] = sinf(deg2rad(pandarXTM_elev_angle_map[laser]));
+ m_cos_elevation_map_[laser] = cosf(deg2rad(pandarXTM_elev_angle_map[laser]));
+ }
+ m_sin_azimuth_map_.resize(MAX_AZIMUTH_DEGREE_NUM);
+ m_cos_azimuth_map_.resize(MAX_AZIMUTH_DEGREE_NUM);
+ for(int i = 0; i < MAX_AZIMUTH_DEGREE_NUM; ++i) {
+ m_sin_azimuth_map_[i] = sinf(i * M_PI / 18000);
+ m_cos_azimuth_map_[i] = cosf(i * M_PI / 18000);
+ }
+
+ scan_phase_ = static_cast(scan_phase * 100.0f);
+ return_mode_ = return_mode;
+
+ last_phase_ = 0;
+ has_scanned_ = false;
+
+ scan_pc_.reset(new pcl::PointCloud);
+ overflow_pc_.reset(new pcl::PointCloud);
+}
+
+bool PandarXTMDecoder::hasScanned()
+{
+ return has_scanned_;
+}
+
+PointcloudXYZIRADT PandarXTMDecoder::getPointcloud()
+{
+ return scan_pc_;
+}
+
+void PandarXTMDecoder::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;
+ }
+ for (int block_id = 0; block_id < packet_.header.chBlockNumber; ++block_id) {
+ int azimuthGap = 0; /* To do */
+ double timestampGap = 0; /* To do */
+ if(last_azimuth_ > packet_.blocks[block_id].azimuth) {
+ azimuthGap = static_cast(packet_.blocks[block_id].azimuth) + (36000 - static_cast(last_azimuth_));
+ } else {
+ azimuthGap = static_cast(packet_.blocks[block_id].azimuth) - static_cast(last_azimuth_);
+ }
+ timestampGap = packet_.usec - last_timestamp_ + 0.001;
+ if (last_azimuth_ != packet_.blocks[block_id].azimuth && \
+ (azimuthGap / timestampGap) < 36000 * 100 ) {
+ /* for all the blocks */
+ if ((last_azimuth_ > packet_.blocks[block_id].azimuth &&
+ start_angle_ <= packet_.blocks[block_id].azimuth) ||
+ (last_azimuth_ < start_angle_ &&
+ start_angle_ <= packet_.blocks[block_id].azimuth)) {
+ has_scanned_ = true;
+ }
+ } else {
+ //printf("last_azimuth_:%d pkt.blocks[block_id].azimuth:%d *******azimuthGap:%d\n", last_azimuth_, pkt.blocks[block_id].azimuth, azimuthGap);
+ }
+ CalcXTPointXYZIT(block_id, packet_.header.chLaserNumber, scan_pc_);
+ last_azimuth_ = packet_.blocks[block_id].azimuth;
+ last_timestamp_ = packet_.usec;
+ }
+}
+
+void PandarXTMDecoder::CalcXTPointXYZIT(int blockid, \
+ char chLaserNumber, boost::shared_ptr> cld) {
+ Block *block = &packet_.blocks[blockid];
+
+ for (int i = 0; i < chLaserNumber; ++i) {
+ /* for all the units in a block */
+ Unit &unit = block->units[i];
+ PointXYZIRADT point{};
+
+ /* skip wrong points */
+ if (unit.distance <= 0.1 || unit.distance > 200.0) {
+ continue;
+ }
+
+ int azimuth = static_cast(pandarXTM_horizontal_azimuth_offset_map[i] * 100 + block->azimuth);
+ if(azimuth < 0)
+ azimuth += 36000;
+ if(azimuth >= 36000)
+ azimuth -= 36000;
+
+ {
+ float xyDistance = unit.distance * m_cos_elevation_map_[i];
+ point.x = static_cast(xyDistance * m_sin_azimuth_map_[azimuth]);
+ point.y = static_cast(xyDistance * m_cos_azimuth_map_[azimuth]);
+ point.z = static_cast(unit.distance * m_sin_elevation_map_[i]);
+ }
+
+ point.intensity = unit.intensity;
+
+ double unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps)
+ point.time_stamp = unix_second + (static_cast(packet_.usec)) / 1000000.0;
+ point.time_stamp += (static_cast(blockXTMOffsetSingle[i] + laserXTMOffset[i]) / 1000000.0f);
+
+ if (packet_.return_mode == 0x3d){
+ point.time_stamp =
+ point.time_stamp + (static_cast(blockXTMOffsetTriple[blockid] +
+ laserXTMOffset[i]) /
+ 1000000.0f);
+ }
+ else if (packet_.return_mode == 0x39 || packet_.return_mode == 0x3b || packet_.return_mode == 0x3c) {
+ point.time_stamp =
+ point.time_stamp + (static_cast(blockXTMOffsetDual[blockid] +
+ laserXTMOffset[i]) /
+ 1000000.0f);
+ } else {
+ point.time_stamp = point.time_stamp + \
+ (static_cast(blockXTMOffsetSingle[blockid] + laserXTMOffset[i]) / \
+ 1000000.0f);
+ }
+
+ point.return_type = packet_.return_mode;
+ point.ring = i;
+ cld->points.emplace_back(point);
+ }
+}
+
+bool PandarXTMDecoder::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.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 < 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)) / (double)1000;
+ 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;
+}
+}
+}
\ No newline at end of file
diff --git a/pandar_pointcloud/src/pandar_cloud.cpp b/pandar_pointcloud/src/pandar_cloud.cpp
index 29be2b0..f3a2e9c 100644
--- a/pandar_pointcloud/src/pandar_cloud.cpp
+++ b/pandar_pointcloud/src/pandar_cloud.cpp
@@ -4,6 +4,7 @@
#include "pandar_pointcloud/decoder/pandar40_decoder.hpp"
#include "pandar_pointcloud/decoder/pandar_qt_decoder.hpp"
#include "pandar_pointcloud/decoder/pandar_xt_decoder.hpp"
+#include "pandar_pointcloud/decoder/pandar_xtm_decoder.hpp"
#include "pandar_pointcloud/decoder/pandar64_decoder.hpp"
#include
@@ -31,7 +32,6 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh)
ROS_ERROR("Unable to load calibration data");
return;
}
-
if (model_ == "Pandar40P" || model_ == "Pandar40M") {
pandar40::Pandar40Decoder::ReturnMode selected_return_mode;
if (return_mode_ == "Strongest")
@@ -82,6 +82,31 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh)
dual_return_distance_threshold_,
selected_return_mode);
}
+ else if (model_ == "PandarXTM") {
+ pandar_xtm::PandarXTMDecoder::ReturnMode selected_return_mode;
+ if (return_mode_ == "First")
+ selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::FIRST;
+ else if (return_mode_ == "Strongest")
+ selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::STRONGEST;
+ else if (return_mode_ == "Last")
+ selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::LAST;
+ else if (return_mode_ == "Dual")
+ selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::DUAL;
+ else if (return_mode_ == "Triple") {
+ //selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::TRIPLE;
+ ROS_ERROR("Triple return mode not implemented, defaulting to dual return mode");
+ selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::DUAL;
+ }
+ else {
+ ROS_ERROR("Invalid return mode, defaulting to dual return mode");
+ selected_return_mode = pandar_xtm::PandarXTMDecoder::ReturnMode::DUAL;
+ }
+ ROS_INFO_STREAM("XTM Decoder");
+ decoder_ = std::make_shared(calibration_, scan_phase_,
+ dual_return_distance_threshold_,
+ selected_return_mode);
+ ROS_INFO_STREAM("XTM Decoder OK");
+ }
else if (model_ == "Pandar64") {
pandar64::Pandar64Decoder::ReturnMode selected_return_mode;
if (return_mode_ == "First")
@@ -108,6 +133,7 @@ PandarCloud::PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh)
node.subscribe("pandar_packets", 10, &PandarCloud::onProcessScan, this, ros::TransportHints().tcpNoDelay(true));
pandar_points_pub_ = node.advertise("pandar_points", 10);
pandar_points_ex_pub_ = node.advertise("pandar_points_ex", 10);
+ ROS_INFO_STREAM("Ready");
}
PandarCloud::~PandarCloud()
@@ -117,8 +143,10 @@ PandarCloud::~PandarCloud()
bool PandarCloud::setupCalibration()
{
if (!calibration_path_.empty() && calibration_.loadFile(calibration_path_) == 0) {
+ ROS_INFO_STREAM("Loaded Calibration from:" << calibration_path_);
return true;
}
+ ROS_INFO_STREAM("Loading Calibration file from device...");
if (tcp_client_) {
std::string content("");
for (size_t i = 0; i < TCP_RETRY_NUM; ++i) {