Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(nebula): relative stamp #33

Merged
merged 2 commits into from
Jun 28, 2023
Merged

fix(nebula): relative stamp #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ced637e
hesai_decoders. update point stamp to be relative to scan
amc-nu Jun 20, 2023
a6c8dd1
decoders. qt128 timestamp
amc-nu Jun 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 3 additions & 14 deletions nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt_128.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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