Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Sep 8, 2024
1 parent 04fd211 commit 1b1c3cf
Show file tree
Hide file tree
Showing 71 changed files with 497 additions and 510 deletions.
78 changes: 42 additions & 36 deletions docs/hesai_decoder_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,17 +11,19 @@ This way, runtime overhead for this generalization is `0`.
### Packet formats

For all handled Hesai sensors, the packet structure follows this rough format:

1. (optional) header: static sensor info and feature flags
2. body: point data
3. tail and other appendices: timestamp, operation mode info

### Decoding steps

For all handled Hesai sensors, decoding a packet follows these steps:

```python
def unpack(packet):
parse_and_validate(packet)
# return group: one (single-return) or more (multi-return)
# return group: one (single-return) or more (multi-return)
# blocks that belong to the same azimuth
for return_group in packet:
if is_start_of_new_scan(return_group):
Expand All @@ -40,17 +42,18 @@ def decode(return_group):
append to pointcloud
```

The steps marked with __*__ are model-specific:
The steps marked with **\*** are model-specific:

* angle correction
* timing correction
* return type assignment
- angle correction
- timing correction
- return type assignment

### Angle correction

There are two approaches between all the supported sensors:
* Calibration file based
* Correction file based (currently only used by AT128)

- Calibration file based
- Correction file based (currently only used by AT128)

For both approaches, sin/cos lookup tables can be computed.
However, the resolution and calculation of these tables is different.
Expand All @@ -60,7 +63,7 @@ However, the resolution and calculation of these tables is different.
For each laser channel, a fixed elevation angle and azimuth angle offset are defined in the calibration file.
Thus, sin/cos for elevation are only a function of the laser channel (not dependent on azimuth) while those for azimuth are a function of azimuth AND elevation.

Lookup tables for elevation can thus be sized with `n_channels`, yielding a maximum size of
Lookup tables for elevation can thus be sized with `n_channels`, yielding a maximum size of
`128 * sizeof(float) = 512B` each.

For azimuth, the size is `n_channels * n_azimuths = n_channels * 360 * azimuth_resolution <= 128 * 36000`.
Expand Down Expand Up @@ -94,9 +97,10 @@ While there is a wide range of different supported return modes (e.g. single (fi
Differences only arise in multi-return (dual or triple) in the output order of the returns, and in the handling of some returns being duplicates (e.g. in dual(first, strongest), the first return coincides with the strongest one).

Here is an exhaustive list of differences:
* For Dual (First, Last) `0x3B`, 128E3X, 128E4X and XT32 reverse the output order (Last, First)
* For Dual (Last, Strongest) `0x39`, all sensors except XT32M place the second strongest return in the even block if last == strongest
* For Dual (First, Strongest) `0x3c`, the same as for `0x39` holds.

- For Dual (First, Last) `0x3B`, 128E3X, 128E4X and XT32 reverse the output order (Last, First)
- For Dual (Last, Strongest) `0x39`, all sensors except XT32M place the second strongest return in the even block if last == strongest
- For Dual (First, Strongest) `0x3c`, the same as for `0x39` holds.

For all other return modes, duplicate points are output if the two returns coincide.

Expand All @@ -119,9 +123,10 @@ Return mode handling has a default implementation that is supplemented by additi
### `AngleCorrector`

The angle corrector has three main tasks:
* compute corrected azimuth/elevation for given azimuth and channel
* implement `hasScanCompleted()` logic that decides where one scan ends and the next starts
* compute and provide lookup tables for sin/cos/etc.

- compute corrected azimuth/elevation for given azimuth and channel
- implement `hasScanCompleted()` logic that decides where one scan ends and the next starts
- compute and provide lookup tables for sin/cos/etc.

The two angle correction types are calibration-based and correction-based. In both approaches, a file from the sensor is used to extract the angle correction for each azimuth/channel.
For all approaches, cos/sin lookup tables in the appropriate size are generated (see requirements section above).
Expand All @@ -133,9 +138,10 @@ It is a template class taking a sensor type `SensorT` from which packet type, an
Thus, this unified decoder is an almost zero-cost abstraction.

Its tasks are:
* parsing an incoming packet
* managing decode/output point buffers
* converting all points in the packet using the sensor-specific functions of `SensorT` where necessary

- parsing an incoming packet
- managing decode/output point buffers
- converting all points in the packet using the sensor-specific functions of `SensorT` where necessary

`HesaiDecoder<SensorT>` is a subclass of the existing `HesaiScanDecoder` to allow all template instantiations to be assigned to variables of the supertype.

Expand All @@ -144,32 +150,32 @@ Its tasks are:
To support a new sensor model, first familiarize with the already implemented decoders.
Then, consult the new sensor's datasheet and identify the following parameters:

| Parameter | Chapter | Possible values | Notes |
|-|-|-|-|
| Header format | 3.1 | `Header12B`, `Header8B`, ... | `Header12B` is the standard and comprises the UDP pre-header and header (6+6B) mentioned in the data sheets |
| Blocks per packet | 3.1 | `2`, `6`, `10`, ... | |
| Number of channels | 3.1 | `32`, `40`, `64`, ... | |
| Unit format | 3.1 | `Unit3B`, `Unit4B`, ... | |
| Angle correction | App. 3 | `CALIBRATION`, `CORRECTION`, ... | The datasheet usually specifies whether a calibration/correction file is used |
| Timing correction | App. 2 | | There is usually a block and channel component. These come in the form of formulas/lookup tables. For most sensors, these depend on return mode and for some, features like high resolution mode, alternate firing etc. might change the timing |
| Return type handling | 3.1 | | Return modes are handled identically for most sensors but some re-order the returns or replace returns if there are duplicates |
| Bytes per second | 1.4 | | |
| Lowest supported frequency | 1.4 | `5 Hz`, `10 Hz`, ... | |

| Chapter | Full title |
|-|-|
|1.4| Introduction > Specifications|
|3.1| Data Structure > Point Cloud Data Packet|
|App. 2| Absolute Time of Point Cloud Data|
|App. 3| Angle Correction|
| Parameter | Chapter | Possible values | Notes |
| -------------------------- | ------- | -------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| Header format | 3.1 | `Header12B`, `Header8B`, ... | `Header12B` is the standard and comprises the UDP pre-header and header (6+6B) mentioned in the data sheets |
| Blocks per packet | 3.1 | `2`, `6`, `10`, ... | |
| Number of channels | 3.1 | `32`, `40`, `64`, ... | |
| Unit format | 3.1 | `Unit3B`, `Unit4B`, ... | |
| Angle correction | App. 3 | `CALIBRATION`, `CORRECTION`, ... | The datasheet usually specifies whether a calibration/correction file is used |
| Timing correction | App. 2 | | There is usually a block and channel component. These come in the form of formulas/lookup tables. For most sensors, these depend on return mode and for some, features like high resolution mode, alternate firing etc. might change the timing |
| Return type handling | 3.1 | | Return modes are handled identically for most sensors but some re-order the returns or replace returns if there are duplicates |
| Bytes per second | 1.4 | | |
| Lowest supported frequency | 1.4 | `5 Hz`, `10 Hz`, ... | |

| Chapter | Full title |
| ------- | ---------------------------------------- |
| 1.4 | Introduction > Specifications |
| 3.1 | Data Structure > Point Cloud Data Packet |
| App. 2 | Absolute Time of Point Cloud Data |
| App. 3 | Angle Correction |

With this information, create a `PacketMySensor` struct and `SensorMySensor` class.
Reuse already-defined structs as much as possible (c.f. `Packet128E3X` and `Packet128E4X`).

Implement timing correction in `SensorMySensor` and define the class constants `float MIN_RANGE`,
`float MAX_RANGE` and `size_t MAX_SCAN_BUFFER_POINTS`.
The former two are used for filtering out too-close and too-far away points while the latter is used to
allocate pointcloud buffers.
allocate pointcloud buffers.
Set `MAX_SCAN_BUFFER_POINTS = bytes_per_second / lowest_supported_frequency` from the parameters found above.

If there are any non-standard features your sensor has, implement them as generically as possible to allow for future sensors to re-use your code.
33 changes: 15 additions & 18 deletions nebula_common/include/nebula_common/hesai/hesai_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
return Status::INVALID_CALIBRATION_FILE;
}
std::ostringstream ss;
ss << ifs.rdbuf(); // reading data
ss << ifs.rdbuf(); // reading data
ifs.close();
return LoadFromString(ss.str());
}
Expand All @@ -70,14 +70,12 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
ss << calibration_content;
std::string line;
constexpr size_t expected_cols = 3;
while(std::getline(ss, line)) {
while (std::getline(ss, line)) {
boost::char_separator<char> sep(",");
boost::tokenizer<boost::char_separator<char>> tok(line, sep);

std::vector<std::string> actual_tokens(tok.begin(), tok.end());
if (actual_tokens.size() < expected_cols
|| actual_tokens.size() > expected_cols
) {
if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) {
std::cerr << "Ignoring line with unexpected data:" << line << std::endl;
continue;
}
Expand All @@ -88,10 +86,9 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
float azimuth = std::stof(actual_tokens[2]);
elev_angle_map[laser_id - 1] = elevation;
azimuth_offset_map[laser_id - 1] = azimuth;
} catch (const std::invalid_argument& ia) {
} catch (const std::invalid_argument & ia) {
continue;
}

}
return Status::OK;
}
Expand Down Expand Up @@ -121,7 +118,8 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
/// @param calibration_file path
/// @param calibration_string calibration string
/// @return Resulting status
inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string)
inline nebula::Status SaveFileFromString(
const std::string & calibration_file, const std::string & calibration_string)
{
std::ofstream ofs(calibration_file);
if (!ofs) {
Expand Down Expand Up @@ -159,9 +157,8 @@ struct HesaiCorrection
inline nebula::Status LoadFromBinary(const std::vector<uint8_t> & buf)
{
size_t index;
for (index = 0; index < buf.size()-1; index++) {
if(buf[index]==0xee && buf[index+1]==0xff)
break;
for (index = 0; index < buf.size() - 1; index++) {
if (buf[index] == 0xee && buf[index + 1] == 0xff) break;
}
delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff));
versionMajor = buf[index + 2] & 0xff;
Expand Down Expand Up @@ -281,7 +278,8 @@ struct HesaiCorrection
/// @param correction_file path
/// @param buf correction binary
/// @return Resulting status
inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector<uint8_t> & buf)
inline nebula::Status SaveFileFromBinary(
const std::string & correction_file, const std::vector<uint8_t> & buf)
{
std::cerr << "Saving in:" << correction_file << "\n";
std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary);
Expand All @@ -291,21 +289,20 @@ struct HesaiCorrection
}
std::cerr << "Writing start...." << buf.size() << "\n";
bool sop_received = false;
for (const auto &byte : buf) {
for (const auto & byte : buf) {
if (!sop_received) {
if (byte == 0xEE) {
std::cerr << "SOP received....\n";
sop_received = true;
}
}
if(sop_received) {
if (sop_received) {
ofs << byte;
}
}
std::cerr << "Closing file\n";
ofs.close();
if(sop_received)
return Status::OK;
if (sop_received) return Status::OK;
return Status::INVALID_CALIBRATION_FILE;
}

Expand Down Expand Up @@ -435,8 +432,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode
case SensorModel::HESAI_PANDARQT128:
if (return_mode == ReturnMode::LAST) return 0;
if (return_mode == ReturnMode::STRONGEST) return 1;
if (return_mode == ReturnMode::DUAL_LAST_STRONGEST
|| return_mode == ReturnMode::DUAL) return 2;
if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL)
return 2;
if (return_mode == ReturnMode::FIRST) return 3;
if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4;
if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5;
Expand Down
36 changes: 14 additions & 22 deletions nebula_common/include/nebula_common/nebula_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@
#define NEBULA_COMMON_H

#include <nebula_common/point_types.hpp>

#include <boost/tokenizer.hpp>

#include <map>
#include <ostream>
#include <string>
Expand Down Expand Up @@ -342,24 +344,11 @@ enum class datatype {
FLOAT64 = 8
};

enum class PtpProfile {
IEEE_1588v2 = 0,
IEEE_802_1AS,
IEEE_802_1AS_AUTO,
PROFILE_UNKNOWN
};
enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, PROFILE_UNKNOWN };

enum class PtpTransportType {
UDP_IP = 0,
L2,
UNKNOWN_TRANSPORT
};
enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT };

enum class PtpSwitchType {
NON_TSN = 0,
TSN,
UNKNOWN_SWITCH
};
enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH };

/// @brief not used?
struct PointField
Expand Down Expand Up @@ -618,8 +607,9 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile)
{
// Hesai
auto tmp_str = ptp_profile;
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
[](unsigned char c){ return std::tolower(c); });
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) {
return std::tolower(c);
});
if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2;
if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS;
if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO;
Expand Down Expand Up @@ -657,8 +647,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport
{
// Hesai
auto tmp_str = transport_type;
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
[](unsigned char c){ return std::tolower(c); });
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) {
return std::tolower(c);
});
if (tmp_str == "udp") return PtpTransportType::UDP_IP;
if (tmp_str == "l2") return PtpTransportType::L2;

Expand Down Expand Up @@ -692,8 +683,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type)
{
// Hesai
auto tmp_str = switch_type;
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
[](unsigned char c){ return std::tolower(c); });
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) {
return std::tolower(c);
});
if (tmp_str == "tsn") return PtpSwitchType::TSN;
if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN;

Expand Down
21 changes: 8 additions & 13 deletions nebula_common/include/nebula_common/point_types.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,20 +63,15 @@ using NebulaPointCloudPtr = pcl::PointCloud<NebulaPoint>::Ptr;
} // namespace drivers
} // namespace nebula

POINT_CLOUD_REGISTER_POINT_STRUCT(nebula::drivers::PointXYZIR,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t,
ring, ring))
POINT_CLOUD_REGISTER_POINT_STRUCT(
nebula::drivers::PointXYZIR,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring))

POINT_CLOUD_REGISTER_POINT_STRUCT(nebula::drivers::PointXYZIRADT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(std::uint16_t, ring, ring)
(float, azimuth, azimuth)
(float, distance, distance)
(std::uint8_t, return_type, return_type)
(double, time_stamp, time_stamp))
POINT_CLOUD_REGISTER_POINT_STRUCT(
nebula::drivers::PointXYZIRADT,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
double, time_stamp, time_stamp))

POINT_CLOUD_REGISTER_POINT_STRUCT(
nebula::drivers::PointXYZICATR,
Expand Down
6 changes: 3 additions & 3 deletions nebula_common/include/nebula_common/robosense/robosense_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,10 +188,10 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase

void CreateCorrectedChannels()
{
for(auto& correction : calibration) {
for (auto & correction : calibration) {
uint16_t channel = 0;
for(const auto& compare:calibration) {
if(compare.elevation < correction.elevation) ++channel;
for (const auto & compare : calibration) {
if (compare.elevation < correction.elevation) ++channel;
}
correction.channel = channel;
}
Expand Down
8 changes: 2 additions & 6 deletions nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,12 +61,8 @@ class VelodyneCalibration
bool initialized;

public:
explicit VelodyneCalibration()
: distance_resolution_m(0.002f), initialized(false)
{
}
explicit VelodyneCalibration(const std::string & calibration_file)
: distance_resolution_m(0.002f)
explicit VelodyneCalibration() : distance_resolution_m(0.002f), initialized(false) {}
explicit VelodyneCalibration(const std::string & calibration_file) : distance_resolution_m(0.002f)
{
read(calibration_file);
}
Expand Down
Loading

0 comments on commit 1b1c3cf

Please sign in to comment.