diff --git a/.cspell.json b/.cspell.json index 8c180631a..4c14cb9f3 100644 --- a/.cspell.json +++ b/.cspell.json @@ -11,6 +11,8 @@ "block_id", "Bpearl", "calib", + "centi", + "ddeg", "DHAVE", "Difop", "extrinsics", diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 50a5d55ab..b38038473 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -19,6 +19,7 @@ #include "nebula_common/nebula_status.hpp" #include "nebula_common/util/string_conversions.hpp" +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include #include namespace nebula { @@ -36,9 +38,10 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase { std::string multicast_ip; uint16_t gnss_port{}; - double scan_phase{}; + uint16_t sync_angle{}; + double cut_angle{}; double dual_return_distance_threshold{}; - std::string calibration_path{}; + std::string calibration_path; uint16_t rotation_speed; uint16_t cloud_min_angle; uint16_t cloud_max_angle; @@ -58,11 +61,13 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "Multicast: " << (arg.multicast_ip.empty() ? "disabled" : "enabled, group " + arg.multicast_ip) << '\n'; os << "GNSS Port: " << arg.gnss_port << '\n'; - os << "Scan Phase: " << arg.scan_phase << '\n'; os << "Rotation Speed: " << arg.rotation_speed << '\n'; + os << "Sync Angle: " << arg.sync_angle << '\n'; + os << "Cut Angle: " << arg.cut_angle << '\n'; os << "FoV Start: " << arg.cloud_min_angle << '\n'; os << "FoV End: " << arg.cloud_max_angle << '\n'; os << "Dual Return Distance Threshold: " << arg.dual_return_distance_threshold << '\n'; + os << "Calibration Path: " << arg.calibration_path << '\n'; os << "PTP Profile: " << arg.ptp_profile << '\n'; os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n'; os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; @@ -76,6 +81,8 @@ struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; virtual nebula::Status SaveToFileFromBytes( const std::string & calibration_file, const std::vector & buf) = 0; + + [[nodiscard]] virtual std::tuple getFovPadding() const = 0; }; /// @brief struct for Hesai calibration configuration @@ -177,6 +184,19 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase ofs.close(); return Status::OK; } + + [[nodiscard]] std::tuple getFovPadding() const override + { + float min = INFINITY; + float max = -INFINITY; + + for (const auto & item : azimuth_offset_map) { + min = std::min(min, item.second); + max = std::max(max, item.second); + } + + return {-max, -min}; + } }; /// @brief struct for Hesai correction configuration (for AT) @@ -360,7 +380,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @param ch The channel id /// @param azi The precision azimuth in (0.01 / 256) degree unit /// @return The azimuth adjustment in 0.01 degree unit - int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const + [[nodiscard]] int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const { unsigned int i = std::floor(1.f * azi / STEP3); unsigned int l = azi - i * STEP3; @@ -372,13 +392,23 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @param ch The channel id /// @param azi The precision azimuth in (0.01 / 256) degree unit /// @return The elevation adjustment in 0.01 degree unit - int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const + [[nodiscard]] int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const { unsigned int i = std::floor(1.f * azi / STEP3); unsigned int l = azi - i * STEP3; float k = 1.f * l / STEP3; return round((1 - k) * elevationOffset[ch * 180 + i] + k * elevationOffset[ch * 180 + i + 1]); } + + [[nodiscard]] std::tuple getFovPadding() const override + { + // TODO(mojomex): calculate instead of hard-coding + // The reason this is tricky is that an upper bound over all azimuth/elevation combinations has + // to be found. For other sensors, this is only a function of elevation, so the search space is + // tiny compared to AT128. We should be able to find an upper bound of `getAzimuthAdjustV3` but + // I have not invested the time for now. + return {-5, 5}; + } }; /* diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp new file mode 100644 index 000000000..805a35fae --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/angles.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +namespace nebula::drivers +{ + +/** + * @brief Tests if `angle` is in the region of the circle defined by `start_angle` and `end_angle`. + * Notably, `end_angle` can be smaller than `start_angle`, in which case the region passes over the + * 360/0 deg bound. This function is unit-independent (but all angles have to have the same unit), + * so degrees, radians, and arbitrary scale factors can be used. + */ +template +bool angle_is_between( + T start_angle, T end_angle, T angle, bool start_inclusive = true, bool end_inclusive = true) +{ + if (!start_inclusive && angle == start_angle) return false; + if (!end_inclusive && angle == end_angle) return false; + + return (start_angle <= angle && angle <= end_angle) || + ((end_angle < start_angle) && (angle <= end_angle || start_angle <= angle)); +} + +/** + * @brief Normalizes an angle to the interval [0; max_angle]. This function is unit-independent. + * `max_angle` is 360 for degrees, 2 * M_PI for radians, and the corresponding scaled value for + * scaled units such as centi-degrees (36000). + */ +template +T normalize_angle(T angle, T max_angle) +{ + T factor = std::floor((1.0 * angle) / max_angle); + return angle - (factor * max_angle); +} + +} // namespace nebula::drivers 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 ce394c726..bc275b347 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 @@ -47,15 +47,10 @@ class AngleCorrector /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0; - /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last - /// azimuth - /// @param current_azimuth The current azimuth value in the sensor's angle resolution - /// @param last_azimuth The last azimuth in the sensor's angle resolution - /// @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 passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) = 0; }; } // namespace nebula::drivers 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 06542420d..b10c664ef 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 @@ -15,10 +15,18 @@ #pragma once #include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_decoders/nebula_decoders_common/angles.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" +#include + +#include +#include #include #include +#include +#include +#include namespace nebula::drivers { @@ -27,30 +35,53 @@ template class AngleCorrectorCalibrationBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; + static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; std::array elevation_angle_rad_{}; std::array azimuth_offset_rad_{}; - std::array block_azimuth_rad_{}; + std::array block_azimuth_rad_{}; std::array elevation_cos_{}; std::array elevation_sin_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; + std::array, MAX_AZIMUTH> azimuth_cos_{}; + std::array, MAX_AZIMUTH> azimuth_sin_{}; public: + uint32_t emit_angle_raw_; + uint32_t timestamp_reset_angle_raw_; + uint32_t fov_start_raw_; + uint32_t fov_end_raw_; + + bool is_360_; + explicit AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration) + const std::shared_ptr & sensor_calibration, + double fov_start_azimuth_deg, double fov_end_azimuth_deg, double scan_cut_azimuth_deg) { if (sensor_calibration == nullptr) { throw std::runtime_error( "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); } + // //////////////////////////////////////// + // Elevation lookup tables + // //////////////////////////////////////// + + int32_t correction_min = INT32_MAX; + int32_t correction_max = INT32_MIN; + + auto round_away_from_zero = [](float value) { + return (value < 0) ? std::floor(value) : std::ceil(value); + }; + for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { float elevation_angle_deg = sensor_calibration->elev_angle_map.at(channel_id); float azimuth_offset_deg = sensor_calibration->azimuth_offset_map.at(channel_id); + int32_t azimuth_offset_raw = round_away_from_zero(azimuth_offset_deg * AngleUnit); + correction_min = std::min(correction_min, azimuth_offset_raw); + correction_max = std::max(correction_max, azimuth_offset_raw); + elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); @@ -58,7 +89,42 @@ class AngleCorrectorCalibrationBased : public AngleCorrector(emit_angle_raw, MAX_AZIMUTH); + + int32_t fov_start_raw = std::floor(fov_start_azimuth_deg * AngleUnit); + fov_start_raw -= correction_max; + fov_start_raw_ = normalize_angle(fov_start_raw, MAX_AZIMUTH); + + int32_t fov_end_raw = std::ceil(fov_end_azimuth_deg * AngleUnit); + fov_end_raw -= correction_min; + fov_end_raw_ = normalize_angle(fov_end_raw, MAX_AZIMUTH); + + // Reset timestamp on FoV start if FoV < 360 deg and scan is cut at FoV end. + // Otherwise, reset timestamp on publish + is_360_ = + normalize_angle(fov_start_azimuth_deg, 360.) == normalize_angle(fov_end_azimuth_deg, 360.); + bool reset_timestamp_on_publish = is_360_ || (normalize_angle(fov_end_azimuth_deg, 360.) != + normalize_angle(scan_cut_azimuth_deg, 360.)); + + if (reset_timestamp_on_publish) { + int32_t timestamp_reset_angle_raw = std::floor(scan_cut_azimuth_deg * AngleUnit); + timestamp_reset_angle_raw -= correction_max; + timestamp_reset_angle_raw_ = normalize_angle(timestamp_reset_angle_raw, MAX_AZIMUTH); + } else { + timestamp_reset_angle_raw_ = fov_start_raw_; + } + + // //////////////////////////////////////// + // Azimuth lookup tables + // //////////////////////////////////////// + + for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH; block_azimuth++) { block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { @@ -74,6 +140,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector + +#include +#include +#include #include +#include #include +#include +#include +#include +#include +#include +#include +#include namespace nebula::drivers { @@ -27,12 +41,23 @@ template class AngleCorrectorCorrectionBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH_LENGTH = 360 * AngleUnit; + static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; const std::shared_ptr correction_; rclcpp::Logger logger_; - std::array cos_{}; - std::array sin_{}; + std::array cos_{}; + std::array sin_{}; + + struct FrameAngleInfo + { + static constexpr uint32_t unset = UINT32_MAX; + uint32_t fov_start = unset; + uint32_t fov_end = unset; + uint32_t timestamp_reset = unset; + uint32_t scan_emit = unset; + }; + + std::vector frame_angle_info_; /// @brief For a given azimuth value, find its corresponding output field /// @param azimuth The azimuth to get the field for @@ -53,9 +78,45 @@ class AngleCorrectorCorrectionBased : public AngleCorrector return field; } + /// @brief For raw encoder angle `azi`, return whether all (any if `any == true`) channels' + /// corrected azimuths are greater (or equal if `eq_ok == true`) than `threshold`. + bool are_corrected_angles_above_threshold(uint32_t azi, double threshold, bool any, bool eq_ok) + { + for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { + auto azi_corr = getCorrectedAngleData(azi, channel_id).azimuth_rad; + if (!any && (azi_corr < threshold || (!eq_ok && azi_corr == threshold))) return false; + if (any && (azi_corr > threshold || (eq_ok && azi_corr == threshold))) return true; + } + + return !any; + } + + /// @brief Find and return the first raw encoder angle between the raw `start` and `end` endcoder + /// angles for which all (any if `any == true`) channels' corrected azimuth is greater than (or + /// equal to if `eq_ok == true`) `threshold`. Return `FrameAngleInfo::unset` if no angle is found. + uint32_t bin_search(uint32_t start, uint32_t end, double threshold, bool any, bool eq_ok) + { + if (start > end) return FrameAngleInfo::unset; + + if (end - start <= 1) { + bool result_start = are_corrected_angles_above_threshold( + normalize_angle(start, MAX_AZIMUTH), threshold, any, eq_ok); + if (result_start) return start; + return end; + } + + uint32_t next = (start + end) / 2; + + bool result_next = are_corrected_angles_above_threshold( + normalize_angle(next, MAX_AZIMUTH), threshold, any, eq_ok); + if (result_next) return bin_search(start, next, threshold, any, eq_ok); + return bin_search(next + 1, end, threshold, any, eq_ok); + } + public: explicit AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_correction) + const std::shared_ptr & sensor_correction, double fov_start_azimuth_deg, + double fov_end_azimuth_deg, double scan_cut_azimuth_deg) : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { @@ -65,45 +126,121 @@ class AngleCorrectorCorrectionBased : public AngleCorrector logger_.set_level(rclcpp::Logger::Level::Debug); - for (size_t i = 0; i < MAX_AZIMUTH_LENGTH; ++i) { - float rad = 2.f * i * M_PI / MAX_AZIMUTH_LENGTH; + // //////////////////////////////////////// + // Trigonometry lookup tables + // //////////////////////////////////////// + + for (size_t i = 0; i < MAX_AZIMUTH; ++i) { + float rad = 2.f * i * M_PIf / MAX_AZIMUTH; cos_[i] = cosf(rad); sin_[i] = sinf(rad); } + + // //////////////////////////////////////// + // Scan start/end correction lookups + // //////////////////////////////////////// + + auto fov_start_rad = deg2rad(fov_start_azimuth_deg); + auto fov_end_rad = deg2rad(fov_end_azimuth_deg); + auto scan_cut_rad = deg2rad(scan_cut_azimuth_deg); + + // For each field (= mirror), find the raw block azimuths corresponding FoV start and end + for (size_t field_id = 0; field_id < correction_->frameNumber; ++field_id) { + auto frame_start = correction_->startFrame[field_id]; + auto frame_end = correction_->endFrame[field_id]; + if (frame_end < frame_start) frame_end += MAX_AZIMUTH; + + FrameAngleInfo & angle_info = frame_angle_info_.emplace_back(); + + angle_info.fov_start = bin_search(frame_start, frame_end, fov_start_rad, true, true); + angle_info.fov_end = bin_search(angle_info.fov_start, frame_end, fov_end_rad, false, true); + angle_info.scan_emit = + bin_search(angle_info.fov_start, angle_info.fov_end, scan_cut_rad, false, true); + angle_info.timestamp_reset = + bin_search(angle_info.fov_start, angle_info.fov_end, scan_cut_rad, true, true); + + if ( + angle_info.fov_start == FrameAngleInfo::unset || + angle_info.fov_end == FrameAngleInfo::unset || + angle_info.scan_emit == FrameAngleInfo::unset || + angle_info.timestamp_reset == FrameAngleInfo::unset) { + throw std::runtime_error("Not all necessary angles found!"); + } + + if (fov_start_rad == scan_cut_rad) { + angle_info.timestamp_reset = angle_info.fov_start; + angle_info.scan_emit = angle_info.fov_start; + } else if (fov_end_rad == scan_cut_rad) { + angle_info.timestamp_reset = angle_info.fov_start; + angle_info.scan_emit = angle_info.fov_end; + } + } } CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override { int field = findField(block_azimuth); - auto elevation = + int32_t elevation = correction_->elevation[channel_id] + correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); - elevation = (MAX_AZIMUTH_LENGTH + elevation) % MAX_AZIMUTH_LENGTH; + elevation = (MAX_AZIMUTH + elevation) % MAX_AZIMUTH; - auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction_->startFrame[field]) * 2 - - correction_->azimuth[channel_id] + - correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); - azimuth = (MAX_AZIMUTH_LENGTH + azimuth) % MAX_AZIMUTH_LENGTH; + int32_t azimuth = + (block_azimuth + MAX_AZIMUTH - correction_->startFrame[field]) * 2 - + correction_->azimuth[channel_id] + + correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + azimuth = (MAX_AZIMUTH + azimuth) % MAX_AZIMUTH; - float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH_LENGTH; - float elevation_rad = 2.f * elevation * M_PI / MAX_AZIMUTH_LENGTH; + float azimuth_rad = 2.f * azimuth * M_PIf / MAX_AZIMUTH; + float elevation_rad = 2.f * elevation * M_PIf / MAX_AZIMUTH; return {azimuth_rad, elevation_rad, sin_[azimuth], cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned( - uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override + bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + { + for (const auto & frame_angles : frame_angle_info_) { + if (angle_is_between(last_azimuth, current_azimuth, frame_angles.scan_emit, false)) + return true; + } + + return false; + } + + bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + { + for (const auto & frame_angles : frame_angle_info_) { + if (angle_is_between(last_azimuth, current_azimuth, frame_angles.timestamp_reset, false)) + return true; + } + + return false; + } + + bool isInsideFoV(uint32_t last_azimuth, uint32_t current_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 - // from the previous field and therefore significantly older than the - // points right of it. - // This also means that the pointcloud timestamp is only at top of second - // if the `sync_azimuth` aligns with the beginning of the field (e.g. 30deg for AT128). - // The absolute point time for points at `sync_azimuth` is still at top of second. - return findField(current_azimuth) != findField(last_azimuth); + for (const auto & frame_angles : frame_angle_info_) { + if ( + angle_is_between(frame_angles.fov_start, frame_angles.fov_end, current_azimuth, false) || + angle_is_between(frame_angles.fov_start, frame_angles.fov_end, last_azimuth, false)) + return true; + } + + return false; + } + + bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) override + { + for (const auto & frame_angles : frame_angle_info_) { + if ( + angle_is_between(frame_angles.timestamp_reset, frame_angles.scan_emit, current_azimuth) || + angle_is_between(frame_angles.timestamp_reset, frame_angles.scan_emit, last_azimuth)) + return true; + } + + return false; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 5684ec179..b1b5bc798 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -14,11 +14,20 @@ #pragma once +#include "nebula_decoders/nebula_decoders_common/angles.hpp" +#include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" +#include +#include +#include +#include #include +#include +#include +#include #include #include #include @@ -30,6 +39,13 @@ namespace nebula::drivers template class HesaiDecoder : public HesaiScanDecoder { + struct ScanCutAngles + { + float fov_min; + float fov_max; + float scan_emit_angle; + }; + protected: /// @brief Configuration for this decoder const std::shared_ptr sensor_configuration_; @@ -47,8 +63,7 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; - /// @brief The last azimuth processed - int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet + /// @brief The timestamp of the last completed scan in nanoseconds uint64_t output_scan_timestamp_ns_ = 0; /// @brief The timestamp of the scan currently in progress @@ -56,6 +71,9 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief Whether a full scan has been processed bool has_scanned_ = false; + ScanCutAngles scan_cut_angles_; + uint32_t last_azimuth_ = 0; + rclcpp::Logger logger_; /// @brief For each channel, its firing offset relative to the block in nanoseconds @@ -114,7 +132,7 @@ class HesaiDecoder : public HesaiScanDecoder continue; } - auto distance = getDistance(unit); + float distance = getDistance(unit); if ( distance < SensorT::MIN_RANGE || SensorT::MAX_RANGE < distance || @@ -154,17 +172,38 @@ class HesaiDecoder : public HesaiScanDecoder } } - NebulaPoint point; + CorrectedAngleData corrected_angle_data = + angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); + float azimuth = corrected_angle_data.azimuth_rad; + + bool in_fov = angle_is_between(scan_cut_angles_.fov_min, scan_cut_angles_.fov_max, azimuth); + if (!in_fov) { + continue; + } + + bool in_current_scan = true; + + if ( + angle_corrector_.isInsideOverlap(last_azimuth_, raw_azimuth) && + angle_is_between( + scan_cut_angles_.scan_emit_angle, scan_cut_angles_.scan_emit_angle + deg2rad(20), + azimuth)) { + in_current_scan = false; + } + + auto pc = in_current_scan ? decode_pc_ : output_pc_; + uint64_t scan_timestamp_ns = + in_current_scan ? decode_scan_timestamp_ns_ : output_scan_timestamp_ns_; + + NebulaPoint & point = pc->emplace_back(); point.distance = distance; point.intensity = unit.reflectivity; - point.time_stamp = - getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id); + point.time_stamp = getPointTimeRelative( + scan_timestamp_ns, packet_timestamp_ns, block_offset + start_block_id, channel_id); point.return_type = static_cast(return_type); point.channel = channel_id; - auto corrected_angle_data = angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); - // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise // corrected angles float xy_distance = distance * corrected_angle_data.cos_elevation; @@ -175,26 +214,10 @@ class HesaiDecoder : public HesaiScanDecoder // The driver wrapper converts to degrees, expects radians point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; - - decode_pc_->emplace_back(point); } } } - /// @brief Checks whether the last processed block was the last block of a scan - /// @param current_phase The azimuth of the last processed block - /// @param sync_phase The azimuth set in the sensor configuration, for which the - /// timestamp is aligned to the full second - /// @return Whether the scan has completed - bool checkScanCompleted(uint32_t current_phase, uint32_t sync_phase) - { - if (last_phase_ == -1) { - return false; - } - - return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase); - } - /// @brief Get the distance of the given unit in meters float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) { @@ -203,15 +226,16 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time /// offset correction for channel and block + /// @param scan_timestamp_ns Start timestamp of the current scan in nanoseconds /// @param packet_timestamp_ns The timestamp of the current PandarPacket in nanoseconds /// @param block_id The block index of the point /// @param channel_id The channel index of the point - uint32_t getPointTimeRelative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) + uint32_t getPointTimeRelative( + uint64_t scan_timestamp_ns, uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) { auto point_to_packet_offset_ns = sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, packet_); - auto packet_to_scan_offset_ns = - static_cast(packet_timestamp_ns - decode_scan_timestamp_ns_); + auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - scan_timestamp_ns); return packet_to_scan_offset_ns + point_to_packet_offset_ns; } @@ -224,7 +248,9 @@ class HesaiDecoder : public HesaiScanDecoder const std::shared_ptr & correction_data) : sensor_configuration_(sensor_configuration), - angle_corrector_(correction_data), + angle_corrector_( + correction_data, sensor_configuration_->cloud_min_angle, + sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle), logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); @@ -235,6 +261,10 @@ class HesaiDecoder : public HesaiScanDecoder decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + + scan_cut_angles_ = { + deg2rad(sensor_configuration_->cloud_min_angle), + deg2rad(sensor_configuration_->cloud_max_angle), deg2rad(sensor_configuration_->cut_angle)}; } int unpack(const std::vector & packet) override @@ -243,42 +273,60 @@ class HesaiDecoder : public HesaiScanDecoder return -1; } + // This is the first scan, set scan timestamp to whatever packet arrived first if (decode_scan_timestamp_ns_ == 0) { - decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_); + decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_) + + sensor_.getEarliestPointTimeOffsetForBlock(0, packet_); } if (has_scanned_) { + output_pc_->clear(); has_scanned_ = false; } const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode); - uint32_t current_azimuth = 0; - for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { - current_azimuth = packet_.body.blocks[block_id].getAzimuth(); + auto block_azimuth = packet_.body.blocks[block_id].getAzimuth(); + + if (angle_corrector_.passedTimestampResetAngle(last_azimuth_, block_azimuth)) { + uint64_t new_scan_timestamp_ns = + hesai_packet::get_timestamp_ns(packet_) + + sensor_.getEarliestPointTimeOffsetForBlock(block_id, packet_); + + if (sensor_configuration_->cut_angle == sensor_configuration_->cloud_max_angle) { + // In the non-360 deg case, if the cut angle and FoV end coincide, the old pointcloud has + // already been swapped and published before the timestamp reset angle is reached. Thus, + // the `decode` pointcloud is now empty and will be decoded to. Reset its timestamp. + decode_scan_timestamp_ns_ = new_scan_timestamp_ns; + } else { + /// When not cutting at the end of the FoV (i.e. the FoV is 360 deg or a cut occurs + /// somewhere within a non-360 deg FoV), the current scan is still being decoded to the + /// `decode` pointcloud but at the same time, points for the next pointcloud are arriving + /// and will be decoded to the `output` pointcloud (please forgive the naming for now). + /// Thus, reset the output pointcloud's timestamp. + output_scan_timestamp_ns_ = new_scan_timestamp_ns; + } + } - bool scan_completed = checkScanCompleted( - current_azimuth, - sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS); + if (!angle_corrector_.isInsideFoV(last_azimuth_, block_azimuth)) { + last_azimuth_ = block_azimuth; + continue; + } - if (scan_completed) { + convertReturns(block_id, n_returns); + + if (angle_corrector_.passedEmitAngle(last_azimuth_, block_azimuth)) { + // The current `decode` pointcloud is ready for publishing, swap buffers to continue with + // the last `output` pointcloud as the `decode pointcloud. std::swap(decode_pc_, output_pc_); - decode_pc_->clear(); + std::swap(decode_scan_timestamp_ns_, output_scan_timestamp_ns_); has_scanned_ = true; - output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; - - // A new scan starts within the current packet, so the new scan's timestamp must be - // calculated as the packet timestamp plus the lowest time offset of any point in the - // remainder of the packet - decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_) + - sensor_.getEarliestPointTimeOffsetForBlock(block_id, packet_); } - convertReturns(block_id, n_returns); - last_phase_ = current_azimuth; + last_azimuth_ = block_azimuth; } - return last_phase_; + return last_azimuth_; } bool hasScanned() override { return has_scanned_; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index 218842657..db07a8545 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -16,21 +16,13 @@ #define NEBULA_HESAI_DRIVER_H #include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_common/point_types.hpp" -#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" -#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp" - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include -#include #include -#include -#include #include #include 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 41f28a592..1cb9557cb 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 @@ -29,16 +29,16 @@ template class AngleCorrectorCalibrationBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; + static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; std::array elevation_angle_rad_{}; std::array azimuth_offset_rad_{}; - std::array block_azimuth_rad_{}; + std::array block_azimuth_rad_{}; std::array elevation_cos_{}; std::array elevation_sin_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_cos_{}; - std::array, MAX_AZIMUTH_LEN> azimuth_sin_{}; + std::array, MAX_AZIMUTH> azimuth_cos_{}; + std::array, MAX_AZIMUTH> azimuth_sin_{}; public: explicit AngleCorrectorCalibrationBased( @@ -62,7 +62,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector elevation_sin_[channel_id] = sinf(elevation_angle_rad_[channel_id]); } - for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH_LEN; block_azimuth++) { + for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH; block_azimuth++) { block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { 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 1f70d0ff6..62f988243 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,12 +92,19 @@ Status HesaiRosOfflineExtractBag::GetParameters( this->declare_parameter("frame_id", "pandar", param_read_only()); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; + descriptor.integer_range = int_range(0, 360, 1); + sensor_configuration.sync_angle = + declare_parameter("sync_angle", 0., param_read_only()); + } + { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; descriptor.floating_point_range = float_range(0, 360, 0.01); - sensor_configuration.scan_phase = this->declare_parameter("scan_phase", 0., descriptor); - this->get_parameter("scan_phase").as_double(); + sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); } calibration_configuration.calibration_file = @@ -122,7 +129,7 @@ Status HesaiRosOfflineExtractBag::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()) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { 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 eba21e544..c1dd6d78c 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -94,12 +94,19 @@ Status HesaiRosOfflineExtractSample::GetParameters( sensor_configuration.frame_id = declare_parameter("frame_id", "pandar", param_read_only()); + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; + descriptor.integer_range = int_range(0, 360, 1); + sensor_configuration.sync_angle = + declare_parameter("sync_angle", 0., param_read_only()); + } + { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; descriptor.floating_point_range = float_range(0, 360, 0.01); - sensor_configuration.scan_phase = - declare_parameter("scan_phase", 0., param_read_only()); + sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); } calibration_configuration.calibration_file = @@ -120,7 +127,7 @@ Status HesaiRosOfflineExtractSample::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()) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { 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 54e757224..f3bb7b073 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 @@ -218,7 +218,7 @@ struct HesaiInventory return os; } - std::string get_str_model() + std::string get_str_model() const { switch (model) { case 0: @@ -400,7 +400,7 @@ struct HesaiLidarStatus return os; } - std::string get_str_gps_pps_lock() + [[nodiscard]] std::string get_str_gps_pps_lock() const { switch (gps_pps_lock) { case 1: @@ -411,7 +411,7 @@ struct HesaiLidarStatus return "Unknown"; } } - std::string get_str_gps_gprmc_status() + [[nodiscard]] std::string get_str_gps_gprmc_status() const { switch (gps_gprmc_status) { case 1: @@ -422,7 +422,7 @@ struct HesaiLidarStatus return "Unknown"; } } - std::string get_str_ptp_clock_status() + [[nodiscard]] std::string get_str_ptp_clock_status() const { switch (ptp_clock_status) { case 0: 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 e41ae449a..471057ea3 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 @@ -130,10 +130,10 @@ class HesaiHwInterface uint8_t error_flags = 0; uint8_t ptc_error_code = 0; - bool ok() { return !error_flags && !ptc_error_code; } + [[nodiscard]] bool ok() const { return !error_flags && !ptc_error_code; } }; - typedef nebula::util::expected, ptc_error_t> ptc_cmd_result_t; + using ptc_cmd_result_t = nebula::util::expected, ptc_error_t>; std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::shared_ptr m_owned_ctx; @@ -331,6 +331,16 @@ class HesaiHwInterface /// @return Resulting status HesaiLidarRangeAll GetLidarRange(); + /** + * @brief Given the HW interface's sensor configuration and a given calibration, set the sensor + * FoV (min and max angles) with appropriate padding around the FoV set in the configuration. This + * compensates for the points lost due to the sensor filtering FoV by raw encoder angle. + * + * @param calibration The calibration file of the sensor + * @return Status Resulting status of setting the FoV + */ + [[nodiscard]] Status checkAndSetLidarRange(const HesaiCalibrationConfigurationBase & calibration); + Status SetClockSource(int clock_source); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG 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 9e5963131..e349e3f3b 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 @@ -7,6 +7,7 @@ #include #include +#include #include // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -18,9 +19,7 @@ #include -namespace nebula -{ -namespace drivers +namespace nebula::drivers { HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, @@ -29,6 +28,7 @@ HesaiHwInterface::HesaiHwInterface() tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} { } + HesaiHwInterface::~HesaiHwInterface() { FinalizeTcpDriver(); @@ -381,8 +381,13 @@ Status HesaiHwInterface::SetSpinRate(uint16_t rpm) Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) { + if (sync_angle < 0 || sync_angle > 360) { + return Status::SENSOR_CONFIG_ERROR; + } + std::vector request_payload; - request_payload.emplace_back(sync_angle & 0xff); + // 360 is converted to 0 + request_payload.emplace_back((sync_angle % 360) & 0xff); request_payload.emplace_back((angle >> 8) & 0xff); request_payload.emplace_back(angle & 0xff); @@ -467,6 +472,9 @@ Status HesaiHwInterface::SetControlPort( Status HesaiHwInterface::SetLidarRange(int method, std::vector data) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::SENSOR_CONFIG_ERROR; + } // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes @@ -479,18 +487,21 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector da return Status::OK; } -Status HesaiHwInterface::SetLidarRange(int start, int end) +Status HesaiHwInterface::SetLidarRange(int start_ddeg, int end_ddeg) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::SENSOR_CONFIG_ERROR; + } // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes std::vector request_payload; int method = 0; request_payload.emplace_back(method & 0xff); - request_payload.emplace_back((start >> 8) & 0xff); - request_payload.emplace_back(start & 0xff); - request_payload.emplace_back((end >> 8) & 0xff); - request_payload.emplace_back(end & 0xff); + request_payload.emplace_back((start_ddeg >> 8) & 0xff); + request_payload.emplace_back(start_ddeg & 0xff); + request_payload.emplace_back((end_ddeg >> 8) & 0xff); + request_payload.emplace_back(end_ddeg & 0xff); auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); @@ -499,18 +510,17 @@ Status HesaiHwInterface::SetLidarRange(int start, int end) HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() { - HesaiLidarRangeAll hesai_range_all{}; + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + throw std::runtime_error("Not supported on this sensor"); + } auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); - - // FIXME(mojomex): this is a hotfix for sensors that do not support this command - if (!response_or_err.has_value()) return hesai_range_all; - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); if (response.size() < 1) { throw std::runtime_error("Response payload too short"); } + HesaiLidarRangeAll hesai_range_all{}; hesai_range_all.method = response[0]; switch (hesai_range_all.method) { case 0: // for all channels @@ -532,6 +542,32 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() return hesai_range_all; } +Status HesaiHwInterface::checkAndSetLidarRange( + const HesaiCalibrationConfigurationBase & calibration) +{ + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::SENSOR_CONFIG_ERROR; + } + + int cloud_min_ddeg = sensor_configuration_->cloud_min_angle * 10; + int cloud_max_ddeg = sensor_configuration_->cloud_max_angle * 10; + + // Only oversize the FoV if it is not already the full 360deg + if (cloud_min_ddeg != 0 || cloud_max_ddeg != 3600) { + auto padding_deg = calibration.getFovPadding(); + cloud_min_ddeg += floor(std::get<0>(padding_deg) * 10); + cloud_max_ddeg += ceil(std::get<1>(padding_deg) * 10); + } + + auto clamp = [](int angle_ddeg) { + while (angle_ddeg < 0) angle_ddeg += 3600; + while (angle_ddeg > 3600) angle_ddeg -= 3600; + return angle_ddeg; + }; + + return SetLidarRange(clamp(cloud_min_ddeg), clamp(cloud_max_ddeg)); +} + Status HesaiHwInterface::SetClockSource(int clock_source) { std::vector request_payload; @@ -619,6 +655,10 @@ Status HesaiHwInterface::SetRotDir(int mode) HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + throw std::runtime_error("Not supported on this sensor"); + } + auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); // FIXME(mojomex): this is a hotfix for sensors that do not support this command @@ -904,17 +944,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; - auto sync_angle = static_cast(hesai_config.sync_angle.value() / 100); - auto scan_phase = static_cast(sensor_configuration->scan_phase); + auto sensor_sync_angle = static_cast(hesai_config.sync_angle.value() / 100); + auto config_sync_angle = sensor_configuration->sync_angle; int sync_flg = 1; - if (scan_phase != sync_angle) { + if (config_sync_angle != sensor_sync_angle) { set_flg = true; } if (sync_flg && set_flg) { PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); - PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); - PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); + PrintInfo("current lidar sync_angle: " + std::to_string(sensor_sync_angle)); + PrintInfo("current configuration sync_angle: " + std::to_string(config_sync_angle)); + std::thread t( + [this, sync_flg, config_sync_angle] { SetSyncAngle(sync_flg, config_sync_angle); }); t.join(); std::this_thread::sleep_for(wait_time); } @@ -950,7 +991,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::this_thread::sleep_for(wait_time); } else { // AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); + SetSyncAngleSyncHttp(1, sensor_configuration->sync_angle); std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile << ", Domain: " << sensor_configuration->ptp_domain @@ -987,27 +1028,27 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #endif set_flg = true; } else { - auto current_cloud_min_angle = hesai_lidar_range_all.start; + auto current_cloud_min_angle_ddeg = hesai_lidar_range_all.start; if ( static_cast(sensor_configuration->cloud_min_angle * 10) != - current_cloud_min_angle.value()) { + current_cloud_min_angle_ddeg.value()) { set_flg = true; PrintInfo( "current lidar range.start: " + - std::to_string(static_cast(current_cloud_min_angle.value()))); + std::to_string(static_cast(current_cloud_min_angle_ddeg.value()))); PrintInfo( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } - auto current_cloud_max_angle = hesai_lidar_range_all.end; + auto current_cloud_max_angle_ddeg = hesai_lidar_range_all.end; if ( static_cast(sensor_configuration->cloud_max_angle * 10) != - current_cloud_max_angle.value()) { + current_cloud_max_angle_ddeg.value()) { set_flg = true; PrintInfo( "current lidar range.end: " + - std::to_string(static_cast(current_cloud_max_angle.value()))); + std::to_string(static_cast(current_cloud_max_angle_ddeg.value()))); PrintInfo( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); @@ -1018,9 +1059,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( std::thread t([this, sensor_configuration] { SetLidarRange( static_cast(sensor_configuration->cloud_min_angle * 10), - static_cast(sensor_configuration->cloud_max_angle * 10) //, - // false - ); + static_cast(sensor_configuration->cloud_max_angle * 10)); }); t.join(); } @@ -1047,6 +1086,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() }); t.join(); + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + return Status::OK; + } + std::thread t2([this] { auto result = GetLidarRange(); std::stringstream ss; @@ -1303,16 +1346,16 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) std::vector nebula_errors; if (error_flags & TCP_ERROR_INCOMPLETE_RESPONSE) { - nebula_errors.push_back("Incomplete response payload"); + nebula_errors.emplace_back("Incomplete response payload"); } if (error_flags & TCP_ERROR_TIMEOUT) { - nebula_errors.push_back("Request timeout"); + nebula_errors.emplace_back("Request timeout"); } if (error_flags & TCP_ERROR_UNEXPECTED_PAYLOAD) { - nebula_errors.push_back("Received payload but expected payload length 0"); + nebula_errors.emplace_back("Received payload but expected payload length 0"); } if (error_flags & TCP_ERROR_UNRELATED_RESPONSE) { - nebula_errors.push_back("Received unrelated response"); + nebula_errors.emplace_back("Received unrelated response"); } ss << boost::algorithm::join(nebula_errors, ", "); @@ -1334,5 +1377,4 @@ T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) return parsed; } -} // namespace drivers -} // namespace nebula +} // namespace nebula::drivers diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index 9da22a169..85361864c 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: Pandar128E4X calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index 0ee7b83ec..1f6b79ff6 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: Pandar40P calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml index 1bb195550..31c695643 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: Pandar64 rotation_speed: 600 return_mode: Dual diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml index 462a4fd78..5a2a8858b 100644 --- a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -15,7 +15,8 @@ max_range: 300.0 cloud_min_angle: 30 cloud_max_angle: 150 - scan_phase: 30.0 + sync_angle: 30 + cut_angle: 150.0 sensor_model: PandarAT128 calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 200 diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml index f9ee8029a..31e046be2 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: PandarQT128 calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml index fcb7f89ac..4d3d119a8 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: PandarQT64 calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml index 51666ea7f..09eaef375 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: PandarXT32 calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml index 01fc35990..6ad0e6566 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -14,7 +14,8 @@ max_range: 300.0 cloud_min_angle: 0 cloud_max_angle: 360 - scan_phase: 0.0 + sync_angle: 0 + cut_angle: 0.0 sensor_model: PandarXT32M calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index 396f16d23..cd77ed937 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -16,22 +16,17 @@ #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include #include #include -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include -#include #include #include -#include -#include namespace nebula { @@ -39,14 +34,12 @@ namespace ros { class HesaiDecoderWrapper { - using get_calibration_result_t = nebula::util::expected< - std::shared_ptr, nebula::Status>; - public: HesaiDecoderWrapper( rclcpp::Node * const parent_node, - const std::shared_ptr & hw_interface, - std::shared_ptr & config); + const std::shared_ptr & config, + const std::shared_ptr & calibration, + bool publish_packets); void ProcessCloudPacket(std::unique_ptr packet_msg); @@ -57,23 +50,9 @@ class HesaiDecoderWrapper const std::shared_ptr & new_calibration); - rcl_interfaces::msg::SetParametersResult OnParameterChange( - const std::vector & p); - nebula::Status Status(); private: - /// @brief Load calibration data from the best available source: - /// 1. If sensor connected, download and save from sensor - /// 2. If downloaded file available, load that file - /// 3. Load the file given by `calibration_file_path` - /// @param calibration_file_path The file to use if no better option is available - /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration - /// options - /// @return The calibration data if successful, or an error code if not - get_calibration_result_t GetCalibrationData( - const std::string & calibration_file_path, bool ignore_others = false); - void PublishCloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher); @@ -89,11 +68,9 @@ class HesaiDecoderWrapper nebula::Status status_; rclcpp::Logger logger_; + rclcpp::Node & parent_node_; - const std::shared_ptr hw_interface_; std::shared_ptr sensor_cfg_; - - std::string calibration_file_path_{}; std::shared_ptr calibration_cfg_ptr_{}; std::shared_ptr driver_ptr_{}; diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 8a70aa0e3..0292c0f8e 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -14,13 +14,10 @@ #pragma once -#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" @@ -29,17 +26,17 @@ #include #include -#include "nebula_msgs/msg/nebula_packet.hpp" +#include +#include #include #include #include -#include -#include #include #include #include +#include #include #include @@ -51,9 +48,12 @@ namespace ros /// @brief Ros wrapper of hesai driver class HesaiRosWrapper final : public rclcpp::Node { + using get_calibration_result_t = nebula::util::expected< + std::shared_ptr, nebula::Status>; + public: explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiRosWrapper() noexcept {}; + ~HesaiRosWrapper() noexcept override = default; /// @brief Get current status of this driver /// @return Current status @@ -79,6 +79,23 @@ class HesaiRosWrapper final : public rclcpp::Node Status ValidateAndSetConfig( std::shared_ptr & new_config); + /// @brief The ROS 2 parameter holding the calibration file path is called differently depending + /// on the sensor model. This function returns the correct parameter name given a model. + /// @param model The sensor model + /// @return std::string The parameter name + std::string getCalibrationParameterName(drivers::SensorModel model) const; + + /// @brief Load calibration data from the best available source: + /// 1. If sensor connected, download and save from sensor + /// 2. If downloaded file available, load that file + /// 3. Load the file given by `calibration_file_path` + /// @param calibration_file_path The file to use if no better option is available + /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration + /// options + /// @return The calibration data if successful, or an error code if not + get_calibration_result_t GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others = false); + Status wrapper_status_; std::shared_ptr sensor_cfg_ptr_{}; diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp index 9706497c6..addc4f478 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -14,8 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" - #include #include #include diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp index d8885574f..87b384d1a 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -105,6 +105,8 @@ class HesaiHwMonitorWrapper std::vector temperature_names_; + bool supports_monitor_; + const std::string MSG_NOT_SUPPORTED = "Not supported"; const std::string MSG_ERROR = "Error"; const std::string MSG_SEP = ": "; diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 9de8b1134..656ebcbb8 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -110,7 +113,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index f83cdc393..d867ea4c4 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -101,7 +104,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index 0b15a0f9b..3559e8764 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -98,7 +101,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "rotation_speed", "return_mode", diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index 08c85e108..81ea068f8 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -57,8 +57,14 @@ "minimum": 30, "maximum": 150 }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase", + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle", + "default": "30", + "minimum": 30, + "maximum": 150 + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle", "default": "30.0", "minimum": 30.0, "maximum": 150.0 @@ -119,7 +125,8 @@ "correction_file", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index efac39914..52cbf3ed7 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -104,7 +107,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index bb16cfcbb..d1680b0fb 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -101,7 +104,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index e4ff2eae9..aac236d7a 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -104,7 +107,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index 43198d162..ea29162fc 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -48,8 +48,11 @@ "cloud_max_angle": { "$ref": "sub/misc.json#/definitions/cloud_max_angle" }, - "scan_phase": { - "$ref": "sub/misc.json#/definitions/scan_phase" + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" }, "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" @@ -104,7 +107,8 @@ "diag_span", "cloud_min_angle", "cloud_max_angle", - "scan_phase", + "sync_angle", + "cut_angle", "sensor_model", "calibration_file", "rotation_speed", diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index caed4db7a..e4c4d5bcd 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/nebula_ros/schema/sub/lidar_hesai.json @@ -32,6 +32,20 @@ "maximum": 1200, "multipleOf": 60 }, + "sync_angle": { + "type": "integer", + "minimum": 0, + "maximum": 359, + "default": "0", + "description": "The angle in whole degrees which should be hit on top-of-second." + }, + "cut_angle": { + "type": "number", + "minimum": 0, + "maximum": 360, + "default": "0.0", + "description": "The angle in degrees at which pointclouds are cut/published." + }, "multicast_ip": { "type": "string", "default": "\"\"", diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index b13d9d782..8ed584eaf 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -2,6 +2,12 @@ #include "nebula_ros/hesai/decoder_wrapper.hpp" +#include +#include +#include + +#include + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula { @@ -12,33 +18,23 @@ using namespace std::chrono_literals; // NOLINT(build/namespaces) HesaiDecoderWrapper::HesaiDecoderWrapper( rclcpp::Node * const parent_node, - const std::shared_ptr & hw_interface, - std::shared_ptr & config) + const std::shared_ptr & config, + const std::shared_ptr & calibration, + bool publish_packets) : status_(nebula::Status::NOT_INITIALIZED), logger_(parent_node->get_logger().get_child("HesaiDecoder")), - hw_interface_(hw_interface), - sensor_cfg_(config) + parent_node_(*parent_node), + sensor_cfg_(config), + calibration_cfg_ptr_(calibration) { - if (!config) { + if (!sensor_cfg_) { throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); } - if (config->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - calibration_file_path_ = - parent_node->declare_parameter("correction_file", param_read_write()); - } else { - calibration_file_path_ = - parent_node->declare_parameter("calibration_file", param_read_write()); - } - - auto calibration_result = GetCalibrationData(calibration_file_path_, false); - - if (!calibration_result.has_value()) { - throw std::runtime_error( - (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + if (!calibration_cfg_ptr_) { + throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); } - calibration_cfg_ptr_ = calibration_result.value(); RCLCPP_INFO_STREAM( logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); @@ -52,8 +48,8 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( (std::stringstream() << "Error instantiating decoder: " << status_).str()); } - // Publish packets only if HW interface is connected - if (hw_interface_) { + // Publish packets only if enabled by the ROS wrapper + if (publish_packets) { current_scan_msg_ = std::make_unique(); packets_pub_ = parent_node->create_publisher( "pandar_packets", rclcpp::SensorDataQoS()); @@ -96,126 +92,6 @@ void HesaiDecoderWrapper::OnCalibrationChange( auto new_driver = std::make_shared(sensor_cfg_, new_calibration); driver_ptr_ = new_driver; calibration_cfg_ptr_ = new_calibration; - calibration_file_path_ = calibration_cfg_ptr_->calibration_file; -} - -rcl_interfaces::msg::SetParametersResult HesaiDecoderWrapper::OnParameterChange( - const std::vector & p) -{ - using rcl_interfaces::msg::SetParametersResult; - - std::string calibration_path = ""; - - // Only one of the two parameters is defined, so not checking for sensor model explicitly here is - // fine - bool got_any = get_param(p, "calibration_file", calibration_path) | - get_param(p, "correction_file", calibration_path); - if (!got_any) { - return rcl_interfaces::build().successful(true).reason(""); - } - - if (!std::filesystem::exists(calibration_path)) { - auto result = SetParametersResult(); - result.successful = false; - result.reason = - "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; - return result; - } - - auto get_calibration_result = GetCalibrationData(calibration_path, true); - if (!get_calibration_result.has_value()) { - auto result = SetParametersResult(); - result.successful = false; - result.reason = - (std::stringstream() << "Could not change calibration file to '" << calibration_path - << "': " << get_calibration_result.error()) - .str(); - return result; - } - - OnCalibrationChange(get_calibration_result.value()); - RCLCPP_INFO_STREAM( - logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); - return rcl_interfaces::build().successful(true).reason(""); -} - -HesaiDecoderWrapper::get_calibration_result_t HesaiDecoderWrapper::GetCalibrationData( - const std::string & calibration_file_path, bool ignore_others) -{ - std::shared_ptr calib; - - if (sensor_cfg_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - calib = std::make_shared(); - } else { - calib = std::make_shared(); - } - - bool hw_connected = hw_interface_ != nullptr; - std::string calibration_file_path_from_sensor; - - { - int ext_pos = calibration_file_path.find_last_of('.'); - calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); - // TODO(mojomex): if multiple different sensors of the same type are used, this will mix up - // their calibration data - calibration_file_path_from_sensor += "_from_sensor_" + sensor_cfg_->sensor_ip; - calibration_file_path_from_sensor += - calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); - } - - // If a sensor is connected, try to download and save its calibration data - if (!ignore_others && hw_connected) { - try { - auto raw_data = hw_interface_->GetLidarCalibrationBytes(); - RCLCPP_INFO(logger_, "Downloaded calibration data from sensor."); - auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); - if (status != Status::OK) { - RCLCPP_ERROR_STREAM(logger_, "Could not save calibration data: " << status); - } else { - RCLCPP_INFO_STREAM( - logger_, "Saved downloaded data to " << calibration_file_path_from_sensor); - } - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(logger_, "Could not download calibration data: " << e.what()); - } - } - - // If saved calibration data from a sensor exists (either just downloaded above, or previously), - // try to load it - if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { - auto status = calib->LoadFromFile(calibration_file_path_from_sensor); - if (status == Status::OK) { - calib->calibration_file = calibration_file_path_from_sensor; - return calib; - } - - RCLCPP_ERROR_STREAM(logger_, "Could not load downloaded calibration data: " << status); - } else if (!ignore_others) { - RCLCPP_ERROR(logger_, "No downloaded calibration data found."); - } - - if (!ignore_others) { - RCLCPP_WARN(logger_, "Falling back to generic calibration file."); - } - - // If downloaded data did not exist or could not be loaded, fall back to a generic file. - // If that file does not exist either, return an error code - if (!std::filesystem::exists(calibration_file_path)) { - RCLCPP_ERROR(logger_, "No calibration data found."); - return nebula::Status(Status::INVALID_CALIBRATION_FILE); - } - - // Try to load the existing fallback calibration file. Return an error if this fails - auto status = calib->LoadFromFile(calibration_file_path); - if (status != Status::OK) { - RCLCPP_ERROR_STREAM( - logger_, "Could not load calibration file at '" << calibration_file_path << "'"); - return status; - } - - // Return the fallback calibration file - calib->calibration_file = calibration_file_path; - return calib; } void HesaiDecoderWrapper::ProcessCloudPacket( @@ -223,8 +99,8 @@ void HesaiDecoderWrapper::ProcessCloudPacket( { // Accumulate packets for recording only if someone is subscribed to the topic (for performance) if ( - hw_interface_ && (packets_pub_->get_subscription_count() > 0 || - packets_pub_->get_intra_process_subscription_count() > 0)) { + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { if (current_scan_msg_->packets.size() == 0) { current_scan_msg_->header.stamp = packet_msg->stamp; } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 2a212b8d1..d4fa54b95 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -4,6 +4,15 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include +#include +#include + +#include +#include +#include +#include +#include #include #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" @@ -21,7 +30,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) hw_monitor_wrapper_(), decoder_wrapper_() { - setvbuf(stdout, NULL, _IONBF, BUFSIZ); + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); wrapper_status_ = DeclareAndGetSensorConfigParams(); @@ -39,8 +48,24 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); } - decoder_wrapper_.emplace( - this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); + auto calibration_result = GetCalibrationData(sensor_cfg_ptr_->calibration_path); + if (!calibration_result.has_value()) { + throw std::runtime_error( + (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + } + + if ( + hw_interface_wrapper_ && + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { + auto status = + hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*calibration_result.value()); + if (status != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not set sensor FoV: " << status).str()); + } + } + + decoder_wrapper_.emplace(this, sensor_cfg_ptr_, calibration_result.value(), launch_hw_); RCLCPP_DEBUG(get_logger(), "Starting stream"); @@ -88,9 +113,21 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.])"; descriptor.floating_point_range = float_range(0, 360, 0.01); - config.scan_phase = declare_parameter("scan_phase", descriptor); + descriptor.description = + "Angle at which to cut the scan. Cannot be equal to the start angle in a non-360 deg " + "FoV. Choose the end angle instead."; + config.cut_angle = declare_parameter("cut_angle", descriptor); + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + if (config.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + descriptor.integer_range = int_range(30, 150, 1); + } else { + descriptor.integer_range = int_range(0, 359, 1); + } + config.sync_angle = declare_parameter("sync_angle", descriptor); } config.min_range = declare_parameter("min_range", param_read_write()); @@ -127,6 +164,10 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() declare_parameter("dual_return_distance_threshold", descriptor); } + std::string calibration_parameter_name = getCalibrationParameterName(config.sensor_model); + config.calibration_path = + declare_parameter(calibration_parameter_name, param_read_write()); + auto _ptp_profile = declare_parameter("ptp_profile", param_read_only()); config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); @@ -171,20 +212,38 @@ Status HesaiRosWrapper::ValidateAndSetConfig( return Status::SENSOR_CONFIG_ERROR; } if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { - RCLCPP_ERROR_STREAM( + RCLCPP_ERROR( get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM( + RCLCPP_ERROR( 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 (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM( - get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + RCLCPP_ERROR(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } + if (!drivers::angle_is_between( + new_config->cloud_min_angle, new_config->cloud_max_angle, new_config->cut_angle)) { + RCLCPP_ERROR(get_logger(), "Cannot cut scan outside of the FoV."); + return Status::SENSOR_CONFIG_ERROR; + } + + bool fov_is_360 = new_config->cloud_min_angle == 0 && new_config->cloud_max_angle == 360; + if (!fov_is_360 && new_config->cut_angle == new_config->cloud_min_angle) { + RCLCPP_ERROR( + get_logger(), "Cannot cut scan right at the start of the FoV. Cut at the end instead."); + return Status::SENSOR_CONFIG_ERROR; + } + + // Handling cutting at 360deg (as opposed to 0deg) for a full 360deg FoV requires a special case + // in the cutting logic. Thus, require the user to cut at 0deg. + if (fov_is_360 && new_config->cut_angle == 360) { + RCLCPP_ERROR(get_logger(), "Cannot cut a 360deg FoV at 360deg. Cut at 0deg instead."); return Status::SENSOR_CONFIG_ERROR; } @@ -254,24 +313,21 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); - std::string _return_mode = ""; + std::string _return_mode{}; + std::string calibration_parameter_name = + getCalibrationParameterName(sensor_cfg_ptr_->sensor_model); + bool got_any = get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | - get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | - get_param(p, "max_range", new_cfg.max_range) | + get_param(p, "sync_angle", new_cfg.sync_angle) | get_param(p, "cut_angle", new_cfg.cut_angle) | + get_param(p, "min_range", new_cfg.min_range) | get_param(p, "max_range", new_cfg.max_range) | get_param(p, "rotation_speed", new_cfg.rotation_speed) | get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | - get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold) | + get_param(p, calibration_parameter_name, new_cfg.calibration_path); - // Currently, HW interface and monitor wrappers have only read-only parameters, so their update - // logic is not implemented - if (decoder_wrapper_) { - auto result = decoder_wrapper_->OnParameterChange(p); - if (!result.successful) { - return result; - } - } + // Currently, all of the sub-wrappers read-only parameters, so they do not be queried for updates if (!got_any) { return rcl_interfaces::build().successful(true).reason(""); @@ -281,9 +337,41 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( new_cfg.return_mode = nebula::drivers::ReturnModeFromStringHesai(_return_mode, sensor_cfg_ptr_->sensor_model); + // //////////////////////////////////////// + // Get and validate new calibration, if any + // //////////////////////////////////////// + + std::shared_ptr new_calibration_ptr{}; + + bool new_calibration_set = new_cfg.calibration_path != sensor_cfg_ptr_->calibration_path; + if (new_calibration_set) { + // Calibration paths set during runtime are always queried from the filesystem, never fetched + // from the sensor. + if (!std::filesystem::exists(new_cfg.calibration_path)) { + auto result = SetParametersResult(); + result.successful = false; + result.reason = + "The given calibration path does not exist, ignoring: '" + new_cfg.calibration_path + "'"; + return result; + } + + // Fail early and do not set the new config if getting calibration data failed. + auto get_calibration_result = GetCalibrationData(new_cfg.calibration_path, true); + if (!get_calibration_result.has_value()) { + auto result = SetParametersResult(); + result.successful = false; + std::stringstream ss{}; + ss << "Could not change calibration file to '" << new_cfg.calibration_path << "': "; + ss << get_calibration_result.error(); + result.reason = ss.str(); + return result; + } + + new_calibration_ptr = get_calibration_result.value(); + } + auto new_cfg_ptr = std::make_shared(new_cfg); auto status = ValidateAndSetConfig(new_cfg_ptr); - if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); auto result = SetParametersResult(); @@ -292,6 +380,22 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( return result; } + // Set calibration (if any) only once all parameters have been validated + if (new_calibration_ptr) { + decoder_wrapper_->OnCalibrationChange(new_calibration_ptr); + RCLCPP_INFO_STREAM(get_logger(), "Changed calibration to '" << new_cfg.calibration_path << "'"); + } + + if ( + new_calibration_ptr && hw_interface_wrapper_ && + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { + auto status = hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*new_calibration_ptr); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + get_logger(), "Sensor configuration updated, but setting hardware FoV failed: " << status); + } + } + return rcl_interfaces::build().successful(true).reason(""); } @@ -315,6 +419,92 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) } } +std::string HesaiRosWrapper::getCalibrationParameterName(drivers::SensorModel model) const +{ + if (model == drivers::SensorModel::HESAI_PANDARAT128) { + return "correction_file"; + } + + return "calibration_file"; +} + +HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others) +{ + std::shared_ptr calib; + const auto & logger = get_logger(); + + if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calib = std::make_shared(); + } else { + calib = std::make_shared(); + } + + std::string calibration_file_path_from_sensor; + + { + int ext_pos = calibration_file_path.find_last_of('.'); + calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); + calibration_file_path_from_sensor += "_from_sensor_" + sensor_cfg_ptr_->sensor_ip; + calibration_file_path_from_sensor += + calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); + } + + // If a sensor is connected, try to download and save its calibration data + if (!ignore_others && launch_hw_) { + try { + auto raw_data = hw_interface_wrapper_->HwInterface()->GetLidarCalibrationBytes(); + RCLCPP_INFO(logger, "Downloaded calibration data from sensor."); + auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM(logger, "Could not save calibration data: " << status); + } else { + RCLCPP_INFO_STREAM( + logger, "Saved downloaded data to " << calibration_file_path_from_sensor); + } + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger, "Could not download calibration data: " << e.what()); + } + } + + // If saved calibration data from a sensor exists (either just downloaded above, or previously), + // try to load it + if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { + auto status = calib->LoadFromFile(calibration_file_path_from_sensor); + if (status == Status::OK) { + calib->calibration_file = calibration_file_path_from_sensor; + return calib; + } + + RCLCPP_ERROR_STREAM(logger, "Could not load downloaded calibration data: " << status); + } else if (!ignore_others) { + RCLCPP_ERROR(logger, "No downloaded calibration data found."); + } + + if (!ignore_others) { + RCLCPP_WARN(logger, "Falling back to generic calibration file."); + } + + // If downloaded data did not exist or could not be loaded, fall back to a generic file. + // If that file does not exist either, return an error code + if (!std::filesystem::exists(calibration_file_path)) { + RCLCPP_ERROR(logger, "No calibration data found."); + return nebula::Status(Status::INVALID_CALIBRATION_FILE); + } + + // Try to load the existing fallback calibration file. Return an error if this fails + auto status = calib->LoadFromFile(calibration_file_path); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + logger, "Could not load calibration file at '" << calibration_file_path << "'"); + return status; + } + + // Return the fallback calibration file + calib->calibration_file = calibration_file_path; + return calib; +} + RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index f9abbd164..f0ba51102 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -2,6 +2,8 @@ #include "nebula_ros/hesai/hw_interface_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" + namespace nebula { namespace ros diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index f94aae744..1a81cfdcc 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -4,6 +4,8 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include + namespace nebula { namespace ros @@ -52,6 +54,8 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( break; } + supports_monitor_ = config->sensor_model != drivers::SensorModel::HESAI_PANDARAT128; + auto result = hw_interface->GetInventory(); current_inventory_.reset(new HesaiInventory(result)); current_inventory_time_.reset(new rclcpp::Time(parent_node->get_clock()->now())); @@ -88,6 +92,9 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() auto fetch_diag_from_sensor = [this]() { OnHesaiStatusTimer(); + + if (!supports_monitor_) return; + if (hw_interface_->UseHttpGetLidarMonitor()) { OnHesaiLidarMonitorTimerHttp(); } else { diff --git a/nebula_tests/data/hesai/40p/1673400149711850138.pcd b/nebula_tests/data/hesai/40p/1673400149711850138.pcd index d49246b11..2ef6c1d27 100644 Binary files a/nebula_tests/data/hesai/40p/1673400149711850138.pcd and b/nebula_tests/data/hesai/40p/1673400149711850138.pcd differ diff --git a/nebula_tests/data/hesai/64/1673403880898440686.pcd b/nebula_tests/data/hesai/64/1673403880898440686.pcd index 7e51c804e..7bd4732fc 100644 Binary files a/nebula_tests/data/hesai/64/1673403880898440686.pcd and b/nebula_tests/data/hesai/64/1673403880898440686.pcd differ diff --git a/nebula_tests/data/hesai/64/all_points/all_points_0.db3 b/nebula_tests/data/hesai/64/all_points/all_points_0.db3 new file mode 100644 index 000000000..d45938c45 Binary files /dev/null and b/nebula_tests/data/hesai/64/all_points/all_points_0.db3 differ diff --git a/nebula_tests/data/hesai/64/all_points/metadata.yaml b/nebula_tests/data/hesai/64/all_points/metadata.yaml new file mode 100644 index 000000000..4a83a0964 --- /dev/null +++ b/nebula_tests/data/hesai/64/all_points/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 299063850 + starting_time: + nanoseconds_since_epoch: 1673403880599376836 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "" + message_count: 4 + compression_format: "" + compression_mode: "" + relative_file_paths: + - all_points_0.db3 + files: + - path: all_points_0.db3 + starting_time: + nanoseconds_since_epoch: 1673403880599376836 + duration: + nanoseconds: 299063850 + message_count: 4 diff --git a/nebula_tests/data/hesai/at128/1679653308704927652.pcd b/nebula_tests/data/hesai/at128/1679653308704927652.pcd index 1a11a3ca8..dd215e9c6 100644 Binary files a/nebula_tests/data/hesai/at128/1679653308704927652.pcd and b/nebula_tests/data/hesai/at128/1679653308704927652.pcd differ diff --git a/nebula_tests/data/hesai/at128/all_points/all_points_0.db3 b/nebula_tests/data/hesai/at128/all_points/all_points_0.db3 new file mode 100644 index 000000000..1e0098d9a Binary files /dev/null and b/nebula_tests/data/hesai/at128/all_points/all_points_0.db3 differ diff --git a/nebula_tests/data/hesai/at128/all_points/metadata.yaml b/nebula_tests/data/hesai/at128/all_points/metadata.yaml new file mode 100644 index 000000000..c2494def5 --- /dev/null +++ b/nebula_tests/data/hesai/at128/all_points/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 298889276 + starting_time: + nanoseconds_since_epoch: 1679653308406038376 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "" + message_count: 4 + compression_format: "" + compression_mode: "" + relative_file_paths: + - all_points_0.db3 + files: + - path: all_points_0.db3 + starting_time: + nanoseconds_since_epoch: 1679653308406038376 + duration: + nanoseconds: 298889276 + message_count: 4 diff --git a/nebula_tests/data/hesai/qt64/1673401196087570870.pcd b/nebula_tests/data/hesai/qt64/1673401196087570870.pcd index 5edff98fb..feef7aef2 100644 Binary files a/nebula_tests/data/hesai/qt64/1673401196087570870.pcd and b/nebula_tests/data/hesai/qt64/1673401196087570870.pcd differ diff --git a/nebula_tests/data/hesai/xt32/1673400678101968885.pcd b/nebula_tests/data/hesai/xt32/1673400678101968885.pcd index d6dca526b..896b4665b 100644 Binary files a/nebula_tests/data/hesai/xt32/1673400678101968885.pcd and b/nebula_tests/data/hesai/xt32/1673400678101968885.pcd differ diff --git a/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd b/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd index 65f85d599..620f19cb8 100644 Binary files a/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd and b/nebula_tests/data/hesai/xt32m/1660893203292679064.pcd differ diff --git a/nebula_tests/hesai/CMakeLists.txt b/nebula_tests/hesai/CMakeLists.txt index 56370ddae..35c5d919c 100644 --- a/nebula_tests/hesai/CMakeLists.txt +++ b/nebula_tests/hesai/CMakeLists.txt @@ -2,6 +2,7 @@ add_library(hesai_ros_decoder_test SHARED hesai_ros_decoder_test.cpp ) + target_include_directories(hesai_ros_decoder_test PUBLIC ${NEBULA_TEST_INCLUDE_DIRS} ) @@ -21,3 +22,17 @@ target_include_directories(hesai_ros_decoder_test_main PUBLIC target_link_libraries(hesai_ros_decoder_test_main hesai_ros_decoder_test ) + +ament_add_gtest(hesai_ros_scan_cutting_test_main + hesai_ros_scan_cutting_test_main.cpp +) + +target_include_directories(hesai_ros_scan_cutting_test_main PUBLIC + ${PROJECT_SOURCE_DIR}/src/hesai + include + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(hesai_ros_scan_cutting_test_main + hesai_ros_decoder_test +) diff --git a/nebula_tests/hesai/hesai_common.hpp b/nebula_tests/hesai/hesai_common.hpp index d01a0edf8..69554e67e 100644 --- a/nebula_tests/hesai/hesai_common.hpp +++ b/nebula_tests/hesai/hesai_common.hpp @@ -34,6 +34,7 @@ namespace test inline void checkPCDs( nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud::Ptr pc_ref) { + ASSERT_GT(pc->points.size(), 0); EXPECT_EQ(pc->points.size(), pc_ref->points.size()); auto bound = std::min(pc->points.size(), pc_ref->points.size()); for (uint32_t i = 0; i < bound; i++) { diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index 192281804..dc423bf43 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -2,15 +2,21 @@ #include "hesai_ros_decoder_test.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_storage/storage_options.hpp" +#include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_common/nebula_common.hpp" + +#include +#include +#include +#include +#include +#include + +#include #include +#include #include #include @@ -79,138 +85,25 @@ Status HesaiRosDecoderTest::GetParameters( calibration_dir /= "hesai"; std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", params_.sensor_model, descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", params_.return_mode, descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", params_.frame_id, descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", params_.scan_phase, descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calibration_dir / params_.calibration_file).string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("min_range", params_.min_range, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", params_.max_range, descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calibration_dir / params_.correction_file).string(), descriptor); - params_.correction_file = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / params_.bag_path).string(), descriptor); - params_.bag_path = this->get_parameter("bag_path").as_string(); - RCLCPP_DEBUG_STREAM(get_logger(), "Bag path relative to test root: " << params_.bag_path); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", params_.storage_id, descriptor); - params_.storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", params_.format, descriptor); - params_.format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", params_.target_topic, descriptor); - params_.target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter( - "dual_return_distance_threshold", params_.dual_return_distance_threshold, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(params_.sensor_model); + sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( + params_.return_mode, sensor_configuration.sensor_model); + sensor_configuration.frame_id = params_.frame_id; + sensor_configuration.sync_angle = params_.sync_angle; + sensor_configuration.cut_angle = params_.cut_angle; + sensor_configuration.cloud_min_angle = params_.fov_start; + sensor_configuration.cloud_max_angle = params_.fov_end; + sensor_configuration.min_range = params_.min_range; + sensor_configuration.max_range = params_.max_range; + + sensor_configuration.calibration_path = (calibration_dir / params_.calibration_file).string(); + params_.bag_path = (bag_root_dir / params_.bag_path).string(); + RCLCPP_DEBUG_STREAM(get_logger(), "Bag path relative to test root: " << params_.bag_path); + params_.storage_id = params_.storage_id; + params_.format = params_.format; + params_.target_topic = params_.target_topic; + sensor_configuration.dual_return_distance_threshold = params_.dual_return_distance_threshold; if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -218,35 +111,33 @@ 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()) { return Status::SENSOR_CONFIG_ERROR; } - if (calibration_configuration.calibration_file.empty()) { + + if (sensor_configuration.calibration_path.empty()) { return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + } + + if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + auto cal_status = correction_configuration.LoadFromFile(sensor_configuration.calibration_path); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); + "Invalid correction file: '" << sensor_configuration.calibration_path << "'"); return cal_status; } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (params_.correction_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(params_.correction_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << params_.correction_file << "'"); - return cal_status; - } + } else { + auto cal_status = calibration_configuration.LoadFromFile(sensor_configuration.calibration_path); + if (cal_status != Status::OK) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Invalid calibration file: '" << sensor_configuration.calibration_path << "'"); + return cal_status; } } - RCLCPP_INFO_STREAM(this->get_logger(), "Sensor Configuration: " << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor configuration: " << sensor_configuration); return Status::OK; } @@ -302,8 +193,6 @@ void HesaiRosDecoderTest::ReadBag( continue; } - std::cerr << "Pointcloud size: " << pointcloud->size() << std::endl; - scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index 775fcf8c0..54e34d60e 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -14,12 +14,11 @@ #pragma once -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" - #include +#include +#include +#include +#include #include #include @@ -45,11 +44,13 @@ struct HesaiRosDecoderTestParams { std::string sensor_model; std::string return_mode; - std::string calibration_file = ""; + std::string calibration_file; std::string bag_path; - std::string correction_file = ""; std::string frame_id = "hesai"; - double scan_phase = 0.; + uint16_t sync_angle = 0; + double cut_angle = 0.; + double fov_start = 0.; + double fov_end = 360.; double min_range = 0.3; double max_range = 300.; std::string storage_id = "sqlite3"; @@ -65,9 +66,11 @@ inline std::ostream & operator<<( << "return_mode=" << arg.return_mode << ", " << "calibration_file=" << arg.calibration_file << ", " << "bag_path=" << arg.bag_path << ", " - << "correction_file=" << arg.correction_file << ", " << "frame_id=" << arg.frame_id << ", " - << "scan_phase=" << arg.scan_phase << ", " + << "sync_angle=" << arg.sync_angle << ", " + << "cut_angle=" << arg.cut_angle << ", " + << "fov_start=" << arg.fov_start << ", " + << "fov_end=" << arg.fov_end << ", " << "min_range=" << arg.min_range << ", " << "max_range=" << arg.max_range << ", " << "storage_id=" << arg.storage_id << ", " diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index c66b05267..1c044ff0a 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -5,12 +5,17 @@ #include "hesai_common.hpp" #include "hesai_ros_decoder_test.hpp" +#include +#include #include #include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -22,15 +27,18 @@ 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.0, 0., 360., 0.3f, + 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "hesai", 0, 0.0, 0., 360., 0.3f, + 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/1679653308406038376", "hesai", 0, + 150.0, 30., 150., 1.f, 180.f}, + {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "hesai", 0, 0.0, 0., 360., + 0.1f, 60.f}, + {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "hesai", 0, 0.0, 0., 360., + 0.05f, 120.f}, + {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "hesai", 0, 0.0, + 0., 360., 0.5f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd) @@ -39,7 +47,7 @@ TEST_P(DecoderTest, TestPcd) rcpputils::fs::path bag_dir(hesai_driver_->params_.bag_path); rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); + auto ref_pointcloud = std::make_shared>(); int check_cnt = 0; auto scan_callback = [&]( @@ -58,7 +66,7 @@ TEST_P(DecoderTest, TestPcd) checkPCDs(pointcloud, ref_pointcloud); check_cnt++; // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); + ref_pointcloud = std::make_shared>(); } }; @@ -70,6 +78,9 @@ TEST_P(DecoderTest, TestPcd) // are not affected by timezones and are always output in UST. TEST_P(DecoderTest, TestTimezone) { + TearDown(); + SetUp(); + // For each pointcloud decoded, this will contain the pointcloud timestamp returned by the driver std::vector decoded_timestamps; @@ -123,16 +134,12 @@ void DecoderTest::SetUp() logger_->set_level(rclcpp::Logger::Level::Info); RCLCPP_DEBUG_STREAM(*logger_, "Testing " << decoder_params.sensor_model); - setvbuf(stdout, NULL, _IONBF, BUFSIZ); std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::executors::SingleThreadedExecutor exec; rclcpp::NodeOptions options; hesai_driver_ = std::make_shared(options, node_name, decoder_params); - exec.add_node(hesai_driver_->get_node_base_interface()); nebula::Status driver_status = hesai_driver_->GetStatus(); if (driver_status != nebula::Status::OK) { diff --git a/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp new file mode 100644 index 000000000..3d26d8103 --- /dev/null +++ b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp @@ -0,0 +1,180 @@ +// Copyright 2024 TIER IV, Inc. + +#include "hesai_ros_scan_cutting_test_main.hpp" + +#include "hesai_ros_decoder_test.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::test +{ +const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[] = { + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 0.0, 0., 360., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 180.0, 0., 360., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 90.0, 90., 270., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 180.0, 90., 270., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 270.0, 90., 270., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 0.0, 270., 90., 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 90.0, 270., 90., 0.3f, 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 31.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 90.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 149.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 150.0, 30., + 150., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 46.0, 45., + 135., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 90.0, 45., + 135., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 134.0, 45., + 135., 1.f, 180.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.dat", "at128/all_points", "hesai", 0, 135.0, 45., + 135., 1.f, 180.f}, +}; + +// Checks that there are never any points outside the defined FoV, and that there are points close +// to the FoV boundaries +TEST_P(ScanCuttingTest, FovAlignment) +{ + int check_cnt = 0; + + auto fov_min_rad = drivers::deg2rad(hesai_driver_->params_.fov_start); + auto fov_max_rad = drivers::deg2rad(hesai_driver_->params_.fov_end); + // The threshold near the FoV borders within which each channel has to have at least one point + auto near_threshold_rad = drivers::deg2rad(.3); + + // Skip the first n clouds (because the recorded rosbags do not always contain full scans and the + // tests would thus fail) + int skip_first = 2; + + auto scan_callback = [&](uint64_t, uint64_t, nebula::drivers::NebulaPointCloudPtr pointcloud) { + if (!pointcloud) return; + + if (skip_first > 0) { + skip_first--; + return; + } + + bool none_outside_fov = true; + std::map has_points_near_start; + std::map has_points_near_end; + + for (const drivers::NebulaPoint & p : pointcloud->points) { + none_outside_fov &= drivers::angle_is_between(fov_min_rad, fov_max_rad, p.azimuth); + + if (has_points_near_start.find(p.channel) == has_points_near_start.end()) { + has_points_near_start.emplace(p.channel, false); + } + auto threshold_angle = drivers::normalize_angle(fov_min_rad + near_threshold_rad, 2 * M_PIf); + has_points_near_start[p.channel] |= drivers::angle_is_between( + drivers::normalize_angle(fov_min_rad, 2 * M_PIf), threshold_angle, p.azimuth); + + if (has_points_near_end.find(p.channel) == has_points_near_end.end()) { + has_points_near_end.emplace(p.channel, false); + } + threshold_angle = drivers::normalize_angle(fov_max_rad - near_threshold_rad, 2 * M_PIf); + has_points_near_end[p.channel] |= drivers::angle_is_between( + threshold_angle, drivers::normalize_angle(fov_max_rad, 2 * M_PIf), p.azimuth); + } + + bool is_at128 = drivers::SensorModelFromString(hesai_driver_->params_.sensor_model) == + drivers::SensorModel::HESAI_PANDARAT128; + // There is a bug in AT128's firm- or hardware that skips a few channels at the beginning of the + // defined FoV. for that reason, relax test conditions in the case where that bug happens. + // Bug observed on SW ver. 3.50.15, FW ver. 3.10b830, RPU ver. 3.50.008 + bool is_at128_bug = is_at128 && hesai_driver_->params_.fov_start <= 30 + 3; + + if (is_at128_bug) { + // The bug affects 32 channels, so 96 valid ones remain + bool most_channels_have_points_near_start = + std::count_if( + has_points_near_start.begin(), has_points_near_start.end(), + [](const auto & entry) { return entry.second; }) >= 96; + + EXPECT_TRUE(most_channels_have_points_near_start); + } else { + bool all_channels_have_points_near_start = std::all_of( + has_points_near_start.begin(), has_points_near_start.end(), + [](const auto & entry) { return entry.second; }); + + EXPECT_TRUE(all_channels_have_points_near_start); + } + + bool all_channels_have_points_near_end = std::all_of( + has_points_near_end.begin(), has_points_near_end.end(), + [](const auto & entry) { return entry.second; }); + EXPECT_TRUE(all_channels_have_points_near_end); + EXPECT_TRUE(none_outside_fov); + check_cnt++; + }; + + hesai_driver_->ReadBag(scan_callback); + EXPECT_GT(check_cnt, 0); +} + +void ScanCuttingTest::SetUp() +{ + auto decoder_params = GetParam(); + logger_ = std::make_shared(rclcpp::get_logger("UnitTest")); + logger_->set_level(rclcpp::Logger::Level::Info); + + RCLCPP_DEBUG_STREAM(*logger_, "Testing " << decoder_params.sensor_model); + + std::string node_name = "nebula_hesai_scan_cutting_test"; + rclcpp::NodeOptions options; + + hesai_driver_ = + std::make_shared(options, node_name, decoder_params); + + nebula::Status driver_status = hesai_driver_->GetStatus(); + if (driver_status != nebula::Status::OK) { + throw std::runtime_error("Could not initialize driver"); + } +} + +void ScanCuttingTest::TearDown() +{ + RCLCPP_INFO_STREAM(*logger_, "Tearing down"); + hesai_driver_.reset(); + logger_.reset(); +} + +INSTANTIATE_TEST_SUITE_P( + TestMain, ScanCuttingTest, testing::ValuesIn(TEST_CONFIGS), + [](const testing::TestParamInfo & p) { + return p.param.sensor_model + "__cut" + std::to_string(static_cast(p.param.cut_angle)) + + "__fov_start" + std::to_string(static_cast(p.param.fov_start)) + "__fov_end" + + std::to_string(static_cast(p.param.fov_end)); + }); + +} // namespace nebula::test + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.hpp b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.hpp new file mode 100644 index 000000000..ebdc6a887 --- /dev/null +++ b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "hesai_ros_decoder_test.hpp" + +#include + +#include + +#include + +namespace nebula::test +{ +class ScanCuttingTest : public ::testing::TestWithParam +{ +protected: + /// @brief Instantiates the Hesai driver node with the given test parameters + void SetUp() override; + + /// @brief Destroys the Hesai driver node + void TearDown() override; + + std::shared_ptr hesai_driver_; + std::shared_ptr logger_; +}; + +} // namespace nebula::test