diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 18b55c77a..e1e9062f0 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -262,7 +262,6 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa const float z_coord = distance * sin_vert_angle; // velodyne z const uint8_t intensity = current_block.data[k + 2]; - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 58a685251..25feaf8f7 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -122,7 +122,8 @@ void Vlp32Decoder::reset_overflow(double time_stamp) // be relative to the overflow's packet timestamp double new_timestamp_seconds = scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; - overflow_point.time_stamp = static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); + overflow_point.time_stamp = + static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); scan_pc_->points.emplace_back(overflow_point); overflow_pc_->points.pop_back(); @@ -281,7 +282,6 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa intensity = raw->blocks[i].data[k + 2]; - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; const float focal_offset = 256 * (1 - corrections.focal_distance / 13100) * diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index 09352e4c6..23442a6f3 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -282,11 +282,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p const float y_coord = -(xy_distance * sin_rot_angle); // velodyne x const float z_coord = distance * sin_vert_angle; // velodyne z const uint8_t intensity = current_block.data[k + 2]; - auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); last_block_timestamp_ = block_timestamp; - if (scan_timestamp_ < 0) { - scan_timestamp_ = block_timestamp; - } double point_time_offset = timing_offsets_[block / 4][firing_order + laser_number / 64];