Skip to content

Commit

Permalink
fix(nebula): relative stamp (autowarefoundation#33)
Browse files Browse the repository at this point in the history 
* hesai_decoders. update point stamp to be relative to scan

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* decoders. qt128 timestamp

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

---------

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
  • Loading branch information
@amc-nu
amc-nu authored Jun 28, 2023
1 parent 39e48c6 commit 427409a
Show file tree
Hide file tree
Showing 9 changed files with 112 additions and 122 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
4 changes: 3 additions & 1 deletion nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_128_e4x_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint32_t>((static_cast<double>(unix_second) +
static_cast<double>(packet_.tail.timestamp_us)/1000000 -
scan_timestamp_)*1000000000);
out_distance = xyDistance;
return point;
}
Expand Down
30 changes: 13 additions & 17 deletions nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_40_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,6 @@ int Pandar40Decoder::unpack(const pandar_msgs::msg::PandarPacket & pandar_packet
}

if (has_scanned_) {
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_pc_ = overflow_pc_;
overflow_pc_.reset(new NebulaPointCloud);
overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
Expand All @@ -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<int>(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<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
*overflow_pc_ += *block_pc;
has_scanned_ = true;
}
Expand Down Expand Up @@ -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<double>(packet_.usec) / 1000000.f;
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
}
auto offset = dual_return
? (static_cast<double>(
block_time_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) /
1000000.0f)
: (static_cast<double>(
block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) /
1000000.0f);
auto point_stamp =
(unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
if (point_stamp < 0) {
point.time_stamp = 0;
} else {
point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
}
? static_cast<double>(
block_time_offset_dual_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.
: static_cast<double>(
block_time_offset_single_return_[block_id] + firing_time_offset_[unit_id]) / 1000000.;
auto point_stamp = unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
if (point_stamp < 0)
point_stamp = 0;
point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);

return point;
}
Expand Down
31 changes: 14 additions & 17 deletions nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_64_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
overflow_pc_.reset(new NebulaPointCloud);
overflow_pc_->reserve(LASER_COUNT * MAX_AZIMUTH_STEPS);
has_scanned_ = false;
Expand All @@ -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<int>(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<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.f;
auto block_pc = dual_return ? convert_dual(block_id) : convert(block_id);
*overflow_pc_ += *block_pc;
has_scanned_ = true;
}
Expand Down Expand Up @@ -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<double>(packet_.usec) / 1000000.f;
}
auto offset =
dual_return
? (static_cast<double>(block_time_offset_dual_[block_id] + firing_time_offset_[unit_id]) /
1000000.0f)
: (static_cast<double>(block_time_offset_single_[block_id] + firing_time_offset_[unit_id]) /
1000000.0f);
auto point_stamp =
(unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
if (point_stamp < 0) {
point.time_stamp = 0;
} else {
point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
}
auto offset = dual_return
? static_cast<double>(
block_time_offset_dual_[block_id] + firing_time_offset_[unit_id]) / 1000000.0
: static_cast<double>(
block_time_offset_single_[block_id] + firing_time_offset_[unit_id]) / 1000000.;
auto point_stamp = unix_second + (static_cast<double>(packet_.usec) / 1000000.0) - offset - scan_timestamp_;
if (point_stamp < 0)
point_stamp = 0;
point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);

return point;
}
Expand Down
13 changes: 8 additions & 5 deletions nebula_decoders/src/nebula_decoders_hesai/decoders/pandar_at_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(packet_.usec) / 1000000.f;
overflow_pc_.reset(new NebulaPointCloud);
has_scanned_ = false;
}
Expand All @@ -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 &&
Expand All @@ -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<double>(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;
Expand Down Expand Up @@ -177,10 +177,13 @@ void PandarATDecoder::CalcXTPointXYZIT(
point.z = static_cast<float>(unit.distance * m_sin_elevation_map_[elevation]);
}
if (scan_timestamp_ < 0) { // invalid timestamp
scan_timestamp_ = packet_.unix_second + static_cast<double>(packet_.usec) / 1000000.f;
scan_timestamp_ = packet_.unix_second + static_cast<double>(packet_.usec) / 1000000.;
}

point.time_stamp = channel_firing_ns[i];
point.time_stamp = static_cast<uint32_t>(
(packet_.unix_second +
static_cast<double>(packet_.usec) / 1000000. -
scan_timestamp_)*1000000000);

switch (packet_.return_mode) {
case STRONGEST_RETURN:
Expand Down
57 changes: 34 additions & 23 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 @@ -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<double>(timegm(&packet_.t)); // sensor-time (ppt/gps)
scan_timestamp_ = unix_second + static_cast<double>(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<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 {
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);
size_t block1size = block1_pt->points.size();
Expand All @@ -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<int>(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<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;
}
}
if (current_phase > last_phase_ && !has_scanned_) {
if (accumulating) {
*scan_pc_ += *block_pc;
} else {
*overflow_pc_ += *block_pc;
Expand Down Expand Up @@ -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<double>(packet_.usec) / 1000000.f;
}
auto offset =
dual_return
? (static_cast<double>(block_time_offset_dual_[block_id] + firing_time_offset1_[unit_id]) /
1000000.0f)
: (static_cast<double>(block_time_offset_single_[block_id] + firing_time_offset2_[unit_id]) /
1000000.0f);
auto point_stamp =
(unix_second + offset + static_cast<double>(packet_.usec) / 1000000.f - scan_timestamp_);
if (point_stamp < 0) {
point.time_stamp = 0;
} else {
point.time_stamp = static_cast<uint32_t>(point_stamp * 10e9);
scan_timestamp_ = unix_second + static_cast<double>(packet_.usec) / 1000000.;
}
auto offset = dual_return
? static_cast<double>(
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.) - offset - scan_timestamp_;
if (point_stamp < 0)
point_stamp = 0;
point.time_stamp = static_cast<uint32_t>(point_stamp*1000000000);

return point;
}
Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
Expand All @@ -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) {
Expand Down
Loading

0 comments on commit 427409a

Please sign in to comment.