fix(decoder): handle weird small packets and mis-labelled data packets

Signed-off-by: David Wong <[email protected]>
tier4 · Jul 8, 2024 · 8397511 · 8397511
1 parent 52ed53e
commit 8397511
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 21 deletions.
diff --git a/nebula_decoders/src/nebula_decoders_seyond/decoders/seyond_decoder.cpp b/nebula_decoders/src/nebula_decoders_seyond/decoders/seyond_decoder.cpp
@@ -42,6 +42,7 @@ SeyondDecoder::SeyondDecoder(
   decode_pc_.reset(new NebulaPointCloud);
   output_pc_.reset(new NebulaPointCloud);
 
+  // TODO(drwnz): Find the right values for each sensor
   decode_pc_->reserve(2000000);
   output_pc_->reserve(2000000);
 
@@ -103,8 +104,11 @@ bool SeyondDecoder::IsPacketValid(const std::vector<uint8_t> & buffer)
   uint16_t magic_number = ((buffer[1] << 8) | buffer[0]);
   uint16_t packet_type = buffer[kSeyondPktTypeIndex];
 
+
   // check packet is data packet
-  if ((magic_number != kSeyondMagicNumberDataPacket) || (packet_type == 2) || (packet_type == 3)) {
+  // TODO(drwnz): [Falcon] sometimes report data packet as 51 not 1.
+  // TODO(drwnz): [Falcon] here packet size check is used to prevent un-decodable short data packet causing errors.
+  if ((magic_number != kSeyondMagicNumberDataPacket) || (packet_type == 2) || (packet_type == 3) || (send_packet_size < 60)) {
     return false;
   }
 
@@ -146,7 +150,6 @@ int SeyondDecoder::unpack(const std::vector<uint8_t> & packet)
   std::vector<uint8_t> packet_copy = packet;
   if (!IsPacketValid(packet_copy)) {
     return -1;
-    std::cout << "Packet invalid" << std::endl;
   }
 
   ProtocolCompatibility(packet_copy);
@@ -164,6 +167,7 @@ int SeyondDecoder::unpack(const std::vector<uint8_t> & packet)
 
   // Publish the whole frame data if scan is complete
   if (current_packet_id_ != packet_id) {
+    // std::cout << "Old packet ID: " << current_packet_id_ << ", New packet ID: " << packet_id << std::endl;
     std::swap(decode_pc_, output_pc_);
     decode_pc_->clear();
     has_scanned_ = true;

diff --git a/nebula_decoders/src/nebula_decoders_seyond/seyond_driver.cpp b/nebula_decoders/src/nebula_decoders_seyond/seyond_driver.cpp
@@ -37,25 +37,6 @@ std::tuple<drivers::NebulaPointCloudPtr, double> SeyondDriver::ParseCloudPacket(
   if (scan_decoder_->hasScanned()) {
     pointcloud = scan_decoder_->getPointcloud();
   }
-
-  // int cnt = 0;
-  // for (auto & packet : seyond_scan->packets) {
-  //   if (scan_decoder_->unpack(packet) == 0) {
-  //     cnt++;
-  //   } else {
-  //     RCLCPP_ERROR_STREAM(
-  //       logger, "Failed to unpack packet with timestamp ");
-  //   }
-  // }
-
-  // pointcloud = scan_decoder_->getPointcloud();
-
-  // if (cnt == 0) {
-  //   RCLCPP_ERROR_STREAM(
-  //     logger, "Scanned " << seyond_scan->packets.size() << " packets, but no "
-  //                        << "pointclouds were generated.");
-  // }
-
   return pointcloud;
 }