Skip to content

Commit

Permalink
update qt_decoder
Browse files Browse the repository at this point in the history
Signed-off-by: Shinnosuke Hirakawa <[email protected]>
  • Loading branch information
0x126 committed Mar 1, 2021
1 parent cb060b4 commit 1c85c27
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@ 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;

struct Unit
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,10 @@ 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;

struct Header
{
uint16_t sob; // 0xFFEE 2bytes
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,15 @@ class PandarQTDecoder : public PacketDecoder
LAST,
};

PandarQTDecoder(Calibration & calibration, float scan_phase = 0.0f);
PandarQTDecoder(Calibration & calibration, float scan_phase = 0.0f, 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);
PointcloudXYZIRADT convert_dual(const int block_id);

std::array<float, UNIT_NUM> elev_angle_;
std::array<float, UNIT_NUM> azimuth_offset_;
Expand All @@ -34,6 +35,7 @@ class PandarQTDecoder : public PacketDecoder
std::array<float, BLOCK_NUM> block_offset_single_;
std::array<float, BLOCK_NUM> block_offset_dual_;

ReturnMode return_mode_;
Packet packet_;

PointcloudXYZIRADT scan_pc_;
Expand Down
2 changes: 1 addition & 1 deletion pandar_pointcloud/src/lib/decoder/pandar40_decoder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ void Pandar40Decoder::unpack(const pandar_msgs::PandarPacket & raw_packet)
has_scanned_ = false;
}

bool dual_return = (packet_.echo == 0x39);
bool dual_return = (packet_.echo == DUAL_ECHO);
auto step = dual_return ? 2 : 1;

for (int block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) {
Expand Down
67 changes: 55 additions & 12 deletions pandar_pointcloud/src/lib/decoder/pandar_qt_decoder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ namespace{
namespace pandar_pointcloud{
namespace pandar_qt{

PandarQTDecoder::PandarQTDecoder(Calibration & calibration, float scan_phase)
PandarQTDecoder::PandarQTDecoder(Calibration & calibration, float scan_phase, ReturnMode return_mode)
{
firing_offset_ = {
12.31, 14.37, 16.43, 18.49, 20.54, 22.6, 24.66, 26.71, 29.16, 31.22, 33.28,
Expand All @@ -38,6 +38,7 @@ PandarQTDecoder::PandarQTDecoder(Calibration & calibration, float scan_phase)
}

scan_phase_ = static_cast<uint16_t>(scan_phase * 100.0f);
return_mode_ = return_mode;

last_phase_ = 0;
has_scanned_ = false;
Expand Down Expand Up @@ -69,12 +70,16 @@ void PandarQTDecoder::unpack(const pandar_msgs::PandarPacket & raw_packet)
has_scanned_ = false;
}

for (int block_id = 0; block_id < BLOCK_NUM; ++block_id) {
auto block_pc = convert(block_id);
int current_phase = (static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
if(current_phase >= last_phase_ && !has_scanned_){
bool dual_return = (packet_.echo == DUAL_ECHO);
auto step = dual_return ? 2 : 1;

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<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
if (current_phase > last_phase_ && !has_scanned_) {
*scan_pc_ += *block_pc;
}else{
} else {
*overflow_pc_ += *block_pc;
has_scanned_ = true;
}
Expand Down Expand Up @@ -113,15 +118,53 @@ PointcloudXYZIRADT PandarQTDecoder::convert(const int block_id)

point.time_stamp = unix_second + (static_cast<double>(packet_.usec)) / 1000000.0;

if(packet_.echo == 0x05){
point.time_stamp +=
(static_cast<double>(block_offset_single_[block_id] + firing_offset_[unit_id]) / 1000000.0f);

block_pc->push_back(point);
}
return block_pc;
}

PointcloudXYZIRADT PandarQTDecoder::convert_dual(const int block_id)
{
PointcloudXYZIRADT block_pc(new pcl::PointCloud<PointXYZIRADT>);

// double unix_second = raw_packet.header.stamp.toSec() // system-time (packet receive time)
double unix_second = static_cast<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)

auto head = block_id + ((return_mode_ == ReturnMode::FIRST) ? 1 : 0);
auto tail = block_id + ((return_mode_ == ReturnMode::LAST) ? 1 : 2);

for (size_t unit_id = 0; unit_id < UNIT_NUM; ++unit_id){
for (int i = head; i < tail; ++i) {
PointXYZIRADT point;
const auto & block = packet_.blocks[i];
const auto& unit = block.units[unit_id];
// skip invalid points
if (unit.distance <= 0.1 || unit.distance > 200.0) {
continue;
}
double xyDistance = unit.distance * cosf(deg2rad(elev_angle_[unit_id]));

point.x = static_cast<float>(
xyDistance * sinf(deg2rad(azimuth_offset_[unit_id] + (static_cast<double>(block.azimuth)) / 100.0)));
point.y = static_cast<float>(
xyDistance * cosf(deg2rad(azimuth_offset_[unit_id] + (static_cast<double>(block.azimuth)) / 100.0)));
point.z = static_cast<float>(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.time_stamp = unix_second + (static_cast<double>(packet_.usec)) / 1000000.0;

point.time_stamp +=
(static_cast<double>(block_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000.0f);
}else{
point.time_stamp +=
(static_cast<double>(block_offset_single_[block_id] + firing_offset_[unit_id]) / 1000000.0f);
}

block_pc->push_back(point);
block_pc->push_back(point);
}
}
return block_pc;
}
Expand Down

0 comments on commit 1c85c27

Please sign in to comment.