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

refactor(velodyne)!: combine Velodyne ROS wrappers into a single node #148

Merged
merged 14 commits into from
May 24, 2024
Merged
Show file tree
Hide file tree
Changes from 13 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
@@ -1,25 +1,26 @@
#ifndef NEBULA_WS_VELODYNE_SCAN_DECODER_HPP
#define NEBULA_WS_VELODYNE_SCAN_DECODER_HPP
#include "nebula_common/point_types.hpp"
#include "nebula_common/velodyne/velodyne_calibration_decoder.hpp"
#include "nebula_common/velodyne/velodyne_common.hpp"

#include <rclcpp/rclcpp.hpp>

#include <velodyne_msgs/msg/velodyne_packet.hpp>
#include <velodyne_msgs/msg/velodyne_scan.hpp>

#include <boost/format.hpp>

#include <angles/angles.h>
#include <pcl/point_cloud.h>

#include <cerrno>
#include <cmath>
#include <cstdint>
#include <memory>
#include <string>
#include <vector>

#include "nebula_common/point_types.hpp"
#include "nebula_common/velodyne/velodyne_calibration_decoder.hpp"
#include "nebula_common/velodyne/velodyne_common.hpp"

#include <velodyne_msgs/msg/velodyne_packet.hpp>
#include <velodyne_msgs/msg/velodyne_scan.hpp>

#include <tuple>
#include <vector>

namespace nebula
{
Expand Down Expand Up @@ -132,23 +133,59 @@ enum RETURN_TYPE {
/// @brief Base class for Velodyne LiDAR decoder
class VelodyneScanDecoder
{
private:
size_t processed_packets_{0};
uint32_t prev_packet_first_azm_phased_{0};
bool has_scanned_{false};

protected:
/// @brief Checks if the current packet completes the ongoing scan.
/// 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 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)
{
if (has_scanned_) {
processed_packets_ = 0;
reset_pointcloud(packet_seconds);
}

has_scanned_ = false;
processed_packets_++;

// Check if scan is complete
uint32_t packet_first_azm = packet[2]; // lower word of azimuth block 0
packet_first_azm |= packet[3] << 8; // higher word of azimuth block 0

uint32_t packet_last_azm = packet[1102];
packet_last_azm |= packet[1103] << 8;

uint32_t packet_first_azm_phased = (36000 + packet_first_azm - phase) % 36000;
uint32_t packet_last_azm_phased = (36000 + packet_last_azm - phase) % 36000;
drwnz marked this conversation as resolved.
Show resolved Hide resolved

has_scanned_ =
processed_packets_ > 1 && (packet_last_azm_phased < packet_first_azm_phased ||
packet_first_azm_phased < prev_packet_first_azm_phased_);

prev_packet_first_azm_phased_ = packet_first_azm_phased;
}

/// @brief Decoded point cloud
drivers::NebulaPointCloudPtr scan_pc_;
/// @brief Point cloud overflowing from one cycle
drivers::NebulaPointCloudPtr overflow_pc_;

uint16_t scan_phase_{};
uint16_t last_phase_{};
bool has_scanned_ = true;
double dual_return_distance_threshold_{}; // Velodyne does this internally, this will not be
// implemented here
double scan_timestamp_{};

/// @brief SensorConfiguration for this decoder
std::shared_ptr<drivers::VelodyneSensorConfiguration> sensor_configuration_;
std::shared_ptr<const drivers::VelodyneSensorConfiguration> sensor_configuration_;
/// @brief Calibration for this decoder
std::shared_ptr<drivers::VelodyneCalibrationConfiguration> calibration_configuration_;
std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> calibration_configuration_;

public:
VelodyneScanDecoder(VelodyneScanDecoder && c) = delete;
Expand All @@ -161,15 +198,16 @@ class VelodyneScanDecoder

/// @brief Virtual function for parsing and shaping VelodynePacket
/// @param pandar_packet
virtual void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;
virtual void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) = 0;
/// @brief Virtual function for parsing VelodynePacket based on packet structure
/// @param pandar_packet
/// @return Resulting flag
virtual bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;

/// @brief Virtual function for getting the flag indicating whether one cycle is ready
/// @return Readied
virtual bool hasScanned() = 0;
bool hasScanned() { return has_scanned_; }

/// @brief Calculation of points in each packet
/// @return # of points
virtual int pointsPerPacket() = 0;
Expand All @@ -178,8 +216,7 @@ class VelodyneScanDecoder
/// @return tuple of Point cloud and timestamp
virtual std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() = 0;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0;
virtual void reset_pointcloud(double time_stamp) = 0;
/// @brief Resetting overflowed point cloud buffer
virtual void reset_overflow(double time_stamp) = 0;
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,23 +25,20 @@ class Vlp16Decoder : public VelodyneScanDecoder
/// @param sensor_configuration SensorConfiguration for this decoder
/// @param calibration_configuration Calibration for this decoder
explicit Vlp16Decoder(
const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
calibration_configuration);
/// @brief Parsing and shaping VelodynePacket
/// @param velodyne_packet
void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
/// @brief Get the flag indicating whether one cycle is ready
/// @return Readied
bool hasScanned() override;
void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
/// @brief Calculation of points in each packet
/// @return # of points
int pointsPerPacket() override;
/// @brief Get the constructed point cloud
/// @return tuple of Point cloud and timestamp
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
void reset_pointcloud(size_t n_pts, double time_stamp) override;
void reset_pointcloud(double time_stamp) override;
/// @brief Resetting overflowed point cloud buffer
void reset_overflow(double time_stamp) override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,23 +24,20 @@ class Vlp32Decoder : public VelodyneScanDecoder
/// @param sensor_configuration SensorConfiguration for this decoder
/// @param calibration_configuration Calibration for this decoder
explicit Vlp32Decoder(
const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
calibration_configuration);
/// @brief Parsing and shaping VelodynePacket
/// @param velodyne_packet
void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
/// @brief Get the flag indicating whether one cycle is ready
/// @return Readied
bool hasScanned() override;
void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
/// @brief Calculation of points in each packet
/// @return # of points
int pointsPerPacket() override;
/// @brief Get the constructed point cloud
/// @return tuple of Point cloud and timestamp
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
void reset_pointcloud(size_t n_pts, double time_stamp) override;
void reset_pointcloud(double time_stamp) override;
/// @brief Resetting overflowed point cloud buffer
void reset_overflow(double time_stamp) override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@
#include <velodyne_msgs/msg/velodyne_scan.hpp>

#include <array>
#include <memory>
#include <tuple>
#include <vector>

namespace nebula
{
Expand All @@ -21,23 +24,20 @@ class Vls128Decoder : public VelodyneScanDecoder
/// @param sensor_configuration SensorConfiguration for this decoder
/// @param calibration_configuration Calibration for this decoder
explicit Vls128Decoder(
const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
calibration_configuration);
/// @brief Parsing and shaping VelodynePacket
/// @param velodyne_packet
void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
/// @brief Get the flag indicating whether one cycle is ready
/// @return Readied
bool hasScanned() override;
void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
/// @brief Calculation of points in each packet
/// @return # of points
int pointsPerPacket() override;
/// @brief Get the constructed point cloud
/// @return tuple of Point cloud and timestamp
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
void reset_pointcloud(size_t n_pts, double time_stamp) override;
void reset_pointcloud(double time_stamp) override;
/// @brief Resetting overflowed point cloud buffer
void reset_overflow(double time_stamp) override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,11 @@
#include <pcl_conversions/pcl_conversions.h>

#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>

namespace nebula
{
Expand All @@ -36,8 +39,9 @@ class VelodyneDriver : NebulaDriverBase
/// @param sensor_configuration SensorConfiguration for this driver
/// @param calibration_configuration CalibrationConfiguration for this driver
VelodyneDriver(
const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
calibration_configuration);

/// @brief Setting CalibrationConfiguration (not used)
/// @param calibration_configuration
Expand All @@ -52,8 +56,8 @@ class VelodyneDriver : NebulaDriverBase
/// @brief Convert VelodyneScan message to point cloud
/// @param velodyne_scan Message
/// @return tuple of Point cloud and timestamp
std::tuple<drivers::NebulaPointCloudPtr, double> ConvertScanToPointcloud(
const std::shared_ptr<velodyne_msgs::msg::VelodyneScan> & velodyne_scan);
std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
const std::vector<uint8_t> & packet, int32_t packet_seconds);
};

} // namespace drivers
Expand Down
Original file line number Diff line number Diff line change
@@ -1,19 +1,20 @@
#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp"

#include <angles/angles.h>

#include <cmath>
#include <utility>

#include <angles/angles.h>

namespace nebula
{
namespace drivers
{
namespace vlp16
{
Vlp16Decoder::Vlp16Decoder(
const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> &
calibration_configuration)
{
sensor_configuration_ = sensor_configuration;
calibration_configuration_ = calibration_configuration;
Expand Down Expand Up @@ -56,11 +57,6 @@ Vlp16Decoder::Vlp16Decoder(
phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
}

bool Vlp16Decoder::hasScanned()
{
return has_scanned_;
}

std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
{
double phase = angles::from_degrees(sensor_configuration_->scan_phase);
Expand All @@ -87,11 +83,9 @@ int Vlp16Decoder::pointsPerPacket()
return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING;
}

void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp)
void Vlp16Decoder::reset_pointcloud(double time_stamp)
{
scan_pc_->points.clear();
max_pts_ = n_pts * pointsPerPacket();
scan_pc_->points.reserve(max_pts_);
reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud
}

Expand Down Expand Up @@ -138,12 +132,14 @@ void Vlp16Decoder::reset_overflow(double time_stamp)
overflow_pc_->points.reserve(max_pts_);
}

void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
{
const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
checkAndHandleScanComplete(packet, packet_seconds, phase_);

const raw_packet_t * raw = (const raw_packet_t *)packet.data();
float last_azimuth_diff = 0;
uint16_t azimuth_next;
const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX];
const uint8_t return_mode = packet[RETURN_MODE_INDEX];
const bool dual_return = (return_mode == RETURN_MODE_DUAL);

for (uint block = 0; block < BLOCKS_PER_PACKET; block++) {
Expand Down Expand Up @@ -203,7 +199,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1];
}
// Apply timestamp if this is the first new packet in the scan.
auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds();
auto block_timestamp = packet_seconds;
if (scan_timestamp_ < 0) {
scan_timestamp_ = block_timestamp;
}
Expand All @@ -216,7 +212,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
continue;
}
{
VelodyneLaserCorrection & corrections =
const VelodyneLaserCorrection & corrections =
calibration_configuration_->velodyne_calibration.laser_corrections[dsr];
float distance = current_return.uint *
calibration_configuration_->velodyne_calibration.distance_resolution_m;
Expand Down
Loading
Loading