Skip to content

Commit

Permalink
chore(velodyne): fix case style
Browse files Browse the repository at this point in the history
Signed-off-by: Max SCHMELLER <[email protected]>
  • Loading branch information
mojomex committed Oct 2, 2024
1 parent d9944b4 commit a82e96a
Show file tree
Hide file tree
Showing 33 changed files with 981 additions and 1,028 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ inline std::ostream & operator<<(std::ostream & os, VelodyneSensorConfiguration
struct VelodyneCalibrationConfiguration : CalibrationConfigurationBase
{
VelodyneCalibration velodyne_calibration;
inline nebula::Status LoadFromFile(const std::string & calibration_file)
inline nebula::Status load_from_file(const std::string & calibration_file)
{
velodyne_calibration.read(calibration_file);
if (!velodyne_calibration.initialized) {
Expand All @@ -62,7 +62,7 @@ struct VelodyneCalibrationConfiguration : CalibrationConfigurationBase
return Status::OK;
}
}
inline nebula::Status SaveFile(const std::string & calibration_file)
inline nebula::Status save_file(const std::string & calibration_file)
{
velodyne_calibration.write(calibration_file);
return Status::OK;
Expand All @@ -72,7 +72,7 @@ struct VelodyneCalibrationConfiguration : CalibrationConfigurationBase
/// @brief Convert return mode name to ReturnMode enum (Velodyne-specific return_mode_from_string)
/// @param return_mode Return mode name (Upper and lower case letters must match)
/// @return Corresponding ReturnMode
inline ReturnMode ReturnModeFromStringVelodyne(const std::string & return_mode)
inline ReturnMode return_mode_from_string_velodyne(const std::string & return_mode)
{
if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST;
if (return_mode == "Last") return ReturnMode::SINGLE_LAST;
Expand Down
104 changes: 52 additions & 52 deletions nebula_common/src/velodyne/velodyne_calibration_decoder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,45 +28,45 @@ namespace nebula
{
namespace drivers
{
const char NUM_LASERS[] = "num_lasers";
const char DISTANCE_RESOLUTION[] = "distance_resolution";
const char LASERS[] = "lasers";
const char LASER_ID[] = "laser_id";
const char ROT_CORRECTION[] = "rot_correction";
const char VERT_CORRECTION[] = "vert_correction";
const char DIST_CORRECTION[] = "dist_correction";
const char TWO_PT_CORRECTION_AVAILABLE[] = "two_pt_correction_available";
const char DIST_CORRECTION_X[] = "dist_correction_x";
const char DIST_CORRECTION_Y[] = "dist_correction_y";
const char VERT_OFFSET_CORRECTION[] = "vert_offset_correction";
const char HORIZ_OFFSET_CORRECTION[] = "horiz_offset_correction";
const char MAX_INTENSITY[] = "max_intensity";
const char MIN_INTENSITY[] = "min_intensity";
const char FOCAL_DISTANCE[] = "focal_distance";
const char FOCAL_SLOPE[] = "focal_slope";
const char g_num_lasers[] = "num_lasers";
const char g_distance_resolution[] = "distance_resolution";
const char g_lasers[] = "lasers";
const char g_laser_id[] = "laser_id";
const char g_rot_correction[] = "rot_correction";
const char g_vert_correction[] = "vert_correction";
const char g_dist_correction[] = "dist_correction";
const char g_two_pt_correction_available[] = "two_pt_correction_available";
const char g_dist_correction_x[] = "dist_correction_x";
const char g_dist_correction_y[] = "dist_correction_y";
const char g_vert_offset_correction[] = "vert_offset_correction";
const char g_horiz_offset_correction[] = "horiz_offset_correction";
const char g_max_intensity[] = "max_intensity";
const char g_min_intensity[] = "min_intensity";
const char g_focal_distance[] = "focal_distance";
const char g_focal_slope[] = "focal_slope";

/** Read calibration for a single laser. */
void operator>>(const YAML::Node & node, std::pair<int, VelodyneLaserCorrection> & correction)
{
node[LASER_ID] >> correction.first;
node[ROT_CORRECTION] >> correction.second.rot_correction;
node[VERT_CORRECTION] >> correction.second.vert_correction;
node[DIST_CORRECTION] >> correction.second.dist_correction;
node[g_laser_id] >> correction.first;
node[g_rot_correction] >> correction.second.rot_correction;
node[g_vert_correction] >> correction.second.vert_correction;
node[g_dist_correction] >> correction.second.dist_correction;
#ifdef HAVE_NEW_YAMLCPP
if (node[TWO_PT_CORRECTION_AVAILABLE])
node[TWO_PT_CORRECTION_AVAILABLE] >> correction.second.two_pt_correction_available;
if (node[g_two_pt_correction_available])
node[g_two_pt_correction_available] >> correction.second.two_pt_correction_available;
#else
if (const YAML::Node * pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE))
*pName >> correction.second.two_pt_correction_available;
#endif
else
correction.second.two_pt_correction_available = false;
node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;
node[g_dist_correction_x] >> correction.second.dist_correction_x;
node[g_dist_correction_y] >> correction.second.dist_correction_y;
node[g_vert_offset_correction] >> correction.second.vert_offset_correction;
#ifdef HAVE_NEW_YAMLCPP
if (node[HORIZ_OFFSET_CORRECTION])
node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction;
if (node[g_horiz_offset_correction])
node[g_horiz_offset_correction] >> correction.second.horiz_offset_correction;
#else
if (const YAML::Node * pName = node.FindValue(HORIZ_OFFSET_CORRECTION))
*pName >> correction.second.horiz_offset_correction;
Expand All @@ -76,8 +76,8 @@ void operator>>(const YAML::Node & node, std::pair<int, VelodyneLaserCorrection>

const YAML::Node * max_intensity_node = NULL;
#ifdef HAVE_NEW_YAMLCPP
if (node[MAX_INTENSITY]) {
const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY];
if (node[g_max_intensity]) {
const YAML::Node max_intensity_node_ref = node[g_max_intensity];
max_intensity_node = &max_intensity_node_ref;
}
#else
Expand All @@ -93,8 +93,8 @@ void operator>>(const YAML::Node & node, std::pair<int, VelodyneLaserCorrection>

const YAML::Node * min_intensity_node = NULL;
#ifdef HAVE_NEW_YAMLCPP
if (node[MIN_INTENSITY]) {
const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY];
if (node[g_min_intensity]) {
const YAML::Node min_intensity_node_ref = node[g_min_intensity];
min_intensity_node = &min_intensity_node_ref;
}
#else
Expand All @@ -107,8 +107,8 @@ void operator>>(const YAML::Node & node, std::pair<int, VelodyneLaserCorrection>
} else {
correction.second.min_intensity = 0;
}
node[FOCAL_DISTANCE] >> correction.second.focal_distance;
node[FOCAL_SLOPE] >> correction.second.focal_slope;
node[g_focal_distance] >> correction.second.focal_distance;
node[g_focal_slope] >> correction.second.focal_slope;

// Calculate cached values
correction.second.cos_rot_correction = cosf(correction.second.rot_correction);
Expand All @@ -123,10 +123,10 @@ void operator>>(const YAML::Node & node, std::pair<int, VelodyneLaserCorrection>
void operator>>(const YAML::Node & node, VelodyneCalibration & calibration)
{
int num_lasers;
node[NUM_LASERS] >> num_lasers;
node[g_num_lasers] >> num_lasers;
float distance_resolution_m;
node[DISTANCE_RESOLUTION] >> distance_resolution_m;
const YAML::Node & lasers = node[LASERS];
node[g_distance_resolution] >> distance_resolution_m;
const YAML::Node & lasers = node[g_lasers];
calibration.laser_corrections.clear();
calibration.num_lasers = num_lasers;
calibration.distance_resolution_m = distance_resolution_m;
Expand Down Expand Up @@ -172,32 +172,32 @@ YAML::Emitter & operator<<(
YAML::Emitter & out, const std::pair<int, VelodyneLaserCorrection> correction)
{
out << YAML::BeginMap;
out << YAML::Key << LASER_ID << YAML::Value << correction.first;
out << YAML::Key << ROT_CORRECTION << YAML::Value << correction.second.rot_correction;
out << YAML::Key << VERT_CORRECTION << YAML::Value << correction.second.vert_correction;
out << YAML::Key << DIST_CORRECTION << YAML::Value << correction.second.dist_correction;
out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE << YAML::Value
out << YAML::Key << g_laser_id << YAML::Value << correction.first;
out << YAML::Key << g_rot_correction << YAML::Value << correction.second.rot_correction;
out << YAML::Key << g_vert_correction << YAML::Value << correction.second.vert_correction;
out << YAML::Key << g_dist_correction << YAML::Value << correction.second.dist_correction;
out << YAML::Key << g_two_pt_correction_available << YAML::Value
<< correction.second.two_pt_correction_available;
out << YAML::Key << DIST_CORRECTION_X << YAML::Value << correction.second.dist_correction_x;
out << YAML::Key << DIST_CORRECTION_Y << YAML::Value << correction.second.dist_correction_y;
out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value
out << YAML::Key << g_dist_correction_x << YAML::Value << correction.second.dist_correction_x;
out << YAML::Key << g_dist_correction_y << YAML::Value << correction.second.dist_correction_y;
out << YAML::Key << g_vert_offset_correction << YAML::Value
<< correction.second.vert_offset_correction;
out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value
out << YAML::Key << g_horiz_offset_correction << YAML::Value
<< correction.second.horiz_offset_correction;
out << YAML::Key << MAX_INTENSITY << YAML::Value << correction.second.max_intensity;
out << YAML::Key << MIN_INTENSITY << YAML::Value << correction.second.min_intensity;
out << YAML::Key << FOCAL_DISTANCE << YAML::Value << correction.second.focal_distance;
out << YAML::Key << FOCAL_SLOPE << YAML::Value << correction.second.focal_slope;
out << YAML::Key << g_max_intensity << YAML::Value << correction.second.max_intensity;
out << YAML::Key << g_min_intensity << YAML::Value << correction.second.min_intensity;
out << YAML::Key << g_focal_distance << YAML::Value << correction.second.focal_distance;
out << YAML::Key << g_focal_slope << YAML::Value << correction.second.focal_slope;
out << YAML::EndMap;
return out;
}

YAML::Emitter & operator<<(YAML::Emitter & out, const VelodyneCalibration & calibration)
{
out << YAML::BeginMap;
out << YAML::Key << NUM_LASERS << YAML::Value << calibration.laser_corrections.size();
out << YAML::Key << DISTANCE_RESOLUTION << YAML::Value << calibration.distance_resolution_m;
out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
out << YAML::Key << g_num_lasers << YAML::Value << calibration.laser_corrections.size();
out << YAML::Key << g_distance_resolution << YAML::Value << calibration.distance_resolution_m;
out << YAML::Key << g_lasers << YAML::Value << YAML::BeginSeq;
for (std::map<int, VelodyneLaserCorrection>::const_iterator it =
calibration.laser_corrections_map.begin();
it != calibration.laser_corrections_map.end(); it++) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,54 +43,54 @@ namespace drivers
/**
* Raw Velodyne packet constants and structures.
*/
static const int SIZE_BLOCK = 100;
static const int RAW_SCAN_SIZE = 3;
static const int SCANS_PER_BLOCK = 32;
static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
static const int g_size_block = 100;
static const int g_raw_scan_size = 3;
static const int g_scans_per_block = 32;
static const int g_block_data_size = (g_scans_per_block * g_raw_scan_size);

static const double ROTATION_RESOLUTION = 0.01; // [deg]
static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100]
static const double g_rotation_resolution = 0.01; // [deg]
static const uint16_t g_rotation_max_units = 36000u; // [deg/100]

static const size_t RETURN_MODE_INDEX = 1204;
static const size_t g_return_mode_index = 1204;

/** @todo make this work for both big and little-endian machines */
static const uint16_t UPPER_BANK = 0xeeff;
static const uint16_t LOWER_BANK = 0xddff;
static const uint16_t g_upper_bank = 0xeeff;
static const uint16_t g_lower_bank = 0xddff;

/** Return Modes **/
static const uint16_t RETURN_MODE_STRONGEST = 55;
static const uint16_t RETURN_MODE_LAST = 56;
static const uint16_t RETURN_MODE_DUAL = 57;
static const uint16_t g_return_mode_strongest = 55;
static const uint16_t g_return_mode_last = 56;
static const uint16_t g_return_mode_dual = 57;

/** Special Defines for VLP16 support **/
static const int VLP16_FIRINGS_PER_BLOCK = 2;
static const int VLP16_SCANS_PER_FIRING = 16;
static const float VLP16_BLOCK_DURATION = 110.592f; // [µs]
static const float VLP16_DSR_TOFFSET = 2.304f; // [µs]
static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs]
static const int g_vlp16_firings_per_block = 2;
static const int g_vlp16_scans_per_firing = 16;
static const float g_vlp16_block_duration = 110.592f; // [µs]
static const float g_vlp16_dsr_toffset = 2.304f; // [µs]
static const float g_vlp16_firing_toffset = 55.296f; // [µs]

/** Special Defines for VLP32 support **/
static const float VLP32_CHANNEL_DURATION = 2.304f; // [µs]
static const float VLP32_SEQ_DURATION = 55.296f; // [µs]
static const float g_vlp32_channel_duration = 2.304f; // [µs]
static const float g_vlp32_seq_duration = 55.296f; // [µs]

/** Special Definitions for VLS128 support **/
static const float VLP128_DISTANCE_RESOLUTION = 0.004f; // [m]
static const float g_vlp128_distance_resolution = 0.004f; // [m]

/** Special Definitions for VLS128 support **/
// These are used to detect which bank of 32 lasers is in this block
static const uint16_t VLS128_BANK_1 = 0xeeff;
static const uint16_t VLS128_BANK_2 = 0xddff;
static const uint16_t VLS128_BANK_3 = 0xccff;
static const uint16_t VLS128_BANK_4 = 0xbbff;
static const uint16_t g_vls128_bank_1 = 0xeeff;
static const uint16_t g_vls128_bank_2 = 0xddff;
static const uint16_t g_vls128_bank_3 = 0xccff;
static const uint16_t g_vls128_bank_4 = 0xbbff;

static const float VLS128_CHANNEL_DURATION =
static const float g_vls128_channel_duration =
2.665f; // [µs] Channels corresponds to one laser firing
static const float VLS128_SEQ_DURATION =
static const float g_vls128_seq_duration =
53.3f; // [µs] Sequence is a set of laser firings including recharging

static const size_t OFFSET_FIRST_AZIMUTH = 2;
static const size_t OFFSET_LAST_AZIMUTH = 1102;
static const uint32_t DEGREE_SUBDIVISIONS = 100;
static const size_t g_offset_first_azimuth = 2;
static const size_t g_offset_last_azimuth = 1102;
static const uint32_t g_degree_subdivisions = 100;

/** \brief Raw Velodyne data block.
*
Expand All @@ -103,7 +103,7 @@ typedef struct raw_block
{
uint16_t header; ///< UPPER_BANK or LOWER_BANK
uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
uint8_t data[BLOCK_DATA_SIZE];
uint8_t data[g_block_data_size];
} raw_block_t;

/** used for unpacking the first two data bytes in a block
Expand All @@ -116,10 +116,10 @@ union two_bytes {
uint8_t bytes[2];
};

static const int PACKET_SIZE = 1206;
static const int BLOCKS_PER_PACKET = 12;
static const int PACKET_STATUS_SIZE = 4;
static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
static const int g_packet_size = 1206;
static const int g_blocks_per_packet = 12;
static const int g_packet_status_size = 4;
static const int g_scans_per_packet = (g_scans_per_block * g_blocks_per_packet);

/** \brief Raw Velodyne packet.
*
Expand All @@ -135,9 +135,9 @@ static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
*/
typedef struct raw_packet
{
raw_block_t blocks[BLOCKS_PER_PACKET];
raw_block_t blocks[g_blocks_per_packet];
uint16_t revolution;
uint8_t status[PACKET_STATUS_SIZE];
uint8_t status[g_packet_status_size];
} raw_packet_t;

/** \brief Velodyne echo types */
Expand Down Expand Up @@ -167,7 +167,7 @@ class VelodyneScanDecoder
/// @param packet The packet buffer to extract azimuths from
/// @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(
void check_and_handle_scan_complete(
const std::vector<uint8_t> & packet, double packet_seconds, const uint32_t phase)
{
if (has_scanned_) {
Expand All @@ -178,13 +178,13 @@ class VelodyneScanDecoder
has_scanned_ = false;
processed_packets_++;

uint32_t packet_first_azm = packet[OFFSET_FIRST_AZIMUTH]; // lower word of azimuth block 0
packet_first_azm |= packet[OFFSET_FIRST_AZIMUTH + 1] << 8; // higher word of azimuth block 0
uint32_t packet_first_azm = packet[g_offset_first_azimuth]; // lower word of azimuth block 0
packet_first_azm |= packet[g_offset_first_azimuth + 1] << 8; // higher word of azimuth block 0

uint32_t packet_last_azm = packet[OFFSET_LAST_AZIMUTH];
packet_last_azm |= packet[OFFSET_LAST_AZIMUTH + 1] << 8;
uint32_t packet_last_azm = packet[g_offset_last_azimuth];
packet_last_azm |= packet[g_offset_last_azimuth + 1] << 8;

const uint32_t max_azi = 360 * DEGREE_SUBDIVISIONS;
const uint32_t max_azi = 360 * g_degree_subdivisions;

uint32_t packet_first_azm_phased = (max_azi + packet_first_azm - phase) % max_azi;
uint32_t packet_last_azm_phased = (max_azi + packet_last_azm - phase) % max_azi;
Expand Down Expand Up @@ -225,15 +225,15 @@ class VelodyneScanDecoder
/// @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;
virtual bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;

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

/// @brief Calculation of points in each packet
/// @return # of points
virtual int pointsPerPacket() = 0;
virtual int points_per_packet() = 0;

/// @brief Virtual function for getting the constructed point cloud
/// @return tuple of Point cloud and timestamp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ namespace drivers
{
namespace vlp16
{
constexpr uint32_t MAX_POINTS = 300000;
constexpr uint32_t max_points = 300000;
/// @brief Velodyne LiDAR decoder (VLP16)
class Vlp16Decoder : public VelodyneScanDecoder
{
Expand All @@ -47,7 +47,7 @@ class Vlp16Decoder : public VelodyneScanDecoder
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;
int points_per_packet() override;
/// @brief Get the constructed point cloud
/// @return tuple of Point cloud and timestamp
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
Expand All @@ -60,10 +60,10 @@ class Vlp16Decoder : public VelodyneScanDecoder
/// @brief Parsing VelodynePacket based on packet structure
/// @param velodyne_packet
/// @return Resulting flag
bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
float sin_rot_table_[ROTATION_MAX_UNITS];
float cos_rot_table_[ROTATION_MAX_UNITS];
float rotation_radians_[ROTATION_MAX_UNITS];
bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
float sin_rot_table_[g_rotation_max_units];
float cos_rot_table_[g_rotation_max_units];
float rotation_radians_[g_rotation_max_units];
int phase_;
int max_pts_;
double last_block_timestamp_;
Expand Down
Loading

0 comments on commit a82e96a

Please sign in to comment.