Skip to content

Commit

Permalink
chore(hesai_decoders): implement clang-tidy fixes (mostly style-relat…
Browse files Browse the repository at this point in the history
…ed) (#183)

* chore(hesai): make namespaces c++17-style one-liners

* chore(hesai): mark single-arg constructor explicit

* chore(hesai): favor using over typedef

* chore(hesai): initialize uninitialized vars

* chore(hesai): clean up imports

* chore(hesai): inline function defined in header

* chore(hesai): fix case style for some functions and variables

* chore(hesai: favor range-based loop over index-based one

* chore(hesai): add missing stdexcept include
  • Loading branch information
mojomex authored Aug 19, 2024
1 parent 8a78e79 commit 5377d1c
Show file tree
Hide file tree
Showing 16 changed files with 87 additions and 145 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -14,15 +14,11 @@

#pragma once

#include "nebula_common/hesai/hesai_common.hpp"

#include <rclcpp/rclcpp.hpp>

#include <cstdint>

namespace nebula
{
namespace drivers
namespace nebula::drivers
{

struct CorrectedAngleData
Expand Down Expand Up @@ -62,5 +58,4 @@ class AngleCorrector
uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0;
};

} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,7 @@
#include <cstdint>
#include <memory>

namespace nebula
{
namespace drivers
namespace nebula::drivers
{

template <size_t ChannelN, size_t AngleUnit>
Expand All @@ -41,7 +39,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
std::array<std::array<float, ChannelN>, MAX_AZIMUTH_LEN> azimuth_sin_{};

public:
AngleCorrectorCalibrationBased(
explicit AngleCorrectorCalibrationBased(
const std::shared_ptr<const HesaiCalibrationConfiguration> & sensor_calibration)
{
if (sensor_calibration == nullptr) {
Expand Down Expand Up @@ -99,5 +97,4 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
}
};

} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,7 @@
#include <cstdint>
#include <memory>

namespace nebula
{
namespace drivers
namespace nebula::drivers
{

template <size_t ChannelN, size_t AngleUnit>
Expand Down Expand Up @@ -109,5 +107,4 @@ class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
}
};

} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers
Original file line number Diff line number Diff line change
Expand Up @@ -19,17 +19,12 @@

#include <rclcpp/rclcpp.hpp>

#include "pandar_msgs/msg/pandar_packet.hpp"
#include "pandar_msgs/msg/pandar_scan.hpp"

#include <memory>
#include <tuple>
#include <utility>
#include <vector>

namespace nebula
{
namespace drivers
namespace nebula::drivers
{

template <typename SensorT>
Expand Down Expand Up @@ -99,7 +94,7 @@ class HesaiDecoder : public HesaiScanDecoder
void convertReturns(size_t start_block_id, size_t n_blocks)
{
uint64_t packet_timestamp_ns = hesai_packet::get_timestamp_ns(packet_);
uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth();
uint32_t raw_azimuth = packet_.body.blocks[start_block_id].getAzimuth();

std::vector<const typename SensorT::packet_t::body_t::block_t::unit_t *> return_units;

Expand Down Expand Up @@ -172,9 +167,9 @@ class HesaiDecoder : public HesaiScanDecoder

// The raw_azimuth and channel are only used as indices, sin/cos functions use the precise
// corrected angles
float xyDistance = distance * corrected_angle_data.cos_elevation;
point.x = xyDistance * corrected_angle_data.sin_azimuth;
point.y = xyDistance * corrected_angle_data.cos_azimuth;
float xy_distance = distance * corrected_angle_data.cos_elevation;
point.x = xy_distance * corrected_angle_data.sin_azimuth;
point.y = xy_distance * corrected_angle_data.cos_azimuth;
point.z = distance * corrected_angle_data.sin_elevation;

// The driver wrapper converts to degrees, expects radians
Expand Down Expand Up @@ -235,8 +230,8 @@ class HesaiDecoder : public HesaiScanDecoder
logger_.set_level(rclcpp::Logger::Level::Debug);
RCLCPP_INFO_STREAM(logger_, *sensor_configuration_);

decode_pc_.reset(new NebulaPointCloud);
output_pc_.reset(new NebulaPointCloud);
decode_pc_ = std::make_shared<NebulaPointCloud>();
output_pc_ = std::make_shared<NebulaPointCloud>();

decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
Expand All @@ -257,10 +252,10 @@ class HesaiDecoder : public HesaiScanDecoder
}

const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode);
uint32_t current_azimuth;
uint32_t current_azimuth = 0;

for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) {
current_azimuth = packet_.body.blocks[block_id].get_azimuth();
current_azimuth = packet_.body.blocks[block_id].getAzimuth();

bool scan_completed = checkScanCompleted(
current_azimuth,
Expand Down Expand Up @@ -295,5 +290,4 @@ class HesaiDecoder : public HesaiScanDecoder
}
};

} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,8 @@
#include <cstddef>
#include <cstdint>
#include <ctime>

namespace nebula
{
namespace drivers
{
namespace hesai_packet
#include <stdexcept>
namespace nebula::drivers::hesai_packet
{

// FIXME(mojomex) This is a workaround for the compiler being pedantic about casting `enum class`s
Expand Down Expand Up @@ -61,7 +57,7 @@ struct DateTime

/// @brief Get seconds since epoch
/// @return Whole seconds since epoch
uint64_t get_seconds() const
[[nodiscard]] uint64_t getSeconds() const
{
std::tm tm{};
tm.tm_year = year - 1900 + YearOffset;
Expand All @@ -82,11 +78,11 @@ struct SecondsSinceEpoch

/// @brief Get seconds since epoch
/// @return Whole seconds since epoch
uint64_t get_seconds() const
[[nodiscard]] uint64_t getSeconds() const
{
uint64_t seconds = 0;
for (int i = 0; i < 5; ++i) {
seconds = (seconds << 8) | this->seconds[i];
for (unsigned char second : this->seconds) {
seconds = (seconds << 8) | second;
}
return seconds;
}
Expand Down Expand Up @@ -147,39 +143,39 @@ struct Block
{
uint16_t azimuth;
UnitT units[UnitN];
typedef UnitT unit_t;
using unit_t = UnitT;

uint32_t get_azimuth() const { return azimuth; }
[[nodiscard]] uint32_t getAzimuth() const { return azimuth; }
};

template <typename UnitT, size_t UnitN>
struct FineAzimuthBlock
{
typedef UnitT unit_t;
using unit_t = UnitT;
uint16_t azimuth;
uint8_t fine_azimuth;
UnitT units[UnitN];

uint32_t get_azimuth() const { return (azimuth << 8) + fine_azimuth; }
[[nodiscard]] uint32_t getAzimuth() const { return (azimuth << 8) + fine_azimuth; }
};

template <typename UnitT, size_t UnitN>
struct SOBBlock
{
typedef UnitT unit_t;
using unit_t = UnitT;

/// @brief Start of Block, 0xFFEE
uint16_t sob;
uint16_t azimuth;
UnitT units[UnitN];

uint32_t get_azimuth() const { return azimuth; }
[[nodiscard]] uint32_t getAzimuth() const { return azimuth; }
};

template <typename BlockT, size_t BlockN>
struct Body
{
typedef BlockT block_t;
using block_t = BlockT;
BlockT blocks[BlockN];
};

Expand Down Expand Up @@ -232,7 +228,7 @@ inline int get_n_returns(uint8_t return_mode)
template <typename PacketT>
uint64_t get_timestamp_ns(const PacketT & packet)
{
return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000;
return packet.tail.date_time.getSeconds() * 1000000000 + packet.tail.timestamp * 1000;
}

/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet,
Expand All @@ -247,6 +243,4 @@ double get_dis_unit(const PacketT & packet)
return packet.header.dis_unit / 1000.;
}

} // namespace hesai_packet
} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers::hesai_packet
Original file line number Diff line number Diff line change
Expand Up @@ -15,18 +15,13 @@
#ifndef NEBULA_WS_HESAI_SCAN_DECODER_HPP
#define NEBULA_WS_HESAI_SCAN_DECODER_HPP

#include "nebula_common/hesai/hesai_common.hpp"
#include "nebula_common/point_types.hpp"

#include "pandar_msgs/msg/pandar_packet.hpp"
#include "pandar_msgs/msg/pandar_scan.hpp"
#include <nebula_common/hesai/hesai_common.hpp>
#include <nebula_common/point_types.hpp>

#include <tuple>
#include <vector>

namespace nebula
{
namespace drivers
namespace nebula::drivers
{
/// @brief Base class for Hesai LiDAR decoder
class HesaiScanDecoder
Expand All @@ -53,6 +48,6 @@ class HesaiScanDecoder
/// @return A tuple of point cloud and timestamp in nanoseconds
virtual std::tuple<drivers::NebulaPointCloudPtr, double> getPointcloud() = 0;
};
} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers

#endif // NEBULA_WS_HESAI_SCAN_DECODER_HPP
Original file line number Diff line number Diff line change
Expand Up @@ -14,18 +14,17 @@

#pragma once

#include "nebula_common/nebula_common.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"

#include <nebula_common/nebula_common.hpp>

#include <algorithm>
#include <type_traits>
#include <vector>

namespace nebula
{
namespace drivers
namespace nebula::drivers
{

enum class AngleCorrectionType { CALIBRATION, CORRECTION };
Expand All @@ -44,7 +43,7 @@ class HesaiSensor
/// blocks)
/// @return true if the reflectivity of the unit is strictly greater than that of all other units
/// in return_units, false otherwise
static bool is_strongest(
static bool isStrongestReturn(
uint32_t return_idx,
const std::vector<const typename PacketT::body_t::block_t::unit_t *> & return_units)
{
Expand All @@ -67,7 +66,7 @@ class HesaiSensor
/// length 2 for dual-return with both units having the same channel but coming from different
/// blocks)
/// @return true if the unit is identical to any other one in return_units, false otherwise
static bool is_duplicate(
static bool isDuplicateReturn(
uint32_t return_idx,
const std::vector<const typename PacketT::body_t::block_t::unit_t *> & return_units)
{
Expand All @@ -87,12 +86,11 @@ class HesaiSensor
};

public:
typedef PacketT packet_t;
typedef typename std::conditional<
using packet_t = PacketT;
using angle_corrector_t = typename std::conditional<
(AngleCorrection == AngleCorrectionType::CALIBRATION),
AngleCorrectorCalibrationBased<PacketT::N_CHANNELS, PacketT::DEGREE_SUBDIVISIONS>,
AngleCorrectorCorrectionBased<PacketT::N_CHANNELS, PacketT::DEGREE_SUBDIVISIONS>>::type
angle_corrector_t;
AngleCorrectorCorrectionBased<PacketT::N_CHANNELS, PacketT::DEGREE_SUBDIVISIONS>>::type;

HesaiSensor() = default;
virtual ~HesaiSensor() = default;
Expand All @@ -115,7 +113,7 @@ class HesaiSensor
int getEarliestPointTimeOffsetForBlock(uint32_t start_block_id, const PacketT & packet)
{
unsigned int n_returns = hesai_packet::get_n_returns(packet.tail.return_mode);
int min_offset_ns = 0xFFFFFFFF; // MAXINT
int min_offset_ns = 0x7FFFFFFF; // MAXINT (max. positive value)

for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) {
for (uint32_t channel_id = 0; channel_id < PacketT::N_CHANNELS; ++channel_id) {
Expand Down Expand Up @@ -145,7 +143,7 @@ class HesaiSensor
hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
const std::vector<const typename PacketT::body_t::block_t::unit_t *> & return_units)
{
if (is_duplicate(return_idx, return_units)) {
if (isDuplicateReturn(return_idx, return_units)) {
return ReturnType::IDENTICAL;
}

Expand All @@ -159,7 +157,7 @@ class HesaiSensor
case hesai_packet::return_mode::SINGLE_LAST:
return ReturnType::LAST;
case hesai_packet::return_mode::DUAL_LAST_STRONGEST:
if (is_strongest(return_idx, return_units)) {
if (isStrongestReturn(return_idx, return_units)) {
return return_idx == 0 ? ReturnType::LAST_STRONGEST : ReturnType::STRONGEST;
} else {
return return_idx == 0 ? ReturnType::LAST : ReturnType::SECONDSTRONGEST;
Expand All @@ -169,7 +167,7 @@ class HesaiSensor
case hesai_packet::return_mode::DUAL_FIRST_LAST:
return return_idx == 0 ? ReturnType::FIRST : ReturnType::LAST;
case hesai_packet::return_mode::DUAL_FIRST_STRONGEST:
if (is_strongest(return_idx, return_units)) {
if (isStrongestReturn(return_idx, return_units)) {
return return_idx == 0 ? ReturnType::FIRST_STRONGEST : ReturnType::STRONGEST;
} else {
return return_idx == 0 ? ReturnType::FIRST : ReturnType::SECONDSTRONGEST;
Expand All @@ -193,5 +191,4 @@ class HesaiSensor
}
};

} // namespace drivers
} // namespace nebula
} // namespace nebula::drivers
Loading

0 comments on commit 5377d1c

Please sign in to comment.