diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128.hpp index 94401f732..3e3bf8c54 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128.hpp @@ -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; @@ -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 = diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp index 9697cec60..d5dcaa9a2 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp @@ -138,7 +138,9 @@ drivers::NebulaPoint Pandar128E4XDecoder::build_point( point.azimuth = block_azimuth_rad_[azimuth] + azimuth_offset_rad_[laser_id]; point.distance = unit_distance; point.elevation = elevation_angle_rad_[laser_id]; - point.time_stamp = unix_second + packet_.tail.timestamp_us - scan_timestamp_; + point.time_stamp = static_cast((static_cast(unix_second) + + static_cast(packet_.tail.timestamp_us)/1000000 - + scan_timestamp_)*1000000000); out_distance = xyDistance; return point; } diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp index 7b2d8a487..186356fc8 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp @@ -72,8 +72,6 @@ int Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet } if (has_scanned_) { - auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; scan_pc_ = overflow_pc_; overflow_pc_.reset(new NebulaPointCloud); overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS); @@ -94,12 +92,15 @@ int Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet } for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { - auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); int current_phase = (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; if (current_phase > last_phase_ && !has_scanned_) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *scan_pc_ += *block_pc; } else { + auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *overflow_pc_ += *block_pc; has_scanned_ = true; } @@ -132,22 +133,17 @@ drivers::NebulaPoint Pandar40Decoder::build_point( point.return_type = return_type; if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; } auto offset = dual_return - ? (static_cast( - block_time_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / - 1000000.0f) - : (static_cast( - block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) / - 1000000.0f); - auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); - if (point_stamp < 0) { - point.time_stamp = 0; - } else { - point.time_stamp = static_cast(point_stamp * 10e9); - } + ? static_cast( + block_time_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / 1000000. + : static_cast( + block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.; + auto point_stamp = unix_second + (static_cast(packet_.usec) / 1000000.0) - offset - scan_timestamp_; + if (point_stamp < 0) + point_stamp = 0; + point.time_stamp = static_cast(point_stamp*1000000000); return point; } diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp index fd7ad0a67..412614f12 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp @@ -70,8 +70,6 @@ int Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet if (has_scanned_) { scan_pc_ = overflow_pc_; - auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; overflow_pc_.reset(new NebulaPointCloud); overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS); has_scanned_ = false; @@ -90,12 +88,15 @@ int Pandar64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet } } for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { - auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); int current_phase = (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; if (current_phase > last_phase_ && !has_scanned_) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *scan_pc_ += *block_pc; } else { + auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *overflow_pc_ += *block_pc; has_scanned_ = true; } @@ -127,21 +128,17 @@ drivers::NebulaPoint Pandar64Decoder::build_point( point.elevation = elevation_angle_rad_[unit_id]; point.return_type = return_type; if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; - } - auto offset = - dual_return - ? (static_cast(block_time_offset_dual_[block_id] + firing_time_offset_[unit_id]) / - 1000000.0f) - : (static_cast(block_time_offset_single_[block_id] + firing_time_offset_[unit_id]) / - 1000000.0f); - auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); - if (point_stamp < 0) { - point.time_stamp = 0; - } else { - point.time_stamp = static_cast(point_stamp * 10e9); + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; } + auto offset = dual_return + ? static_cast( + block_time_offset_dual_[block_id] + firing_time_offset_[unit_id]) / 1000000.0 + : static_cast( + block_time_offset_single_[block_id] + firing_time_offset_[unit_id]) / 1000000.; + auto point_stamp = unix_second + (static_cast(packet_.usec) / 1000000.0) - offset - scan_timestamp_; + if (point_stamp < 0) + point_stamp = 0; + point.time_stamp = static_cast(point_stamp*1000000000); return point; } diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp index 9517a27a6..0af59b119 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp @@ -76,8 +76,6 @@ int PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet if (has_scanned_) { scan_pc_ = overflow_pc_; - - scan_timestamp_ = packet_.unix_second + static_cast(packet_.usec) / 1000000.f; overflow_pc_.reset(new NebulaPointCloud); has_scanned_ = false; } @@ -87,7 +85,6 @@ int PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet packet_.blocks[block_id].azimuth * LIDAR_AZIMUTH_UNIT + packet_.blocks[block_id].fine_azimuth); - auto block_pc = convert(block_id); //* int count = 0, field = 0; while (count < correction_configuration_->frameNumber && @@ -103,9 +100,12 @@ int PandarATDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet if (max_azimuth_ < azimuth) { max_azimuth_ = azimuth; } + scan_timestamp_ = packet_.unix_second + static_cast(packet_.usec) / 1000000.f; + auto block_pc = convert(block_id); *overflow_pc_ += *block_pc; has_scanned_ = true; } else { + auto block_pc = convert(block_id); *scan_pc_ += *block_pc; } last_azimuth_ = azimuth; @@ -177,10 +177,13 @@ void PandarATDecoder::CalcXTPointXYZIT( point.z = static_cast(unit.distance * m_sin_elevation_map_[elevation]); } if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = packet_.unix_second + static_cast(packet_.usec) / 1000000.f; + scan_timestamp_ = packet_.unix_second + static_cast(packet_.usec) / 1000000.; } - point.time_stamp = channel_firing_ns[i]; + point.time_stamp = static_cast( + (packet_.unix_second + + static_cast(packet_.usec) / 1000000. - + scan_timestamp_)*1000000000); switch (packet_.return_mode) { case STRONGEST_RETURN: diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp index 79635e47c..39a5ad730 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_128_decoder.cpp @@ -82,20 +82,29 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac if (has_scanned_) { scan_pc_ = overflow_pc_; - auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; overflow_pc_.reset(new NebulaPointCloud); overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS); has_scanned_ = false; } bool dual_return = is_dual_return(); + auto unix_second = static_cast(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(packet_.blocks[block_id + 1].azimuth) - scan_phase_ + 36000) % 36000; + if (current_phase > last_phase_ && !has_scanned_) { + accumulating = true; + } + else { + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; + accumulating = false; + } auto block1_pt = convert(block_id); auto block2_pt = convert(block_id + 1); size_t block1size = block1_pt->points.size(); @@ -113,19 +122,25 @@ int PandarQT128Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pac block_pc->points.emplace_back(block1_pt->points[i]); } } - current_phase = - (static_cast(packet_.blocks[block_id + 1].azimuth) - scan_phase_ + 36000) % 36000; + } } else // single { for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id++) { - block_pc = convert(block_id); - *block_pc += *block_pc; current_phase = (static_cast(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(packet_.usec) / 1000000.; + accumulating = false; + } + block_pc = convert(block_id); + *block_pc += *block_pc; } } - if (current_phase > last_phase_ && !has_scanned_) { + if (accumulating) { *scan_pc_ += *block_pc; } else { *overflow_pc_ += *block_pc; @@ -159,21 +174,17 @@ drivers::NebulaPoint PandarQT128Decoder::build_point( point.return_type = first_return_type_; } if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; - } - auto offset = - dual_return - ? (static_cast(block_time_offset_dual_[block_id] + firing_time_offset1_[unit_id]) / - 1000000.0f) - : (static_cast(block_time_offset_single_[block_id] + firing_time_offset2_[unit_id]) / - 1000000.0f); - auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); - if (point_stamp < 0) { - point.time_stamp = 0; - } else { - point.time_stamp = static_cast(point_stamp * 10e9); + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; } + auto offset = dual_return + ? static_cast( + block_time_offset_dual_[block_id] + firing_time_offset2_[unit_id]) / 1000000. + : static_cast( + block_time_offset_single_[block_id] + firing_time_offset1_[unit_id]) / 1000000.; + auto point_stamp = unix_second + (static_cast(packet_.usec) / 1000000.) - offset - scan_timestamp_; + if (point_stamp < 0) + point_stamp = 0; + point.time_stamp = static_cast(point_stamp*1000000000); return point; } @@ -261,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; } @@ -284,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; @@ -297,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) { diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp index da307d41a..e664b191d 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_qt_64_decoder.cpp @@ -70,7 +70,6 @@ int PandarQT64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pack if (has_scanned_) { scan_pc_ = overflow_pc_; - scan_timestamp_ = std::numeric_limits::max(); overflow_pc_.reset(new NebulaPointCloud); overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS); has_scanned_ = false; @@ -90,12 +89,15 @@ int PandarQT64Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_pack } for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { - auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); int current_phase = (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; if (current_phase > last_phase_ && !has_scanned_) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *scan_pc_ += *block_pc; } else { + auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *overflow_pc_ += *block_pc; has_scanned_ = true; } @@ -127,21 +129,17 @@ drivers::NebulaPoint PandarQT64Decoder::build_point( point.elevation = elevation_angle_rad_[unit_id]; point.return_type = return_type; if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; - } - auto offset = - dual_return - ? (static_cast(block_time_offset_dual_[block_id] + firing_offset_[unit_id]) / - 1000000.0f) - : (static_cast(block_time_offset_single_[block_id] + firing_offset_[unit_id]) / - 1000000.0f); - auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); - if (point_stamp < 0) { - point.time_stamp = 0; - } else { - point.time_stamp = static_cast(point_stamp * 10e9); + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; } + auto offset = dual_return + ? static_cast( + block_time_offset_dual_[block_id] + firing_offset_[unit_id]) / 1000000. + : static_cast( + block_time_offset_single_[block_id] + firing_offset_[unit_id]) / 1000000.; + auto point_stamp = unix_second + (static_cast(packet_.usec) / 1000000.0) - offset - scan_timestamp_; + if (point_stamp < 0) + point_stamp = 0; + point.time_stamp = static_cast(point_stamp*1000000000); return point; } diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp index 117a05426..77b880b07 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xt_decoder.cpp @@ -63,8 +63,6 @@ int PandarXTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet if (has_scanned_) { scan_pc_ = overflow_pc_; - auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; overflow_pc_.reset(new NebulaPointCloud); overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS); has_scanned_ = false; @@ -74,12 +72,15 @@ int PandarXTDecoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet auto step = dual_return ? 2 : 1; for (size_t block_id = 0; block_id < BLOCKS_PER_PACKET; block_id += step) { - auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); int current_phase = (static_cast(packet_.blocks[block_id].azimuth) - scan_phase_ + 36000) % 36000; if (current_phase > last_phase_ && !has_scanned_) { + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *scan_pc_ += *block_pc; } else { + auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; + auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id); *overflow_pc_ += *block_pc; has_scanned_ = true; } @@ -108,18 +109,14 @@ drivers::NebulaPoint PandarXTDecoder::build_point(int block_id, int unit_id, uin point.elevation = elevation_angle_rad_[unit_id]; point.return_type = return_type; if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; - } - auto offset = - (static_cast(block_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / - 1000000.0f); - auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); - if (point_stamp < 0) { - point.time_stamp = 0; - } else { - point.time_stamp = static_cast(point_stamp * 10e9); + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; } + auto offset = static_cast( + block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.0; + auto point_stamp = unix_second + (static_cast(packet_.usec) / 1000000.0) - offset - scan_timestamp_; + if (point_stamp < 0) + point_stamp = 0; + point.time_stamp = static_cast(point_stamp*1000000000); return point; } @@ -194,18 +191,14 @@ drivers::NebulaPointCloudPtr PandarXTDecoder::convert_dual(size_t block_id) point.distance = unit.distance; if (scan_timestamp_ < 0) { // invalid timestamp - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; - } - auto offset = - (static_cast(block_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / - 1000000.0f); - auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); - if (point_stamp < 0) { - point.time_stamp = 0; - } else { - point.time_stamp = static_cast(point_stamp * 10e9); + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.; } + auto offset = static_cast( + block_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.0; + auto point_stamp = unix_second + (static_cast(packet_.usec) / 1000000.0) - offset - scan_timestamp_; + if (point_stamp < 0) + point_stamp = 0; + point.time_stamp = static_cast(point_stamp*1000000000); if (identical_flg) { point.return_type = static_cast(nebula::drivers::ReturnType::IDENTICAL); diff --git a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xtm_decoder.cpp b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xtm_decoder.cpp index 229da8f4e..a2e17edca 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xtm_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_xtm_decoder.cpp @@ -74,8 +74,6 @@ int_least32_t PandarXTMDecoder::unpack(const pandar_msgs::msg::PandarPacket & pa if (has_scanned_) { scan_pc_ = overflow_pc_; - auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) - scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; overflow_pc_.reset(new NebulaPointCloud); overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS); has_scanned_ = false; @@ -92,7 +90,6 @@ int_least32_t PandarXTMDecoder::unpack(const pandar_msgs::msg::PandarPacket & pa static_cast(packet_.blocks[block_id].azimuth) - static_cast(last_azimuth_); } timestampGap = packet_.usec - last_timestamp_ + 0.001; - auto block_pc = convert(block_id); if ( last_azimuth_ != packet_.blocks[block_id].azimuth && (azimuthGap / timestampGap) < 36000 * 100) { @@ -101,10 +98,14 @@ int_least32_t PandarXTMDecoder::unpack(const pandar_msgs::msg::PandarPacket & pa (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)) { + auto unix_second = static_cast(timegm(&packet_.t)); // sensor-time (ppt/gps) + scan_timestamp_ = unix_second + static_cast(packet_.usec) / 1000000.f; + auto block_pc = convert(block_id); *overflow_pc_ += *block_pc; has_scanned_ = true; } } else { + auto block_pc = convert(block_id); *scan_pc_ += *block_pc; } last_azimuth_ = packet_.blocks[block_id].azimuth; @@ -196,7 +197,7 @@ void PandarXTMDecoder::CalcXTPointXYZIT( break; } auto point_stamp = - (unix_second + offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); + (unix_second - offset + static_cast(packet_.usec) / 1000000.f - scan_timestamp_); if (point_stamp < 0) { point.time_stamp = 0; } else {