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.

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

fix(velodyne): propagate sub-second part of timestamps to pointcloud timestamp #201

Merged
merged 2 commits into from
Oct 1, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -165,10 +165,10 @@ class VelodyneScanDecoder
/// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until
/// the Velodyne decoder uses the same structure as Hesai/Robosense
/// @param packet The packet buffer to extract azimuths from
/// @param packet_seconds The seconds-since-epoch part of the packet's timestamp
/// @param packet_seconds The packet's timestamp in seconds, including the sub-second part
/// @param phase The sensor's scan phase used for scan cutting
void checkAndHandleScanComplete(
const std::vector<uint8_t> & packet, int32_t packet_seconds, const uint32_t phase)
const std::vector<uint8_t> & packet, double packet_seconds, const uint32_t phase)
{
if (has_scanned_) {
processed_packets_ = 0;
Expand Down Expand Up @@ -221,7 +221,7 @@ class VelodyneScanDecoder

/// @brief Virtual function for parsing and shaping VelodynePacket
/// @param pandar_packet
virtual void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) = 0;
virtual void unpack(const std::vector<uint8_t> & packet, double packet_seconds) = 0;
/// @brief Virtual function for parsing VelodynePacket based on packet structure
/// @param pandar_packet
/// @return Resulting flag
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ class Vlp16Decoder : public VelodyneScanDecoder
calibration_configuration);
/// @brief Parsing and shaping VelodynePacket
/// @param velodyne_packet
void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
void unpack(const std::vector<uint8_t> & packet, double packet_seconds) override;
/// @brief Calculation of points in each packet
/// @return # of points
int pointsPerPacket() override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class Vlp32Decoder : public VelodyneScanDecoder
calibration_configuration);
/// @brief Parsing and shaping VelodynePacket
/// @param velodyne_packet
void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
void unpack(const std::vector<uint8_t> & packet, double packet_seconds) override;
/// @brief Calculation of points in each packet
/// @return # of points
int pointsPerPacket() override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class Vls128Decoder : public VelodyneScanDecoder
calibration_configuration);
/// @brief Parsing and shaping VelodynePacket
/// @param velodyne_packet
void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
void unpack(const std::vector<uint8_t> & packet, double packet_seconds) override;
/// @brief Calculation of points in each packet
/// @return # of points
int pointsPerPacket() override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ class VelodyneDriver : NebulaDriverBase
/// @param velodyne_scan Message
/// @return tuple of Point cloud and timestamp
std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
const std::vector<uint8_t> & packet, int32_t packet_seconds);
const std::vector<uint8_t> & packet, double packet_seconds);
};

} // namespace drivers
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ void Vlp16Decoder::reset_overflow(double time_stamp)
overflow_pc_->points.reserve(max_pts_);
}

void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, double packet_seconds)
{
checkAndHandleScanComplete(packet, packet_seconds, phase_);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ void Vlp32Decoder::reset_overflow(double time_stamp)
overflow_pc_->points.reserve(max_pts_);
}

void Vlp32Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
void Vlp32Decoder::unpack(const std::vector<uint8_t> & packet, double packet_seconds)
{
checkAndHandleScanComplete(packet, packet_seconds, phase_);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ void Vls128Decoder::reset_overflow(double time_stamp)
overflow_pc_->points.reserve(max_pts_);
}

void Vls128Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
void Vls128Decoder::unpack(const std::vector<uint8_t> & packet, double packet_seconds)
{
checkAndHandleScanComplete(packet, packet_seconds, phase_);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ Status VelodyneDriver::SetCalibrationConfiguration(
}

std::tuple<drivers::NebulaPointCloudPtr, double> VelodyneDriver::ParseCloudPacket(
const std::vector<uint8_t> & packet, int32_t packet_seconds)
const std::vector<uint8_t> & packet, double packet_seconds)
{
std::tuple<drivers::NebulaPointCloudPtr, double> pointcloud;

Expand Down
5 changes: 4 additions & 1 deletion nebula_ros/src/velodyne/decoder_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@

#include "nebula_ros/velodyne/decoder_wrapper.hpp"

#include <rclcpp/time.hpp>

namespace nebula
{
namespace ros
Expand Down Expand Up @@ -176,7 +178,8 @@
nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr;
{
std::lock_guard lock(mtx_driver_ptr_);
pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data, packet_msg->stamp.sec);
pointcloud_ts =
driver_ptr_->ParseCloudPacket(packet_msg->data, rclcpp::Time(packet_msg->stamp).seconds());

Check warning on line 182 in nebula_ros/src/velodyne/decoder_wrapper.cpp

View check run for this annotation

Codecov / codecov/patch

nebula_ros/src/velodyne/decoder_wrapper.cpp#L182

Added line #L182 was not covered by tests
pointcloud = std::get<0>(pointcloud_ts);
}

Expand Down
Loading