Skip to content

Commit

Permalink
decoders. qt128 timestamp
Browse files Browse the repository at this point in the history 
Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
  • Loading branch information
@amc-nu
amc-nu committed Jun 23, 2023
1 parent ced637e commit a6c8dd1
Show file tree
Hide file tree
Showing 3 changed files with 14 additions and 25 deletions.
17 changes: 3 additions & 14 deletions nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,15 +13,13 @@ namespace drivers
{
namespace pandar_qt_128
{
constexpr uint16_t MAX_AZIMUTH_STEPS = 900; // High Res mode
constexpr uint16_t MAX_AZIMUTH_STEPS = 36000; // High Res mode
// constexpr float DISTANCE_UNIT = 0.004f; // 4mm
constexpr double MIN_RANGE = 0.05;
constexpr double MAX_RANGE = 50.0;

// Head
constexpr size_t HEAD_SIZE = 12;
constexpr size_t PRE_HEADER_SIZE = 6;
constexpr size_t HEADER_SIZE = 6;
// Body
constexpr size_t BLOCKS_PER_PACKET = 2;
constexpr size_t BLOCK_HEADER_AZIMUTH = 2;
Expand All @@ -31,31 +29,22 @@ constexpr size_t CRC_SIZE = 4;
constexpr size_t BLOCK_SIZE = UNIT_SIZE * LASER_COUNT + BLOCK_HEADER_AZIMUTH;
constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCKS_PER_PACKET + CRC_SIZE;
// Functional Safety
constexpr size_t FS_VERSION_SIZE = 1;
constexpr size_t LIDAR_STATE_SIZE = 1;
constexpr size_t FAULT_SIZE = 1;
constexpr size_t OUT_FAULT_SIZE = 2;
constexpr size_t RESERVED_SIZE = 8;
constexpr size_t CRC2_SIZE = 4;
constexpr size_t PACKET_FS_SIZE = 17;
// Tail
constexpr size_t RESERVED2_SIZE = 5;
constexpr size_t MODE_FLAG_SIZE = 1;
constexpr size_t RESERVED3_SIZE = 6;
constexpr size_t RETURN_MODE_SIZE = 1;
constexpr size_t MOTOR_SPEED_SIZE = 2;
constexpr size_t UTC_SIZE = 6;
constexpr size_t TIMESTAMP_SIZE = 4;
constexpr size_t FACTORY_SIZE = 1;
constexpr size_t SEQUENCE_SIZE = 4;
constexpr size_t CRC3_SIZE = 4;
constexpr size_t PACKET_TAIL_SIZE = 34;
constexpr size_t PACKET_TAIL_WITHOUT_UDP_SEQ_CRC_SIZE = 26;
constexpr size_t PACKET_TAIL_TIMESTAMP_OFFSET = 1076;

// Cyber Security
constexpr size_t SIGNATURE_SIZE = 32;

constexpr size_t SKIP_SIZE = CRC_SIZE + PACKET_FS_SIZE + RESERVED2_SIZE;
constexpr size_t SKIP_SIZE = PACKET_FS_SIZE + RESERVED2_SIZE;

// All
constexpr size_t PACKET_SIZE =
Expand Down
19 changes: 11 additions & 8 deletions nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,22 +88,22 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac
}

bool dual_return = is_dual_return();
auto unix_second = static_cast<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)

drivers::NebulaPointCloudPtr block_pc(new NebulaPointCloud);
int current_phase;
int cnt2;
bool accumulating;
if (dual_return) {
for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += 2) {

current_phase =
(static_cast<int>(packet_.blocks[block_id + 1].azimuth) - scan_phase_ + 36000) % 36000;
if (current_phase > last_phase_ && !has_scanned_) {
accumulating = true;
}
else {
auto unix_second = static_cast<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
accumulating = false;
}
auto block1_pt = convert(block_id);
auto block2_pt = convert(block_id + 1);
Expand All @@ -127,12 +127,15 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac
} else // single
{
for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id++) {

current_phase =
(static_cast<int>(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000;
if (current_phase > last_phase_ && !has_scanned_) {
accumulating = true;
}
else {
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
accumulating = false;
}
block_pc = convert(block_id);
*block_pc += *block_pc;
}
Expand Down Expand Up @@ -175,10 +178,10 @@ drivers::NebulaPoint PandarQT128Decoder::build_point(
}
auto offset = dual_return
? static_cast<double>(
block_time_offset_dual_[block_id] + firing_time_offset2_[unit_id]) / 1000000.0
block_time_offset_dual_[block_id] + firing_time_offset2_[unit_id]) / 1000000.
: static_cast<double>(
block_time_offset_single_[block_id] + firing_time_offset1_[unit_id]) / 1000000.;
auto point_stamp = unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
auto point_stamp = unix_second + (static_cast<double>(packet_.usec) / 1000000.) - offset - scan_timestamp_;
if (point_stamp < 0)
point_stamp = 0;
point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);
Expand Down Expand Up @@ -269,6 +272,7 @@ bool PandarQT128Decoder::parsePacket(const pandar_msgs::msg::PandarPacket & pand
index += HEAD_SIZE;

if (packet_.header.sob != 0xEEFF) {
std::cerr << "Incorrect packet received" << std::endl;
return false;
}

Expand All @@ -292,8 +296,8 @@ bool PandarQT128Decoder::parsePacket(const pandar_msgs::msg::PandarPacket & pand
index += MODE_FLAG_SIZE;
index += RESERVED3_SIZE;
packet_.return_mode = buf[index] & 0xff; // Return Mode
index += RETURN_MODE_SIZE;

index = PACKET_TAIL_TIMESTAMP_OFFSET;
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;
Expand All @@ -305,7 +309,6 @@ bool PandarQT128Decoder::parsePacket(const pandar_msgs::msg::PandarPacket & pand

packet_.usec = (buf[index] & 0xff) | (buf[index + 1] & 0xff) << 8 |
((buf[index + 2] & 0xff) << 16) | ((buf[index + 3] & 0xff) << 24);
index += TIMESTAMP_SIZE;

// in case of time error
if (packet_.t.tm_year >= 200) {
Expand Down
3 changes: 0 additions & 3 deletions nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,6 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback(
RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed.");
return;
};
pcl::PCDWriter writer;
auto fn = std::to_string(std::get<1>(pointcloud_ts)) + ".pcd";
writer.writeASCII( "/tmp/" + fn, *pointcloud);
if (
nebula_points_pub_->get_subscription_count() > 0 ||
nebula_points_pub_->get_intra_process_subscription_count() > 0) {
Expand Down

0 comments on commit a6c8dd1

Please sign in to comment.