diff --git a/docs/hesai_decoder_design.md b/docs/hesai_decoder_design.md index a3d48f0a3..827b87773 100644 --- a/docs/hesai_decoder_design.md +++ b/docs/hesai_decoder_design.md @@ -11,6 +11,7 @@ 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 @@ -18,10 +19,11 @@ For all handled Hesai sensors, the packet structure follows this rough format: ### 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): @@ -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. @@ -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`. @@ -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. @@ -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). @@ -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` is a subclass of the existing `HesaiScanDecoder` to allow all template instantiations to be assigned to variables of the supertype. @@ -144,24 +150,24 @@ 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`). @@ -169,7 +175,7 @@ Reuse already-defined structs as much as possible (c.f. `Packet128E3X` and `Pack 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. diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 9c6e5d555..7194ff96d 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -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()); } @@ -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 sep(","); boost::tokenizer> tok(line, sep); std::vector 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; } @@ -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; } @@ -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) { @@ -159,9 +157,8 @@ struct HesaiCorrection inline nebula::Status LoadFromBinary(const std::vector & 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; @@ -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 & buf) + inline nebula::Status SaveFileFromBinary( + const std::string & correction_file, const std::vector & buf) { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -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; } @@ -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; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 9d04a267a..c1f41cd11 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,7 +2,9 @@ #define NEBULA_COMMON_H #include + #include + #include #include #include @@ -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 @@ -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; @@ -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; @@ -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; diff --git a/nebula_common/include/nebula_common/point_types.hpp b/nebula_common/include/nebula_common/point_types.hpp index 260b7b9db..240b7de5a 100644 --- a/nebula_common/include/nebula_common/point_types.hpp +++ b/nebula_common/include/nebula_common/point_types.hpp @@ -63,20 +63,15 @@ using NebulaPointCloudPtr = pcl::PointCloud::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, diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index 1984642dc..7d3f8bf5c 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -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; } diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp index 9cbbb53ea..67bda0d86 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_calibration_decoder.hpp @@ -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); } diff --git a/nebula_common/src/nebula_common.cpp b/nebula_common/src/nebula_common.cpp index 7af599032..bae2ae063 100644 --- a/nebula_common/src/nebula_common.cpp +++ b/nebula_common/src/nebula_common.cpp @@ -60,7 +60,7 @@ pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( point.ring = p.channel; point.azimuth = rad2deg(p.azimuth) * 100.0; point.distance = p.distance; - point.time_stamp = stamp + static_cast(p.time_stamp)*1e-9; + point.time_stamp = stamp + static_cast(p.time_stamp) * 1e-9; output_pointcloud->points.emplace_back(point); } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 42fd0a008..b3b2a4efa 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -52,8 +52,9 @@ class AngleCorrector /// @param sync_azimuth The azimuth set in the sensor configuration, for which the /// timestamp is aligned to the full second /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; + virtual bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index 3e0c999b0..0bbe1181d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -81,10 +81,10 @@ class AngleCorrectorCalibrationBased : public AngleCorrector (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; uint32_t last_diff_from_sync = (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; - + return current_diff_from_sync < last_diff_from_sync; } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index be60078de..cf4ab9851 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -29,8 +29,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector { // Assumes that: // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) - // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.) - // These assumptions hold for AT128E2X. + // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg + // etc.) These assumptions hold for AT128E2X. int field = sensor_correction_->frameNumber - 1; for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) { if (azimuth < sensor_correction_->startFrame[i]) return field; @@ -84,7 +84,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override + bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override { // For AT128, the scan is always cut at the beginning of the field: // If we would cut at `sync_azimuth`, the points left of it would be diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index dec30dfcd..f02454e3f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -221,7 +221,8 @@ uint64_t get_timestamp_ns(const PacketT & packet) return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000; } -/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, multiplied by this value, yield the distance in meters. +/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, +/// multiplied by this value, yield the distance in meters. /// @tparam PacketT The packet type /// @param packet The packet to get the distance unit from /// @return The distance unit in meters diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp index 78f8a11bc..07f6521fb 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp @@ -178,4 +178,4 @@ class HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 65155efb4..4720c0197 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -272,4 +272,4 @@ class Pandar128E3X : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 41cee89b3..93f5f3b8f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -23,11 +23,11 @@ typedef Packet128E3X Packet128E4X; // FIXME(mojomex): // The OT128 datasheet has entirely different numbers (and more azimuth states). // With the current sensor version, the numbers from the new datasheet are incorrect -// (clouds do not sync to ToS but ToS+.052s) +// (clouds do not sync to ToS but ToS+.052s) class Pandar128E4X : public HesaiSensor { private: -enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; + enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; static constexpr int firing_time_offset_static_ns_[128] = { 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 49758, 43224, 36690, 30156, 2378, @@ -120,4 +120,4 @@ enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp index 12003721e..a76b637fe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp @@ -73,4 +73,4 @@ class Pandar40 : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp index b7a6640f3..b8b225818 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp @@ -53,4 +53,4 @@ class Pandar64 : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp index e3c85e857..8e5486ce7 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp @@ -85,4 +85,4 @@ class PandarAT128 }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp index ac136bc20..dc295d58c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp @@ -100,4 +100,4 @@ class PandarQT128 : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp index 1b7c4e54d..2d929e478 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp @@ -62,4 +62,4 @@ class PandarQT64 : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp index c9a5e7326..33e9dc682 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp @@ -74,4 +74,4 @@ class PandarXT32 : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp index 080876470..a896d1fbb 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp @@ -56,4 +56,4 @@ class PandarXT32M : public HesaiSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp index 60b345861..b9f820dcf 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp @@ -54,4 +54,4 @@ class AngleCorrector }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp index 99bd0c02f..212f7344b 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp @@ -82,4 +82,4 @@ class AngleCorrectorCalibrationBased : public AngleCorrector }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp index 92e58e304..c405aec2e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp @@ -457,4 +457,4 @@ class BpearlV3 : public RobosenseSensor< } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp index 681ad2ba2..aace6eb6d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp @@ -376,4 +376,4 @@ class BpearlV4 : public RobosenseSensor< } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp index 0780e3c24..c9bb20f52 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp @@ -452,4 +452,4 @@ class Helios }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp index 79630bc23..326d54d7d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp @@ -265,4 +265,4 @@ class RobosenseDecoder : public RobosenseScanDecoder }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp index 86b9bc4aa..cd12ee7e4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp @@ -81,4 +81,4 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp index d97dc4e1c..5fe7e25ed 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp @@ -39,4 +39,4 @@ class RobosenseInfoDecoderBase }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp index 607c44aac..de2a76799 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp @@ -34,7 +34,6 @@ struct Timestamp } }; - struct Unit { big_uint16_buf_t distance; @@ -137,9 +136,8 @@ struct ChannelAngleCorrection [[nodiscard]] float getAngle() const { - return sign.value() == ANGLE_SIGN_FLAG - ? static_cast(angle.value()) / 100.0f - : static_cast(angle.value()) / -100.0f; + return sign.value() == ANGLE_SIGN_FLAG ? static_cast(angle.value()) / 100.0f + : static_cast(angle.value()) / -100.0f; } }; @@ -264,4 +262,4 @@ std::string get_float_value(const uint16_t & raw_angle) } // namespace robosense_packet } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp index 4e8b3c25a..f91ed2a7f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp @@ -121,4 +121,4 @@ class RobosenseSensor } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index e08d0c1dd..e1136ac8a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -1,7 +1,14 @@ #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 +#include +#include + #include #include @@ -10,16 +17,8 @@ #include #include #include -#include - -#include "nebula_common/point_types.hpp" -#include "nebula_common/velodyne/velodyne_calibration_decoder.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" - -#include -#include - #include +#include namespace nebula { @@ -55,8 +54,8 @@ static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] static const float 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 VLP32_CHANNEL_DURATION = 2.304f; // [µs] +static const float VLP32_SEQ_DURATION = 55.296f; // [µs] /** Special Definitions for VLS128 support **/ static const float VLP128_DISTANCE_RESOLUTION = 0.004f; // [m] diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 5a8cea2ec..b4687d8fe 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -1,10 +1,10 @@ #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp" +#include + #include #include -#include - namespace nebula { namespace drivers @@ -231,9 +231,10 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa float azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) / - VLP16_BLOCK_DURATION) - corrections.rot_correction * 180.0 / M_PI * 100; - - if (azimuth_corrected_f < 0.0){ + VLP16_BLOCK_DURATION) - + corrections.rot_correction * 180.0 / M_PI * 100; + + if (azimuth_corrected_f < 0.0) { azimuth_corrected_f += 36000.0; } const uint16_t azimuth_corrected = diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 92ee10df0..108b46183 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -1,10 +1,10 @@ #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp" +#include + #include #include -#include - namespace nebula { namespace drivers @@ -173,9 +173,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa // This makes the assumption the difference between the last block and the next packet is the // same as the last to the second to last. // Assumes RPM doesn't change much between blocks. - azimuth_diff = (i == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) - ? 0 - : last_azimuth_diff; + azimuth_diff = + (i == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff; } for (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { @@ -232,7 +231,9 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) { const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; - float azimuth_corrected_f = azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - corrections.rot_correction * 180.0 / M_PI * 100; + float azimuth_corrected_f = + azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - + corrections.rot_correction * 180.0 / M_PI * 100; if (azimuth_corrected_f < 0) { azimuth_corrected_f += 36000; } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index 2e7c644db..2afb02a95 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -1,10 +1,10 @@ #include "nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp" +#include + #include #include -#include - namespace nebula { namespace drivers @@ -249,9 +249,11 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p } // Correct for the laser rotation as a function of timing during the firings. - float azimuth_corrected_f = azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - corrections.rot_correction * 180.0 / M_PI * 100; - - if (azimuth_corrected_f < 0.0){ + float azimuth_corrected_f = azimuth + + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - + corrections.rot_correction * 180.0 / M_PI * 100; + + if (azimuth_corrected_f < 0.0) { azimuth_corrected_f += 36000.0; } const uint16_t azimuth_corrected = ((uint16_t)std::round(azimuth_corrected_f)) % 36000; diff --git a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp index c1910d294..6b32d0289 100644 --- a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp @@ -38,9 +38,8 @@ #include #include -#include - #include +#include #include namespace nebula diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 9c1266347..bed5431dd 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -92,7 +92,10 @@ Status HesaiRosOfflineExtractBag::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractBag::GetStatus() {return wrapper_status_;} +Status HesaiRosOfflineExtractBag::GetStatus() +{ + return wrapper_status_; +} Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -119,7 +122,7 @@ Status HesaiRosOfflineExtractBag::GetParameters( sensor_configuration.return_mode = // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); + this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -343,7 +346,7 @@ Status HesaiRosOfflineExtractBag::ReadBag() writer_->open(storage_options_w, converter_options_w); writer_->create_topic( {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), - ""}); + ""}); needs_open = false; } writer_->write(bag_message); diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index a57d9b96e..b5034e86a 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -92,7 +92,10 @@ Status HesaiRosOfflineExtractSample::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractSample::GetStatus() {return wrapper_status_;} +Status HesaiRosOfflineExtractSample::GetStatus() +{ + return wrapper_status_; +} Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -119,7 +122,7 @@ Status HesaiRosOfflineExtractSample::GetParameters( sensor_configuration.return_mode = // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); + this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); } { rcl_interfaces::msg::ParameterDescriptor descriptor; diff --git a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp index 913898b7c..0bba14063 100644 --- a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp @@ -56,7 +56,10 @@ Status VelodyneRosOfflineExtractBag::InitializeDriver( return driver_ptr_->GetStatus(); } -Status VelodyneRosOfflineExtractBag::GetStatus() {return wrapper_status_;} +Status VelodyneRosOfflineExtractBag::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosOfflineExtractBag::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -352,7 +355,7 @@ Status VelodyneRosOfflineExtractBag::ReadBag() writer_->open(storage_options_w, converter_options_w); writer_->create_topic( {bag_message->topic_name, "velodyne_msgs/msg/VelodyneScan", - rmw_get_serialization_format(), ""}); + rmw_get_serialization_format(), ""}); needs_open = false; } writer_->write(bag_message); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 5b46415e7..ac6bce4e6 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -1,9 +1,9 @@ #ifndef NEBULA_HW_INTERFACE_BASE_H #define NEBULA_HW_INTERFACE_BASE_H +#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "boost_udp_driver/udp_driver.hpp" #include #include diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index 415225309..a39601aa1 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -4,8 +4,8 @@ #include #include -#include #include +#include namespace nebula { diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 2f5639373..d5530c1d2 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -78,9 +78,9 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; +const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; +const int PTP_SYNC_INTERVAL = 1; // Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) const int PTP_LOG_MIN_DELAY_INTERVAL = 0; diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 13626debb..f496689c3 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -1,4 +1,5 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" + #include // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -69,7 +70,8 @@ std::shared_ptr> HesaiHwInterface::SendReceive( bool success = false; std::stringstream ss; - ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) + << " (" << len << ") "; std::string log_tag = ss.str(); PrintDebug(log_tag + "Entering lock"); @@ -86,9 +88,13 @@ std::shared_ptr> HesaiHwInterface::SendReceive( tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, [this, log_tag, &success](const std::vector & header_bytes) { - size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); - if (payload_len == 0) { success = true; } + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | + (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug( + log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + if (payload_len == 0) { + success = true; + } }, [this, log_tag, &recv_buf, &success](const std::vector & payload_bytes) { PrintDebug(log_tag + "Received payload"); @@ -105,7 +111,7 @@ std::shared_ptr> HesaiHwInterface::SendReceive( }, [this, log_tag, &tm]() { PrintDebug(log_tag + "Unlocking mutex"); - tm.unlock(); + tm.unlock(); PrintDebug(log_tag + "Unlocked mutex"); }); this->IOContextRun(); diff --git a/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg b/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg index acf5751bd..944d41abf 100644 --- a/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg +++ b/nebula_messages/robosense_msgs/msg/RobosenseInfoPacket.msg @@ -1,2 +1,2 @@ string lidar_model -RobosensePacket packet \ No newline at end of file +RobosensePacket packet diff --git a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg index 713682931..40b73bd17 100644 --- a/nebula_messages/robosense_msgs/msg/RobosensePacket.msg +++ b/nebula_messages/robosense_msgs/msg/RobosensePacket.msg @@ -1,2 +1,2 @@ builtin_interfaces/Time stamp -uint8[1248] data \ No newline at end of file +uint8[1248] data diff --git a/nebula_messages/robosense_msgs/msg/RobosenseScan.msg b/nebula_messages/robosense_msgs/msg/RobosenseScan.msg index 0c2b5518b..25bd36a37 100644 --- a/nebula_messages/robosense_msgs/msg/RobosenseScan.msg +++ b/nebula_messages/robosense_msgs/msg/RobosenseScan.msg @@ -1,2 +1,2 @@ std_msgs/Header header -RobosensePacket[] packets \ No newline at end of file +RobosensePacket[] packets diff --git a/nebula_messages/robosense_msgs/package.xml b/nebula_messages/robosense_msgs/package.xml index 29077edb2..e41802175 100644 --- a/nebula_messages/robosense_msgs/package.xml +++ b/nebula_messages/robosense_msgs/package.xml @@ -1,22 +1,22 @@ - robosense_msgs - 0.0.0 - Robosense message types for Nebula - Mehmet Emin BASOGLU - TODO: License declaration + robosense_msgs + 0.0.0 + Robosense message types for Nebula + Mehmet Emin BASOGLU + TODO: License declaration - ament_cmake_auto - rosidl_default_generators + ament_cmake_auto + rosidl_default_generators - builtin_interfaces - std_msgs + builtin_interfaces + std_msgs - rosidl_default_runtime - rosidl_interface_packages + rosidl_default_runtime + rosidl_interface_packages - - ament_cmake - + + ament_cmake + diff --git a/nebula_ros/config/hesai/Pandar128E4X.yaml b/nebula_ros/config/hesai/Pandar128E4X.yaml index b4f5df758..12489eb95 100644 --- a/nebula_ros/config/hesai/Pandar128E4X.yaml +++ b/nebula_ros/config/hesai/Pandar128E4X.yaml @@ -16,4 +16,4 @@ calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/Pandar128E4X.csv" setup_sensor: True - online: True \ No newline at end of file + online: True diff --git a/nebula_ros/config/hesai/PandarQT64.yaml b/nebula_ros/config/hesai/PandarQT64.yaml index b2caccca8..303479b90 100644 --- a/nebula_ros/config/hesai/PandarQT64.yaml +++ b/nebula_ros/config/hesai/PandarQT64.yaml @@ -16,4 +16,4 @@ calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarQT64.csv" setup_sensor: True - online: True \ No newline at end of file + online: True diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp index 52518fcd0..a8081b372 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp @@ -13,11 +13,11 @@ #include #include -#include - #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" +#include + namespace nebula { namespace ros diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index 1802f87b2..29f929822 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -1,11 +1,11 @@ #ifndef NEBULA_HesaiHwInterfaceRosWrapper_H #define NEBULA_HesaiHwInterfaceRosWrapper_H +#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" #include #include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp index c23b76c55..09cc8d829 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -1,24 +1,22 @@ #ifndef NEBULA_HesaiHwMonitorRosWrapper_H #define NEBULA_HesaiHwMonitorRosWrapper_H +#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" #include #include #include #include -#include - -#include #include #include #include +#include #include namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp index 74e33231e..18bc6f2b8 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp @@ -81,7 +81,8 @@ class RobosenseHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterf /// @brief Callback for receiving RobosensePacket /// @param difop_buffer Received DIFOP packet - void ReceiveInfoDataCallback(std::unique_ptr difop_buffer); + void ReceiveInfoDataCallback( + std::unique_ptr difop_buffer); }; } // namespace ros diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml index 75c7414da..842fecb36 100644 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -87,9 +87,9 @@ - @@ -112,4 +112,4 @@ - \ No newline at end of file + diff --git a/nebula_ros/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml index 6e2ab6dda..3c42285be 100644 --- a/nebula_ros/launch/robosense_launch.all_hw.xml +++ b/nebula_ros/launch/robosense_launch.all_hw.xml @@ -50,4 +50,4 @@ - \ No newline at end of file + diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index 6f27d547e..5403f4cfc 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -43,8 +43,7 @@ HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); pandar_scan_sub_ = create_subscription( "pandar_packets", qos, std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); @@ -91,8 +90,8 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = @@ -101,7 +100,8 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( } auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void HesaiDriverRosWrapper::PublishCloud( @@ -138,7 +138,10 @@ Status HesaiDriverRosWrapper::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } +Status HesaiDriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} Status HesaiDriverRosWrapper::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -288,47 +291,50 @@ Status HesaiDriverRosWrapper::GetParameters( hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - + bool run_local = !launch_hw; if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { std::string calibration_file_path_from_sensor; if (launch_hw && !calibration_configuration.calibration_file.empty()) { int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); + calibration_file_path_from_sensor += + calibration_configuration.calibration_file.substr(0, ext_pos); calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr( + ext_pos, calibration_configuration.calibration_file.size() - ext_pos); } - if(launch_hw) { + if (launch_hw) { run_local = false; RCLCPP_INFO_STREAM( this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver() == Status::OK) { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - } else { - run_local = true; - } - }); + << sensor_configuration.sensor_ip << "'"); + std::future future = std::async( + std::launch::async, + [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + if (hw_interface_.InitializeTcpDriver() == Status::OK) { + auto str = hw_interface_.GetLidarCalibrationString(); + auto rt = + calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); + RCLCPP_ERROR_STREAM(get_logger(), str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); + } + } else { + run_local = true; + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(5000)); if (status == std::future_status::timeout) { @@ -337,36 +343,38 @@ Status HesaiDriverRosWrapper::GetParameters( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); + this->get_logger(), + "Acquired calibration data from sensor: '" << sensor_configuration.sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "The calibration has been saved in '" << calibration_file_path_from_sensor << "'"); } } - if(run_local) { + if (run_local) { RCLCPP_WARN_STREAM(get_logger(), "Running locally"); bool run_org = false; if (calibration_file_path_from_sensor.empty()) { run_org = true; } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + RCLCPP_INFO_STREAM( + get_logger(), "Trying to load file: " << calibration_file_path_from_sensor); + auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_file_path_from_sensor << "'"); } } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); + if (run_org) { + RCLCPP_INFO_STREAM( + get_logger(), "Trying to load file: " << calibration_configuration.calibration_file); if (calibration_configuration.calibration_file.empty()) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = @@ -377,54 +385,61 @@ Status HesaiDriverRosWrapper::GetParameters( this->get_logger(), "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); } } } } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 std::string correction_file_path_from_sensor; if (launch_hw && !correction_file_path.empty()) { int ext_pos = correction_file_path.find_last_of('.'); correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); + correction_file_path_from_sensor += + correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); + std::future future = std::async( + std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, + &run_local, &launch_hw]() { + if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { + RCLCPP_INFO_STREAM(this->get_logger(), "Trying to acquire calibration data from sensor"); + auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); + RCLCPP_INFO_STREAM( + get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary( + correction_file_path_from_sensor, received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromBinary failed:" << correction_file_path_from_sensor + << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), "LoadFromBinary success" + << "\n"); + run_local = false; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "LoadFromBinary failed" + << ". Falling back to offline calibration file."); + run_local = true; + } + } else { + RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); run_local = true; } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); + }); if (!run_local) { std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); @@ -433,30 +448,29 @@ Status HesaiDriverRosWrapper::GetParameters( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); + this->get_logger(), + "Acquired correction data from sensor: '" << sensor_configuration.sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "The correction has been saved in '" << correction_file_path_from_sensor << "'"); } } - if(run_local) { + if (run_local) { bool run_org = false; if (correction_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); + auto cal_status = correction_configuration.LoadFromFile(correction_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "Load correction data from: '" << correction_file_path_from_sensor << "'"); } } - if(run_org) { + if (run_org) { if (correction_file_path.empty()) { RCLCPP_ERROR_STREAM( this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); @@ -468,15 +482,14 @@ Status HesaiDriverRosWrapper::GetParameters( RCLCPP_ERROR_STREAM( this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); + this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); } } } } - } // end AT128 + } // end AT128 // Do not use outside of this location hw_interface_.~HesaiHwInterface(); RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 1ac59b55b..7d0d53896 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -23,34 +23,30 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions std::static_pointer_cast(sensor_cfg_ptr)); #if not defined(TEST_PCAP) Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) - { + if (this->retry_hw_) { int cnt = 0; RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { + while (rt == Status::ERROR_1) { cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); rt = hw_interface_.InitializeTcpDriver(); } } - if(rt != Status::ERROR_1){ - try{ + if (rt != Status::ERROR_1) { + try { std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - for (std::thread & th : thread_pool) { - th.join(); - } + thread_pool.emplace_back([this] { + auto result = hw_interface_.GetInventory(); + RCLCPP_INFO_STREAM(get_logger(), result); + hw_interface_.SetTargetModel(result.model); + }); + for (std::thread & th : thread_pool) { + th.join(); + } - } - catch (...) - { + } catch (...) { std::cout << "catch (...) in parent" << std::endl; RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); } @@ -58,10 +54,10 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions hw_interface_.CheckAndSetConfig(); updateParameters(); } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); } #endif @@ -146,7 +142,8 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions StreamStart(); } -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { +HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() +{ RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); hw_interface_.FinalizeTcpDriver(); } @@ -159,8 +156,14 @@ Status HesaiHwInterfaceRosWrapper::StreamStart() return interface_status_; } -Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } +Status HesaiHwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status HesaiHwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -362,9 +365,9 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { + sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportTypeFromString( + this->get_parameter("ptp_transport_type").as_string()); + if (static_cast(sensor_configuration.ptp_profile) > 0) { sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; } } @@ -390,18 +393,23 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); } - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + if (sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } - if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); + if ( + sensor_configuration.ptp_transport_type == + nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); return Status::SENSOR_CONFIG_ERROR; } - if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + if (sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); return Status::SENSOR_CONFIG_ERROR; } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index df484a27a..916b2624e 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -86,7 +86,8 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback( } auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void VelodyneDriverRosWrapper::PublishCloud( @@ -111,7 +112,10 @@ Status VelodyneDriverRosWrapper::InitializeDriver( return driver_ptr_->GetStatus(); } -Status VelodyneDriverRosWrapper::GetStatus() { return wrapper_status_; } +Status VelodyneDriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} Status VelodyneDriverRosWrapper::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp index cbbc19e05..38dda5472 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp @@ -62,8 +62,14 @@ Status VelodyneHwInterfaceRosWrapper::StreamStart() return interface_status_; } -Status VelodyneHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status VelodyneHwInterfaceRosWrapper::Shutdown() { return Status::OK; } +Status VelodyneHwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status VelodyneHwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} Status VelodyneHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) diff --git a/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp index 172e61c83..1e4f49dd6 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp @@ -143,10 +143,19 @@ VelodyneHwMonitorRosWrapper::VelodyneHwMonitorRosWrapper(const rclcpp::NodeOptio }); } -Status VelodyneHwMonitorRosWrapper::MonitorStart() { return interface_status_; } +Status VelodyneHwMonitorRosWrapper::MonitorStart() +{ + return interface_status_; +} -Status VelodyneHwMonitorRosWrapper::MonitorStop() { return Status::OK; } -Status VelodyneHwMonitorRosWrapper::Shutdown() { return Status::OK; } +Status VelodyneHwMonitorRosWrapper::MonitorStop() +{ + return Status::OK; +} +Status VelodyneHwMonitorRosWrapper::Shutdown() +{ + return Status::OK; +} Status VelodyneHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) diff --git a/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml b/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml index 536456380..54e2fec22 100644 --- a/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml +++ b/nebula_tests/data/velodyne/vlp32/1713492677464078412/metadata.yaml @@ -23,4 +23,4 @@ rosbag2_bagfile_information: nanoseconds_since_epoch: 1713492677464078412 duration: nanoseconds: 376987098 - message_count: 5 \ No newline at end of file + message_count: 5 diff --git a/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml b/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml index 81489216f..1ee009d7e 100644 --- a/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml +++ b/nebula_tests/data/velodyne/vls128/1614315746471294674/metadata.yaml @@ -23,4 +23,4 @@ rosbag2_bagfile_information: nanoseconds_since_epoch: 1585897255313147068 duration: nanoseconds: 263164997 - message_count: 4 \ No newline at end of file + message_count: 4 diff --git a/nebula_tests/hesai/hesai_common.hpp b/nebula_tests/hesai/hesai_common.hpp index 784d8cd55..9d0914f2b 100644 --- a/nebula_tests/hesai/hesai_common.hpp +++ b/nebula_tests/hesai/hesai_common.hpp @@ -69,5 +69,5 @@ void printPCD(nebula::drivers::NebulaPointCloudPtr pp) } } -} // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace test +} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index a07a4dabd..71c49a7c0 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -235,8 +235,7 @@ Status HesaiRosDecoderTest::GetParameters( if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if ( - sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 96ff00c3c..471c6ead9 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -19,72 +19,15 @@ namespace test { const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { - { - "Pandar40P", - "Dual", - "Pandar40P.csv", - "40p/1673400149412331409", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "Pandar64", - "Dual", - "Pandar64.csv", - "64/1673403880599376836", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "PandarAT128", - "LastStrongest", - "PandarAT128.csv", - "at128/1679653308406038376", - "PandarAT128.dat", - "hesai", - 0, - 1.f, - 180.f - }, - { - "PandarQT64", - "Dual", - "PandarQT64.csv", - "qt64/1673401195788312575", - "", - "hesai", - 0, - 0.1f, - 60.f - }, - { - "PandarXT32", - "Dual", - "PandarXT32.csv", - "xt32/1673400677802009732", - "", - "hesai", - 0, - 0.05f, - 120.f - }, - { - "PandarXT32M", - "LastStrongest", - "PandarXT32M.csv", - "xt32m/1660893203042895158", - "", - "hesai", - 0, - 0.5f, - 300.f - }}; + {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "", "hesai", 0, 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "", "hesai", 0, 0.3f, 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.csv", "at128/1679653308406038376", + "PandarAT128.dat", "hesai", 0, 1.f, 180.f}, + {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "", "hesai", 0, 0.1f, 60.f}, + {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "", "hesai", 0, 0.05f, + 120.f}, + {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "", "hesai", 0, + 0.5f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp index dca0f05d7..495fc9985 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp @@ -2,9 +2,10 @@ #include "hesai_ros_decoder_test.hpp" -#include #include +#include + namespace nebula { namespace test diff --git a/nebula_tests/velodyne/CMakeLists.txt b/nebula_tests/velodyne/CMakeLists.txt index f166997bd..d96c85ad2 100644 --- a/nebula_tests/velodyne/CMakeLists.txt +++ b/nebula_tests/velodyne/CMakeLists.txt @@ -59,4 +59,4 @@ include target_link_libraries(velodyne_ros_decoder_test_main_vlp32 ${PCL_LIBRARIES} velodyne_ros_decoder_test_vlp32 -) \ No newline at end of file +) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp index fa3604485..2dca9e2b0 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp @@ -57,7 +57,10 @@ Status VelodyneRosDecoderTest::InitializeDriver( return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } +Status VelodyneRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index 5fe5e1dbc..5faff7a2e 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -57,7 +57,10 @@ Status VelodyneRosDecoderTest::InitializeDriver( return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } +Status VelodyneRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -222,8 +225,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter( - "target_topic", "/velodyne_packets", descriptor); + this->declare_parameter("target_topic", "/velodyne_packets", descriptor); target_topic = this->get_parameter("target_topic").as_string(); } diff --git a/scripts/hesai_config.py b/scripts/hesai_config.py index fc45b7a65..471f3cae1 100644 --- a/scripts/hesai_config.py +++ b/scripts/hesai_config.py @@ -1,11 +1,13 @@ from __future__ import annotations +from argparse import ArgumentParser +from dataclasses import dataclass +from ipaddress import IPv4Address +from ipaddress import IPv4Network +from ipaddress import ip_interface import socket import struct import sys -from argparse import ArgumentParser -from dataclasses import dataclass -from ipaddress import IPv4Address, IPv4Network, ip_interface from typing import Any from rich import print diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 521dfd347..1d80e7539 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -1,11 +1,13 @@ #%% +import argparse +import glob import os -import pandas as pd import re -import glob -import argparse + from matplotlib import pyplot as plt +import pandas as pd + def parse_logs(run_name): dfs = [] @@ -21,7 +23,7 @@ def parse_logs(run_name): dfs.append(pd.DataFrame(records)) df = pd.concat(dfs) - + for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms df['d_total'] = sum([df[c] for c in df.columns if c.startswith("d_")]) # type: ignore @@ -46,7 +48,7 @@ def plot_timing_comparison(run_names): ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') - + d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) for j, col in enumerate(d_columns): @@ -85,4 +87,4 @@ def plot_timing_comparison(run_names): parser.add_argument("run_names", nargs='+', help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.") args = parser.parse_args() - plot_timing_comparison(args.run_names) \ No newline at end of file + plot_timing_comparison(args.run_names) diff --git a/scripts/profiling_runner.bash b/scripts/profiling_runner.bash index 029faf647..aa0513655 100755 --- a/scripts/profiling_runner.bash +++ b/scripts/profiling_runner.bash @@ -2,10 +2,9 @@ # Configuration -print_usage () { +print_usage() { # Print optionally passed error message - if [ -n "$1" ] - then + if [ -n "$1" ]; then echo "$1" fi echo "Usage: $0 [-t|--timeout ] [-m|--sensor-model ] [-n|--n-runs ] [-f|--maxfreq ] [-c|--taskset-cores ] [-b|--rosbag-path ]" @@ -18,15 +17,14 @@ print_usage () { } # Default parameters -runtime=20 # seconds (rosbag runtime is timeout-3s for startup) -n_runs=3 # number of runs per test -maxfreq=2300000 # 2.3 GHz -n_cores=$(nproc --all) # number of cores of the CPU -taskset_cores=0-$(( $n_cores - 1 )) # cores to run Nebula on -nebula_dir=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )/.." &> /dev/null && pwd ) - -if [ $# -lt 1 ] -then +runtime=20 # seconds (rosbag runtime is timeout-3s for startup) +n_runs=3 # number of runs per test +maxfreq=2300000 # 2.3 GHz +n_cores=$(nproc --all) # number of cores of the CPU +taskset_cores=0-$((n_cores - 1)) # cores to run Nebula on +nebula_dir=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." &>/dev/null && pwd) + +if [ $# -lt 1 ]; then print_usage "No run name specified" exit 1 fi @@ -35,62 +33,59 @@ run_name=$1 shift #do the parsing -while [[ $# -gt 0 ]] -do +while [[ $# -gt 0 ]]; do key="$1" case $key in - -t|--runtime) - runtime="$2" - shift - shift - ;; - -m|--sensor-model) - sensor_model="$2" - shift - shift - ;; - -n|--n-runs) - n_runs="$2" - shift - shift - ;; - -f|--maxfreq) - maxfreq="$2" - shift - shift - ;; - -c|--taskset-cores) - taskset_cores="$2" - shift - shift - ;; - -d|--nebula-dir) - nebula_dir="$2" - shift - shift - ;; - -b|--rosbag-path) - rosbag_path="$2" - shift - shift - ;; - *) - print_usage "Unknown option $1" - exit 1 - ;; + -t | --runtime) + runtime="$2" + shift + shift + ;; + -m | --sensor-model) + sensor_model="$2" + shift + shift + ;; + -n | --n-runs) + n_runs="$2" + shift + shift + ;; + -f | --maxfreq) + maxfreq="$2" + shift + shift + ;; + -c | --taskset-cores) + taskset_cores="$2" + shift + shift + ;; + -d | --nebula-dir) + nebula_dir="$2" + shift + shift + ;; + -b | --rosbag-path) + rosbag_path="$2" + shift + shift + ;; + *) + print_usage "Unknown option $1" + exit 1 + ;; esac done # Enf of configuration -if [ -z "$rosbag_path" ] -then +if [ -z "$rosbag_path" ]; then print_usage "No --rosbag-path specified" exit 1 fi -if [ -z "$sensor_model" ] -then +if [ -z "$sensor_model" ]; then print_usage "No --sensor-model specified" exit 1 fi @@ -102,11 +97,10 @@ echo "Outputting to '$output_dir/$run_name-[1-$n_runs].log'" cd "$nebula_dir" -lockfreq () { - for (( i=0; i<$n_cores; i++ )) - do - echo userspace | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor > /dev/null - echo $maxfreq | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_setspeed > /dev/null +lockfreq() { + for ((i = 0; i < n_cores; i++)); do + echo userspace | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor >/dev/null + echo $maxfreq | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_setspeed >/dev/null echo -n "CPU $i's frequency is: " sudo cat /sys/devices/system/cpu/cpu$i/cpufreq/cpuinfo_cur_freq done @@ -114,10 +108,9 @@ lockfreq () { sudo sh -c "echo 0 > /sys/devices/system/cpu/cpufreq/boost" } -unlockfreq () { - for (( i=0; i<$n_cores; i++ )) - do - echo schedutil | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor > /dev/null +unlockfreq() { + for ((i = 0; i < n_cores; i++)); do + echo schedutil | sudo tee /sys/devices/system/cpu/cpufreq/policy$i/scaling_governor >/dev/null echo -n "CPU $i's frequency is: " sudo cat /sys/devices/system/cpu/cpu$i/cpufreq/cpuinfo_cur_freq done @@ -137,15 +130,14 @@ ros2 daemon start echo "Setting up for test run" lockfreq -for (( i=1; i<=$n_runs; i++ )) -do +for ((i = 1; i <= n_runs; i++)); do echo "Running iteration $i of $n_runs" # set the log level to debug for all ros 2 nodes - timeout -s INT $runtime taskset -c "$taskset_cores" ros2 launch nebula_ros nebula_launch.py "sensor_model:=$sensor_model" launch_hw:=false debug_logging:=true > "$output_dir/$run_name-$i.log" 2>&1 & - (sleep 3 && timeout -s KILL $(( $runtime - 3 )) nohup ros2 bag play -l "$rosbag_path") > /dev/null & + timeout -s INT $runtime taskset -c "$taskset_cores" ros2 launch nebula_ros nebula_launch.py "sensor_model:=$sensor_model" launch_hw:=false debug_logging:=true >"$output_dir/$run_name-$i.log" 2>&1 & + (sleep 3 && timeout -s KILL $((runtime - 3)) nohup ros2 bag play -l "$rosbag_path") >/dev/null & wait done echo "Unlocking frequency" -unlockfreq \ No newline at end of file +unlockfreq