diff --git a/.cspell.json b/.cspell.json index 75b26f454..30dc5cda2 100644 --- a/.cspell.json +++ b/.cspell.json @@ -6,23 +6,39 @@ "adctp", "Adctp", "AT", + "autosar", "block_id", + "Bpearl", + "calib", + "DHAVE", + "Difop", "extrinsics", + "fout", "gprmc", + "gptp", + "Helios", "Hesai", + "Idat", + "ipaddr", + "manc", "memcpy", "mkdoxy", + "Msop", "nohup", "nproc", + "ntoa", "pandar", "PANDAR", "PANDARAT", "PANDARQT", "PANDARXT", "Pdelay", + "Piyush", + "piyushk", "QT", "rclcpp", "schedutil", + "srvs", "STD_COUT", "stds", "struct", @@ -30,23 +46,9 @@ "UDP_SEQ", "vccint", "Vccint", + "Vdat", "XT", "XTM", - "DHAVE", - "Bpearl", - "Helios", - "Msop", - "Difop", - "gptp", - "Idat", - "Vdat", - "autosar", - "srvs", - "manc", - "ipaddr", - "ntoa", - "piyushk", - "Piyush", - "fout" + "yukkysaito" ] } diff --git a/.gitignore b/.gitignore index e966ffdfb..f068ec8be 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,6 @@ site/ # qcreator stuff CMakeLists.txt.user + +# pre-commit +node_modules/ diff --git a/build_depends.repos b/build_depends.repos index e28379f1a..e9278685a 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -2,5 +2,5 @@ repositories: # TCP version of transport drivers transport_drivers: type: git - url: https://github.com/MapIV/transport_drivers - version: boost + url: https://github.com/mojomex/transport_drivers + version: mutable-buffer-in-udp-callback diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 4c146736c..154f8e453 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -40,6 +40,8 @@ ament_auto_add_library(nebula_common SHARED src/velodyne/velodyne_calibration_decoder.cpp ) +target_link_libraries(nebula_common yaml-cpp) + ament_auto_package() # Set ROS_DISTRO macros diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 9c6e5d555..8e0600999 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -8,13 +8,16 @@ #include #include #include +#include #include +#include +#include namespace nebula { namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : LidarConfigurationBase +struct HesaiSensorConfiguration : public LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -43,24 +46,38 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con return os; } +struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase +{ + virtual nebula::Status LoadFromBytes(const std::vector & buf) = 0; + virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; + virtual nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) = 0; +}; + /// @brief struct for Hesai calibration configuration -struct HesaiCalibrationConfiguration : CalibrationConfigurationBase +struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase { std::map elev_angle_map; std::map azimuth_offset_map; - inline nebula::Status LoadFromFile(const std::string & calibration_file) + inline nebula::Status LoadFromFile(const std::string & calibration_file) override { std::ifstream ifs(calibration_file); if (!ifs) { return Status::INVALID_CALIBRATION_FILE; } std::ostringstream ss; - ss << ifs.rdbuf(); // reading data + ss << ifs.rdbuf(); // reading data ifs.close(); return LoadFromString(ss.str()); } + nebula::Status LoadFromBytes(const std::vector & buf) + { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return LoadFromString(calibration_string); + } + /// @brief Loading calibration data /// @param calibration_content /// @return Resulting status @@ -70,14 +87,12 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase ss << calibration_content; std::string line; constexpr size_t expected_cols = 3; - while(std::getline(ss, line)) { + while (std::getline(ss, line)) { boost::char_separator sep(","); boost::tokenizer> tok(line, sep); std::vector actual_tokens(tok.begin(), tok.end()); - if (actual_tokens.size() < expected_cols - || actual_tokens.size() > expected_cols - ) { + if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) { std::cerr << "Ignoring line with unexpected data:" << line << std::endl; continue; } @@ -88,10 +103,9 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase float azimuth = std::stof(actual_tokens[2]); elev_angle_map[laser_id - 1] = elevation; azimuth_offset_map[laser_id - 1] = azimuth; - } catch (const std::invalid_argument& ia) { + } catch (const std::invalid_argument & ia) { continue; } - } return Status::OK; } @@ -99,7 +113,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveFile(const std::string & calibration_file) + inline nebula::Status SaveToFile(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -117,11 +131,19 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } + nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) override + { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return SaveFileFromString(calibration_file, calibration_string); + } + /// @brief Saving calibration data from string /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) + inline nebula::Status SaveFileFromString( + const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -134,7 +156,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase }; /// @brief struct for Hesai correction configuration (for AT) -struct HesaiCorrection +struct HesaiCorrection : public HesaiCalibrationConfigurationBase { uint16_t delimiter; uint8_t versionMajor; @@ -156,12 +178,11 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param buf Binary buffer /// @return Resulting status - inline nebula::Status LoadFromBinary(const std::vector & buf) + inline nebula::Status LoadFromBytes(const std::vector & buf) override { size_t index; - for (index = 0; index < buf.size()-1; index++) { - if(buf[index]==0xee && buf[index+1]==0xff) - break; + for (index = 0; index < buf.size() - 1; index++) { + if (buf[index] == 0xee && buf[index + 1] == 0xff) break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -258,7 +279,7 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param correction_file path /// @return Resulting status - inline nebula::Status LoadFromFile(const std::string & correction_file) + inline nebula::Status LoadFromFile(const std::string & correction_file) override { std::ifstream ifs(correction_file, std::ios::in | std::ios::binary); if (!ifs) { @@ -268,10 +289,10 @@ struct HesaiCorrection // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read((char *)&c, sizeof(unsigned char)); + ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); buf.emplace_back(c); } - LoadFromBinary(buf); + LoadFromBytes(buf); ifs.close(); return Status::OK; @@ -281,7 +302,8 @@ struct HesaiCorrection /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) + inline nebula::Status SaveToFileFromBytes( + const std::string & correction_file, const std::vector & buf) override { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -291,21 +313,20 @@ struct HesaiCorrection } std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; - for (const auto &byte : buf) { + for (const auto & byte : buf) { if (!sop_received) { if (byte == 0xEE) { std::cerr << "SOP received....\n"; sop_received = true; } } - if(sop_received) { + if (sop_received) { ofs << byte; } } std::cerr << "Closing file\n"; ofs.close(); - if(sop_received) - return Status::OK; + if (sop_received) return Status::OK; return Status::INVALID_CALIBRATION_FILE; } @@ -435,8 +456,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARQT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST - || return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL) + return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 9d04a267a..e43ab49a3 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,7 +2,10 @@ #define NEBULA_COMMON_H #include + #include + +#include #include #include #include @@ -342,24 +345,11 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { - IEEE_1588v2 = 0, - IEEE_802_1AS, - IEEE_802_1AS_AUTO, - PROFILE_UNKNOWN -}; +enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; -enum class PtpTransportType { - UDP_IP = 0, - L2, - UNKNOWN_TRANSPORT -}; +enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; -enum class PtpSwitchType { - NON_TSN = 0, - TSN, - UNKNOWN_SWITCH -}; +enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; /// @brief not used? struct PointField @@ -618,13 +608,14 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; - return PtpProfile::PROFILE_UNKNOWN; + return PtpProfile::UNKNOWN_PROFILE; } /// @brief Convert PtpProfile enum to string (Overloading the << operator) @@ -643,7 +634,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile case PtpProfile::IEEE_802_1AS_AUTO: os << "IEEE_802.1AS Automotive"; break; - case PtpProfile::PROFILE_UNKNOWN: + case PtpProfile::UNKNOWN_PROFILE: os << "UNKNOWN"; break; } @@ -657,8 +648,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -692,8 +684,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index d3643cfb4..25cb5f83d 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -1,23 +1,22 @@ #pragma once -#include -#include #include +#include +#include namespace nebula { namespace util { -struct bad_expected_access : public std::exception { - bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} +struct bad_expected_access : public std::exception +{ + explicit bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} - const char* what() const noexcept override { - return msg_.c_str(); - } + const char * what() const noexcept override { return msg_.c_str(); } private: - const std::string msg_; + const std::string msg_; }; /// @brief A poor man's backport of C++23's std::expected. @@ -39,14 +38,16 @@ struct expected /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained. /// @return The value of type `T` - T value() { + T value() + { if (!has_value()) { throw bad_expected_access("value() called but containing error"); } - return std::get(value_); + return std::get(value_); } - /// @brief Return the contained value, or, if an error is contained, return the given `default_` instead. + /// @brief Return the contained value, or, if an error is contained, return the given `default_` + /// instead. /// @return The contained value, if any, else `default_` T value_or(const T & default_) { @@ -54,36 +55,41 @@ struct expected return default_; } - /// @brief Return the contained value, or, if an error is contained, throw `runtime_error(error_msg)`. - /// @return The contained value if no error is thrown - T value_or_throw(const std::string & error_msg) { + /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg) + /// @param error_msg The message to be included in the thrown exception + /// @return The value + T value_or_throw(const std::string & error_msg) + { if (has_value()) return value(); throw std::runtime_error(error_msg); } /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained. /// @return The error of type `E` - E error() { + E error() + { if (has_value()) { throw bad_expected_access("error() called but containing value"); } - return std::get(value_); + return std::get(value_); } - /// @brief Return the contained error, or, if a value is contained, return the given `default_` instead. + /// @brief Return the contained error, or, if a value is contained, return the given `default_` + /// instead. /// @return The contained error, if any, else `default_` - E error_or(const E & default_) { + E error_or(const E & default_) + { if (!has_value()) return error(); - return default_; - } + return default_; + } - expected(const T & value) { value_ = value; } + expected(const T & value) { value_ = value; } // NOLINT(runtime/explicit) - expected(const E & error) { value_ = error; } + expected(const E & error) { value_ = error; } // NOLINT(runtime/explicit) private: std::variant value_; }; } // namespace util -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 42fd0a008..02d7cd535 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 @@ -23,19 +23,11 @@ struct CorrectedAngleData /// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry /// lookup tables +template class AngleCorrector { -protected: - const std::shared_ptr sensor_calibration_; - const std::shared_ptr sensor_correction_; - public: - AngleCorrector( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction) - { - } + using correction_data_t = CorrectionDataT; /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their /// sin/cos values. @@ -52,8 +44,9 @@ class AngleCorrector /// @param sync_azimuth The azimuth set in the sensor configuration, for which the /// timestamp is aligned to the full second /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; + virtual bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index 3e0c999b0..da9a1b579 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 @@ -4,6 +4,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include +#include namespace nebula { @@ -11,7 +12,7 @@ namespace drivers { template -class AngleCorrectorCalibrationBased : public AngleCorrector +class AngleCorrectorCalibrationBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; @@ -27,9 +28,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector public: AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction) + const std::shared_ptr & sensor_calibration) { if (sensor_calibration == nullptr) { throw std::runtime_error( @@ -37,8 +36,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector } for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { - float elevation_angle_deg = sensor_calibration->elev_angle_map[channel_id]; - float azimuth_offset_deg = sensor_calibration->azimuth_offset_map[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); elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); @@ -81,10 +80,10 @@ class AngleCorrectorCalibrationBased : public AngleCorrector (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; uint32_t last_diff_from_sync = (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; - + return current_diff_from_sync < last_diff_from_sync; } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index be60078de..619c5a8fe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -4,8 +4,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include - -#define _(x) '"' << #x << "\": " << x << ", " +#include namespace nebula { @@ -13,10 +12,11 @@ namespace drivers { template -class AngleCorrectorCorrectionBased : public AngleCorrector +class AngleCorrectorCorrectionBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LENGTH = 360 * AngleUnit; + const std::shared_ptr correction_; rclcpp::Logger logger_; std::array cos_{}; @@ -29,28 +29,26 @@ class AngleCorrectorCorrectionBased : public AngleCorrector { // Assumes that: // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) - // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.) - // These assumptions hold for AT128E2X. - int field = sensor_correction_->frameNumber - 1; - for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) { - if (azimuth < sensor_correction_->startFrame[i]) return field; + // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg + // etc.) These assumptions hold for AT128E2X. + int field = correction_->frameNumber - 1; + for (size_t i = 0; i < correction_->frameNumber; ++i) { + if (azimuth < correction_->startFrame[i]) return field; field = i; } - // This is never reached if sensor_correction_ is correct + // This is never reached if correction_ is correct return field; } public: - AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction), - logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) + explicit AngleCorrectorCorrectionBased( + const std::shared_ptr & sensor_correction) + : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { throw std::runtime_error( - "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); + "Cannot instantiate AngleCorrectorCorrectionBased without correction data"); } logger_.set_level(rclcpp::Logger::Level::Debug); @@ -64,17 +62,16 @@ class AngleCorrectorCorrectionBased : public AngleCorrector CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override { - const auto & correction = AngleCorrector::sensor_correction_; int field = findField(block_azimuth); auto elevation = - correction->elevation[channel_id] + - correction->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + correction_->elevation[channel_id] + + correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); elevation = (MAX_AZIMUTH_LENGTH + elevation) % MAX_AZIMUTH_LENGTH; - auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction->startFrame[field]) * 2 - - correction->azimuth[channel_id] + - correction->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + 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; float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH_LENGTH; @@ -84,7 +81,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override + bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override { // For AT128, the scan is always cut at the beginning of the field: // If we would cut at `sync_azimuth`, the points left of it would be diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 7bdfd069e..a277137d2 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 @@ -8,6 +8,11 @@ #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include + namespace nebula { namespace drivers @@ -18,7 +23,7 @@ class HesaiDecoder : public HesaiScanDecoder { protected: /// @brief Configuration for this decoder - const std::shared_ptr sensor_configuration_; + const std::shared_ptr sensor_configuration_; /// @brief The sensor definition, used for return mode and time offset handling SensorT sensor_{}; @@ -34,13 +39,13 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; /// @brief The last azimuth processed - int last_phase_; + 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_; + uint64_t output_scan_timestamp_ns_ = 0; /// @brief The timestamp of the scan currently in progress - uint64_t decode_scan_timestamp_ns_; + uint64_t decode_scan_timestamp_ns_ = 0; /// @brief Whether a full scan has been processed - bool has_scanned_; + bool has_scanned_ = false; rclcpp::Logger logger_; @@ -52,17 +57,17 @@ class HesaiDecoder : public HesaiScanDecoder block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) + bool parsePacket(const std::vector & packet) { - if (pandar_packet.size < sizeof(typename SensorT::packet_t)) { + if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:" + logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { // FIXME(mojomex) do validation? // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); return true; @@ -174,6 +179,10 @@ class HesaiDecoder : public HesaiScanDecoder /// @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); } @@ -200,20 +209,17 @@ class HesaiDecoder : public HesaiScanDecoder public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder (can be nullptr if - /// correction_configuration is set) - /// @param correction_configuration Correction for this decoder (can be nullptr if - /// calibration_configuration is set) + /// @param correction_data Calibration data for this decoder explicit HesaiDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + correction_data) : sensor_configuration_(sensor_configuration), - angle_corrector_(calibration_configuration, correction_configuration), + angle_corrector_(correction_data), logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); - RCLCPP_INFO_STREAM(logger_, sensor_configuration_); + RCLCPP_INFO_STREAM(logger_, *sensor_configuration_); decode_pc_.reset(new NebulaPointCloud); output_pc_.reset(new NebulaPointCloud); @@ -222,9 +228,9 @@ class HesaiDecoder : public HesaiScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override + int unpack(const std::vector & packet) override { - if (!parsePacket(pandar_packet)) { + if (!parsePacket(packet)) { return -1; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index 730888c91..aa807e567 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -190,7 +190,7 @@ struct PacketBase /// @brief Get the number of returns for a given return mode /// @param return_mode The return mode /// @return The number of returns -int get_n_returns(uint8_t return_mode) +inline int get_n_returns(uint8_t return_mode) { switch (return_mode) { case return_mode::SINGLE_FIRST: @@ -221,7 +221,8 @@ uint64_t get_timestamp_ns(const PacketT & packet) return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000; } -/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, multiplied by this value, yield the distance in meters. +/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, +/// multiplied by this value, yield the distance in meters. /// @tparam PacketT The packet type /// @param packet The packet to get the distance unit from /// @return The distance unit in meters diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index f0b4e3f8b..153435bbe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -8,6 +8,7 @@ #include "pandar_msgs/msg/pandar_scan.hpp" #include +#include namespace nebula { @@ -26,9 +27,9 @@ class HesaiScanDecoder HesaiScanDecoder() = default; /// @brief Parses PandarPacket and add its points to the point cloud - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return The last azimuth processed - virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + virtual int unpack(const std::vector & packet) = 0; /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready 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 c83788965..4c211efcc 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 @@ -14,15 +14,18 @@ #include #include +#include #include #include +#include +#include namespace nebula { namespace drivers { /// @brief Hesai driver -class HesaiDriver : NebulaDriverBase +class HesaiDriver { private: /// @brief Current driver status @@ -30,16 +33,22 @@ class HesaiDriver : NebulaDriverBase /// @brief Decoder according to the model std::shared_ptr scan_decoder_; + template + std::shared_ptr InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); + public: HesaiDriver() = delete; /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver (for AT) + /// @param calibration_configuration CalibrationConfiguration for this driver (either + /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128) explicit HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration = nullptr); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Get current status of this driver /// @return Current status @@ -49,13 +58,13 @@ class HesaiDriver : NebulaDriverBase /// @param calibration_configuration /// @return Resulting status Status SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) override; + const HesaiCalibrationConfigurationBase & calibration_configuration); /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan); + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 49be4adac..b3afa886b 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -18,62 +18,63 @@ namespace nebula namespace drivers { HesaiDriver::HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_data) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; + switch (sensor_configuration->sensor_model) { - case SensorModel::UNKNOWN: - driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; - break; case SensorModel::HESAI_PANDAR64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + throw std::runtime_error("Invalid sensor model."); default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); - break; } } -std::tuple HesaiDriver::ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan) +template +std::shared_ptr HesaiDriver::InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) +{ + using CalibT = typename SensorT::angle_corrector_t::correction_data_t; + return std::make_shared>( + sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); +} + +std::tuple HesaiDriver::ParseCloudPacket( + const std::vector & packet) { std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); @@ -83,26 +84,23 @@ std::tuple HesaiDriver::ConvertScanToPoint return pointcloud; } - int cnt = 0, last_azimuth = 0; - for (auto & packet : pandar_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); - cnt++; - } + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); } - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); - } + // todo + // if (cnt == 0) { + // RCLCPP_ERROR_STREAM( + // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " + // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // } return pointcloud; } Status HesaiDriver::SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) + const HesaiCalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( "SetCalibrationConfiguration. Not yet implemented (" + diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index a7681323c..b8bbe345c 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -15,18 +15,20 @@ #ifndef NEBULA_HesaiRosOfflineExtractBag_H #define NEBULA_HesaiRosOfflineExtractBag_H -#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 "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include +#include +#include #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include #include #include @@ -35,7 +37,7 @@ namespace nebula { namespace ros { -class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractBag final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -46,12 +48,7 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrap Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); Status GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp index 711502e6a..4551bd848 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp @@ -15,18 +15,18 @@ #ifndef NEBULA_HesaiRosOfflineExtractSample_H #define NEBULA_HesaiRosOfflineExtractSample_H -#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 "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include +#include +#include #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include #include #include @@ -36,7 +36,7 @@ namespace nebula namespace ros { /// @brief Offline hesai driver usage example (Output PCD data) -class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractSample final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -51,17 +51,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosW /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/package.xml b/nebula_examples/package.xml index 6f1163279..f63847cca 100644 --- a/nebula_examples/package.xml +++ b/nebula_examples/package.xml @@ -28,8 +28,8 @@ velodyne_msgs yaml-cpp + ament_cmake_gtest ament_lint_auto - ament_lint_common ament_cmake diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 9c1266347..649085018 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 @@ -56,12 +56,11 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,171 +68,56 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( Status HesaiRosOfflineExtractBag::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractBag::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) +Status HesaiRosOfflineExtractBag::GetStatus() { - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); + return wrapper_status_; } -Status HesaiRosOfflineExtractBag::GetStatus() {return wrapper_status_;} - Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + 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; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); 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", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + 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(); } + + calibration_configuration.calibration_file = + this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_num", 3, descriptor); - out_num = this->get_parameter("out_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("skip_num", 3, descriptor); - skip_num = this->get_parameter("skip_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 1; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("only_xyz", false, descriptor); - only_xyz = this->get_parameter("only_xyz").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = + this->declare_parameter("correction_file", "", param_read_only()); } + bag_path = this->declare_parameter("bag_path", "", param_read_only()); + storage_id = this->declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = this->declare_parameter("out_path", "", param_read_only()); + format = this->declare_parameter("format", "cdr", param_read_only()); + out_num = this->declare_parameter("out_num", 3, param_read_only()); + skip_num = this->declare_parameter("skip_num", 3, param_read_only()); + only_xyz = this->declare_parameter("only_xyz", false, param_read_only()); + target_topic = this->declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } @@ -296,75 +180,83 @@ Status HesaiRosOfflineExtractBag::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; - pcl::PCDWriter writer; + pcl::PCDWriter pcd_writer; - std::unique_ptr writer_; - bool needs_open = true; + std::unique_ptr bag_writer{}; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - int cnt = 0; - int out_cnt = 0; - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - - if (needs_open) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( - {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), - ""}); - needs_open = false; - } - writer_->write(bag_message); - cnt++; - if (skip_num < cnt) { - out_cnt++; - if (only_xyz) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); - } - // writer.writeASCII((o_dir / fn).string(), *pointcloud); - } - if (out_num <= out_cnt) { - break; + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + int cnt = 0; + int out_cnt = 0; + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + nebula_msgs::msg::NebulaPackets nebula_msg; + nebula_msg.header = extracted_msg.header; + for (auto & pkt : extracted_msg.packets) { + std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(pkt_data); + auto pointcloud = std::get<0>(pointcloud_ts); + + nebula_msgs::msg::NebulaPacket nebula_pkt; + nebula_pkt.stamp = pkt.stamp; + nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` + nebula_msg.packets.push_back(nebula_pkt); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + if (!bag_writer) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + bag_writer = std::make_unique(); + bag_writer->open(storage_options_w, converter_options_w); + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), + ""}); + } + + bag_writer->write(bag_message); + cnt++; + if (skip_num < cnt) { + out_cnt++; + if (only_xyz) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); } } + + if (out_num <= out_cnt) { + break; + } } - // close on scope exit } return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp index f0eed5055..db6a66285 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp @@ -36,7 +36,6 @@ int main(int argc, char * argv[]) if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); driver_status = hesai_driver->ReadBag(); - // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index a57d9b96e..720236446 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -56,12 +56,11 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,144 +68,53 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( Status HesaiRosOfflineExtractSample::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractSample::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) +Status HesaiRosOfflineExtractSample::GetStatus() { - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); + return wrapper_status_; } -Status HesaiRosOfflineExtractSample::GetStatus() {return wrapper_status_;} - Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + sensor_configuration.frame_id = + declare_parameter("frame_id", "pandar", param_read_only()); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); 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", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + descriptor.floating_point_range = float_range(0, 360, 0.01); + sensor_configuration.scan_phase = + declare_parameter("scan_phase", 0., param_read_only()); } + + calibration_configuration.calibration_file = + declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = declare_parameter("correction_file", "", param_read_only()); } + bag_path = declare_parameter("bag_path", "", param_read_only()); + storage_id = declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = declare_parameter("out_path", "", param_read_only()); + format = declare_parameter("format", "cdr", param_read_only()); + target_topic = declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } @@ -266,46 +174,46 @@ Status HesaiRosOfflineExtractSample::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; pcl::PCDWriter writer; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - // nebula::drivers::NebulaPointCloudPtr pointcloud = - // driver_ptr_->ConvertScanToPointcloud( - // std::make_shared(extracted_msg)); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - writer.writeBinary((o_dir / fn).string(), *pointcloud); + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + for (auto & pkt : extracted_msg.packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + + if (!pointcloud) { + continue; } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + writer.writeBinary((o_dir / fn).string(), *pointcloud); } - // close on scope exit } + return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp index b504605e6..a4839dac0 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp @@ -37,7 +37,6 @@ int main(int argc, char * argv[]) if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); driver_status = hesai_driver->ReadBag(); - // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } 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 a6d78c45e..13ad0ed53 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 @@ -10,7 +10,7 @@ #include #include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -100,7 +100,7 @@ struct HesaiPtpDiagTime os << ", "; os << "gmTimeBaseIndicator: " << arg.gmTimeBaseIndicator; os << ", "; - //FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect + // FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect os << "lastGmPhaseChange: " << std::string(std::begin(arg.lastGmPhaseChange), std::end(arg.lastGmPhaseChange)); os << ", "; @@ -158,7 +158,9 @@ struct HesaiInventory os << "sn: " << std::string(arg.sn, strnlen(arg.sn, sizeof(arg.sn))); os << ", "; os << "date_of_manufacture: " - << std::string(arg.date_of_manufacture, strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); + << std::string( + arg.date_of_manufacture, + strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); os << ", "; os << "mac: "; @@ -256,7 +258,7 @@ struct HesaiConfig uint8_t motor_status; uint8_t vlan_flag; big_uint16_buf_t vlan_id; - uint8_t clock_data_fmt; //FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets + uint8_t clock_data_fmt; // FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets uint8_t noise_filtering; uint8_t reflectivity_mapping; uint8_t reserved[6]; @@ -266,29 +268,17 @@ struct HesaiConfig std::ios initial_format(nullptr); initial_format.copyfmt(os); - os << "ipaddr: " - << +arg.ipaddr[0] << "." - << +arg.ipaddr[1] << "." - << +arg.ipaddr[2] << "." + os << "ipaddr: " << +arg.ipaddr[0] << "." << +arg.ipaddr[1] << "." << +arg.ipaddr[2] << "." << +arg.ipaddr[3]; os << ", "; - os << "mask: " - << +arg.mask[0] << "." - << +arg.mask[1] << "." - << +arg.mask[2] << "." + os << "mask: " << +arg.mask[0] << "." << +arg.mask[1] << "." << +arg.mask[2] << "." << +arg.mask[3]; os << ", "; - os << "gateway: " - << +arg.gateway[0] << "." - << +arg.gateway[1] << "." - << +arg.gateway[2] << "." + os << "gateway: " << +arg.gateway[0] << "." << +arg.gateway[1] << "." << +arg.gateway[2] << "." << +arg.gateway[3]; os << ", "; - os << "dest_ipaddr: " - << +arg.dest_ipaddr[0] << "." - << +arg.dest_ipaddr[1] << "." - << +arg.dest_ipaddr[2] << "." - << +arg.dest_ipaddr[3]; + os << "dest_ipaddr: " << +arg.dest_ipaddr[0] << "." << +arg.dest_ipaddr[1] << "." + << +arg.dest_ipaddr[2] << "." << +arg.dest_ipaddr[3]; os << ", "; os << "dest_LiDAR_udp_port: " << arg.dest_LiDAR_udp_port; os << ", "; @@ -351,7 +341,7 @@ struct HesaiLidarStatus big_uint32_buf_t startup_times; big_uint32_buf_t total_operation_time; uint8_t ptp_clock_status; - uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet + uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarStatus const & arg) { @@ -463,7 +453,7 @@ struct HesaiPtpConfig int8_t logAnnounceInterval; int8_t logSyncInterval; int8_t logMinDelayReqInterval; - //FIXME: this format is not correct for OT128, or for AT128 on 802.1AS + // FIXME: this format is not correct for OT128, or for AT128 on 802.1AS friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpConfig const & arg) { @@ -489,7 +479,7 @@ struct HesaiPtpConfig /// @brief struct of PTC_COMMAND_LIDAR_MONITOR struct HesaiLidarMonitor { - //FIXME: this format is not correct for OT128 + // FIXME: this format is not correct for OT128 big_int32_buf_t input_voltage; big_int32_buf_t input_current; big_int32_buf_t input_power; 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 1b21ddae2..10a1ec93f 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 @@ -30,6 +30,7 @@ #include #include +#include #include namespace nebula @@ -103,7 +104,7 @@ const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; /// @brief Hardware interface of hesai driver -class HesaiHwInterface : NebulaHwInterfaceBase +class HesaiHwInterface { private: struct ptc_error_t @@ -120,15 +121,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::shared_ptr m_owned_ctx; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; - std::shared_ptr sensor_configuration_; - std::shared_ptr calibration_configuration_; - size_t azimuth_index_{}; - size_t mtu_size_{}; - std::unique_ptr scan_cloud_ptr_; - std::function - is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function buffer)> - scan_reception_callback_; /**This function pointer is called when the scan is complete*/ + std::shared_ptr sensor_configuration_; + std::function & buffer)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ + + std::mutex mtx_inflight_tcp_request_; int prev_phase_{}; @@ -172,6 +169,12 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return A string description of all errors in this code std::string PrettyPrintPTCError(ptc_error_t error_code); + /// @brief Checks if the data size matches that of the struct to be parsed, and parses the struct. + /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is + /// printed and the struct is parsed with the first sizeof(T) bytes. + template + T CheckSizeAndParse(const std::vector & data); + /// @brief Send a PTC request with an optional payload, and return the full response payload. /// Blocking. /// @param command_id PTC command number. @@ -198,17 +201,17 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status @@ -217,12 +220,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); @@ -388,13 +390,13 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param hesai_config Current HesaiConfig /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config); + std::shared_ptr sensor_configuration, HesaiConfig hesai_config); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_lidar_range_all Current HesaiLidarRangeAll /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all); /// @brief Checking the current settings and changing the difference point /// @return Resulting status 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 fccb2a622..29b260098 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 @@ -17,40 +17,19 @@ HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - scan_cloud_ptr_{std::make_unique()} + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} { } HesaiHwInterface::~HesaiHwInterface() { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif - if (tcp_driver_) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................tcp_driver_ is available" << std::endl; -#endif - if (tcp_driver_ && tcp_driver_->isOpen()) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: tcp_driver_->close();" << std::endl; -#endif - tcp_driver_->close(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: tcp_driver_->close();" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: if(tcp_driver_)" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif + FinalizeTcpDriver(); } HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( const uint8_t command_id, const std::vector & payload) { + std::lock_guard lock(mtx_inflight_tcp_request_); + uint32_t len = payload.size(); std::vector send_buf; @@ -64,15 +43,16 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( send_buf.emplace_back(len & 0xff); send_buf.insert(send_buf.end(), payload.begin(), payload.end()); - // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can access valid memory + // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can + // access valid memory auto recv_buf = std::make_shared>(); auto response_complete = std::make_shared(false); - // Low byte is for PTC error code, the rest is nebula-specific auto error_code = std::make_shared(); std::stringstream ss; - ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) + << " (" << len << ") "; std::string log_tag = ss.str(); PrintDebug(log_tag + "Entering lock"); @@ -88,21 +68,26 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( PrintDebug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, - [this, log_tag, command_id, response_complete, error_code](const std::vector & header_bytes) { + [this, log_tag, command_id, response_complete, + error_code](const std::vector & header_bytes) { error_code->ptc_error_code = header_bytes[3]; - size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); - // If command_id in the response does not match, we got a response for another command (or rubbish), probably as a - // result of too many simultaneous TCP connections to the sensor (e.g. from GUI, Web UI, another nebula instance, etc.) + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | + (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug( + log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + // If command_id in the response does not match, we got a response for another command (or + // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. + // from GUI, Web UI, another nebula instance, etc.) if (header_bytes[2] != command_id) { error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; } - if (payload_len == 0) { - *response_complete = true; + if (payload_len == 0) { + *response_complete = true; } }, - [this, log_tag, recv_buf, response_complete, error_code](const std::vector & payload_bytes) { + [this, log_tag, recv_buf, response_complete, + error_code](const std::vector & payload_bytes) { PrintDebug(log_tag + "Received payload"); // Header had payload length 0 (thus, header callback processed request successfully already), @@ -118,7 +103,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( }, [this, log_tag, &tm]() { PrintDebug(log_tag + "Unlocking mutex"); - tm.unlock(); + tm.unlock(); PrintDebug(log_tag + "Unlocked mutex"); }); this->IOContextRun(); @@ -144,65 +129,10 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( } Status HesaiHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - HesaiStatus status = Status::OK; - mtu_size_ = MTU_SIZE; - is_solid_state = false; - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - if ( - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P) { - azimuth_index_ = 2; - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR40_PACKET_SIZE || packet_size == PANDAR40P_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT64) { - azimuth_index_ = 12; // 12 + 258 * [0-3] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARQT64_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT128) { - azimuth_index_ = 12; // 12 + 514 * [0-1] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARQT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARXT32_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32M) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARXT32M_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { - azimuth_index_ = 12; // 12 + 4 * 128 * [0-1] - is_solid_state = true; - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARAT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { - azimuth_index_ = 8; // 8 + 192 * [0-5] - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR64_PACKET_SIZE || packet_size == PANDAR64_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { - azimuth_index_ = 12; // 12 - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR128_E4X_EXTENDED_PACKET_SIZE || - packet_size == PANDAR128_E4X_PACKET_SIZE); - }; - } else { - status = Status::INVALID_SENSOR_MODEL; - } - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); return Status::OK; } @@ -239,68 +169,23 @@ Status HesaiHwInterface::SensorInterfaceStart() } Status HesaiHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function &)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); - if (!is_valid_packet_(buffer.size())) { - PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); - return; - } - const uint32_t buffer_size = buffer.size(); - pandar_msgs::msg::PandarPacket pandar_packet; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, pandar_packet.data.begin()); - pandar_packet.size = buffer_size; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - pandar_packet.stamp.sec = static_cast(now_secs); - pandar_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); - scan_cloud_ptr_->packets.emplace_back(pandar_packet); - - int current_phase = 0; - bool comp_flg = false; - - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_] & 0xff) + ((data[azimuth_index_ + 1] & 0xff) << 8); - if (is_solid_state) { - current_phase = (static_cast(current_phase) + 36000 - 0) % 12000; - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } else { - current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; - - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } - - if (comp_flg) { // Scan complete - if (scan_reception_callback_) { - scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; - // Callback - scan_reception_callback_(std::move(scan_cloud_ptr_)); - scan_cloud_ptr_ = std::make_unique(); - } - } + cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +Status HesaiHwInterface::GetSensorConfiguration( + const SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; @@ -344,7 +229,9 @@ Status HesaiHwInterface::InitializeTcpDriver() Status HesaiHwInterface::FinalizeTcpDriver() { try { - tcp_driver_->close(); + if (tcp_driver_) { + tcp_driver_->close(); + } } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; @@ -372,7 +259,8 @@ std::vector HesaiHwInterface::GetLidarCalibrationBytes() std::string HesaiHwInterface::GetLidarCalibrationString() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - auto calib_data = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto calib_data = + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); std::string calib_string(calib_data.begin(), calib_data.end()); return calib_string; } @@ -381,106 +269,67 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagStatus)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiPtpDiagStatus hesai_ptp_diag_status; - memcpy(&hesai_ptp_diag_status, response.data(), sizeof(HesaiPtpDiagStatus)); + auto diag_status = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << diag_status; PrintInfo(ss.str()); - return hesai_ptp_diag_status; + return diag_status; } HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagPort)) { - throw std::runtime_error("Unexpected payload size"); - } - HesaiPtpDiagPort hesai_ptp_diag_port; - memcpy(&hesai_ptp_diag_port, response.data(), sizeof(HesaiPtpDiagPort)); + auto diag_port = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; + ss << "HesaiHwInterface::GetPtpDiagPort: " << diag_port; PrintInfo(ss.str()); - return hesai_ptp_diag_port; + return diag_port; } HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagTime)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiPtpDiagTime hesai_ptp_diag_time; - memcpy(&hesai_ptp_diag_time, response.data(), sizeof(HesaiPtpDiagTime)); + auto diag_time = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; + ss << "HesaiHwInterface::GetPtpDiagTime: " << diag_time; PrintInfo(ss.str()); - return hesai_ptp_diag_time; + return diag_time; } HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); + auto response_or_err = + SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagGrandmaster)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - memcpy(&hesai_ptp_diag_grandmaster, response.data(), sizeof(HesaiPtpDiagGrandmaster)); + auto diag_grandmaster = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << diag_grandmaster; PrintInfo(ss.str()); - return hesai_ptp_diag_grandmaster; + return diag_grandmaster; } HesaiInventory HesaiHwInterface::GetInventory() { auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiInventory)) { - throw std::runtime_error("Unexpected payload size"); - } else if (response.size() > sizeof(HesaiInventory)) { - PrintError("HesaiInventory from Sensor has unknown format. Will parse anyway."); - } - - HesaiInventory hesai_inventory; - memcpy(&hesai_inventory, response.data(), sizeof(HesaiInventory)); - - return hesai_inventory; + return CheckSizeAndParse(response); } HesaiConfig HesaiHwInterface::GetConfig() { auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiConfig)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiConfig hesai_config; - memcpy(&hesai_config, response.data(), sizeof(HesaiConfig)); - + auto hesai_config = CheckSizeAndParse(response); std::cout << "Config: " << hesai_config << std::endl; return hesai_config; } @@ -489,15 +338,7 @@ HesaiLidarStatus HesaiHwInterface::GetLidarStatus() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiLidarStatus)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiLidarStatus hesai_status; - memcpy(&hesai_status, response.data(), sizeof(HesaiLidarStatus)); - - return hesai_status; + return CheckSizeAndParse(response); } Status HesaiHwInterface::SetSpinRate(uint16_t rpm) @@ -645,15 +486,15 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() if (response.size() != 5) { throw std::runtime_error("Unexpected response payload"); } - + memcpy(&hesai_range_all.start, &response[1], 2); memcpy(&hesai_range_all.end, &response[3], 2); break; case 1: // for each channel - //TODO + // TODO(yukkysaito) break; case 2: // multi-section FOV - //TODO + // TODO(yukkysaito) break; } @@ -710,7 +551,7 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); if (response.size() < sizeof(HesaiPtpConfig)) { - throw std::runtime_error("Unexpected payload size"); + throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); } else if (response.size() > sizeof(HesaiPtpConfig)) { PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } @@ -749,15 +590,7 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiLidarMonitor)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiLidarMonitor hesai_lidar_monitor; - memcpy(&hesai_lidar_monitor, response.data(), sizeof(HesaiLidarMonitor)); - - return hesai_lidar_monitor; + return CheckSizeAndParse(response); } void HesaiHwInterface::IOContextRun() @@ -931,9 +764,9 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config) + std::shared_ptr sensor_configuration, HesaiConfig hesai_config) { - using namespace std::chrono_literals; + using namespace std::chrono_literals; // NOLINT(build/namespaces) #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif @@ -965,7 +798,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_rotation_speed = hesai_config.spin_rate; if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { - PrintInfo("current lidar rotation_speed: " + std::to_string(static_cast(current_rotation_speed.value()))); + PrintInfo( + "current lidar rotation_speed: " + + std::to_string(static_cast(current_rotation_speed.value()))); PrintInfo( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); @@ -983,8 +818,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( bool set_flg = false; std::stringstream ss; - ss << static_cast(hesai_config.dest_ipaddr[0]) << "." << static_cast(hesai_config.dest_ipaddr[1]) << "." - << static_cast(hesai_config.dest_ipaddr[2]) << "." << static_cast(hesai_config.dest_ipaddr[3]); + ss << static_cast(hesai_config.dest_ipaddr[0]) << "." + << static_cast(hesai_config.dest_ipaddr[1]) << "." + << static_cast(hesai_config.dest_ipaddr[2]) << "." + << static_cast(hesai_config.dest_ipaddr[3]); auto current_host_addr = ss.str(); if (sensor_configuration->host_ip != current_host_addr) { set_flg = true; @@ -995,7 +832,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_host_dport = hesai_config.dest_LiDAR_udp_port; if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo("current lidar dest_LiDAR_udp_port: " + std::to_string(static_cast(current_host_dport.value()))); + PrintInfo( + "current lidar dest_LiDAR_udp_port: " + + std::to_string(static_cast(current_host_dport.value()))); PrintInfo( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } @@ -1003,7 +842,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_host_tport = hesai_config.dest_gps_udp_port; if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo("current lidar dest_gps_udp_port: " + std::to_string(static_cast(current_host_tport.value()))); + PrintInfo( + "current lidar dest_gps_udp_port: " + + std::to_string(static_cast(current_host_tport.value()))); PrintInfo( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } @@ -1090,7 +931,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1107,18 +948,26 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( set_flg = true; } else { auto current_cloud_min_angle = hesai_lidar_range_all.start; - if (static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle.value()) { + if ( + static_cast(sensor_configuration->cloud_min_angle * 10) != + current_cloud_min_angle.value()) { set_flg = true; - PrintInfo("current lidar range.start: " + std::to_string(static_cast(current_cloud_min_angle.value()))); + PrintInfo( + "current lidar range.start: " + + std::to_string(static_cast(current_cloud_min_angle.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; - if (static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle.value()) { + if ( + static_cast(sensor_configuration->cloud_max_angle * 10) != + current_cloud_max_angle.value()) { set_flg = true; - PrintInfo("current lidar range.end: " + std::to_string(static_cast(current_cloud_max_angle.value()))); + PrintInfo( + "current lidar range.end: " + + std::to_string(static_cast(current_cloud_max_angle.value()))); PrintInfo( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); @@ -1154,7 +1003,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t.join(); @@ -1164,7 +1013,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1358,7 +1207,8 @@ void HesaiHwInterface::PrintDebug(const std::vector & bytes) PrintDebug(ss.str()); } -std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { +std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) +{ if (error_code.ok()) { return "No error"; } @@ -1368,10 +1218,11 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { std::stringstream ss; if (ptc_error) { - ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(ptc_error) << ' '; + ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex + << static_cast(ptc_error) << ' '; } - switch(ptc_error) { + switch (ptc_error) { case PTC_ERROR_CODE_NO_ERROR: break; case PTC_ERROR_CODE_INVALID_INPUT_PARAM: @@ -1429,5 +1280,19 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { return ss.str(); } +template +T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) +{ + if (data.size() < sizeof(T)) { + throw std::runtime_error("Attempted to parse too-small payload"); + } else if (data.size() > sizeof(T)) { + PrintError("Sensor returned longer payload than expected. Will parse anyway."); + } + + T parsed; + memcpy(&parsed, data.data(), sizeof(T)); + return parsed; +} + } // namespace drivers } // namespace nebula diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index dd81e1ac0..324d38074 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -23,6 +23,8 @@ find_package(nebula_decoders REQUIRED) find_package(nebula_hw_interfaces REQUIRED) find_package(yaml-cpp REQUIRED) find_package(robosense_msgs REQUIRED) +find_package(nebula_msgs REQUIRED) +find_package(pandar_msgs REQUIRED) include_directories( include @@ -33,34 +35,17 @@ include_directories( ) link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) ## Hesai -# Hw Interface -ament_auto_add_library(hesai_hw_ros_wrapper SHARED - src/hesai/hesai_hw_interface_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_ros_wrapper - PLUGIN "HesaiHwInterfaceRosWrapper" - EXECUTABLE hesai_hw_interface_ros_wrapper_node - ) - -# Monitor -ament_auto_add_library(hesai_hw_monitor_ros_wrapper SHARED - src/hesai/hesai_hw_monitor_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_monitor_ros_wrapper - PLUGIN "HesaiHwMonitorRosWrapper" - EXECUTABLE hesai_hw_monitor_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(hesai_driver_ros_wrapper SHARED - src/hesai/hesai_decoder_ros_wrapper.cpp +ament_auto_add_library(hesai_ros_wrapper SHARED + src/hesai/hesai_ros_wrapper.cpp + src/hesai/decoder_wrapper.cpp + src/hesai/hw_interface_wrapper.cpp + src/hesai/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp ) -rclcpp_components_register_node(hesai_driver_ros_wrapper - PLUGIN "HesaiDriverRosWrapper" - EXECUTABLE hesai_driver_ros_wrapper_node +rclcpp_components_register_node(hesai_ros_wrapper + PLUGIN "HesaiRosWrapper" + EXECUTABLE hesai_ros_wrapper_node ) ## Velodyne diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp new file mode 100644 index 000000000..aca385ce3 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include + +template +class mt_queue +{ +private: + std::mutex mutex_; + std::condition_variable cv_not_empty_, cv_not_full_; + std::deque queue_; + size_t capacity_; + +public: + explicit mt_queue(size_t capacity) : capacity_(capacity) {} + + bool try_push(T && value) + { + std::unique_lock lock(this->mutex_); + bool can_push = queue_.size() < capacity_; + if (can_push) { + queue_.push_front(std::move(value)); + } + this->cv_not_empty_.notify_all(); + return can_push; + } + + void push(T && value) + { + std::unique_lock lock(this->mutex_); + this->cv_not_full_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); + queue_.push_front(std::move(value)); + this->cv_not_empty_.notify_all(); + } + + T pop() + { + std::unique_lock lock(this->mutex_); + this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); + T return_value(std::move(this->queue_.back())); + this->queue_.pop_back(); + this->cv_not_full_.notify_all(); + + return return_value; + } +}; diff --git a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp new file mode 100644 index 000000000..6d4e38504 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp new file mode 100644 index 000000000..e6320f91e --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +class WatchdogTimer +{ + using watchdog_cb_t = std::function; + +public: + WatchdogTimer( + rclcpp::Node & node, const std::chrono::microseconds & expected_update_interval, + const watchdog_cb_t & callback) + : node_(node), + callback_(callback), + expected_update_interval_ns_( + std::chrono::duration_cast(expected_update_interval).count()) + { + timer_ = + node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); + } + + void update() { last_update_ns_ = node_.get_clock()->now().nanoseconds(); } + +private: + void onTimer() + { + uint64_t now_ns = node_.get_clock()->now().nanoseconds(); + + bool is_late = (now_ns - last_update_ns_) > expected_update_interval_ns_; + + callback_(!is_late); + } + + rclcpp::Node & node_; + rclcpp::TimerBase::SharedPtr timer_; + std::atomic last_update_ns_; + const watchdog_cb_t callback_; + + const uint64_t expected_update_interval_ns_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp new file mode 100644 index 000000000..b2bdf856c --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -0,0 +1,98 @@ +#pragma once + +#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 + +namespace nebula +{ +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); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr & new_config); + + void OnCalibrationChange( + 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); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + 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_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; + + std::shared_ptr cloud_watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp deleted file mode 100644 index 52518fcd0..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef NEBULA_HesaiDriverRosWrapper_H -#define NEBULA_HesaiDriverRosWrapper_H - -#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 "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of hesai driver -class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - rclcpp::Subscription::SharedPtr pandar_scan_sub_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; - rclcpp::Publisher::SharedPtr aw_points_ex_pub_; - rclcpp::Publisher::SharedPtr aw_points_base_pub_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - drivers::HesaiHwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - - /*** - * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher - * @param pointcloud unique pointer containing the point cloud to publish - * @param publisher - */ - void PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher); - -public: - explicit HesaiDriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for PandarScan subscriber - /// @param scan_msg Received PandarScan - void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - -private: - /// @brief File path of Correction data (Only required only for AT) - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 1802f87b2..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef NEBULA_HesaiHwInterfaceRosWrapper_H -#define NEBULA_HesaiHwInterfaceRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of hesai driver -class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Received Hesai message publisher - rclcpp::Publisher::SharedPtr pandar_scan_pub_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving PandarScan - /// @param scan_buffer Received PandarScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); - -public: - explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwInterfaceRosWrapper() noexcept override; - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - uint16_t delay_hw_ms_; - bool retry_hw_; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp deleted file mode 100644 index cf8739279..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ /dev/null @@ -1,166 +0,0 @@ -#ifndef NEBULA_HesaiHwMonitorRosWrapper_H -#define NEBULA_HesaiHwMonitorRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware monitor ros wrapper of hesai driver -class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; - -public: - explicit HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwMonitorRosWrapper() noexcept override; - /// @brief Not used - /// @return Current status - Status MonitorStart() override; - /// @brief Not used - /// @return Status::OK - Status MonitorStop() override; - /// @brief Not used - /// @return Status::OK - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - diagnostic_updater::Updater diagnostics_updater_; - /// @brief Initializing diagnostics - void InitializeHesaiDiagnostics(); - /// @brief Callback of the timer for getting the current lidar status - void OnHesaiStatusTimer(); - /// @brief Callback of the timer for getting the current lidar monitor via http - void OnHesaiLidarMonitorTimerHttp(); - /// @brief Callback of the timer for getting the current lidar monitor via tcp - void OnHesaiLidarMonitorTimer(); - // void OnHesaiDiagnosticsTimer(); - // void OnHesaiStatusTimer(); - - /// @brief Check status information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check ptp information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check temperature information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check rpm information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via http - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via tcp - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - - rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; - rclcpp::TimerBase::SharedPtr diagnostics_update_monitor_timer_; - rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_; - std::unique_ptr current_status; - std::unique_ptr current_monitor; - std::unique_ptr current_config; - std::unique_ptr current_inventory; - std::unique_ptr current_lidar_monitor_tree; - std::unique_ptr current_status_time; - std::unique_ptr current_config_time; - std::unique_ptr current_inventory_time; - std::unique_ptr current_lidar_monitor_time; - uint8_t current_diag_status; - uint8_t current_monitor_status; - - uint16_t diag_span_; - uint16_t delay_monitor_ms_; - std::mutex mtx_diag; - std::mutex mtx_status; - std::mutex mtx_lidar_monitor; - // std::timed_mutex mtx_lidar_monitor; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Get value from property_tree - /// @param pt property_tree - /// @param key Pey string - /// @return Value - std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); - /// @brief Making fixed precision string - /// @param val Target value - /// @param pre Precision - /// @return Created string - std::string GetFixedPrecisionString(double val, int pre = 2); - - std::string info_model; - std::string info_serial; - rclcpp::CallbackGroup::SharedPtr cbg_r_; - rclcpp::CallbackGroup::SharedPtr cbg_m_; - rclcpp::CallbackGroup::SharedPtr cbg_m2_; - - const char * not_supported_message; - const char * error_message; - std::string message_sep; - - std::vector temperature_names; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwMonitorRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp new file mode 100644 index 000000000..0a60d3e8e --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -0,0 +1,91 @@ +#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" + +#include +#include +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of hesai driver +class HesaiRosWrapper final : public rclcpp::Node +{ +public: + explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); + ~HesaiRosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + void ReceiveCloudPacketCallback(std::vector & packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & new_config); + + Status wrapper_status_; + + std::shared_ptr sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_; + + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp new file mode 100644 index 000000000..0791af5e2 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwInterfaceWrapper +{ +public: + HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & new_config); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + bool setup_sensor_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..0b270623f --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwMonitorWrapper +{ +public: + HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & /* new_config */) + { + } + + nebula::Status Status(); + +private: + void InitializeHesaiDiagnostics(); + + std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); + + std::string GetFixedPrecisionString(double val, int pre); + + void OnHesaiStatusTimer(); + + void OnHesaiLidarMonitorTimerHttp(); + + void OnHesaiLidarMonitorTimer(); + + void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + rclcpp::Logger logger_; + diagnostic_updater::Updater diagnostics_updater_; + nebula::Status status_; + + const std::shared_ptr hw_interface_; + rclcpp::Node * const parent_node_; + + uint16_t diag_span_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; + rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; + + std::unique_ptr current_status_{}; + std::unique_ptr current_monitor_{}; + std::unique_ptr current_config_{}; + std::unique_ptr current_inventory_{}; + std::unique_ptr current_lidar_monitor_tree_{}; + + std::unique_ptr current_status_time_{}; + std::unique_ptr current_config_time_{}; + std::unique_ptr current_inventory_time_{}; + std::unique_ptr current_lidar_monitor_time_{}; + + uint8_t current_diag_status_; + uint8_t current_monitor_status_; + + std::mutex mtx_lidar_status_; + std::mutex mtx_lidar_monitor_; + + std::string info_model_; + std::string info_serial_; + + std::vector temperature_names_; + + bool setup_sensor; + const std::string MSG_NOT_SUPPORTED = "Not supported"; + const std::string MSG_ERROR = "Error"; + const std::string MSG_SEP = ": "; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index f4a1cd41c..10c7aa218 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,34 +27,27 @@ - - - - - - - - - - - - - - + + + + - + + + + @@ -64,33 +57,13 @@ - - - - - - - - - - - - - - - - - - - - - + + diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml deleted file mode 100644 index 75c7414da..000000000 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ /dev/null @@ -1,115 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index b394498fb..6eee53443 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -26,6 +26,7 @@ def get_sensor_make(sensor_name): return "Continental", None return "unrecognized_sensor_model", None + def get_plugin_name(sensor_make, sensor_model): if sensor_make.lower() != "continental": return sensor_make @@ -34,15 +35,22 @@ def get_plugin_name(sensor_make, sensor_model): else: return "invalid_plugin" + def is_hw_monitor_available(sensor_make): return sensor_make.lower() != "continental" + def launch_setup(context, *args, **kwargs): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) calibration_file = LaunchConfiguration("calibration_file").perform(context) - correction_file = LaunchConfiguration("correction_file").perform(context) sensor_make, sensor_extension = get_sensor_make(sensor_model) + + if sensor_make.lower() == "hesai": + raise ValueError( + "\n `nebula_launch.py` is deprecated. For Hesai sensors, use `hesai_launch_all_hw.xml` instead." + ) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") nebula_ros_share_dir = get_package_share_directory("nebula_ros") @@ -50,23 +58,36 @@ def launch_setup(context, *args, **kwargs): sensor_params_fp = LaunchConfiguration("config_file").perform(context) if sensor_params_fp == "": warnings.warn("No config file provided, using sensor model default", RuntimeWarning) - sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml") + sensor_params_fp = os.path.join( + nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml" + ) if not os.path.exists(sensor_params_fp): sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", "BaseParams.yaml") - assert os.path.exists(sensor_params_fp), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) + assert os.path.exists( + sensor_params_fp + ), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) sensor_calib_fp = sensor_corr_fp = "" if sensor_extension is not None: # Velodyne and Hesai - sensor_calib_fp = os.path.join(nebula_decoders_share_dir, "calibration", sensor_make.lower(), sensor_model + sensor_extension) - assert os.path.exists(sensor_calib_fp), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) if sensor_model.lower() == "pandarat128": sensor_corr_fp = os.path.splitext(sensor_calib_fp)[0] + ".dat" - assert os.path.exists(sensor_corr_fp), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) + assert os.path.exists( + sensor_corr_fp + ), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) with open(sensor_params_fp, "r") as f: - sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] + sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] nodes = [] launch_hw = LaunchConfiguration("launch_hw").perform(context) == "true" @@ -75,8 +96,8 @@ def launch_setup(context, *args, **kwargs): # HwInterface ComposableNode( package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"HwInterfaceRosWrapper", - name=sensor_make.lower()+"_hw_interface_ros_wrapper_node", + plugin=get_plugin_name(sensor_make, sensor_model) + "HwInterfaceRosWrapper", + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", parameters=[ sensor_params, { @@ -84,7 +105,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, "setup_sensor": LaunchConfiguration("setup_sensor"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), @@ -95,14 +115,13 @@ def launch_setup(context, *args, **kwargs): ), ) - if launch_hw and is_hw_monitor_available(sensor_make): nodes.append( # HwMonitor ComposableNode( package="nebula_ros", - plugin=sensor_make+"HwMonitorRosWrapper", - name=sensor_make.lower()+"_hw_monitor_ros_wrapper_node", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", parameters=[ sensor_params, { @@ -110,7 +129,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, }, ], ) @@ -118,8 +136,8 @@ def launch_setup(context, *args, **kwargs): nodes.append( ComposableNode( package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"DriverRosWrapper", - name=sensor_make.lower()+"_driver_ros_wrapper_node", + plugin=get_plugin_name(sensor_make, sensor_model) + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", parameters=[ sensor_params, { @@ -127,7 +145,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, "launch_hw": LaunchConfiguration("launch_hw"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), @@ -146,7 +163,7 @@ def launch_setup(context, *args, **kwargs): container_kwargs = {} if LaunchConfiguration("debug_logging").perform(context) == "true": - container_kwargs["ros_arguments"] = ['--log-level', 'debug'] + container_kwargs["ros_arguments"] = ["--log-level", "debug"] container = ComposableNodeContainer( name="nebula_ros_node", diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 87bc000c9..a0bdec51f 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -21,6 +21,8 @@ nebula_common nebula_decoders nebula_hw_interfaces + nebula_msgs + pandar_msgs pcl_conversions rclcpp rclcpp_components diff --git a/nebula_ros/src/common/parameter_descriptors.cpp b/nebula_ros/src/common/parameter_descriptors.cpp new file mode 100644 index 000000000..b62cb8a9e --- /dev/null +++ b/nebula_ros/src/common/parameter_descriptors.cpp @@ -0,0 +1,34 @@ +#include "nebula_ros/common/parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp new file mode 100644 index 000000000..6da62b273 --- /dev/null +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -0,0 +1,316 @@ +#include "nebula_ros/hesai/decoder_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +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) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("HesaiDecoder")), + hw_interface_(hw_interface), + sensor_cfg_(config) +{ + if (!config) { + 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()); + } + + calibration_cfg_ptr_ = calibration_result.value(); + RCLCPP_INFO_STREAM( + logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); + + RCLCPP_INFO(logger_, "Starting Decoder"); + + driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_) { + current_scan_msg_ = std::make_unique(); + packets_pub_ = parent_node->create_publisher( + "pandar_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = + parent_node->create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node->create_publisher("aw_points_ex", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); +} + +void HesaiDecoderWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +void HesaiDecoderWrapper::OnCalibrationChange( + const std::shared_ptr & new_calibration) +{ + std::lock_guard lock(mtx_driver_ptr_); + 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( + std::unique_ptr packet_msg) +{ + // 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)) { + if (current_scan_msg_->packets.size() == 0) { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + pandar_msgs::msg::PandarPacket pandar_packet_msg{}; + pandar_packet_msg.stamp = packet_msg->stamp; + pandar_packet_msg.size = packet_msg->data.size(); + std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); + } + + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud = std::get<0>(pointcloud_ts); + } + + // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th + // emits one) + if (pointcloud == nullptr) { + // Since this ends the function early, the `cloud_watchdog_` will not be updated. + // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no + // packets come in), the watchdog will log a warning automatically + return; + } + + cloud_watchdog_->update(); + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_subscription_count() > 0) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_cloud_xyzi = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + } + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + } +} + +void HesaiDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +nebula::Status HesaiDecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp deleted file mode 100644 index 6f27d547e..000000000 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,488 +0,0 @@ -#include "nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_driver_ros_wrapper", options), hw_interface_() -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = - GetParameters(sensor_configuration, calibration_configuration, correction_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); - pandar_scan_sub_ = create_subscription( - "pandar_packets", qos, - std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - nebula_points_pub_ = - this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); - aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); -} - -void HesaiDriverRosWrapper::ReceiveScanMsgCallback( - const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) -{ - auto t_start = std::chrono::high_resolution_clock::now(); - std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - - if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); - return; - }; - if ( - nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_subscription_count() > 0) { - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - } - if ( - aw_points_base_pub_->get_subscription_count() > 0 || - aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - } - if ( - aw_points_ex_pub_->get_subscription_count() > 0 || - aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - } - - auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); -} - -void HesaiDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration), - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } - -Status HesaiDriverRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - { - 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("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").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("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - 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", ""); - 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", 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 = false; - 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", 0., 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("min_range", 0.3, 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", 300., 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").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.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - bool launch_hw; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("launch_hw", "", descriptor); - launch_hw = this->get_parameter("launch_hw").as_bool(); - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - 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) { - return Status::SENSOR_CONFIG_ERROR; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - bool run_local = !launch_hw; - if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - std::string calibration_file_path_from_sensor; - if (launch_hw && !calibration_configuration.calibration_file.empty()) { - int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); - calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); - } - if(launch_hw) { - run_local = false; - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver() == Status::OK) { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - } else { - run_local = true; - } - }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(5000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_local) { - RCLCPP_WARN_STREAM(get_logger(), "Running locally"); - bool run_org = false; - if (calibration_file_path_from_sensor.empty()) { - run_org = true; - } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); - if (calibration_configuration.calibration_file.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); - } - } - } - } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 - std::string correction_file_path_from_sensor; - if (launch_hw && !correction_file_path.empty()) { - int ext_pos = correction_file_path.find_last_of('.'); - correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); - correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); - } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); - if (!run_local) { - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_local) { - bool run_org = false; - if (correction_file_path_from_sensor.empty()) { - run_org = true; - } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_org) { - if (correction_file_path.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); - } - } - } - } - } // end AT128 - // Do not use outside of this location - hw_interface_.~HesaiHwInterface(); - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 1ac59b55b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); -#if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) - { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); - } - } - - if(rt != Status::ERROR_1){ - try{ - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - } - catch (...) - { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); - } -#endif - - hw_interface_.RegisterScanCallback( - std::bind(&HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); - pandar_scan_pub_ = - this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - -#if not defined(TEST_PCAP) - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - } -#endif - -#ifdef WITH_DEBUG_STDOUT_HesaiHwInterfaceRosWrapper - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(0); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarCalib(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagStatus(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagPort(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagTime(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagGrandmaster(ios); - }); - // thread_pool.emplace_back([&hw_interface_]{ - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetInventory(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarStatus(ios); - }); - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(1); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarRange(ios); - }); - for (std::thread & th : thread_pool) { - // hw_interface_.IOServiceRun(); - th.join(); - } - } - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - std::cout << "GetLidarCalib" << std::endl; - hw_interface_.GetLidarCalib(ios); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - } -#endif - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); -} - -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); -} - -Status HesaiHwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - return interface_status_; -} - -Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwInterfaceRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) -{ - { - 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", ""); - 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("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("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").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("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - 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", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - 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.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("setup_sensor", true, descriptor); - this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_hw_ms", 0, descriptor); - this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("retry_hw", true, descriptor); - this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); - } - { - 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("ptp_profile", ""); - sensor_configuration.ptp_profile = - nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").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("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { - sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; - } - } - { - 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("ptp_switch_type", ""); - sensor_configuration.ptp_switch_type = - nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(127).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("ptp_domain", 0, descriptor); - sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); - } - - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); - return Status::SENSOR_CONFIG_ERROR; - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - 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) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_configuration_.frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - std::string return_mode_str; - if ( - get_param(p, "sensor_model", sensor_model_str) || - get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || - get_param(p, "sensor_ip", new_param.sensor_ip) || - get_param(p, "frame_id", new_param.frame_id) || - get_param(p, "data_port", new_param.data_port) || - get_param(p, "gnss_port", new_param.gnss_port) || - get_param(p, "scan_phase", new_param.scan_phase) || - get_param(p, "packet_mtu_size", new_param.packet_mtu_size) || - get_param(p, "rotation_speed", new_param.rotation_speed) || - get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || - get_param(p, "cloud_max_angle", new_param.cloud_max_angle) || - get_param(p, "dual_return_distance_threshold", new_param.dual_return_distance_threshold)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - if (0 < return_mode_str.length()) - new_param.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -std::vector HesaiHwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - std::ostringstream os_return_mode; - os_return_mode << sensor_configuration_.return_mode; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("gnss_port", sensor_configuration_.gnss_port), - rclcpp::Parameter("scan_phase", sensor_configuration_.scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_configuration_.packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_configuration_.rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_configuration_.cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_configuration_.cloud_max_angle), - rclcpp::Parameter( - "dual_return_distance_threshold", sensor_configuration_.dual_return_distance_threshold)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp deleted file mode 100644 index e64bf443b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,620 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_monitor_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) -{ - cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; - - switch (sensor_cfg_ptr->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - while(hw_interface_.InitializeTcpDriver() == Status::ERROR_1) - { - RCLCPP_WARN(this->get_logger(), "Could not initialize TCP driver, retrying in 8s..."); - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - } - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); - RCLCPP_INFO_STREAM(this->get_logger(), result); - info_model = result.get_str_model(); - info_serial = std::string(std::begin(result.sn), std::end(result.sn)); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); -} - -Status HesaiHwMonitorRosWrapper::MonitorStart() { return interface_status_; } - -Status HesaiHwMonitorRosWrapper::MonitorStop() { return Status::OK; } -Status HesaiHwMonitorRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwMonitorRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) -{ - { - 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", ""); - 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 = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("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("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").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("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - 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", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - 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.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - 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) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("diag_span", 1000, descriptor); - this->diag_span_ = this->get_parameter("diag_span").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_monitor_ms", 0, descriptor); - this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwMonitorRosWrapper::InitializeHesaiDiagnostics() -{ - RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); - using std::chrono_literals::operator""s; - std::ostringstream os; - auto hardware_id = info_model + ": " + info_serial; - diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); - - diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorRosWrapper::HesaiCheckStatus); - diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add( - "hesai_temperature", this, &HesaiHwMonitorRosWrapper::HesaiCheckTemperature); - diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorRosWrapper::HesaiCheckRpm); - - current_status.reset(); - current_monitor.reset(); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - - auto fetch_diag_from_sensor = [this](){ - OnHesaiStatusTimer(); - if (hw_interface_.UseHttpGetLidarMonitor()) { - OnHesaiLidarMonitorTimerHttp(); - } else { - OnHesaiLidarMonitorTimer(); - } - }; - - fetch_diagnostics_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); - - if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltage); - } - - auto on_timer_update = [this] { - RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); - auto now = this->get_clock()->now(); - auto dif = (now - *current_status_time).seconds(); - - RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); - - if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - dif = (now - *current_lidar_monitor_time).seconds(); - RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); - if (diag_span_ * 2.0 < dif * 1000) { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - diagnostics_updater_.force_update(); - }; - diagnostics_update_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); - - RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); -} - -std::string HesaiHwMonitorRosWrapper::GetPtreeValue( - boost::property_tree::ptree * pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; - } -} -std::string HesaiHwMonitorRosWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwMonitorRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(get_logger(), p); - RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - uint16_t new_diag_span = 0; - if (get_param(p, "diag_span", new_diag_span)) { - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - return *result; -} - -void HesaiHwMonitorRosWrapper::OnHesaiStatusTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarStatus(); - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); - try { - hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_tree = - std::make_unique(hw_interface_.ParseJson(str)); - }); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END"); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarMonitor(); - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); -} - -void HesaiHwMonitorRosWrapper::HesaiCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - diagnostics.add("system_uptime", std::to_string(current_status->system_uptime.value())); - diagnostics.add("startup_times", std::to_string(current_status->startup_times.value())); - diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time.value())); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckPtp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - auto gps_status = current_status->get_str_gps_pps_lock(); - auto gprmc_status = current_status->get_str_gps_gprmc_status(); - auto ptp_status = current_status->get_str_ptp_clock_status(); - std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); - std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); - std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); - diagnostics.add("gps_pps_lock", gps_status); - diagnostics.add("gps_gprmc_status", gprmc_status); - diagnostics.add("ptp_clock_status", ptp_status); - if (gps_status != "UNKNOWN") { - msg.emplace_back("gps_pps_lock: " + gps_status); - } - if (gprmc_status != "UNKNOWN") { - msg.emplace_back("gprmc_status: " + gprmc_status); - } - if (ptp_status != "UNKNOWN") { - msg.emplace_back("ptp_status: " + ptp_status); - } - if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckTemperature( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - for (size_t i = 0; i < std::size(current_status->temperature); i++) { - diagnostics.add( - temperature_names[i], GetFixedPrecisionString(current_status->temperature[i].value() * 0.01)); - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add("motor_speed", std::to_string(current_status->motor_speed.value())); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_lidar_monitor_tree) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - std::string key = ""; - - std::string mes; - key = "lidarInCur"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - key = "lidarInVol"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_monitor) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage.value() * 0.01) + " V"); - diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor->input_current.value() * 0.01) + " mA"); - diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor->input_power.value() * 0.01) + " W"); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - - HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); - } - - RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp new file mode 100644 index 000000000..8449bdfe5 --- /dev/null +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -0,0 +1,315 @@ +#include "nebula_ros/hesai/hesai_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + hw_monitor_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + 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_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessCloudPacket(std::move(packet_queue_.pop())); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::HesaiSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", "", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", "", param_read_write()); + config.return_mode = drivers::ReturnModeFromStringHesai(_return_mode, config.sensor_model); + + config.host_ip = declare_parameter("host_ip", "255.255.255.255", param_read_only()); + config.sensor_ip = + declare_parameter("sensor_ip", "192.168.1.201", param_read_only()); + config.data_port = declare_parameter("data_port", 2368, param_read_only()); + config.gnss_port = declare_parameter("gnss_port", 2369, param_read_only()); + config.frame_id = declare_parameter("frame_id", "pandar", param_read_write()); + + { + 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", 0., descriptor); + } + + config.min_range = declare_parameter("min_range", 0.3, param_read_write()); + config.max_range = declare_parameter("max_range", 300., param_read_write()); + config.packet_mtu_size = declare_parameter("packet_mtu_size", 1500, param_read_only()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + uint16_t default_value; + RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model); + if (config.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { + descriptor.additional_constraints = "200, 400"; + descriptor.integer_range = int_range(200, 400, 200); + default_value = 200; + } else { + descriptor.additional_constraints = "300, 600, 1200"; + descriptor.integer_range = int_range(300, 1200, 300); + default_value = 600; + } + config.rotation_speed = + declare_parameter("rotation_speed", default_value, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_min_angle = declare_parameter("cloud_min_angle", 0, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_max_angle = declare_parameter("cloud_max_angle", 360, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; + descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); + config.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + } + + auto _ptp_profile = declare_parameter("ptp_profile", "", param_read_only()); + config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); + + auto _ptp_transport = declare_parameter("ptp_transport_type", "", param_read_only()); + config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); + + if ( + config.ptp_transport_type != drivers::PtpTransportType::L2 && + config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && + config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_WARN_STREAM( + get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" + << _ptp_profile + << "' only supports 'L2'. Setting it to 'L2'."); + config.ptp_transport_type = drivers::PtpTransportType::L2; + set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); + } + + auto _ptp_switch = declare_parameter("ptp_switch_type", "", param_read_only()); + config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(0, 127, 1); + config.ptp_domain = declare_parameter("ptp_domain", 0, descriptor); + } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status HesaiRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_ERROR_STREAM( + 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( + 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'"); + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (hw_monitor_wrapper_) { + hw_monitor_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void HesaiRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + return; + } + + for (auto & pkt : scan_msg->packets) { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + +Status HesaiRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status HesaiRosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); +} + +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); + + std::string _return_mode = ""; + 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, "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); + + // 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; + } + } + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (_return_mode.length() > 0) + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + + 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(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +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 new file mode 100644 index 000000000..18da9da83 --- /dev/null +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -0,0 +1,82 @@ +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: hw_interface_(new nebula::drivers::HesaiHwInterface()), + logger_(parent_node->get_logger().get_child("HwInterface")), + status_(Status::NOT_INITIALIZED) +{ + setup_sensor_ = parent_node->declare_parameter("setup_sensor", true, param_read_only()); + bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); + + status_ = hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(config)); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); + hw_interface_->SetTargetModel(config->sensor_model); + + int retry_count = 0; + + while (true) { + status_ = hw_interface_->InitializeTcpDriver(); + if (status_ == Status::OK || !retry_connect) { + break; + } + + retry_count++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count); + } + + if (status_ == Status::OK) { + try { + auto inventory = hw_interface_->GetInventory(); + RCLCPP_INFO_STREAM(logger_, inventory); + hw_interface_->SetTargetModel(inventory.model); + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); + } + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } + } else { + RCLCPP_ERROR_STREAM( + logger_, "Failed to get model from sensor... Set from config: " << config->sensor_model); + } + + status_ = Status::OK; +} + +void HesaiHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(new_config)); + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } +} + +Status HesaiHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr HesaiHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..da1f78794 --- /dev/null +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -0,0 +1,362 @@ +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + diagnostics_updater_(parent_node), + status_(Status::OK), + hw_interface_(hw_interface), + parent_node_(parent_node) +{ + diag_span_ = parent_node->declare_parameter("diag_span", 1000, param_read_only()); + + switch (config->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names_.emplace_back("Bottom circuit board T1"); + temperature_names_.emplace_back("Bottom circuit board T2"); + temperature_names_.emplace_back("Laser emitting board RT_L1 (Internal)"); + temperature_names_.emplace_back("Laser emitting board RT_L2"); + temperature_names_.emplace_back("Receiving board RT_R"); + temperature_names_.emplace_back("Receiving board RT2"); + temperature_names_.emplace_back("Top circuit RT3"); + temperature_names_.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names_.emplace_back("Bottom circuit RT1"); + temperature_names_.emplace_back("Bottom circuit RT2"); + temperature_names_.emplace_back("Internal Temperature"); + temperature_names_.emplace_back("Laser emitting board RT1"); + temperature_names_.emplace_back("Laser emitting board RT2"); + temperature_names_.emplace_back("Receiving board RT1"); + temperature_names_.emplace_back("Top circuit RT1"); + temperature_names_.emplace_back("Top circuit RT2"); + break; + } + + auto result = hw_interface->GetInventory(); + current_inventory_.reset(new HesaiInventory(result)); + current_inventory_time_.reset(new rclcpp::Time(parent_node->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model_ = result.get_str_model(); + info_serial_ = std::string(std::begin(result.sn), std::end(result.sn)); + RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); + InitializeHesaiDiagnostics(); +} + +void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() +{ + RCLCPP_INFO_STREAM(logger_, "InitializeHesaiDiagnostics"); + using std::chrono_literals::operator""s; + std::ostringstream os; + auto hardware_id = info_model_ + ": " + info_serial_; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hardware_id); + + diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::HesaiCheckStatus); + diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::HesaiCheckPtp); + diagnostics_updater_.add( + "hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::HesaiCheckRpm); + + current_status_.reset(); + current_monitor_.reset(); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + auto fetch_diag_from_sensor = [this]() { + OnHesaiStatusTimer(); + if (hw_interface_->UseHttpGetLidarMonitor()) { + OnHesaiLidarMonitorTimerHttp(); + } else { + OnHesaiLidarMonitorTimer(); + } + }; + + fetch_diagnostics_timer_ = parent_node_->create_wall_timer( + std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); + + if (hw_interface_->UseHttpGetLidarMonitor()) { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltageHttp); + } else { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltage); + } + + auto on_timer_update = [this] { + RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); + auto now = parent_node_->get_clock()->now(); + auto dif = (now - *current_status_time_).seconds(); + + RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + dif = (now - *current_lidar_monitor_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "dif(monitor): " << dif); + if (diag_span_ * 2.0 < dif * 1000) { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + diagnostics_updater_.force_update(); + }; + diagnostics_update_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); + + RCLCPP_DEBUG_STREAM(logger_, "add_timer"); +} + +std::string HesaiHwMonitorWrapper::GetPtreeValue( + boost::property_tree::ptree * pt, const std::string & key) +{ + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return MSG_NOT_SUPPORTED; + } +} +std::string HesaiHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} + +void HesaiHwMonitorWrapper::OnHesaiStatusTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer" << std::endl); + try { + auto result = hw_interface_->GetLidarStatus(); + std::scoped_lock lock(mtx_lidar_status_); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_status_.reset(new HesaiLidarStatus(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer END" << std::endl); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp"); + try { + hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string & str) { + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_tree_ = + std::make_unique(hw_interface_->ParseJson(str)); + }); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp END"); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer"); + try { + auto result = hw_interface_->GetLidarMonitor(); + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_monitor_.reset(new HesaiLidarMonitor(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer END"); +} + +void HesaiHwMonitorWrapper::HesaiCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime.value())); + diagnostics.add("startup_times", std::to_string(current_status_->startup_times.value())); + diagnostics.add( + "total_operation_time", std::to_string(current_status_->total_operation_time.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + auto gps_status = current_status_->get_str_gps_pps_lock(); + auto gprmc_status = current_status_->get_str_gps_gprmc_status(); + auto ptp_status = current_status_->get_str_ptp_clock_status(); + std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); + std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); + std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); + diagnostics.add("gps_pps_lock", gps_status); + diagnostics.add("gps_gprmc_status", gprmc_status); + diagnostics.add("ptp_clock_status", ptp_status); + if (gps_status != "UNKNOWN") { + msg.emplace_back("gps_pps_lock: " + gps_status); + } + if (gprmc_status != "UNKNOWN") { + msg.emplace_back("gprmc_status: " + gprmc_status); + } + if (ptp_status != "UNKNOWN") { + msg.emplace_back("ptp_status: " + ptp_status); + } + if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckTemperature( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + for (size_t i = 0; i < std::size(current_status_->temperature); i++) { + diagnostics.add( + temperature_names_[i], + GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_lidar_monitor_tree_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + std::string key = ""; + + std::string mes; + key = "lidarInCur"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + key = "lidarInVol"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltage( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_monitor_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add( + "input_voltage", + GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); + diagnostics.add( + "input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + + " m" + "A"); + diagnostics.add( + "input_power", + GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +Status HesaiHwMonitorWrapper::Status() +{ + return Status::OK; +} +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index a07a4dabd..289e8a711 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -46,12 +46,11 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -59,26 +58,11 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( Status HesaiRosDecoderTest::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } @@ -235,8 +219,7 @@ Status HesaiRosDecoderTest::GetParameters( if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if ( - sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { @@ -289,9 +272,8 @@ void HesaiRosDecoderTest::ReadBag( storage_options.uri = params_.bag_path; storage_options.storage_id = params_.storage_id; - converter_options.output_serialization_format = params_.format; //"cdr"; + converter_options.output_serialization_format = params_.format; rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); rosbag2_cpp::Reader bag_reader(std::make_unique()); bag_reader.open(storage_options, converter_options); @@ -310,12 +292,21 @@ void HesaiRosDecoderTest::ReadBag( "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp); auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - auto scan_timestamp = std::get<1>(pointcloud_ts); - pointcloud = std::get<0>(pointcloud_ts); - scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + for (auto & pkt : extracted_msg_ptr->packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + auto scan_timestamp = std::get<1>(pointcloud_ts); + + if (!pointcloud) { + 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 a0477a366..f12e56020 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #ifndef _SRC_CALIBRATION_DIR_PATH #define _SRC_CALIBRATION_DIR_PATH "" @@ -48,9 +50,27 @@ struct HesaiRosDecoderTestParams double dual_return_distance_threshold = 0.1; }; +inline std::ostream & operator<<( + std::ostream & os, nebula::ros::HesaiRosDecoderTestParams const & arg) +{ + return os << "sensor_model=" << arg.sensor_model << ", " + << "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 << ", " + << "min_range=" << arg.min_range << ", " + << "max_range=" << arg.max_range << ", " + << "storage_id=" << arg.storage_id << ", " + << "format=" << arg.format << ", " + << "target_topic=" << arg.target_topic << ", " + << "dual_return_distance_threshold=" << arg.dual_return_distance_threshold << ", "; +} + /// @brief Testing decoder of pandar 40p (Keeps HesaiDriverRosWrapper structure as much as /// possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test +class HesaiRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -65,17 +85,7 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations (Magic numbers for Pandar40P is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 96ff00c3c..7f80f6f48 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -12,6 +12,8 @@ #include #include #include +#include +#include namespace nebula { @@ -19,72 +21,15 @@ namespace test { const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { - { - "Pandar40P", - "Dual", - "Pandar40P.csv", - "40p/1673400149412331409", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "Pandar64", - "Dual", - "Pandar64.csv", - "64/1673403880599376836", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "PandarAT128", - "LastStrongest", - "PandarAT128.csv", - "at128/1679653308406038376", - "PandarAT128.dat", - "hesai", - 0, - 1.f, - 180.f - }, - { - "PandarQT64", - "Dual", - "PandarQT64.csv", - "qt64/1673401195788312575", - "", - "hesai", - 0, - 0.1f, - 60.f - }, - { - "PandarXT32", - "Dual", - "PandarXT32.csv", - "xt32/1673400677802009732", - "", - "hesai", - 0, - 0.05f, - 120.f - }, - { - "PandarXT32M", - "LastStrongest", - "PandarXT32M.csv", - "xt32m/1660893203042895158", - "", - "hesai", - 0, - 0.5f, - 300.f - }}; + {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "", "hesai", 0, 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "", "hesai", 0, 0.3f, 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.csv", "at128/1679653308406038376", + "PandarAT128.dat", "hesai", 0, 1.f, 180.f}, + {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "", "hesai", 0, 0.1f, 60.f}, + {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "", "hesai", 0, 0.05f, + 120.f}, + {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "", "hesai", 0, + 0.5f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd) @@ -142,6 +87,8 @@ TEST_P(DecoderTest, TestTimezone) auto gmt = timezone; hesai_driver_->ReadBag(scan_callback); + ASSERT_GT(decoded_timestamps.size(), 0U); + // Then, reset driver and timestamps vector for the next decode run TearDown(); SetUp(); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index b00a9433d..047b2c661 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -13,7 +13,9 @@ #include #include +#include #include +#include namespace nebula { @@ -57,7 +59,10 @@ Status VelodyneRosDecoderTest::InitializeDriver( return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } +Status VelodyneRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -315,10 +320,9 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; + converter_options.output_serialization_format = format; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); { rosbag2_cpp::Reader bag_reader(std::make_unique()); diff --git a/ros2_socketcan b/ros2_socketcan new file mode 160000 index 000000000..4ced5284f --- /dev/null +++ b/ros2_socketcan @@ -0,0 +1 @@ +Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/scripts/plot_instrumentation.py b/scripts/plot_instrumentation.py new file mode 100755 index 000000000..7540aa35f --- /dev/null +++ b/scripts/plot_instrumentation.py @@ -0,0 +1,79 @@ +#!/usr/bin/python3 + +import argparse +from collections import defaultdict +import json +import os +import re + +from matplotlib import pyplot as plt +import pandas as pd + + +def condition_data(log_file: str): + with open(log_file, "r") as f: + lines = f.readlines() + lines = [re.search(r'(\{"type":.*?"tag":.*?\})', line) for line in lines] + lines = [re.sub(r'([0-9])"', r"\1", line[1]) for line in lines if line] + lines = [json.loads(line) for line in lines if line] + + cols = defaultdict(list) + + for record in lines: + for key in record.keys(): + if key in ["tag", "type"]: + continue + + colname = f"{record['type']}.{key}.{record['tag']}" + cols[colname] += [num for num in record[key] if num is not None] + + def quantile_filter(series, quantile): + q = series.quantile(quantile) + return series[series <= q] + + cols = {k: pd.Series(v, name=k) / 1e3 for k, v in cols.items()} + cols = {k: quantile_filter(v, 0.999) for k, v in cols.items()} + + for v in cols.values(): + v: pd.Series + v.attrs["file"] = os.path.basename(os.path.splitext(log_file)[0]) + + return cols + + +def plot(conditioned_logs): + + conditioned_logs = {k: [dic[k] for dic in conditioned_logs] for k in conditioned_logs[0]} + + fig, axs = plt.subplots(1, len(conditioned_logs), figsize=(15, 8), dpi=120) + + handles, labels = [], [] + + for i, k in enumerate(sorted(conditioned_logs.keys())): + k: str + v = conditioned_logs[k] + ax: plt.Axes = axs[i] + art = ax.boxplot(v) + handles.append(art) + labels.append(k) + ax.set_ylabel("dt [µs]") + ax.set_title(k.replace(".", "\n")) + ax.set_xticks([1 + i for i in range(len(v))], [ser.attrs["file"] for ser in v]) + ax.tick_params(axis="x", labelrotation=90) + + fig.tight_layout() + plt.savefig("instrumentation.png") + plt.show() + + +def main(args): + conditioned_logs = [condition_data(f) for f in args.log_files] + plot(conditioned_logs) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("log_files", nargs="+") + args = parser.parse_args() + + main(args) diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 521dfd347..07dbee302 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -1,11 +1,13 @@ -#%% +# %% +import argparse +import glob import os -import pandas as pd import re -import glob -import argparse + from matplotlib import pyplot as plt +import pandas as pd + def parse_logs(run_name): dfs = [] @@ -15,22 +17,20 @@ def parse_logs(run_name): with open(path) as f: lines = f.readlines() lines = filter(lambda line: "PROFILING" in line, lines) - lines = [re.sub(r'.*PROFILING (\{.*?\}).*', r'\1', line) for line in lines] + lines = [re.sub(r".*PROFILING (\{.*?\}).*", r"\1", line) for line in lines] records = [eval(line) for line in lines] dfs.append(pd.DataFrame(records)) df = pd.concat(dfs) - + for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms - df['d_total'] = sum([df[c] for c in df.columns if c.startswith("d_")]) # type: ignore return df + def plot_timing_comparison(run_names): - scenario_dfs = { - run_name: parse_logs(run_name) for run_name in run_names - } + scenario_dfs = {run_name: parse_logs(run_name) for run_name in run_names} n_cols = sum(1 for c in next(iter(scenario_dfs.values())).columns if c.startswith("d_")) @@ -41,16 +41,20 @@ def plot_timing_comparison(run_names): boxes = axs[1:] for i, (label, df) in enumerate(scenario_dfs.items()): - durations = df['d_total'] + # durations = df['d_total'] - ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') + # ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): - ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') - + ax_n.plot( + df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".", color="black" + ) + for col in filter(lambda col: col.startswith("d_"), df.columns): + ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".") + d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) for j, col in enumerate(d_columns): - if (i == 0): + if i == 0: boxes[j].set_ylabel(f"{col} (ms)") boxes[j].boxplot(df[col], positions=[i], labels=[label]) @@ -58,13 +62,11 @@ def plot_timing_comparison(run_names): ax_d.set_ylabel("time (ms)") ax_d.set_xlabel("iteration") - df_means = pd.DataFrame({ - label: df.describe().loc["mean"] for label, df in scenario_dfs.items() - }) + df_means = pd.DataFrame( + {label: df.describe().loc["mean"] for label, df in scenario_dfs.items()} + ) - df_stds = pd.DataFrame({ - label: df.describe().loc["std"] for label, df in scenario_dfs.items() - }) + df_stds = pd.DataFrame({label: df.describe().loc["std"] for label, df in scenario_dfs.items()}) df_means = df_means.rename(index=lambda label: f"{label} AVG") df_stds = df_stds.rename(index=lambda label: f"{label} STD") @@ -74,15 +76,23 @@ def plot_timing_comparison(run_names): baseline_name = run_names[0] df_base = df_stat[baseline_name] for label in df_base.index: - df_stat.loc[f"{label} % rel to {baseline_name}", :] = df_stat.loc[label, :] / df_base.loc[label] * 100 + df_stat.loc[f"{label} % rel to {baseline_name}", :] = ( + df_stat.loc[label, :] / df_base.loc[label] * 100 + ) print(df_stat.to_markdown()) plt.show() + + # %% if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("run_names", nargs='+', help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.") + parser.add_argument( + "run_names", + nargs="+", + help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.", + ) args = parser.parse_args() - plot_timing_comparison(args.run_names) \ No newline at end of file + plot_timing_comparison(args.run_names) diff --git a/transport_drivers b/transport_drivers new file mode 160000 index 000000000..abf8aa8e1 --- /dev/null +++ b/transport_drivers @@ -0,0 +1 @@ +Subproject commit abf8aa8e171f33e0acd6d845c3d795192c964e9c