diff --git a/.cspell.json b/.cspell.json index a52c07b1c..75b26f454 100644 --- a/.cspell.json +++ b/.cspell.json @@ -6,39 +6,23 @@ "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", @@ -46,9 +30,23 @@ "UDP_SEQ", "vccint", "Vccint", - "Vdat", "XT", "XTM", - "yukkysaito" + "DHAVE", + "Bpearl", + "Helios", + "Msop", + "Difop", + "gptp", + "Idat", + "Vdat", + "autosar", + "srvs", + "manc", + "ipaddr", + "ntoa", + "piyushk", + "Piyush", + "fout" ] -} \ No newline at end of file +} diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml deleted file mode 100644 index 00482c2b5..000000000 --- a/.github/workflows/json-schema-check.yaml +++ /dev/null @@ -1,40 +0,0 @@ -name: json-schema-check - -on: - pull_request: - workflow_dispatch: - -jobs: - check-if-relevant-files-changed: - runs-on: ubuntu-latest - outputs: - run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} - steps: - - uses: actions/checkout@v4 - - uses: dorny/paths-filter@v3 - id: paths_filter - with: - filters: | - json_or_yaml: - - '**/schema/*.schema.json' - - '**/schema/sub/.json' - - '**/config/**/*.param.yaml' - - json-schema-check: - needs: check-if-relevant-files-changed - if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v4 - - - name: Run json-schema-check - uses: autowarefoundation/autoware-github-actions/json-schema-check@v1 - - no-relevant-changes: - needs: check-if-relevant-files-changed - if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' - runs-on: ubuntu-latest - steps: - - name: Dummy step - run: echo "No relevant changes, passing check" \ No newline at end of file diff --git a/.gitignore b/.gitignore index f068ec8be..e966ffdfb 100644 --- a/.gitignore +++ b/.gitignore @@ -29,6 +29,3 @@ site/ # qcreator stuff CMakeLists.txt.user - -# pre-commit -node_modules/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b4c2e9f2f..13c31ce62 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v4.1.0 hooks: - id: check-json - id: check-merge-conflict @@ -18,51 +18,51 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.40.0 + rev: v0.30.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v4.0.0-alpha.8 + rev: v2.5.1 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.35.1 + rev: v1.26.3 hooks: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.8.0 + rev: v0.4.0 hooks: - id: prettier-package-xml - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.10.0.1 + rev: v0.8.0.3 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.8.0-1 + rev: v3.4.2-1 hooks: - id: shfmt args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.13.2 + rev: 5.12.0 hooks: - id: isort - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 22.1.0 hooks: - id: black args: [--line-length=100] - repo: https://github.com/PyCQA/flake8 - rev: 7.0.0 + rev: 4.0.1 hooks: - id: flake8 additional_dependencies: @@ -78,12 +78,12 @@ repos: ] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.5 + rev: v13.0.0 hooks: - id: clang-format - repo: https://github.com/cpplint/cpplint - rev: 1.6.1 + rev: 1.5.5 hooks: - id: cpplint args: [--quiet] diff --git a/build_depends.repos b/build_depends.repos index e9278685a..e28379f1a 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/mojomex/transport_drivers - version: mutable-buffer-in-udp-callback + url: https://github.com/MapIV/transport_drivers + version: boost diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 154f8e453..4c146736c 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -40,8 +40,6 @@ 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 8e0600999..9c6e5d555 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -8,16 +8,13 @@ #include #include #include -#include #include -#include -#include namespace nebula { namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : public LidarConfigurationBase +struct HesaiSensorConfiguration : LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -46,38 +43,24 @@ 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 : public HesaiCalibrationConfigurationBase +struct HesaiCalibrationConfiguration : CalibrationConfigurationBase { std::map elev_angle_map; std::map azimuth_offset_map; - inline nebula::Status LoadFromFile(const std::string & calibration_file) override + inline nebula::Status LoadFromFile(const std::string & calibration_file) { 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 @@ -87,12 +70,14 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase 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; } @@ -103,9 +88,10 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase 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; } @@ -113,7 +99,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveToFile(const std::string & calibration_file) + inline nebula::Status SaveFile(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -131,19 +117,11 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase 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) { @@ -156,7 +134,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase }; /// @brief struct for Hesai correction configuration (for AT) -struct HesaiCorrection : public HesaiCalibrationConfigurationBase +struct HesaiCorrection { uint16_t delimiter; uint8_t versionMajor; @@ -178,11 +156,12 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @brief Load correction data from file /// @param buf Binary buffer /// @return Resulting status - inline nebula::Status LoadFromBytes(const std::vector & buf) override + inline nebula::Status LoadFromBinary(const std::vector & buf) { size_t index; - for (index = 0; index < buf.size() - 1; index++) { - if (buf[index] == 0xee && buf[index + 1] == 0xff) break; + for (index = 0; index < buf.size()-1; index++) { + if(buf[index]==0xee && buf[index+1]==0xff) + break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -279,7 +258,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @brief Load correction data from file /// @param correction_file path /// @return Resulting status - inline nebula::Status LoadFromFile(const std::string & correction_file) override + inline nebula::Status LoadFromFile(const std::string & correction_file) { std::ifstream ifs(correction_file, std::ios::in | std::ios::binary); if (!ifs) { @@ -289,10 +268,10 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); + ifs.read((char *)&c, sizeof(unsigned char)); buf.emplace_back(c); } - LoadFromBytes(buf); + LoadFromBinary(buf); ifs.close(); return Status::OK; @@ -302,8 +281,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveToFileFromBytes( - const std::string & correction_file, const std::vector & buf) override + inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -313,20 +291,21 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase } 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; } @@ -456,8 +435,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 e43ab49a3..9d04a267a 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,10 +2,7 @@ #define NEBULA_COMMON_H #include - #include - -#include #include #include #include @@ -345,11 +342,24 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; +enum class PtpProfile { + IEEE_1588v2 = 0, + IEEE_802_1AS, + IEEE_802_1AS_AUTO, + PROFILE_UNKNOWN +}; -enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; +enum class PtpTransportType { + UDP_IP = 0, + L2, + UNKNOWN_TRANSPORT +}; -enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; +enum class PtpSwitchType { + NON_TSN = 0, + TSN, + UNKNOWN_SWITCH +}; /// @brief not used? struct PointField @@ -608,14 +618,13 @@ 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::UNKNOWN_PROFILE; + return PtpProfile::PROFILE_UNKNOWN; } /// @brief Convert PtpProfile enum to string (Overloading the << operator) @@ -634,7 +643,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::UNKNOWN_PROFILE: + case PtpProfile::PROFILE_UNKNOWN: os << "UNKNOWN"; break; } @@ -648,9 +657,8 @@ 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; @@ -684,9 +692,8 @@ 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 25cb5f83d..d3643cfb4 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -1,22 +1,23 @@ #pragma once -#include -#include #include +#include +#include namespace nebula { namespace util { -struct bad_expected_access : public std::exception -{ - explicit bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} +struct bad_expected_access : public std::exception { + 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. @@ -38,16 +39,14 @@ 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_) { @@ -55,41 +54,36 @@ struct expected return default_; } - /// @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) - { + /// @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) { 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; } // NOLINT(runtime/explicit) + expected(const T & value) { value_ = value; } - expected(const E & error) { value_ = error; } // NOLINT(runtime/explicit) + expected(const E & error) { value_ = error; } private: std::variant value_; }; } // namespace util -} // namespace nebula +} // namespace nebula \ No newline at end of file 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 02d7cd535..42fd0a008 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,11 +23,19 @@ 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: - using correction_data_t = CorrectionDataT; + AngleCorrector( + const std::shared_ptr & sensor_calibration, + const std::shared_ptr & sensor_correction) + : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction) + { + } /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their /// sin/cos values. @@ -44,9 +52,8 @@ 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 +} // namespace nebula \ No newline at end of file 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 da9a1b579..3e0c999b0 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,7 +4,6 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include -#include namespace nebula { @@ -12,7 +11,7 @@ namespace drivers { template -class AngleCorrectorCalibrationBased : public AngleCorrector +class AngleCorrectorCalibrationBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; @@ -28,7 +27,9 @@ class AngleCorrectorCalibrationBased : public AngleCorrector & sensor_calibration) + const std::shared_ptr & sensor_calibration, + const std::shared_ptr & sensor_correction) + : AngleCorrector(sensor_calibration, sensor_correction) { if (sensor_calibration == nullptr) { throw std::runtime_error( @@ -36,8 +37,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrectorelev_angle_map.at(channel_id); - float azimuth_offset_deg = sensor_calibration->azimuth_offset_map.at(channel_id); + float elevation_angle_deg = sensor_calibration->elev_angle_map[channel_id]; + float azimuth_offset_deg = sensor_calibration->azimuth_offset_map[channel_id]; elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); @@ -80,10 +81,10 @@ class AngleCorrectorCalibrationBased : public AngleCorrector -#include + +#define _(x) '"' << #x << "\": " << x << ", " namespace nebula { @@ -12,11 +13,10 @@ 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,26 +29,28 @@ 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 = correction_->frameNumber - 1; - for (size_t i = 0; i < correction_->frameNumber; ++i) { - if (azimuth < 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 = sensor_correction_->frameNumber - 1; + for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) { + if (azimuth < sensor_correction_->startFrame[i]) return field; field = i; } - // This is never reached if correction_ is correct + // This is never reached if sensor_correction_ is correct return field; } public: - explicit AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_correction) - : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) + AngleCorrectorCorrectionBased( + const std::shared_ptr & sensor_calibration, + const std::shared_ptr & sensor_correction) + : AngleCorrector(sensor_calibration, sensor_correction), + logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { throw std::runtime_error( - "Cannot instantiate AngleCorrectorCorrectionBased without correction data"); + "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); } logger_.set_level(rclcpp::Logger::Level::Debug); @@ -62,16 +64,17 @@ 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; @@ -81,8 +84,7 @@ 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 2e2bba6ba..7bdfd069e 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,11 +8,6 @@ #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" -#include -#include -#include -#include - namespace nebula { namespace drivers @@ -23,7 +18,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_{}; @@ -39,13 +34,13 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; /// @brief The last azimuth processed - int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet + int last_phase_; /// @brief The timestamp of the last completed scan in nanoseconds - uint64_t output_scan_timestamp_ns_ = 0; + uint64_t output_scan_timestamp_ns_; /// @brief The timestamp of the scan currently in progress - uint64_t decode_scan_timestamp_ns_ = 0; + uint64_t decode_scan_timestamp_ns_; /// @brief Whether a full scan has been processed - bool has_scanned_ = false; + bool has_scanned_; rclcpp::Logger logger_; @@ -57,17 +52,17 @@ class HesaiDecoder : public HesaiScanDecoder block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. - /// @param packet The incoming PandarPacket + /// @param pandar_packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const std::vector & packet) + bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) { - if (packet.size() < sizeof(typename SensorT::packet_t)) { + if (pandar_packet.size < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" + logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:" << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) { // FIXME(mojomex) do validation? // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); return true; @@ -179,10 +174,6 @@ 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); } @@ -209,17 +200,20 @@ class HesaiDecoder : public HesaiScanDecoder public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - /// @param correction_data Calibration data 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) explicit HesaiDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - correction_data) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration, + const std::shared_ptr & correction_configuration) : sensor_configuration_(sensor_configuration), - angle_corrector_(correction_data), + angle_corrector_(calibration_configuration, correction_configuration), 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); @@ -228,9 +222,9 @@ class HesaiDecoder : public HesaiScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const std::vector & packet) override + int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override { - if (!parsePacket(packet)) { + if (!parsePacket(pandar_packet)) { return -1; } @@ -253,7 +247,7 @@ class HesaiDecoder : public HesaiScanDecoder sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS); if (scan_completed) { - std::swap(decode_pc_, output_pc_); + std::swap(decode_pc_, output_pc_); decode_pc_->clear(); has_scanned_ = true; output_scan_timestamp_ns_ = decode_scan_timestamp_ns_; 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 aa807e567..730888c91 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 -inline int get_n_returns(uint8_t return_mode) +int get_n_returns(uint8_t return_mode) { switch (return_mode) { case return_mode::SINGLE_FIRST: @@ -221,8 +221,7 @@ 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 153435bbe..f0b4e3f8b 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,7 +8,6 @@ #include "pandar_msgs/msg/pandar_scan.hpp" #include -#include namespace nebula { @@ -27,9 +26,9 @@ class HesaiScanDecoder HesaiScanDecoder() = default; /// @brief Parses PandarPacket and add its points to the point cloud - /// @param packet The incoming PandarPacket + /// @param pandar_packet The incoming PandarPacket /// @return The last azimuth processed - virtual int unpack(const std::vector & packet) = 0; + virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_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 4c211efcc..c83788965 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,18 +14,15 @@ #include #include -#include #include #include -#include -#include namespace nebula { namespace drivers { /// @brief Hesai driver -class HesaiDriver +class HesaiDriver : NebulaDriverBase { private: /// @brief Current driver status @@ -33,22 +30,16 @@ class HesaiDriver /// @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 (either - /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128) + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @param correction_configuration CorrectionConfiguration for this driver (for AT) explicit HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration, + const std::shared_ptr & correction_configuration = nullptr); /// @brief Get current status of this driver /// @return Current status @@ -58,13 +49,13 @@ class HesaiDriver /// @param calibration_configuration /// @return Resulting status Status SetCalibrationConfiguration( - const HesaiCalibrationConfigurationBase & calibration_configuration); + const CalibrationConfigurationBase & calibration_configuration) override; /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ParseCloudPacket( - const std::vector & packet); + std::tuple ConvertScanToPointcloud( + const std::shared_ptr & pandar_scan); }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index 02209c5b2..7c319e0fe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -20,7 +20,6 @@ #include #include -#include namespace nebula { @@ -133,57 +132,23 @@ enum RETURN_TYPE { /// @brief Base class for Velodyne LiDAR decoder class VelodyneScanDecoder { -private: - size_t processed_packets_{0}; - uint32_t prev_packet_first_azm_phased_{0}; - bool has_scanned_{false}; - protected: - /// @brief Checks if the current packet completes the ongoing scan. - /// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until - /// the Velodyne decoder uses the same structure as Hesai/Robosense - /// @param packet The packet buffer to extract azimuths from - /// @param packet_seconds The seconds-since-epoch part of the packet's timestamp - /// @param phase The sensor's scan phase used for scan cutting - void checkAndHandleScanComplete(const std::vector & packet, int32_t packet_seconds, const uint32_t phase) { - if (has_scanned_) { - processed_packets_ = 0; - reset_pointcloud(packet_seconds); - } - - has_scanned_ = false; - processed_packets_++; - - // Check if scan is complete - uint32_t packet_first_azm = packet[2]; // lower word of azimuth block 0 - packet_first_azm |= packet[3] << 8; // higher word of azimuth block 0 - - uint32_t packet_last_azm = packet[1102]; - packet_last_azm |= packet[1103] << 8; - - uint32_t packet_first_azm_phased = (36000 + packet_first_azm - phase) % 36000; - uint32_t packet_last_azm_phased = (36000 + packet_last_azm - phase) % 36000; - - has_scanned_ = - processed_packets_ > 1 && (packet_last_azm_phased < packet_first_azm_phased || - packet_first_azm_phased < prev_packet_first_azm_phased_); - - prev_packet_first_azm_phased_ = packet_first_azm_phased; - } - /// @brief Decoded point cloud drivers::NebulaPointCloudPtr scan_pc_; /// @brief Point cloud overflowing from one cycle drivers::NebulaPointCloudPtr overflow_pc_; + uint16_t scan_phase_{}; + uint16_t last_phase_{}; + bool has_scanned_ = true; double dual_return_distance_threshold_{}; // Velodyne does this internally, this will not be // implemented here double scan_timestamp_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_configuration_; /// @brief Calibration for this decoder - std::shared_ptr calibration_configuration_; + std::shared_ptr calibration_configuration_; public: VelodyneScanDecoder(VelodyneScanDecoder && c) = delete; @@ -196,7 +161,7 @@ class VelodyneScanDecoder /// @brief Virtual function for parsing and shaping VelodynePacket /// @param pandar_packet - virtual void unpack(const std::vector & packet, int32_t packet_seconds) = 0; + virtual void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0; /// @brief Virtual function for parsing VelodynePacket based on packet structure /// @param pandar_packet /// @return Resulting flag @@ -204,10 +169,7 @@ class VelodyneScanDecoder /// @brief Virtual function for getting the flag indicating whether one cycle is ready /// @return Readied - bool hasScanned() { - return has_scanned_; - } - + virtual bool hasScanned() = 0; /// @brief Calculation of points in each packet /// @return # of points virtual int pointsPerPacket() = 0; @@ -216,7 +178,8 @@ class VelodyneScanDecoder /// @return tuple of Point cloud and timestamp virtual std::tuple get_pointcloud() = 0; /// @brief Resetting point cloud buffer - virtual void reset_pointcloud(double time_stamp) = 0; + /// @param n_pts # of points + virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0; /// @brief Resetting overflowed point cloud buffer virtual void reset_overflow(double time_stamp) = 0; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp index be920a3e1..730f7045f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp @@ -25,11 +25,14 @@ class Vlp16Decoder : public VelodyneScanDecoder /// @param sensor_configuration SensorConfiguration for this decoder /// @param calibration_configuration Calibration for this decoder explicit Vlp16Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const std::vector & packet, int32_t packet_seconds) override; + void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; + /// @brief Get the flag indicating whether one cycle is ready + /// @return Readied + bool hasScanned() override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -37,7 +40,8 @@ class Vlp16Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - void reset_pointcloud(double time_stamp) override; + /// @param n_pts # of points + void reset_pointcloud(size_t n_pts, double time_stamp) override; /// @brief Resetting overflowed point cloud buffer void reset_overflow(double time_stamp) override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp index 4d5cb938b..0ffa1e3ce 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp @@ -24,11 +24,14 @@ class Vlp32Decoder : public VelodyneScanDecoder /// @param sensor_configuration SensorConfiguration for this decoder /// @param calibration_configuration Calibration for this decoder explicit Vlp32Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const std::vector & packet, int32_t packet_seconds) override; + void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; + /// @brief Get the flag indicating whether one cycle is ready + /// @return Readied + bool hasScanned() override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -36,7 +39,8 @@ class Vlp32Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - void reset_pointcloud(double time_stamp) override; + /// @param n_pts # of points + void reset_pointcloud(size_t n_pts, double time_stamp) override; /// @brief Resetting overflowed point cloud buffer void reset_overflow(double time_stamp) override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp index af84a33f4..e440096e9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp @@ -21,11 +21,14 @@ class Vls128Decoder : public VelodyneScanDecoder /// @param sensor_configuration SensorConfiguration for this decoder /// @param calibration_configuration Calibration for this decoder explicit Vls128Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration); /// @brief Parsing and shaping VelodynePacket /// @param velodyne_packet - void unpack(const std::vector & packet, int32_t packet_seconds) override; + void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; + /// @brief Get the flag indicating whether one cycle is ready + /// @return Readied + bool hasScanned() override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -33,7 +36,8 @@ class Vls128Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - void reset_pointcloud(double time_stamp) override; + /// @param n_pts # of points + void reset_pointcloud(size_t n_pts, double time_stamp) override; /// @brief Resetting overflowed point cloud buffer void reset_overflow(double time_stamp) override; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp index c5caf00b8..d0257b82d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp @@ -36,8 +36,8 @@ class VelodyneDriver : NebulaDriverBase /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver VelodyneDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration); /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration @@ -52,8 +52,8 @@ class VelodyneDriver : NebulaDriverBase /// @brief Convert VelodyneScan message to point cloud /// @param velodyne_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ParseCloudPacket( - const std::vector & packet, int32_t packet_seconds); + std::tuple ConvertScanToPointcloud( + const std::shared_ptr & velodyne_scan); }; } // 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 269db2a17..49be4adac 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -18,90 +18,91 @@ namespace nebula namespace drivers { HesaiDriver::HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_data) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration, + const std::shared_ptr & correction_configuration) { // 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_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_.reset(new HesaiDecoder( + sensor_configuration, calibration_configuration, correction_configuration)); 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; } } -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 HesaiDriver::ConvertScanToPointcloud( + const std::shared_ptr & pandar_scan) { std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); - if (driver_status_ != nebula::Status::OK) - { + if (driver_status_ != nebula::Status::OK) { RCLCPP_ERROR(logger, "Driver not OK."); return pointcloud; } - scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); + 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++; + } } - // todo - // if (cnt == 0) { - // RCLCPP_ERROR_STREAM( - // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - // << "pointclouds were generated. Last azimuth: " << last_azimuth); - // } + 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 HesaiCalibrationConfigurationBase & calibration_configuration) + const CalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( "SetCalibrationConfiguration. Not yet implemented (" + diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 298431f7b..338163d60 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -12,8 +12,8 @@ namespace drivers namespace vlp16 { Vlp16Decoder::Vlp16Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) { sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; @@ -56,6 +56,11 @@ Vlp16Decoder::Vlp16Decoder( phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); } +bool Vlp16Decoder::hasScanned() +{ + return has_scanned_; +} + std::tuple Vlp16Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -82,9 +87,11 @@ int Vlp16Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING; } -void Vlp16Decoder::reset_pointcloud(double time_stamp) +void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { scan_pc_->points.clear(); + max_pts_ = n_pts * pointsPerPacket(); + scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -131,14 +138,12 @@ void Vlp16Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_seconds) +void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { - checkAndHandleScanComplete(packet, packet_seconds, phase_); - - const raw_packet_t * raw = (const raw_packet_t *)packet.data(); + const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = packet[RETURN_MODE_INDEX]; + const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (uint block = 0; block < BLOCKS_PER_PACKET; block++) { @@ -198,7 +203,7 @@ void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_se block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1]; } // Apply timestamp if this is the first new packet in the scan. - auto block_timestamp = packet_seconds; + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -211,7 +216,7 @@ void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_se continue; } { - const VelodyneLaserCorrection & corrections = + VelodyneLaserCorrection & corrections = calibration_configuration_->velodyne_calibration.laser_corrections[dsr]; float distance = current_return.uint * calibration_configuration_->velodyne_calibration.distance_resolution_m; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 3b82bf522..ec3e06745 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -12,8 +12,8 @@ namespace drivers namespace vlp32 { Vlp32Decoder::Vlp32Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) { sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; @@ -56,6 +56,11 @@ Vlp32Decoder::Vlp32Decoder( } } +bool Vlp32Decoder::hasScanned() +{ + return has_scanned_; +} + std::tuple Vlp32Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -80,10 +85,12 @@ int Vlp32Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vlp32Decoder::reset_pointcloud(double time_stamp) +void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); + max_pts_ = n_pts * pointsPerPacket(); + scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -130,12 +137,10 @@ void Vlp32Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_seconds) +void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { - checkAndHandleScanComplete(packet, packet_seconds, phase_); - - const raw_packet_t * raw = (const raw_packet_t *)packet.data(); - uint8_t return_mode = packet[RETURN_MODE_INDEX]; + const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; + uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (int i = 0; i < BLOCKS_PER_PACKET; i++) { @@ -165,7 +170,7 @@ void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_se i % 2 ? raw->blocks[i - 1].data[k + 1] : raw->blocks[i + 1].data[k + 1]; } // Apply timestamp if this is the first new packet in the scan. - auto block_timestamp = packet_seconds; + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index a3bff8fdc..c42f9b0b5 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -12,8 +12,8 @@ namespace drivers namespace vls128 { Vls128Decoder::Vls128Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) { sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; @@ -56,6 +56,11 @@ Vls128Decoder::Vls128Decoder( } } +bool Vls128Decoder::hasScanned() +{ + return has_scanned_; +} + std::tuple Vls128Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -82,10 +87,12 @@ int Vls128Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vls128Decoder::reset_pointcloud(double time_stamp) +void Vls128Decoder::reset_pointcloud(size_t n_pts, double time_stamp) { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); + max_pts_ = n_pts * pointsPerPacket(); + scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -132,14 +139,12 @@ void Vls128Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_seconds) +void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { - checkAndHandleScanComplete(packet, packet_seconds, phase_); - - const raw_packet_t * raw = (const raw_packet_t *)packet.data(); + const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = packet[RETURN_MODE_INDEX]; + const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (uint block = 0; block < static_cast(BLOCKS_PER_PACKET - (4 * dual_return)); block++) { @@ -218,7 +223,7 @@ void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_s block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1]; } // Apply timestamp if this is the first new packet in the scan. - auto block_timestamp = packet_seconds; + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -235,7 +240,7 @@ void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_s j + bank_origin; // offset the laser in this block by which block it's in const uint firing_order = laser_number / 8; // VLS-128 fires 8 lasers at a time - const VelodyneLaserCorrection & corrections = + VelodyneLaserCorrection & corrections = calibration_configuration_->velodyne_calibration.laser_corrections[laser_number]; float distance = current_return.uint * VLP128_DISTANCE_RESOLUTION; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp index 310e75697..b0f664c8c 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -9,8 +9,8 @@ namespace nebula namespace drivers { VelodyneDriver::VelodyneDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_configuration) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; @@ -46,24 +46,20 @@ Status VelodyneDriver::SetCalibrationConfiguration( calibration_configuration.calibration_file + ")"); } -std::tuple VelodyneDriver::ParseCloudPacket( - const std::vector & packet, int32_t packet_seconds) +std::tuple VelodyneDriver::ConvertScanToPointcloud( + const std::shared_ptr & velodyne_scan) { std::tuple pointcloud; - - if (driver_status_ != nebula::Status::OK) - { - auto logger = rclcpp::get_logger("VelodyneDriver"); - RCLCPP_ERROR(logger, "Driver not OK."); - return pointcloud; - } - - scan_decoder_->unpack(packet, packet_seconds); - if (scan_decoder_->hasScanned()) - { + if (driver_status_ == nebula::Status::OK) { + scan_decoder_->reset_pointcloud( + velodyne_scan->packets.size(), rclcpp::Time(velodyne_scan->packets.front().stamp).seconds()); + for (auto & packet : velodyne_scan->packets) { + scan_decoder_->unpack(packet); + } pointcloud = scan_decoder_->get_pointcloud(); + } else { + std::cout << "not ok driver_status_ = " << driver_status_ << std::endl; } - return pointcloud; } Status VelodyneDriver::GetStatus() 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 b8bbe345c..a7681323c 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,20 +15,18 @@ #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 -#include -#include -#include +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" #include #include @@ -37,7 +35,7 @@ namespace nebula { namespace ros { -class HesaiRosOfflineExtractBag final : public rclcpp::Node +class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -48,7 +46,12 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration); + std::shared_ptr calibration_configuration) override; + + Status InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration, + std::shared_ptr correction_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 8e26515bf..711502e6a 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp @@ -36,7 +36,7 @@ namespace nebula namespace ros { /// @brief Offline hesai driver usage example (Output PCD data) -class HesaiRosOfflineExtractSample final : public rclcpp::Node +class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosWrapperBase { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -51,7 +51,17 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_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); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp index 33966d955..c1910d294 100644 --- a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp @@ -48,21 +48,21 @@ namespace nebula namespace ros { /// @brief Offline velodyne driver usage example (Output PCD data) -class VelodyneRosOfflineExtractBag final : public rclcpp::Node +class VelodyneRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @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); + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; /// @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 f63847cca..6f1163279 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 649085018..9c1266347 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,11 +56,12 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - correction_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_), - calibration_cfg_ptr_); + std::static_pointer_cast(calibration_cfg_ptr_)); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -68,55 +69,170 @@ 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), - calibration_configuration); + std::static_pointer_cast(calibration_configuration)); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractBag::GetStatus() +Status HesaiRosOfflineExtractBag::InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration, + std::shared_ptr correction_configuration) { - return wrapper_status_; + // 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 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 = 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; descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - descriptor.floating_point_range = float_range(0, 360, 0.01); - sensor_configuration.scan_phase = this->declare_parameter("scan_phase", 0., descriptor); - this->get_parameter("scan_phase").as_double(); + 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(); } - - calibration_configuration.calibration_file = - this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_file_path = - this->declare_parameter("correction_file", "", 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("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(); } - - 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; @@ -180,83 +296,75 @@ Status HesaiRosOfflineExtractBag::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } + // return Status::OK; - pcl::PCDWriter pcd_writer; + pcl::PCDWriter writer; - std::unique_ptr bag_writer{}; + std::unique_ptr writer_; + bool needs_open = true; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - 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); + 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; } - } - - if (out_num <= out_cnt) { - break; } } + // close on scope exit } return Status::OK; } 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 8e134f8aa..a57d9b96e 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -56,11 +56,12 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - correction_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_), - calibration_cfg_ptr_); + std::static_pointer_cast(calibration_cfg_ptr_)); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -68,20 +69,31 @@ 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), - calibration_configuration); + std::static_pointer_cast(calibration_configuration)); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractSample::GetStatus() +Status HesaiRosOfflineExtractSample::InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration, + std::shared_ptr correction_configuration) { - return wrapper_status_; + // 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 HesaiRosOfflineExtractSample::GetStatus() {return wrapper_status_;} + Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, @@ -107,7 +119,7 @@ Status HesaiRosOfflineExtractSample::GetParameters( sensor_configuration.return_mode = // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); + this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -284,19 +296,12 @@ Status HesaiRosOfflineExtractSample::ReadBag() // nebula::drivers::NebulaPointCloudPtr pointcloud = // driver_ptr_->ConvertScanToPointcloud( // std::make_shared(extracted_msg)); - 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"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - writer.writeBinary((o_dir / fn).string(), *pointcloud); - } + 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); } } // close on scope exit diff --git a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp index ce3a69966..913898b7c 100644 --- a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp @@ -33,22 +33,26 @@ VelodyneRosOfflineExtractBag::VelodyneRosOfflineExtractBag( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_ ,calibration_cfg_ptr_); + 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_); } Status VelodyneRosOfflineExtractBag::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(calibration_configuration)); return driver_ptr_->GetStatus(); } @@ -334,47 +338,38 @@ Status VelodyneRosOfflineExtractBag::ReadBag() // 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"; - for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( - std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.data.size())), - pkt.stamp.sec); - auto pointcloud = std::get<0>(pointcloud_ts); - - if (!pointcloud) { - continue; - } - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - 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, "velodyne_msgs/msg/VelodyneScan", - 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); - } - } - if (out_num <= out_cnt) { - break; + 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, "velodyne_msgs/msg/VelodyneScan", + 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); } } + if (out_num <= out_cnt) { + break; + } } } // close on scope exit 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 13ad0ed53..a6d78c45e 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; // NOLINT(build/namespaces) +using namespace boost::endian; 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,9 +158,7 @@ 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: "; @@ -258,7 +256,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]; @@ -268,17 +266,29 @@ 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 << ", "; @@ -341,7 +351,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) { @@ -453,7 +463,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) { @@ -479,7 +489,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 10a1ec93f..1b21ddae2 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,7 +30,6 @@ #include #include -#include #include namespace nebula @@ -104,7 +103,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 +class HesaiHwInterface : NebulaHwInterfaceBase { private: struct ptc_error_t @@ -121,11 +120,15 @@ class HesaiHwInterface 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::function & buffer)> - cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ - - std::mutex mtx_inflight_tcp_request_; + 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*/ int prev_phase_{}; @@ -169,12 +172,6 @@ class HesaiHwInterface /// @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. @@ -201,17 +198,17 @@ class HesaiHwInterface /// @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(std::vector & buffer); + void ReceiveSensorPacketCallback(const std::vector & buffer) final; /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart(); + Status SensorInterfaceStart() final; /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop(); + Status SensorInterfaceStop() final; /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status @@ -220,11 +217,12 @@ class HesaiHwInterface /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration); + std::shared_ptr sensor_configuration) final; /// @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(); @@ -390,13 +388,13 @@ class HesaiHwInterface /// @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/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index 858e29488..fcb907a15 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp @@ -11,16 +11,17 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif - +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" +#include "nebula_common/velodyne/velodyne_common.hpp" +#include "nebula_common/velodyne/velodyne_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" -#include -#include -#include -#include - #include +#include +#include + #include #include @@ -31,20 +32,30 @@ namespace nebula namespace drivers { /// @brief Hardware interface of velodyne driver -class VelodyneHwInterface +class VelodyneHwInterface : NebulaHwInterfaceBase { private: std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; - std::shared_ptr sensor_configuration_; - std::function&)> - cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ + std::shared_ptr sensor_configuration_; + std::shared_ptr calibration_configuration_; + 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*/ + + uint16_t packet_first_azm_ = 0; + uint16_t packet_first_azm_phased_ = 0; + uint16_t packet_last_azm_ = 0; + uint16_t packet_last_azm_phased_ = 0; + uint16_t prev_packet_first_azm_phased_ = 0; + uint16_t phase_ = 0; + uint processed_packets_ = 0; std::shared_ptr boost_ctx_; std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> http_client_driver_; - std::mutex mtx_inflight_request_; - std::string TARGET_STATUS{"/cgi/status.json"}; std::string TARGET_DIAG{"/cgi/diag.json"}; std::string TARGET_SNAPSHOT{"/cgi/snapshot.hdl"}; @@ -56,9 +67,6 @@ class VelodyneHwInterface std::string TARGET_RESET{"/cgi/reset"}; void StringCallback(const std::string & str); - std::string HttpGetRequest(const std::string & endpoint); - std::string HttpPostRequest(const std::string & endpoint, const std::string & body); - /// @brief Get a one-off HTTP client to communicate with the hardware /// @param ctx IO Context /// @param hcd Got http client driver @@ -78,7 +86,7 @@ class VelodyneHwInterface /// @param tree Current settings (property_tree) /// @return Resulting status VelodyneStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, boost::property_tree::ptree tree); std::shared_ptr parent_node_logger; @@ -98,38 +106,38 @@ class VelodyneHwInterface /// @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(std::vector & buffer); + void ReceiveSensorPacketCallback(const std::vector & buffer) final; /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart(); + Status SensorInterfaceStart() final; /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop(); + Status SensorInterfaceStop() final; /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration); + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status Status GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration); + CalibrationConfigurationBase & calibration_configuration) final; /// @brief Initializing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status InitializeSensorConfiguration( - std::shared_ptr sensor_configuration); + std::shared_ptr sensor_configuration); /// @brief Setting sensor configuration with InitializeSensorConfiguration & /// CheckAndSetConfigBySnapshotAsync /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration); + std::shared_ptr sensor_configuration) final; /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status Status RegisterScanCallback( - std::function& packet)> scan_callback); + std::function)> scan_callback); /// @brief Parsing JSON string to property_tree /// @param str JSON string @@ -209,6 +217,95 @@ class VelodyneHwInterface /// @return Resulting status VelodyneStatus SetNetDhcp(bool use_dhcp); + /// @brief Initializing HTTP client (async) + /// @return Resulting status + VelodyneStatus InitHttpClientAsync(); + /// @brief Getting the current operational state and parameters of the sensor (async) + /// @param str_callback Callback function for received JSON string + /// @return Resulting status + VelodyneStatus GetStatusAsync(std::function str_callback); + /// @brief Getting the current operational state and parameters of the sensor (async) + /// @return Resulting status + VelodyneStatus GetStatusAsync(); + /// @brief Getting diagnostic information from the sensor (async) + /// @param str_callback Callback function for received JSON string + /// @return Resulting status + VelodyneStatus GetDiagAsync(std::function str_callback); + /// @brief Getting diagnostic information from the sensor (async) + /// @return Resulting status + VelodyneStatus GetDiagAsync(); + /// @brief Getting current sensor configuration and status data (async) + /// @param str_callback Callback function for received JSON string + /// @return Resulting status + VelodyneStatus GetSnapshotAsync(std::function str_callback); + /// @brief Getting current sensor configuration and status data (async) + /// @return Resulting status + VelodyneStatus GetSnapshotAsync(); + /// @brief Checking the current settings and changing the difference point + /// @return Resulting status + VelodyneStatus CheckAndSetConfigBySnapshotAsync( + std::shared_ptr sensor_configuration); + /// @brief Setting Motor RPM (async) + /// @param rpm the RPM of the motor + /// @return Resulting status + VelodyneStatus SetRpmAsync(uint16_t rpm); + /// @brief Setting Field of View Start (async) + /// @param fov_start FOV start + /// @return Resulting status + VelodyneStatus SetFovStartAsync(uint16_t fov_start); + /// @brief Setting Field of View End (async) + /// @param fov_end FOV end + /// @return Resulting status + VelodyneStatus SetFovEndAsync(uint16_t fov_end); + /// @brief Setting Return Type (async) + /// @param return_mode ReturnMode + /// @return Resulting status + VelodyneStatus SetReturnTypeAsync(ReturnMode return_mode); + /// @brief Save Configuration to the LiDAR memory (async) + /// @return Resulting status + VelodyneStatus SaveConfigAsync(); + /// @brief Resets the sensor (async) + /// @return Resulting status + VelodyneStatus ResetSystemAsync(); + /// @brief Turn laser state on (async) + /// @return Resulting status + VelodyneStatus LaserOnAsync(); + /// @brief Turn laser state off (async) + /// @return Resulting status + VelodyneStatus LaserOffAsync(); + /// @brief Turn laser state on/off (async) + /// @param on is ON + /// @return Resulting status + VelodyneStatus LaserOnOffAsync(bool on); + /// @brief Setting host (destination) IP address (async) + /// @param addr destination IP address + /// @return Resulting status + VelodyneStatus SetHostAddrAsync(std::string addr); + /// @brief Setting host (destination) data port (async) + /// @param dport destination data port + /// @return Resulting status + VelodyneStatus SetHostDportAsync(uint16_t dport); + /// @brief Setting host (destination) telemetry port (async) + /// @param tport destination telemetry port + /// @return Resulting status + VelodyneStatus SetHostTportAsync(uint16_t tport); + /// @brief Setting network (sensor) IP address (async) + /// @param addr sensor IP address + /// @return Resulting status + VelodyneStatus SetNetAddrAsync(std::string addr); + /// @brief Setting the network mask of the sensor (async) + /// @param mask Network mask + /// @return Resulting status + VelodyneStatus SetNetMaskAsync(std::string mask); + /// @brief Setting the gateway address of the sensor (async) + /// @param gateway Gateway address + /// @return Resulting status + VelodyneStatus SetNetGatewayAsync(std::string gateway); + /// @brief This determines if the sensor is to rely on a DHCP server for its IP address (async) + /// @param use_dhcp DHCP on + /// @return Resulting status + VelodyneStatus SetNetDhcpAsync(bool use_dhcp); + /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); 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 29b260098..fccb2a622 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,19 +17,40 @@ 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)} + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, + scan_cloud_ptr_{std::make_unique()} { } HesaiHwInterface::~HesaiHwInterface() { - FinalizeTcpDriver(); +#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 } 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; @@ -43,16 +64,15 @@ 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"); @@ -68,26 +88,21 @@ 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), @@ -103,7 +118,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(); @@ -129,10 +144,65 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( } Status HesaiHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - sensor_configuration_ = - std::static_pointer_cast(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; + } return Status::OK; } @@ -169,23 +239,68 @@ Status HesaiHwInterface::SensorInterfaceStart() } Status HesaiHwInterface::RegisterScanCallback( - std::function &)> scan_callback) + std::function)> scan_callback) { - cloud_packet_callback_ = std::move(scan_callback); + scan_reception_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) { - cloud_packet_callback_(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(); + } + } } Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration( - const SensorConfigurationBase & sensor_configuration) +Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; @@ -229,9 +344,7 @@ Status HesaiHwInterface::InitializeTcpDriver() Status HesaiHwInterface::FinalizeTcpDriver() { try { - if (tcp_driver_) { - tcp_driver_->close(); - } + tcp_driver_->close(); } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; @@ -259,8 +372,7 @@ 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; } @@ -269,67 +381,106 @@ 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({}))); - auto diag_status = CheckSizeAndParse(response); + + 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)); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << diag_status; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; PrintInfo(ss.str()); - return diag_status; + return hesai_ptp_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({}))); - auto diag_port = CheckSizeAndParse(response); + + 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)); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << diag_port; + ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; PrintInfo(ss.str()); - return diag_port; + return hesai_ptp_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({}))); - auto diag_time = CheckSizeAndParse(response); + + 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)); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << diag_time; + ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; PrintInfo(ss.str()); - return diag_time; + return hesai_ptp_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({}))); - auto diag_grandmaster = CheckSizeAndParse(response); + + 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)); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << diag_grandmaster; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; PrintInfo(ss.str()); - return diag_grandmaster; + return hesai_ptp_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({}))); - return CheckSizeAndParse(response); + + 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; } 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({}))); - auto hesai_config = CheckSizeAndParse(response); + + if (response.size() != sizeof(HesaiConfig)) { + throw std::runtime_error("Unexpected payload size"); + } + + HesaiConfig hesai_config; + memcpy(&hesai_config, response.data(), sizeof(HesaiConfig)); + std::cout << "Config: " << hesai_config << std::endl; return hesai_config; } @@ -338,7 +489,15 @@ 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({}))); - return CheckSizeAndParse(response); + + 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; } Status HesaiHwInterface::SetSpinRate(uint16_t rpm) @@ -486,15 +645,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(yukkysaito) + //TODO break; case 2: // multi-section FOV - // TODO(yukkysaito) + //TODO break; } @@ -551,7 +710,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("HesaiPtpConfig has unexpected payload size"); + throw std::runtime_error("Unexpected payload size"); } else if (response.size() > sizeof(HesaiPtpConfig)) { PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } @@ -590,7 +749,15 @@ 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({}))); - return CheckSizeAndParse(response); + + 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; } void HesaiHwInterface::IOContextRun() @@ -764,9 +931,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; // NOLINT(build/namespaces) + using namespace std::chrono_literals; #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif @@ -798,9 +965,7 @@ 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)); @@ -818,10 +983,8 @@ 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; @@ -832,9 +995,7 @@ 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)); } @@ -842,9 +1003,7 @@ 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)); } @@ -931,7 +1090,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 @@ -948,26 +1107,18 @@ 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)); @@ -1003,7 +1154,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(); @@ -1013,7 +1164,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 @@ -1207,8 +1358,7 @@ 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"; } @@ -1218,11 +1368,10 @@ 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: @@ -1280,19 +1429,5 @@ 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_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index fdc1051ba..8a8bfefbe 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -7,50 +7,33 @@ namespace drivers VelodyneHwInterface::VelodyneHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, + scan_cloud_ptr_{std::make_unique()}, boost_ctx_{new boost::asio::io_context()}, http_client_driver_{new ::drivers::tcp_driver::HttpClientDriver(boost_ctx_)} { } -std::string VelodyneHwInterface::HttpGetRequest(const std::string & endpoint) -{ - std::lock_guard lock(mtx_inflight_request_); - if (!http_client_driver_->client()->isOpen()) { - http_client_driver_->client()->open(); - } - - std::string response = http_client_driver_->get(endpoint); - http_client_driver_->client()->close(); - return response; -} - -std::string VelodyneHwInterface::HttpPostRequest(const std::string & endpoint, const std::string & body) -{ - std::lock_guard lock(mtx_inflight_request_); - if (!http_client_driver_->client()->isOpen()) { - http_client_driver_->client()->open(); - } - - std::string response = http_client_driver_->post(endpoint, body); - http_client_driver_->client()->close(); - return response; -} - Status VelodyneHwInterface::InitializeSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - sensor_configuration_ = sensor_configuration; - return Status::OK; + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); + phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); + + GetDiagAsync(); + GetStatusAsync(); + Status status = Status::OK; + return status; } Status VelodyneHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - auto snapshot = GetSnapshot(); - auto tree = ParseJson(snapshot); - VelodyneStatus status = CheckAndSetConfig(sensor_configuration, tree); - - return status; + auto velodyne_sensor_configuration = + std::static_pointer_cast(sensor_configuration); + VelodyneStatus status = CheckAndSetConfigBySnapshotAsync(velodyne_sensor_configuration); + Status rt = status; + return rt; } Status VelodyneHwInterface::SensorInterfaceStart() @@ -72,19 +55,49 @@ Status VelodyneHwInterface::SensorInterfaceStart() } Status VelodyneHwInterface::RegisterScanCallback( - std::function& packet)> scan_callback) + std::function)> scan_callback) { - cloud_packet_callback_ = std::move(scan_callback); + scan_reception_callback_ = std::move(scan_callback); return Status::OK; } -void VelodyneHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) -{ - if (!cloud_packet_callback_) { - return; +void VelodyneHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +{ + // Process current packet + const uint32_t buffer_size = buffer.size(); + velodyne_msgs::msg::VelodynePacket velodyne_packet; + std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, velodyne_packet.data.begin()); + 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(); + velodyne_packet.stamp.sec = static_cast(now_secs); + velodyne_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); + scan_cloud_ptr_->packets.emplace_back(velodyne_packet); + processed_packets_++; + + // Check if scan is complete + packet_first_azm_ = scan_cloud_ptr_->packets.back().data[2]; // lower word of azimuth block 0 + packet_first_azm_ |= scan_cloud_ptr_->packets.back().data[3] + << 8; // higher word of azimuth block 0 + + packet_last_azm_ = scan_cloud_ptr_->packets.back().data[1102]; + packet_last_azm_ |= scan_cloud_ptr_->packets.back().data[1103] << 8; + + packet_first_azm_phased_ = (36000 + packet_first_azm_ - phase_) % 36000; + packet_last_azm_phased_ = (36000 + packet_last_azm_ - phase_) % 36000; + + if (processed_packets_ > 1) { + if ( + packet_last_azm_phased_ < packet_first_azm_phased_ || + packet_first_azm_phased_ < prev_packet_first_azm_phased_) { + // Callback + scan_reception_callback_(std::move(scan_cloud_ptr_)); + scan_cloud_ptr_ = std::make_unique(); + processed_packets_ = 0; + } } - - cloud_packet_callback_(buffer); + prev_packet_first_azm_phased_ = packet_first_azm_phased_; } Status VelodyneHwInterface::SensorInterfaceStop() { @@ -99,6 +112,13 @@ Status VelodyneHwInterface::GetSensorConfiguration(SensorConfigurationBase & sen return Status::ERROR_1; } +Status VelodyneHwInterface::GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) +{ + PrintDebug(calibration_configuration.calibration_file); + return Status::ERROR_1; +} + VelodyneStatus VelodyneHwInterface::InitHttpClient() { try { @@ -113,6 +133,42 @@ VelodyneStatus VelodyneHwInterface::InitHttpClient() return Status::OK; } +VelodyneStatus VelodyneHwInterface::InitHttpClientAsync() +{ + try { + http_client_driver_->init_client(sensor_configuration_->sensor_ip, 80); + } catch (const std::exception & ex) { + VelodyneStatus status = Status::HTTP_CONNECTION_ERROR; + return status; + } + return Status::OK; +} + +VelodyneStatus VelodyneHwInterface::GetHttpClientDriverOnce( + std::shared_ptr ctx, + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) +{ + hcd = std::unique_ptr<::drivers::tcp_driver::HttpClientDriver>( + new ::drivers::tcp_driver::HttpClientDriver(ctx)); + try { + hcd->init_client(sensor_configuration_->sensor_ip, 80); + } catch (const std::exception & ex) { + Status status = Status::HTTP_CONNECTION_ERROR; + std::cerr << status << sensor_configuration_->sensor_ip << "," << 80 << std::endl; + return Status::HTTP_CONNECTION_ERROR; + } + return Status::OK; +} + +VelodyneStatus VelodyneHwInterface::GetHttpClientDriverOnce( + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd) +{ + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd_tmp; + auto st = GetHttpClientDriverOnce(std::make_shared(), hcd_tmp); + hcd = std::move(hcd_tmp); + return st; +} + void VelodyneHwInterface::StringCallback(const std::string & str) { std::cout << "VelodyneHwInterface::StringCallback: " << str << std::endl; @@ -132,20 +188,15 @@ boost::property_tree::ptree VelodyneHwInterface::ParseJson(const std::string & s } VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, boost::property_tree::ptree tree) { - VelodyneStatus status; - const auto & OK = VelodyneStatus::OK; - std::string target_key = "config.returns"; auto current_return_mode_str = tree.get(target_key); auto current_return_mode = nebula::drivers::ReturnModeFromStringVelodyne(tree.get(target_key)); if (sensor_configuration->return_mode != current_return_mode) { - status = SetReturnType(sensor_configuration->return_mode); - if (status != OK) return status; - + SetReturnTypeAsync(sensor_configuration->return_mode); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_return_mode_str << std::endl; std::cout << "current_return_mode: " << current_return_mode << std::endl; @@ -156,9 +207,7 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.rpm"; auto current_rotation_speed = tree.get(target_key); if (sensor_configuration->rotation_speed != current_rotation_speed) { - status = SetRpm(sensor_configuration->rotation_speed); - if (status != OK) return status; - + SetRpmAsync(sensor_configuration->rotation_speed); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_rotation_speed << std::endl; std::cout << "sensor_configuration->rotation_speed: " << sensor_configuration->rotation_speed @@ -173,9 +222,7 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_min_angle = 359; } if (setting_cloud_min_angle != current_cloud_min_angle) { - status = SetFovStart(setting_cloud_min_angle); - if (status != OK) return status; - + SetFovStartAsync(setting_cloud_min_angle); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_min_angle << std::endl; std::cout << "sensor_configuration->cloud_min_angle: " << setting_cloud_min_angle << std::endl; @@ -189,9 +236,7 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_max_angle = 359; } if (setting_cloud_max_angle != current_cloud_max_angle) { - status = SetFovEnd(setting_cloud_max_angle); - if (status != OK) return status; - + SetFovEndAsync(setting_cloud_max_angle); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_max_angle << std::endl; std::cout << "sensor_configuration->cloud_max_angle: " << setting_cloud_max_angle << std::endl; @@ -200,9 +245,7 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.addr"; auto current_host_addr = tree.get(target_key); if (sensor_configuration->host_ip != current_host_addr) { - status = SetHostAddr(sensor_configuration->host_ip); - if (status != OK) return status; - + SetHostAddrAsync(sensor_configuration->host_ip); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_addr << std::endl; std::cout << "sensor_configuration->host_ip: " << sensor_configuration->host_ip << std::endl; @@ -211,9 +254,7 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.dport"; auto current_host_dport = tree.get(target_key); if (sensor_configuration->data_port != current_host_dport) { - status = SetHostDport(sensor_configuration->data_port); - if (status != OK) return status; - + SetHostDportAsync(sensor_configuration->data_port); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_dport << std::endl; std::cout << "sensor_configuration->data_port: " << sensor_configuration->data_port @@ -223,35 +264,46 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.tport"; auto current_host_tport = tree.get(target_key); if (sensor_configuration->gnss_port != current_host_tport) { - status = SetHostTport(sensor_configuration->gnss_port); - if (status != OK) return status; - + SetHostTportAsync(sensor_configuration->gnss_port); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_tport << std::endl; std::cout << "sensor_configuration->gnss_port: " << sensor_configuration->gnss_port << std::endl; } - return OK; + return Status::WAITING_FOR_SENSOR_RESPONSE; } // sync std::string VelodyneHwInterface::GetStatus() { - return HttpGetRequest(TARGET_STATUS); + auto rt = http_client_driver_->get(TARGET_STATUS); + http_client_driver_->client()->close(); + // str_cb(rt); + // return Status::OK; + return rt; } std::string VelodyneHwInterface::GetDiag() { - auto rt = HttpGetRequest(TARGET_DIAG); + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + std::cout << "GetHttpClientDriverOnce" << std::endl; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return ""; + } + auto rt = hcd->get(TARGET_DIAG); std::cout << "read_response: " << rt << std::endl; return rt; } std::string VelodyneHwInterface::GetSnapshot() { - return HttpGetRequest(TARGET_SNAPSHOT); + auto rt = http_client_driver_->get(TARGET_SNAPSHOT); + http_client_driver_->client()->close(); + return rt; } VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) @@ -259,7 +311,8 @@ VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) if (rpm < 300 || 1200 < rpm || rpm % 60 != 0) { return VelodyneStatus::INVALID_RPM_ERROR; } - auto rt = HttpPostRequest(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); + auto rt = http_client_driver_->post(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -269,7 +322,8 @@ VelodyneStatus VelodyneHwInterface::SetFovStart(uint16_t fov_start) if (359 < fov_start) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = HttpPostRequest(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); + auto rt = http_client_driver_->post(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -279,7 +333,8 @@ VelodyneStatus VelodyneHwInterface::SetFovEnd(uint16_t fov_end) if (359 < fov_end) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = HttpPostRequest(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); + auto rt = http_client_driver_->post(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -300,7 +355,8 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re default: return VelodyneStatus::INVALID_RETURN_MODE_ERROR; } - auto rt = HttpPostRequest(TARGET_SETTING, body_str); + auto rt = http_client_driver_->post(TARGET_SETTING, body_str); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -308,7 +364,8 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re VelodyneStatus VelodyneHwInterface::SaveConfig() { std::string body_str = "submit"; - auto rt = HttpPostRequest(TARGET_SAVE, body_str); + auto rt = http_client_driver_->post(TARGET_SAVE, body_str); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -316,7 +373,8 @@ VelodyneStatus VelodyneHwInterface::SaveConfig() VelodyneStatus VelodyneHwInterface::ResetSystem() { std::string body_str = "reset_system"; - auto rt = HttpPostRequest(TARGET_RESET, body_str); + auto rt = http_client_driver_->post(TARGET_RESET, body_str); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -324,7 +382,8 @@ VelodyneStatus VelodyneHwInterface::ResetSystem() VelodyneStatus VelodyneHwInterface::LaserOn() { std::string body_str = "laser=on"; - auto rt = HttpPostRequest(TARGET_SETTING, body_str); + auto rt = http_client_driver_->post(TARGET_SETTING, body_str); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -332,7 +391,8 @@ VelodyneStatus VelodyneHwInterface::LaserOn() VelodyneStatus VelodyneHwInterface::LaserOff() { std::string body_str = "laser=off"; - auto rt = HttpPostRequest(TARGET_SETTING, body_str); + auto rt = http_client_driver_->post(TARGET_SETTING, body_str); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } @@ -340,61 +400,417 @@ VelodyneStatus VelodyneHwInterface::LaserOff() VelodyneStatus VelodyneHwInterface::LaserOnOff(bool on) { std::string body_str = (boost::format("laser=%s") % (on ? "on" : "off")).str(); - auto rt = HttpPostRequest(TARGET_SETTING, body_str); + auto rt = http_client_driver_->post(TARGET_SETTING, body_str); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostAddr(std::string addr) { - auto rt = HttpPostRequest(TARGET_HOST, (boost::format("addr=%s") % addr).str()); + auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("addr=%s") % addr).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostDport(uint16_t dport) { - auto rt = HttpPostRequest(TARGET_HOST, (boost::format("dport=%d") % dport).str()); + auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("dport=%d") % dport).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostTport(uint16_t tport) { - auto rt = HttpPostRequest(TARGET_HOST, (boost::format("tport=%d") % tport).str()); + auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("tport=%d") % tport).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetAddr(std::string addr) { - auto rt = HttpPostRequest(TARGET_NET, (boost::format("addr=%s") % addr).str()); + auto rt = http_client_driver_->post(TARGET_NET, (boost::format("addr=%s") % addr).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetMask(std::string mask) { - auto rt = HttpPostRequest(TARGET_NET, (boost::format("mask=%s") % mask).str()); + auto rt = http_client_driver_->post(TARGET_NET, (boost::format("mask=%s") % mask).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetGateway(std::string gateway) { - auto rt = HttpPostRequest(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); + auto rt = http_client_driver_->post(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetDhcp(bool use_dhcp) { - auto rt = HttpPostRequest( + auto rt = http_client_driver_->post( TARGET_NET, (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); + http_client_driver_->client()->close(); StringCallback(rt); return Status::OK; } +VelodyneStatus VelodyneHwInterface::GetStatusAsync( + std::function str_callback) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncGet(str_callback, TARGET_STATUS); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::GetStatusAsync() +{ + return GetStatusAsync([this](const std::string & str) { StringCallback(str); }); +} + +VelodyneStatus VelodyneHwInterface::GetDiagAsync( + std::function str_callback) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncGet(str_callback, TARGET_DIAG); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::GetDiagAsync() +{ + return GetDiagAsync([this](const std::string & str) { StringCallback(str); }); +} + +VelodyneStatus VelodyneHwInterface::GetSnapshotAsync( + std::function str_callback) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + hcd->asyncGet(str_callback, TARGET_SNAPSHOT); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::GetSnapshotAsync() +{ + return GetSnapshotAsync([this](const std::string & str) { ParseJson(str); }); +} + +VelodyneStatus VelodyneHwInterface::CheckAndSetConfigBySnapshotAsync( + std::shared_ptr sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; + + return GetSnapshotAsync([this](const std::string & str) { + auto tree = ParseJson(str); + std::cout << "ParseJson OK\n"; + CheckAndSetConfig( + std::static_pointer_cast(sensor_configuration_), tree); + }); +} + +VelodyneStatus VelodyneHwInterface::SetRpmAsync(uint16_t rpm) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + if (rpm < 300 || 1200 < rpm || rpm % 60 != 0) { + return VelodyneStatus::INVALID_RPM_ERROR; + } + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, + (boost::format("rpm=%d") % rpm).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetFovStartAsync(uint16_t fov_start) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + if (359 < fov_start) { + return VelodyneStatus::INVALID_FOV_ERROR; + } + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_FOV, + (boost::format("start=%d") % fov_start).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetFovEndAsync(uint16_t fov_end) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + if (359 < fov_end) { + return VelodyneStatus::INVALID_FOV_ERROR; + } + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_FOV, + (boost::format("end=%d") % fov_end).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetReturnTypeAsync(nebula::drivers::ReturnMode return_mode) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + std::string body_str = ""; + switch (return_mode) { + case nebula::drivers::ReturnMode::SINGLE_STRONGEST: + body_str = "returns=Strongest"; + break; + case nebula::drivers::ReturnMode::SINGLE_LAST: + body_str = "returns=Last"; + break; + case nebula::drivers::ReturnMode::DUAL_ONLY: + body_str = "returns=Dual"; + break; + default: + return VelodyneStatus::INVALID_RETURN_MODE_ERROR; + } + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SaveConfigAsync() +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + std::string body_str = "submit"; + hcd->asyncPost([this](const std::string & str) { StringCallback(str); }, TARGET_SAVE, body_str); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::ResetSystemAsync() +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + std::string body_str = "reset_system"; + hcd->asyncPost([this](const std::string & str) { StringCallback(str); }, TARGET_RESET, body_str); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::LaserOnAsync() +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + std::string body_str = "laser=on"; + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::LaserOffAsync() +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + std::string body_str = "laser=off"; + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::LaserOnOffAsync(bool on) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + std::string body_str = (boost::format("laser=%s") % (on ? "on" : "off")).str(); + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_SETTING, body_str); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetHostAddrAsync(std::string addr) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_HOST, + (boost::format("addr=%s") % addr).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetHostDportAsync(uint16_t dport) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_HOST, + (boost::format("dport=%d") % dport).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetHostTportAsync(uint16_t tport) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_HOST, + (boost::format("tport=%d") % tport).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetNetAddrAsync(std::string addr) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_NET, + (boost::format("addr=%s") % addr).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetNetMaskAsync(std::string mask) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_NET, + (boost::format("mask=%s") % mask).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetNetGatewayAsync(std::string gateway) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_NET, + (boost::format("gateway=%s") % gateway).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + +VelodyneStatus VelodyneHwInterface::SetNetDhcpAsync(bool use_dhcp) +{ + auto ctx = std::make_shared(); + std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; + auto st = GetHttpClientDriverOnce(ctx, hcd); + if (st != Status::OK) { + return st; + } + + hcd->asyncPost( + [this](const std::string & str) { StringCallback(str); }, TARGET_NET, + (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); + ctx->run(); + return Status::WAITING_FOR_SENSOR_RESPONSE; +} + void VelodyneHwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; diff --git a/nebula_messages/nebula_msgs/CMakeLists.txt b/nebula_messages/nebula_msgs/CMakeLists.txt index e2b798d9d..2ffa068a5 100644 --- a/nebula_messages/nebula_msgs/CMakeLists.txt +++ b/nebula_messages/nebula_msgs/CMakeLists.txt @@ -21,4 +21,4 @@ rosidl_generate_interfaces(${PROJECT_NAME} std_msgs ) -ament_package() \ No newline at end of file +ament_package() diff --git a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg index be5898eb1..f6ba285d8 100644 --- a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg +++ b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg @@ -1,2 +1,2 @@ builtin_interfaces/Time stamp -uint8[] data \ No newline at end of file +uint8[] data diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml index 3eb05e603..cdd135c37 100644 --- a/nebula_messages/nebula_msgs/package.xml +++ b/nebula_messages/nebula_msgs/package.xml @@ -22,4 +22,4 @@ ament_cmake - \ No newline at end of file + diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index d756a3d51..dd81e1ac0 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -23,8 +23,6 @@ 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 @@ -35,31 +33,63 @@ include_directories( ) link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) ## Hesai -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 +# 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 ) -rclcpp_components_register_node(hesai_ros_wrapper - PLUGIN "HesaiRosWrapper" - EXECUTABLE hesai_ros_wrapper_node +rclcpp_components_register_node(hesai_driver_ros_wrapper + PLUGIN "HesaiDriverRosWrapper" + EXECUTABLE hesai_driver_ros_wrapper_node ) ## Velodyne -ament_auto_add_library(velodyne_ros_wrapper SHARED - src/velodyne/velodyne_ros_wrapper.cpp - src/velodyne/decoder_wrapper.cpp - src/velodyne/hw_interface_wrapper.cpp - src/velodyne/hw_monitor_wrapper.cpp - src/common/parameter_descriptors.cpp +# Hw Interface +ament_auto_add_library(velodyne_hw_ros_wrapper SHARED + src/velodyne/velodyne_hw_interface_ros_wrapper.cpp + ) +rclcpp_components_register_node(velodyne_hw_ros_wrapper + PLUGIN "VelodyneHwInterfaceRosWrapper" + EXECUTABLE velodyne_hw_ros_wrapper_node + ) + + +# Monitor +ament_auto_add_library(velodyne_hw_monitor_ros_wrapper SHARED + src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp + ) +rclcpp_components_register_node(velodyne_hw_monitor_ros_wrapper + PLUGIN "VelodyneHwMonitorRosWrapper" + EXECUTABLE velodyne_hw_monitor_ros_wrapper_node ) -rclcpp_components_register_node(velodyne_ros_wrapper - PLUGIN "VelodyneRosWrapper" - EXECUTABLE velodyne_ros_wrapper_node +# DriverDecoder +ament_auto_add_library(velodyne_driver_ros_wrapper SHARED + src/velodyne/velodyne_decoder_ros_wrapper.cpp + ) +rclcpp_components_register_node(velodyne_driver_ros_wrapper + PLUGIN "VelodyneDriverRosWrapper" + EXECUTABLE velodyne_driver_ros_wrapper_node ) ## Robosense diff --git a/nebula_ros/config/lidar/robosense/Bpearl.param.yaml b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml deleted file mode 100644 index 1be1f64c5..000000000 --- a/nebula_ros/config/lidar/robosense/Bpearl.param.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**: - ros__parameters: - host_ip: "255.255.255.255" - sensor_ip: "192.168.1.201" - data_port: 2368 - frame_id: data_link - diag_span: 1000 - scan_phase: 0.0 - setup_sensor: true - sensor_model: Bpearl - return_mode: Dual - gnss_port: 2369 - packet_mtu_size: 1500 - cloud_min_angle: 0 - cloud_max_angle: 360 - dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/robosense/Helios.param.yaml b/nebula_ros/config/lidar/robosense/Helios.param.yaml deleted file mode 100644 index b745d6e93..000000000 --- a/nebula_ros/config/lidar/robosense/Helios.param.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**: - ros__parameters: - host_ip: "255.255.255.255" - sensor_ip: "192.168.1.201" - data_port: 2368 - frame_id: data_link - diag_span: 1000 - scan_phase: 0.0 - setup_sensor: true - sensor_model: Helios - return_mode: Dual - gnss_port: 2369 - packet_mtu_size: 1500 - cloud_min_angle: 0 - cloud_max_angle: 360 - dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml deleted file mode 100644 index c8b49ed78..000000000 --- a/nebula_ros/config/lidar/velodyne/VLP16.param.yaml +++ /dev/null @@ -1,21 +0,0 @@ -/**: - ros__parameters: - calibration_file: $(find-pkg-share nebula_decoders)/calibration/velodyne/$(var sensor_model).yaml - setup_sensor: true - advanced_diagnostics: false - launch_hw: true - sensor_model: VLP16 - return_mode: Dual - gnss_port: 2369 - min_range: 0.3 - max_range: 300 - packet_mtu_size: 1500 - rotation_speed: 600 - cloud_min_angle: 0 - cloud_max_angle: 360 - diag_span: 1000 - scan_phase: 0 - host_ip: "255.255.255.255" - sensor_ip: "192.168.1.201" - data_port: 2368 - frame_id: data_link \ No newline at end of file diff --git a/nebula_ros/config/lidar/velodyne/VLP32.param.yaml b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml deleted file mode 100644 index d2f328476..000000000 --- a/nebula_ros/config/lidar/velodyne/VLP32.param.yaml +++ /dev/null @@ -1,21 +0,0 @@ -/**: - ros__parameters: - calibration_file: "$(find-pkg-share nebula_decoders)/calibration/velodyne/VLP32.yaml" - setup_sensor: true - diag_span: 1000 - advanced_diagnostics: false - launch_hw: false - sensor_model: VLP32 - return_mode: Dual - host_ip: "255.255.255.255" - sensor_ip: "192.168.1.201" - data_port: 2368 - gnss_port: 2369 - frame_id: velodyne - scan_phase: 0.0 - min_range: 0.3 - max_range: 300.0 - packet_mtu_size: 1500 - rotation_speed: 360 - cloud_min_angle: 0 - cloud_max_angle: 360 diff --git a/nebula_ros/config/lidar/velodyne/VLS128.param.yaml b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml deleted file mode 100644 index 19c1cfdb7..000000000 --- a/nebula_ros/config/lidar/velodyne/VLS128.param.yaml +++ /dev/null @@ -1,21 +0,0 @@ -/**: - ros__parameters: - calibration_file: "$(find-pkg-share nebula_decoders)/calibration/velodyne/VLS128.yaml" - setup_sensor: true - diag_span: 1000 - advanced_diagnostics: false - launch_hw: false - sensor_model: VLS128 - return_mode: Dual - host_ip: "255.255.255.255" - sensor_ip: "192.168.1.201" - data_port: 2368 - gnss_port: 2369 - frame_id: velodyne - scan_phase: 0.0 - min_range: 0.3 - max_range: 300.0 - packet_mtu_size: 1500 - rotation_speed: 360 - cloud_min_angle: 0 - cloud_max_angle: 360 diff --git a/nebula_ros/config/robosense/Bpearl.yaml b/nebula_ros/config/robosense/Bpearl.yaml new file mode 100644 index 000000000..9ccb42e56 --- /dev/null +++ b/nebula_ros/config/robosense/Bpearl.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + sensor_model: "Bpearl" # See readme for supported models + sensor_ip: "192.168.1.200" # Lidar Sensor IP + host_ip: "192.168.1.102" # Broadcast IP from Sensor + frame_id: "robosense" + data_port: 6699 # LiDAR Data Port (MSOP) + gnss_port: 7788 # LiDAR GNSS Port (DIFOP) + scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] + diag_span: 1000 # milliseconds + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/robosense/Helios.yaml b/nebula_ros/config/robosense/Helios.yaml new file mode 100644 index 000000000..24350822d --- /dev/null +++ b/nebula_ros/config/robosense/Helios.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + sensor_model: "Helios" # See readme for supported models + sensor_ip: "192.168.1.200" # Lidar Sensor IP + host_ip: "192.168.1.102" # Broadcast IP from Sensor + frame_id: "robosense" + data_port: 6699 # LiDAR Data Port (MSOP) + gnss_port: 7788 # LiDAR GNSS Port (DIFOP) + scan_phase: 0.0 # Angle where scans begin (degrees, [0.,360.] + diag_span: 1000 # milliseconds + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/velodyne/VLP16.yaml b/nebula_ros/config/velodyne/VLP16.yaml new file mode 100644 index 000000000..1daf3a4c6 --- /dev/null +++ b/nebula_ros/config/velodyne/VLP16.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + sensor_model: "VLP16" + sensor_ip: "192.168.1.201" + host_ip: "255.255.255.255" + frame_id: "velodyne" + data_port: 2368 + gnss_port: 2369 + setup_sensor: True + online: True diff --git a/nebula_ros/config/velodyne/VLP32.yaml b/nebula_ros/config/velodyne/VLP32.yaml new file mode 100644 index 000000000..ef5ae14c1 --- /dev/null +++ b/nebula_ros/config/velodyne/VLP32.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + sensor_model: "VLP32" + sensor_ip: "192.168.1.201" + host_ip: "255.255.255.255" + frame_id: "velodyne" + data_port: 2368 + gnss_port: 2369 + setup_sensor: True + online: True diff --git a/nebula_ros/config/velodyne/VLS128.yaml b/nebula_ros/config/velodyne/VLS128.yaml new file mode 100644 index 000000000..db73ecc05 --- /dev/null +++ b/nebula_ros/config/velodyne/VLS128.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + sensor_model: "VLS128" + sensor_ip: "192.168.1.201" + host_ip: "255.255.255.255" + frame_id: "velodyne" + data_port: 2368 + gnss_port: 2369 + setup_sensor: True + online: True diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp deleted file mode 100644 index aca385ce3..000000000 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ /dev/null @@ -1,50 +0,0 @@ -#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 deleted file mode 100644 index 6d4e38504..000000000 --- a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#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 deleted file mode 100644 index e6320f91e..000000000 --- a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#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 deleted file mode 100644 index b2bdf856c..000000000 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#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 new file mode 100644 index 000000000..52518fcd0 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp @@ -0,0 +1,106 @@ +#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 new file mode 100644 index 000000000..1802f87b2 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -0,0 +1,100 @@ +#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 new file mode 100644 index 000000000..cf8739279 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -0,0 +1,166 @@ +#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 deleted file mode 100644 index 0a60d3e8e..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ /dev/null @@ -1,91 +0,0 @@ -#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 deleted file mode 100644 index 0791af5e2..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#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 deleted file mode 100644 index 0b270623f..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#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/include/nebula_ros/velodyne/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp deleted file mode 100644 index 283b68368..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once - -#include "nebula_ros/common/parameter_descriptors.hpp" -#include "nebula_ros/common/watchdog_timer.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -class VelodyneDecoderWrapper -{ - using get_calibration_result_t = nebula::util::expected< - std::shared_ptr, nebula::Status>; - -public: - VelodyneDecoderWrapper( - 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 file - /// @param calibration_file_path The file to read from - /// @return The calibration data if successful, or an error code if not - get_calibration_result_t GetCalibrationData(const std::string & calibration_file_path); - - 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_{}; - velodyne_msgs::msg::VelodyneScan::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 \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp deleted file mode 100644 index 5c857fa12..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include "nebula_ros/common/parameter_descriptors.hpp" - -#include - -#include -#include - -#include - -namespace nebula -{ -namespace ros -{ -class VelodyneHwInterfaceWrapper -{ -public: - VelodyneHwInterfaceWrapper(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 \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp new file mode 100644 index 000000000..5bc79b6c8 --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp @@ -0,0 +1,83 @@ +#ifndef NEBULA_VelodyneDriverRosWrapper_H +#define NEBULA_VelodyneDriverRosWrapper_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_common/velodyne/velodyne_common.hpp" +#include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" +#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Ros wrapper of velodyne driver +class VelodyneDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +{ + std::shared_ptr driver_ptr_; + Status wrapper_status_; + rclcpp::Subscription::SharedPtr velodyne_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_; + + /// @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 Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @param calibration_configuration Output of CalibrationConfiguration + /// @return Resulting status + Status GetParameters( + drivers::VelodyneSensorConfiguration & sensor_configuration, + drivers::VelodyneCalibrationConfiguration & calibration_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 VelodyneDriverRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Callback for VelodyneScan subscriber + /// @param scan_msg Received VelodyneScan + void ReceiveScanMsgCallback(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg); + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_VelodyneDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..5a0e094c3 --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp @@ -0,0 +1,202 @@ +#ifndef NEBULA_VelodyneHwInterfaceRosWrapper_H +#define NEBULA_VelodyneHwInterfaceRosWrapper_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/velodyne/velodyne_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" + +#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 interface ros wrapper of velodyne driver +class VelodyneHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase +{ + drivers::VelodyneHwInterface hw_interface_; + Status interface_status_; + + drivers::VelodyneSensorConfiguration sensor_configuration_; + drivers::VelodyneCalibrationConfiguration calibration_configuration_; + + /// @brief Received Velodyne message publisher + rclcpp::Publisher::SharedPtr velodyne_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 VelodyneScan + /// @param scan_buffer Received VelodyneScan + void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + +public: + explicit VelodyneHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + /// @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::VelodyneSensorConfiguration & sensor_configuration); + +private: // ROS Diagnostics + /* +diagnostic_updater::Updater diagnostics_updater_; +void InitializeVelodyneDiagnostics(); +*/ + + /// @brief Get value from property_tree + /// @param pt property_tree + /// @param key Pey string + /// @return Value + std::string GetPtreeValue( + std::shared_ptr pt, const std::string & key); + /* + rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; +*/ + std::shared_ptr current_diag_tree; + /* + void OnVelodyneDiagnosticsTimer(); + void VelodyneCheckTopHv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopAdTemp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopLm20Temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopPwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopPwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopPwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopPwrRaw(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckTopPwrVccint(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotIOut(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotPwr12v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotLm20Temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotPwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotPwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotPwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotPwrVIn(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckBotPwr125v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckVhv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckAdcNf(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckAdcStats(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckIxe(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckAdctpStat(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; +*/ + std::shared_ptr current_status_tree; + /* + void OnVelodyneStatusTimer(); + void VelodyneCheckGpsPpsState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckGpsPosition(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckMotorState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckMotorRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckMotorLock(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckMotorPhase(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void VelodyneCheckLaserState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void VelodyneCheckSnapshot(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void OnVelodyneSnapshotTimer(); + rclcpp::TimerBase::SharedPtr diagnostics_snapshot_timer_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; +*/ + std::shared_ptr current_snapshot; + std::shared_ptr current_snapshot_tree; + std::shared_ptr current_snapshot_time; + // rclcpp::Time current_snapshot_time; + // std::shared_ptr current_diag_status; + uint8_t current_diag_status; + + uint16_t diag_span_; + std::mutex mtx_diag; + std::mutex mtx_status; + std::mutex mtx_config_; + + void curl_callback(std::string err, std::string body); + /* + const char* key_volt_temp_top_hv; + const char* key_volt_temp_top_ad_temp; + const char* key_volt_temp_top_lm20_temp; + const char* key_volt_temp_top_pwr_5v; + const char* key_volt_temp_top_pwr_2_5v; + const char* key_volt_temp_top_pwr_3_3v; + const char* key_volt_temp_top_pwr_raw; + const char* key_volt_temp_top_pwr_vccint; + const char* key_volt_temp_bot_i_out; + const char* key_volt_temp_bot_pwr_1_2v; + const char* key_volt_temp_bot_lm20_temp; + const char* key_volt_temp_bot_pwr_5v; + const char* key_volt_temp_bot_pwr_2_5v; + const char* key_volt_temp_bot_pwr_3_3v; + const char* key_volt_temp_bot_pwr_v_in; + const char* key_volt_temp_bot_pwr_1_25v; + const char* key_vhv; + const char* key_adc_nf; + const char* key_adc_stats; + const char* key_ixe; + const char* key_adctp_stat; + const char* key_status_gps_pps_state; + const char* key_status_gps_pps_position; + const char* key_status_motor_state; + const char* key_status_motor_rpm; + const char* key_status_motor_lock; + const char* key_status_motor_phase; + const char* key_status_laser_state; +*/ + const char * not_supported_message; + + 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(); + + // rclcpp::callback_group::CallbackGroup::SharedPtr cbg_; + rclcpp::CallbackGroup::SharedPtr cbg_r_; + rclcpp::CallbackGroup::SharedPtr cbg_m_; +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_VelodyneHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp similarity index 68% rename from nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp rename to nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp index e695f3114..3f589bbbf 100644 --- a/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp @@ -1,64 +1,92 @@ -#pragma once +#ifndef NEBULA_VelodyneHwMonitorRosWrapper_H +#define NEBULA_VelodyneHwMonitorRosWrapper_H -#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/velodyne/velodyne_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" +#include #include -#include -#include #include +#include -#include -#include -#include -#include - -#include -#include +#include namespace nebula { namespace ros { -class VelodyneHwMonitorWrapper +/// @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) { -public: - VelodyneHwMonitorWrapper( - rclcpp::Node * const parent_node, - const std::shared_ptr & hw_interface, - std::shared_ptr & config); - - void OnConfigChange( - const std::shared_ptr & /* new_config */) - { + 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; +} - nebula::Status Status(); - -private: - void InitializeVelodyneDiagnostics(); +/// @brief Hardware monitor ros wrapper of velodyne driver +class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase +{ + drivers::VelodyneHwInterface hw_interface_; + Status interface_status_; - /// @brief Callback of the timer for getting the current lidar status - void OnVelodyneStatusTimer(); + drivers::VelodyneSensorConfiguration sensor_configuration_; + drivers::VelodyneCalibrationConfiguration calibration_configuration_; - /// @brief Callback of the timer for getting the current lidar snapshot - void OnVelodyneSnapshotTimer(); - - /// @brief Callback of the timer for getting the current lidar status & updating the diagnostics - void OnVelodyneDiagnosticsTimer(); + /// @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 VelodyneHwMonitorRosWrapper(const rclcpp::NodeOptions & options); + + /// @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::VelodyneSensorConfiguration & sensor_configuration); + +private: // ROS Diagnostics + diagnostic_updater::Updater diagnostics_updater_; + /// @brief Initializing diagnostics + void InitializeVelodyneDiagnostics(); /// @brief Get value from property_tree /// @param pt property_tree - /// @param mtx_pt the mutex associated with `pt` /// @param key Pey string /// @return Value std::string GetPtreeValue( - std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key); - + std::shared_ptr 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); + rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; + std::shared_ptr current_diag_tree; + /// @brief Callback of the timer for getting the current lidar status & updating the diagnostics + void OnVelodyneDiagnosticsTimer(); /// @brief Getting top:hv from the current property_tree /// @return tuple @@ -214,6 +242,8 @@ class VelodyneHwMonitorWrapper void VelodyneCheckAdctpStat(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; std::shared_ptr current_status_tree; + /// @brief Callback of the timer for getting the current lidar status + void OnVelodyneStatusTimer(); /// @brief Check gps:pps_state from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper void VelodyneCheckGpsPpsState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); @@ -256,111 +286,150 @@ class VelodyneHwMonitorWrapper /// @param diagnostics DiagnosticStatusWrapper void VelodyneCheckVoltage(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_; - - std::shared_ptr sensor_configuration_; - - uint16_t diag_span_; - bool show_advanced_diagnostics_; - - + /// @brief Callback of the timer for getting the current lidar snapshot + void OnVelodyneSnapshotTimer(); rclcpp::TimerBase::SharedPtr diagnostics_snapshot_timer_; rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; - rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; - std::shared_ptr current_snapshot; std::shared_ptr current_snapshot_tree; - std::shared_ptr current_diag_tree; std::shared_ptr current_snapshot_time; + // rclcpp::Time current_snapshot_time; + // std::shared_ptr current_diag_status; uint8_t current_diag_status; - std::mutex mtx_snapshot_; - std::mutex mtx_status_; - std::mutex mtx_diag_; - - std::string info_model_; - std::string info_serial_; - - static constexpr auto key_volt_temp_top_hv = "volt_temp.top.hv"; - static constexpr auto key_volt_temp_top_ad_temp = "volt_temp.top.ad_temp"; // only32 - static constexpr auto key_volt_temp_top_lm20_temp = "volt_temp.top.lm20_temp"; - static constexpr auto key_volt_temp_top_pwr_5v = "volt_temp.top.pwr_5v"; - static constexpr auto key_volt_temp_top_pwr_2_5v = "volt_temp.top.pwr_2_5v"; - static constexpr auto key_volt_temp_top_pwr_3_3v = "volt_temp.top.pwr_3_3v"; - static constexpr auto key_volt_temp_top_pwr_5v_raw = "volt_temp.top.pwr_5v_raw"; // only16 - static constexpr auto key_volt_temp_top_pwr_raw = "volt_temp.top.pwr_raw"; // only32 - static constexpr auto key_volt_temp_top_pwr_vccint = "volt_temp.top.pwr_vccint"; - static constexpr auto key_volt_temp_bot_i_out = "volt_temp.bot.i_out"; - static constexpr auto key_volt_temp_bot_pwr_1_2v = "volt_temp.bot.pwr_1_2v"; - static constexpr auto key_volt_temp_bot_lm20_temp = "volt_temp.bot.lm20_temp"; - static constexpr auto key_volt_temp_bot_pwr_5v = "volt_temp.bot.pwr_5v"; - static constexpr auto key_volt_temp_bot_pwr_2_5v = "volt_temp.bot.pwr_2_5v"; - static constexpr auto key_volt_temp_bot_pwr_3_3v = "volt_temp.bot.pwr_3_3v"; - static constexpr auto key_volt_temp_bot_pwr_v_in = "volt_temp.bot.pwr_v_in"; - static constexpr auto key_volt_temp_bot_pwr_1_25v = "volt_temp.bot.pwr_1_25v"; - static constexpr auto key_vhv = "vhv"; - static constexpr auto key_adc_nf = "adc_nf"; - static constexpr auto key_adc_stats = "adc_stats"; - static constexpr auto key_ixe = "ixe"; - static constexpr auto key_adctp_stat = "adctp_stat"; - static constexpr auto key_status_gps_pps_state = "gps.pps_state"; - static constexpr auto key_status_gps_pps_position = "gps.position"; - static constexpr auto key_status_motor_state = "motor.state"; - static constexpr auto key_status_motor_rpm = "motor.rpm"; - static constexpr auto key_status_motor_lock = "motor.lock"; - static constexpr auto key_status_motor_phase = "motor.phase"; - static constexpr auto key_status_laser_state = "laser.state"; - - static constexpr auto name_volt_temp_top_hv = "Top HV"; - static constexpr auto name_volt_temp_top_ad_temp = "Top A/D TD"; - static constexpr auto name_volt_temp_top_lm20_temp = "Top Temp"; - static constexpr auto name_volt_temp_top_pwr_5v = "Top 5v"; - static constexpr auto name_volt_temp_top_pwr_2_5v = "Top 2.5v"; - static constexpr auto name_volt_temp_top_pwr_3_3v = "Top 3.3v"; - static constexpr auto name_volt_temp_top_pwr_5v_raw = "Top 5v(RAW)"; - static constexpr auto name_volt_temp_top_pwr_raw = "Top RAW"; - static constexpr auto name_volt_temp_top_pwr_vccint = "Top VCCINT"; - static constexpr auto name_volt_temp_bot_i_out = "Bot I out"; - static constexpr auto name_volt_temp_bot_pwr_1_2v = "Bot 1.2v"; - static constexpr auto name_volt_temp_bot_lm20_temp = "Bot Temp"; - static constexpr auto name_volt_temp_bot_pwr_5v = "Bot 5v"; - static constexpr auto name_volt_temp_bot_pwr_2_5v = "Bot 2.5v"; - static constexpr auto name_volt_temp_bot_pwr_3_3v = "Bot 3.3v"; - static constexpr auto name_volt_temp_bot_pwr_v_in = "Bot V in"; - static constexpr auto name_volt_temp_bot_pwr_1_25v = "Bot 1.25v"; // N/A? - static constexpr auto name_vhv = "VHV"; - static constexpr auto name_adc_nf = "adc_nf"; - static constexpr auto name_adc_stats = "adc_stats"; - static constexpr auto name_ixe = "ixe"; - static constexpr auto name_adctp_stat = "adctp_stat"; - static constexpr auto name_status_gps_pps_state = "GPS PPS"; - static constexpr auto name_status_gps_pps_position = "GPS Position"; - static constexpr auto name_status_motor_state = "Motor State"; - static constexpr auto name_status_motor_rpm = "Motor RPM"; - static constexpr auto name_status_motor_lock = "Motor Lock"; - static constexpr auto name_status_motor_phase = "Motor Phase"; - static constexpr auto name_status_laser_state = "Laser State"; - - const std::string message_sep{": "}; - static constexpr auto not_supported_message = "Not supported"; - static constexpr auto error_message = "Error"; - - static constexpr auto key_info_model = "info.model"; - static constexpr auto key_info_serial = "info.serial"; - - static constexpr auto temperature_cold_message = "temperature cold"; - static constexpr auto temperature_hot_message = "temperature hot"; - - static constexpr auto voltage_low_message = "voltage low"; - static constexpr auto voltage_high_message = "voltage high"; - - static constexpr auto ampere_low_message = "ampere low"; - static constexpr auto ampere_high_message = "ampere high"; + uint16_t diag_span_; + std::mutex mtx_diag; + std::mutex mtx_status; + std::mutex mtx_config_; + + /// @brief Test callback function for getting json with curl + /// @param err Error + /// @param body Received body + void curl_callback(std::string err, std::string body); + + const char * key_volt_temp_top_hv; + const char * key_volt_temp_top_ad_temp; + const char * key_volt_temp_top_lm20_temp; + const char * key_volt_temp_top_pwr_5v; + const char * key_volt_temp_top_pwr_2_5v; + const char * key_volt_temp_top_pwr_3_3v; + const char * key_volt_temp_top_pwr_5v_raw; + const char * key_volt_temp_top_pwr_raw; + const char * key_volt_temp_top_pwr_vccint; + const char * key_volt_temp_bot_i_out; + const char * key_volt_temp_bot_pwr_1_2v; + const char * key_volt_temp_bot_lm20_temp; + const char * key_volt_temp_bot_pwr_5v; + const char * key_volt_temp_bot_pwr_2_5v; + const char * key_volt_temp_bot_pwr_3_3v; + const char * key_volt_temp_bot_pwr_v_in; + const char * key_volt_temp_bot_pwr_1_25v; + const char * key_vhv; + const char * key_adc_nf; + const char * key_adc_stats; + const char * key_ixe; + const char * key_adctp_stat; + const char * key_status_gps_pps_state; + const char * key_status_gps_pps_position; + const char * key_status_motor_state; + const char * key_status_motor_rpm; + const char * key_status_motor_lock; + const char * key_status_motor_phase; + const char * key_status_laser_state; + + /* + const char* name_volt_temp_top_hv; + const char* name_volt_temp_top_ad_temp; + const char* name_volt_temp_top_lm20_temp; + const char* name_volt_temp_top_pwr_5v; + const char* name_volt_temp_top_pwr_2_5v; + const char* name_volt_temp_top_pwr_3_3v; + const char* name_volt_temp_top_pwr_raw; + const char* name_volt_temp_top_pwr_vccint; + const char* name_volt_temp_bot_i_out; + const char* name_volt_temp_bot_pwr_1_2v; + const char* name_volt_temp_bot_lm20_temp; + const char* name_volt_temp_bot_pwr_5v; + const char* name_volt_temp_bot_pwr_2_5v; + const char* name_volt_temp_bot_pwr_3_3v; + const char* name_volt_temp_bot_pwr_v_in; + const char* name_volt_temp_bot_pwr_1_25v; + const char* name_vhv; + const char* name_adc_nf; + const char* name_adc_stats; + const char* name_ixe; + const char* name_adctp_stat; + const char* name_status_gps_pps_state; + const char* name_status_gps_pps_position; + const char* name_status_motor_state; + const char* name_status_motor_rpm; + const char* name_status_motor_lock; + const char* name_status_motor_phase; + const char* name_status_laser_state; + */ + + std::string name_volt_temp_top_hv; + std::string name_volt_temp_top_ad_temp; + std::string name_volt_temp_top_lm20_temp; + std::string name_volt_temp_top_pwr_5v; + std::string name_volt_temp_top_pwr_2_5v; + std::string name_volt_temp_top_pwr_3_3v; + std::string name_volt_temp_top_pwr_5v_raw; + std::string name_volt_temp_top_pwr_raw; + std::string name_volt_temp_top_pwr_vccint; + std::string name_volt_temp_bot_i_out; + std::string name_volt_temp_bot_pwr_1_2v; + std::string name_volt_temp_bot_lm20_temp; + std::string name_volt_temp_bot_pwr_5v; + std::string name_volt_temp_bot_pwr_2_5v; + std::string name_volt_temp_bot_pwr_3_3v; + std::string name_volt_temp_bot_pwr_v_in; + std::string name_volt_temp_bot_pwr_1_25v; + std::string name_vhv; + std::string name_adc_nf; + std::string name_adc_stats; + std::string name_ixe; + std::string name_adctp_stat; + std::string name_status_gps_pps_state; + std::string name_status_gps_pps_position; + std::string name_status_motor_state; + std::string name_status_motor_rpm; + std::string name_status_motor_lock; + std::string name_status_motor_phase; + std::string name_status_laser_state; + + const char * not_supported_message; + const char * error_message; + std::string message_sep; + + const char * key_info_model; + const char * key_info_serial; + + std::string temperature_cold_message; + std::string temperature_hot_message; + std::string voltage_low_message; + std::string voltage_high_message; + std::string ampere_low_message; + std::string ampere_high_message; + + std::string info_model; + std::string info_serial; + + bool use_advanced_diagnostics; + + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + // rclcpp::callback_group::CallbackGroup::SharedPtr cbg_; + rclcpp::CallbackGroup::SharedPtr cbg_r_; + rclcpp::CallbackGroup::SharedPtr cbg_m_; }; + } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula + +#endif // NEBULA_VelodyneHwMonitorRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp deleted file mode 100644 index a7110bd4a..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" -#include "nebula_ros/velodyne/decoder_wrapper.hpp" -#include "nebula_ros/velodyne/hw_interface_wrapper.hpp" -#include "nebula_ros/velodyne/hw_monitor_wrapper.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ - -/// @brief Ros wrapper of velodyne driver -class VelodyneRosWrapper final : public rclcpp::Node -{ -public: - explicit VelodyneRosWrapper(const rclcpp::NodeOptions & options); - ~VelodyneRosWrapper() 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/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 10c7aa218..f4a1cd41c 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,27 +27,34 @@ + + - - - - + + + + + + + + + + + + - - - - + @@ -57,13 +64,33 @@ + + + + + + + + + + + + + + + + - - + + + + + diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml new file mode 100644 index 000000000..75c7414da --- /dev/null +++ b/nebula_ros/launch/hesai_launch_component.launch.xml @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index 6eee53443..b394498fb 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -26,7 +26,6 @@ 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 @@ -35,22 +34,15 @@ 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") @@ -58,36 +50,23 @@ 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" @@ -96,8 +75,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, { @@ -105,6 +84,7 @@ 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"), @@ -115,13 +95,14 @@ 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, { @@ -129,6 +110,7 @@ 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, }, ], ) @@ -136,8 +118,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, { @@ -145,6 +127,7 @@ 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"), @@ -163,7 +146,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/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml index 52a013523..6e2ab6dda 100644 --- a/nebula_ros/launch/robosense_launch.all_hw.xml +++ b/nebula_ros/launch/robosense_launch.all_hw.xml @@ -1,10 +1,53 @@ - - + + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index d2f030b29..c39437039 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -1,10 +1,67 @@ - - + + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index a0bdec51f..87bc000c9 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -21,8 +21,6 @@ nebula_common nebula_decoders nebula_hw_interfaces - nebula_msgs - pandar_msgs pcl_conversions rclcpp rclcpp_components diff --git a/nebula_ros/schema/Bpearl.schema.json b/nebula_ros/schema/Bpearl.schema.json deleted file mode 100644 index 0622e2089..000000000 --- a/nebula_ros/schema/Bpearl.schema.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Robosense Bpearl parameters.", - "type": "object", - "definitions": { - "Bpearl": { - "type": "object", - "properties": { - }, - "allOf": [ - { - "$ref": "sub/robosense.json#/definitions/robosense" - } - ], - "required": [ - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/Bpearl" - } - }, - "required": [ - "ros__parameters" - ], - "additionalProperties": false - } - }, - "required": [ - "/**" - ], - "additionalProperties": false -} \ No newline at end of file diff --git a/nebula_ros/schema/Helios.schema.json b/nebula_ros/schema/Helios.schema.json deleted file mode 100644 index 4485f4aad..000000000 --- a/nebula_ros/schema/Helios.schema.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Robosense Helios parameters.", - "type": "object", - "definitions": { - "Helios": { - "type": "object", - "properties": { - }, - "allOf": [ - { - "$ref": "sub/robosense.json#/definitions/robosense" - } - ], - "required": [ - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/Helios" - } - }, - "required": [ - "ros__parameters" - ], - "additionalProperties": false - } - }, - "required": [ - "/**" - ], - "additionalProperties": false -} \ No newline at end of file diff --git a/nebula_ros/schema/VLP16.schema.json b/nebula_ros/schema/VLP16.schema.json deleted file mode 100644 index 4aadb40a4..000000000 --- a/nebula_ros/schema/VLP16.schema.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Velodyne VLP16 parameters.", - "type": "object", - "definitions": { - "VLP16": { - "type": "object", - "properties": { - }, - "allOf": [ - { - "$ref": "sub/velodyne.json#/definitions/velodyne" - } - ], - "required": [ - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/VLP16" - } - }, - "required": [ - "ros__parameters" - ], - "additionalProperties": false - } - }, - "required": [ - "/**" - ], - "additionalProperties": false -} \ No newline at end of file diff --git a/nebula_ros/schema/VLP32.schema.json b/nebula_ros/schema/VLP32.schema.json deleted file mode 100644 index 9adb5d98b..000000000 --- a/nebula_ros/schema/VLP32.schema.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Velodyne VLP32 parameters.", - "type": "object", - "definitions": { - "VLP32": { - "type": "object", - "properties": { - }, - "allOf": [ - { - "$ref": "sub/velodyne.json#/definitions/velodyne" - } - ], - "required": [ - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/VLP32" - } - }, - "required": [ - "ros__parameters" - ], - "additionalProperties": false - } - }, - "required": [ - "/**" - ], - "additionalProperties": false -} \ No newline at end of file diff --git a/nebula_ros/schema/VLS128.schema.json b/nebula_ros/schema/VLS128.schema.json deleted file mode 100644 index 8b5d6375b..000000000 --- a/nebula_ros/schema/VLS128.schema.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Velodyne VLS128 parameters.", - "type": "object", - "definitions": { - "VLS128": { - "type": "object", - "properties": { - }, - "allOf": [ - { - "$ref": "sub/velodyne.json#/definitions/velodyne" - } - ], - "required": [ - ] - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/VLS128" - } - }, - "required": [ - "ros__parameters" - ], - "additionalProperties": false - } - }, - "required": [ - "/**" - ], - "additionalProperties": false -} \ No newline at end of file diff --git a/nebula_ros/schema/sub/lidar.json b/nebula_ros/schema/sub/lidar.json deleted file mode 100644 index b12e242fd..000000000 --- a/nebula_ros/schema/sub/lidar.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR common parameters.", - "type": "object", - "definitions": { - "lidar": { - "type": "object", - "properties": { - "diag_span": { - "type": "integer", - "default": 1000, - "minimum": 0, - "readOnly": true, - "description": "Diagnostics rate." - }, - "scan_phase": { - "type": "number", - "default": 0.0, - "minimum": 0.0, - "maximum": 360.0, - "description": "Sensor scan phase." - } - }, - "allOf": [ - { - "$ref": "nebula.json#/definitions/nebula" - } - ], - "required": [ - "diag_span", - "scan_phase" - ] - } - } -} \ No newline at end of file diff --git a/nebula_ros/schema/sub/nebula.json b/nebula_ros/schema/sub/nebula.json deleted file mode 100644 index 6f977cf41..000000000 --- a/nebula_ros/schema/sub/nebula.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Sensors common parameters.", - "type": "object", - "definitions": { - "nebula": { - "type": "object", - "properties": { - "host_ip": { - "type": "string", - "default": "255.255.255.255", - "pattern": "^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$", - "readOnly": true, - "description": "Host IPv4 address." - }, - "sensor_ip": { - "type": "string", - "default": "192.168.1.201", - "pattern": "^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$", - "readOnly": true, - "description": "Sensor IPv4 address." - }, - "data_port": { - "type": "integer", - "default": 2368, - "minimum": 0, - "readOnly": true, - "description": "Sensor data port." - }, - "frame_id": { - "type": "string", - "default": "data_link", - "description": "Sensor data frame_id." - } - }, - "required": [ - "host_ip", - "sensor_ip", - "data_port", - "frame_id" - ] - } - } -} \ No newline at end of file diff --git a/nebula_ros/schema/sub/robosense.json b/nebula_ros/schema/sub/robosense.json deleted file mode 100644 index 0e23e8433..000000000 --- a/nebula_ros/schema/sub/robosense.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Robosense common parameters.", - "type": "object", - "definitions": { - "robosense": { - "type": "object", - "properties": { - "setup_sensor": { - "type": "boolean", - "default": true, - "readOnly": true, - "description": "Enable sensor setup on hw-driver." - }, - "sensor_model": { - "type": "string", - "readOnly": true, - "enum": [ - "Helios", - "Bpearl" - ], - "description": "LiDAR model." - }, - "return_mode": { - "type": "string", - "enum": [ - "Dual", - "Strongest", - "Last", - "First" - ], - "description": "LiDAR return mode." - }, - "gnss_port": { - "type": "integer", - "default": 2369, - "minimum": 0, - "readOnly": true, - "description": "LiDAR GNSS port." - }, - "packet_mtu_size": { - "type": "integer", - "default": 1500, - "minimum": 0, - "readOnly": true, - "description": "Packet MTU size." - }, - "cloud_min_angle": { - "type": "integer", - "default": 0, - "minimum": 0, - "maximum": 360, - "description": "Field of View, start degrees." - }, - "cloud_max_angle": { - "type": "integer", - "default": 360, - "minimum": 0, - "maximum": 360, - "description": "Field of View, end degrees." - }, - "dual_return_distance_threshold": { - "type": "number", - "default": 0.1, - "minimum": 0.01, - "maximum": 0.5, - "description": "Distance threshold between two neighboring points for dual return mode." - } - }, - "allOf": [ - { - "$ref": "lidar.json#/definitions/lidar" - } - ], - "required": [ - "setup_sensor", - "sensor_model", - "return_mode", - "gnss_port", - "packet_mtu_size", - "cloud_min_angle", - "cloud_max_angle", - "dual_return_distance_threshold" - ] - } - } -} \ No newline at end of file diff --git a/nebula_ros/schema/sub/velodyne.json b/nebula_ros/schema/sub/velodyne.json deleted file mode 100644 index 1c2e186b8..000000000 --- a/nebula_ros/schema/sub/velodyne.json +++ /dev/null @@ -1,123 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "LiDAR Velodyne common parameters.", - "type": "object", - "definitions": { - "velodyne": { - "type": "object", - "properties": { - "calibration_file": { - "type": "string", - "default": "$(find-pkg-share nebula_decoders)/calibration/velodyne/$(var sensor_model).yaml", - "pattern": "^.*\\.yaml$", - "description": "Sensor calibration file." - }, - "setup_sensor": { - "type": "boolean", - "default": true, - "readOnly": true, - "description": "Enable sensor setup on hw-driver." - }, - "advanced_diagnostics": { - "type": "boolean", - "default": false, - "readOnly": true, - "description": "Enable advanced diagnostics." - }, - "launch_hw": { - "type": "boolean", - "readOnly": false, - "description": "Launch hardware." - }, - "sensor_model": { - "type": "string", - "readOnly": true, - "enum": [ - "VLP16", - "VLP32", - "VLS128" - ], - "description": "LiDAR model." - }, - "return_mode": { - "type": "string", - "enum": [ - "SingleStrongest", - "SingleLast", - "Dual", - "SingleFirst" - ], - "description": "LiDAR return mode." - }, - "gnss_port": { - "type": "integer", - "default": 2369, - "minimum": 0, - "readOnly": true, - "description": "LiDAR GNSS port." - }, - "min_range": { - "type": "number", - "default": 0.3, - "minimum": 0.0, - "description": "Sensor minimum single point range." - }, - "max_range": { - "type": "number", - "default": 300.0, - "minimum": 0.0, - "description": "Sensor maximum single point range." - }, - "packet_mtu_size": { - "type": "integer", - "default": 1500, - "minimum": 0, - "readOnly": true, - "description": "Packet MTU size." - }, - "rotation_speed": { - "type": "integer", - "default": 600, - "minimum": 300, - "maximum": 1200, - "multipleOf": 60, - "description": "Motor RPM, the sensor's internal spin rate." - }, - "cloud_min_angle": { - "type": "integer", - "default": 0, - "minimum": 0, - "maximum": 360, - "description": "Field of View, start degrees." - }, - "cloud_max_angle": { - "type": "integer", - "default": 360, - "minimum": 0, - "maximum": 360, - "description": "Field of View, end degrees." - } - }, - "allOf": [ - { - "$ref": "lidar.json#/definitions/lidar" - } - ], - "required": [ - "calibration_file", - "setup_sensor", - "advanced_diagnostics", - "launch_hw", - "sensor_model", - "return_mode", - "gnss_port", - "min_range", - "max_range", - "packet_mtu_size", - "rotation_speed", - "cloud_min_angle", - "cloud_max_angle" - ] - } - } -} \ No newline at end of file diff --git a/nebula_ros/src/common/parameter_descriptors.cpp b/nebula_ros/src/common/parameter_descriptors.cpp deleted file mode 100644 index b62cb8a9e..000000000 --- a/nebula_ros/src/common/parameter_descriptors.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#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 deleted file mode 100644 index f51378e47..000000000 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ /dev/null @@ -1,316 +0,0 @@ -#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"; - 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 new file mode 100644 index 000000000..6f27d547e --- /dev/null +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -0,0 +1,488 @@ +#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 new file mode 100644 index 000000000..1ac59b55b --- /dev/null +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -0,0 +1,510 @@ +#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 new file mode 100644 index 000000000..e64bf443b --- /dev/null +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -0,0 +1,620 @@ +#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 deleted file mode 100644 index 8449bdfe5..000000000 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ /dev/null @@ -1,315 +0,0 @@ -#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 deleted file mode 100644 index 18da9da83..000000000 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#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 deleted file mode 100644 index da1f78794..000000000 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ /dev/null @@ -1,362 +0,0 @@ -#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_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp deleted file mode 100644 index d07383fbe..000000000 --- a/nebula_ros/src/velodyne/decoder_wrapper.cpp +++ /dev/null @@ -1,242 +0,0 @@ -#include "nebula_ros/velodyne/decoder_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ - -using namespace std::chrono_literals; - -VelodyneDecoderWrapper::VelodyneDecoderWrapper(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("VelodyneDecoder")) - , hw_interface_(hw_interface) - , sensor_cfg_(config) -{ - if (!config) - { - throw std::runtime_error("VelodyneDecoderWrapper cannot be instantiated without a valid config!"); - } - - calibration_file_path_ = parent_node->declare_parameter("calibration_file", param_read_write()); - auto calibration_result = GetCalibrationData(calibration_file_path_); - - 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("velodyne_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("velodyne_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 VelodyneDecoderWrapper::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 VelodyneDecoderWrapper::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 VelodyneDecoderWrapper::OnParameterChange(const std::vector& p) -{ - using namespace rcl_interfaces::msg; - - std::string calibration_path = ""; - - bool got_any = get_param(p, "calibration_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); - 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(""); -} - -VelodyneDecoderWrapper::get_calibration_result_t -VelodyneDecoderWrapper::GetCalibrationData(const std::string& calibration_file_path) -{ - auto calib = std::make_shared(); - - // 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 VelodyneDecoderWrapper::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; - } - - velodyne_msgs::msg::VelodynePacket velodyne_packet_msg{}; - velodyne_packet_msg.stamp = packet_msg->stamp; - std::copy(packet_msg->data.begin(), packet_msg->data.end(), velodyne_packet_msg.data.begin()); - current_scan_msg_->packets.emplace_back(std::move(velodyne_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, packet_msg->stamp.sec); - pointcloud = std::get<0>(pointcloud_ts); - } - - if (pointcloud == nullptr) - { - // todo - // RCLCPP_WARN_STREAM(logger_, "Empty cloud parsed."); - 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 VelodyneDecoderWrapper::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 VelodyneDecoderWrapper::Status() -{ - std::lock_guard lock(mtx_driver_ptr_); - - if (!driver_ptr_) - { - return nebula::Status::NOT_INITIALIZED; - } - - return driver_ptr_->GetStatus(); -} -} // namespace ros -} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp deleted file mode 100644 index 38091f89b..000000000 --- a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include "nebula_ros/velodyne/hw_interface_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ - -VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( - rclcpp::Node* const parent_node, std::shared_ptr& config) - : hw_interface_(new nebula::drivers::VelodyneHwInterface()) - , logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")) - , status_(Status::NOT_INITIALIZED) -{ - setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); - - hw_interface_->SetLogger(std::make_shared(parent_node->get_logger().get_child("HwInterface"))); - status_ = hw_interface_->InitializeSensorConfiguration(config); - - if (status_ != Status::OK) { - throw std::runtime_error((std::stringstream{} << "Could not initialize HW interface: " << status_).str()); - } - - status_ = hw_interface_->InitHttpClient(); - - if (status_ != Status::OK) { - throw std::runtime_error((std::stringstream{} << "Could not initialize HTTP client: " << status_).str()); - } - - if (setup_sensor_) { - RCLCPP_INFO_STREAM(logger_, "Setting sensor configuration"); - status_ = hw_interface_->SetSensorConfiguration(config); - } - - if (status_ != Status::OK) { - throw std::runtime_error((std::stringstream{} << "Could not set sensor configuration: " << status_).str()); - } - - status_ = Status::OK; -} - -void VelodyneHwInterfaceWrapper::OnConfigChange( - const std::shared_ptr& new_config) -{ - hw_interface_->InitializeSensorConfiguration(new_config); - hw_interface_->InitHttpClient(); - if (setup_sensor_) { - hw_interface_->SetSensorConfiguration(new_config); - } -} - -Status VelodyneHwInterfaceWrapper::Status() -{ - return status_; -} - -std::shared_ptr VelodyneHwInterfaceWrapper::HwInterface() const -{ - return hw_interface_; -} - -} // namespace ros -} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..df484a27a --- /dev/null +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -0,0 +1,281 @@ +#include "nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +VelodyneDriverRosWrapper::VelodyneDriverRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("velodyne_driver_ros_wrapper", options) +{ + drivers::VelodyneCalibrationConfiguration calibration_configuration; + drivers::VelodyneSensorConfiguration sensor_configuration; + + wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_DEBUG_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_DEBUG_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast(sensor_cfg_ptr_), + std::static_pointer_cast(calibration_cfg_ptr_)); + + RCLCPP_DEBUG_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + + velodyne_scan_sub_ = create_subscription( + "velodyne_packets", rclcpp::SensorDataQoS(), + std::bind(&VelodyneDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); + nebula_points_pub_ = this->create_publisher( + "velodyne_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 VelodyneDriverRosWrapper::ReceiveScanMsgCallback( + const velodyne_msgs::msg::VelodyneScan::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); + double cloud_stamp = std::get<1>(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, cloud_stamp); + 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 VelodyneDriverRosWrapper::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 VelodyneDriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_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 VelodyneDriverRosWrapper::GetStatus() { return wrapper_status_; } + +Status VelodyneDriverRosWrapper::GetParameters( + drivers::VelodyneSensorConfiguration & sensor_configuration, + drivers::VelodyneCalibrationConfiguration & calibration_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::ReturnModeFromString(this->get_parameter("return_mode").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", "velodyne", 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 = 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(); + } + + { + 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(); + } + double view_direction; + { + 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("view_direction", 0., descriptor); + view_direction = this->get_parameter("view_direction").as_double(); + } + double view_width = 2.0 * M_PI; + { + 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("view_width", 2.0 * M_PI, descriptor); + view_width = this->get_parameter("view_width").as_double(); + } + + if (sensor_configuration.sensor_model != nebula::drivers::SensorModel::VELODYNE_HDL64) { + { + 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(); + } + } else { + double min_angle = fmod(fmod(view_direction + view_width / 2, 2 * M_PI) + 2 * M_PI, 2 * M_PI); + double max_angle = fmod(fmod(view_direction - view_width / 2, 2 * M_PI) + 2 * M_PI, 2 * M_PI); + sensor_configuration.cloud_min_angle = 100 * (2 * M_PI - min_angle) * 180 / M_PI + 0.5; + sensor_configuration.cloud_max_angle = 100 * (2 * M_PI - max_angle) * 180 / M_PI + 0.5; + if (sensor_configuration.cloud_min_angle == sensor_configuration.cloud_max_angle) { + // avoid returning empty cloud if min_angle = max_angle + sensor_configuration.cloud_min_angle = 0; + sensor_configuration.cloud_max_angle = 36000; + } + } + + 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; + } + + if (calibration_configuration.calibration_file.empty()) { + 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; + } + } + + RCLCPP_INFO_STREAM( + this->get_logger(), "Sensor model: " << sensor_configuration.sensor_model + << ", Return mode: " << sensor_configuration.return_mode + << ", Scan Phase: " << sensor_configuration.scan_phase); + return Status::OK; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneDriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..cbbc19e05 --- /dev/null +++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp @@ -0,0 +1,326 @@ +#include "nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +VelodyneHwInterfaceRosWrapper::VelodyneHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("velodyne_hw_interface_ros_wrapper", options), hw_interface_() +{ + not_supported_message = "Not supported"; + + 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; + } + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + // Initialize sensor_configuration + RCLCPP_INFO_STREAM(this->get_logger(), "Initialize sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.InitializeSensorConfiguration"); + hw_interface_.InitializeSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + if (this->setup_sensor) { + RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + updateParameters(); + } + + // register scan callback and publisher + hw_interface_.RegisterScanCallback(std::bind( + &VelodyneHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); + velodyne_scan_pub_ = this->create_publisher( + "velodyne_packets", + rclcpp::SensorDataQoS(rclcpp::KeepLast(10)).best_effort().durability_volatile()); + + if (this->setup_sensor) { + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&VelodyneHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + } + + auto status = StreamStart(); + if (status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "UDP Driver Started"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), status); + } +} + +Status VelodyneHwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + return interface_status_; +} + +Status VelodyneHwInterfaceRosWrapper::StreamStop() { return Status::OK; } +Status VelodyneHwInterfaceRosWrapper::Shutdown() { return Status::OK; } + +Status VelodyneHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status VelodyneHwInterfaceRosWrapper::GetParameters( + drivers::VelodyneSensorConfiguration & 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::ReturnModeFromString(this->get_parameter("return_mode").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("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", "velodyne", 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 = 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_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; + descriptor.additional_constraints = "range from 300 to 1200, in increments of 60"; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(300).set__to_value(1200).set__step(1); + 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_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(); + } + + 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 VelodyneHwInterfaceRosWrapper::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; + velodyne_scan_pub_->publish(*scan_buffer); +} + +std::string VelodyneHwInterfaceRosWrapper::GetPtreeValue( + std::shared_ptr pt, const std::string & key) +{ + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return not_supported_message; + } +} + +rcl_interfaces::msg::SetParametersResult VelodyneHwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + std::cout << "add_on_set_parameters_callback" << std::endl; + std::cout << p << std::endl; + std::cout << sensor_configuration_ << std::endl; + + drivers::VelodyneSensorConfiguration new_param{sensor_configuration_}; + std::cout << new_param << std::endl; + 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)) { // || + + 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; + // Update sensor_configuration + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + std::cout << "add_on_set_parameters_callback success" << std::endl; + + return *result; +} + +std::vector +VelodyneHwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + std::cout << "!!!!!!!!!!!updateParameters!!!!!!!!!!!!" << std::endl; + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + std::ostringstream os_return_mode; + os_return_mode << sensor_configuration_.return_mode; + std::cout << "set_parameters start" << std::endl; + 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) //, + }); + std::cout << "set_parameters fin" << std::endl; + return results; +} +RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneHwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp similarity index 52% rename from nebula_ros/src/velodyne/hw_monitor_wrapper.cpp rename to nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp index 11bbff4c4..172e61c83 100644 --- a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp @@ -1,221 +1,626 @@ -#include "nebula_ros/velodyne/hw_monitor_wrapper.hpp" +#include "nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp" + +#include +#include + +#include +#include + +#include namespace nebula { namespace ros { -VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper(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) - , sensor_configuration_(config) +VelodyneHwMonitorRosWrapper::VelodyneHwMonitorRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("velodyne_hw_monitor_ros_wrapper", options), + hw_interface_(), + diagnostics_updater_(this) { - diag_span_ = parent_node->declare_parameter("diag_span", param_read_only()); - show_advanced_diagnostics_ = parent_node->declare_parameter("advanced_diagnostics", param_read_only()); + cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + cbg_m_ = 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; + } + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + // Initialize sensor_configuration + RCLCPP_INFO_STREAM(this->get_logger(), "Initialize sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.InitializeSensorConfiguration"); + hw_interface_.InitializeSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&VelodyneHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); + + key_volt_temp_top_hv = "volt_temp.top.hv"; + key_volt_temp_top_ad_temp = "volt_temp.top.ad_temp"; // only32 + key_volt_temp_top_lm20_temp = "volt_temp.top.lm20_temp"; + key_volt_temp_top_pwr_5v = "volt_temp.top.pwr_5v"; + key_volt_temp_top_pwr_2_5v = "volt_temp.top.pwr_2_5v"; + key_volt_temp_top_pwr_3_3v = "volt_temp.top.pwr_3_3v"; + key_volt_temp_top_pwr_5v_raw = "volt_temp.top.pwr_5v_raw"; // only16 + key_volt_temp_top_pwr_raw = "volt_temp.top.pwr_raw"; // only32 + key_volt_temp_top_pwr_vccint = "volt_temp.top.pwr_vccint"; + key_volt_temp_bot_i_out = "volt_temp.bot.i_out"; + key_volt_temp_bot_pwr_1_2v = "volt_temp.bot.pwr_1_2v"; + key_volt_temp_bot_lm20_temp = "volt_temp.bot.lm20_temp"; + key_volt_temp_bot_pwr_5v = "volt_temp.bot.pwr_5v"; + key_volt_temp_bot_pwr_2_5v = "volt_temp.bot.pwr_2_5v"; + key_volt_temp_bot_pwr_3_3v = "volt_temp.bot.pwr_3_3v"; + key_volt_temp_bot_pwr_v_in = "volt_temp.bot.pwr_v_in"; + key_volt_temp_bot_pwr_1_25v = "volt_temp.bot.pwr_1_25v"; + key_vhv = "vhv"; + key_adc_nf = "adc_nf"; + key_adc_stats = "adc_stats"; + key_ixe = "ixe"; + key_adctp_stat = "adctp_stat"; + key_status_gps_pps_state = "gps.pps_state"; + key_status_gps_pps_position = "gps.position"; + key_status_motor_state = "motor.state"; + key_status_motor_rpm = "motor.rpm"; + key_status_motor_lock = "motor.lock"; + key_status_motor_phase = "motor.phase"; + key_status_laser_state = "laser.state"; + + name_volt_temp_top_hv = "Top HV"; + name_volt_temp_top_ad_temp = "Top A/D TD"; + name_volt_temp_top_lm20_temp = "Top Temp"; + name_volt_temp_top_pwr_5v = "Top 5v"; + name_volt_temp_top_pwr_2_5v = "Top 2.5v"; + name_volt_temp_top_pwr_3_3v = "Top 3.3v"; + name_volt_temp_top_pwr_5v_raw = "Top 5v(RAW)"; + name_volt_temp_top_pwr_raw = "Top RAW"; + name_volt_temp_top_pwr_vccint = "Top VCCINT"; + name_volt_temp_bot_i_out = "Bot I out"; + name_volt_temp_bot_pwr_1_2v = "Bot 1.2v"; + name_volt_temp_bot_lm20_temp = "Bot Temp"; + name_volt_temp_bot_pwr_5v = "Bot 5v"; + name_volt_temp_bot_pwr_2_5v = "Bot 2.5v"; + name_volt_temp_bot_pwr_3_3v = "Bot 3.3v"; + name_volt_temp_bot_pwr_v_in = "Bot V in"; + name_volt_temp_bot_pwr_1_25v = "Bot 1.25v"; // N/A? + name_vhv = "VHV"; + name_adc_nf = "adc_nf"; + name_adc_stats = "adc_stats"; + name_ixe = "ixe"; + name_adctp_stat = "adctp_stat"; + name_status_gps_pps_state = "GPS PPS"; + name_status_gps_pps_position = "GPS Position"; + name_status_motor_state = "Motor State"; + name_status_motor_rpm = "Motor RPM"; + name_status_motor_lock = "Motor Lock"; + name_status_motor_phase = "Motor Phase"; + name_status_laser_state = "Laser State"; + + message_sep = ": "; + + not_supported_message = "Not supported"; + error_message = "Error"; + + key_info_model = "info.model"; + key_info_serial = "info.serial"; + + temperature_cold_message = "temperature cold"; + temperature_hot_message = "temperature hot"; + voltage_low_message = "voltage low"; + voltage_high_message = "voltage high"; + ampere_low_message = "ampere low"; + ampere_high_message = "ampere high"; std::cout << "Get model name and serial." << std::endl; - auto str = hw_interface_->GetSnapshot(); - current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); - current_snapshot_tree = - std::make_shared(hw_interface_->ParseJson(str)); - current_diag_tree = - std::make_shared(current_snapshot_tree->get_child("diag")); - current_status_tree = - std::make_shared(current_snapshot_tree->get_child("status")); - current_snapshot.reset(new std::string(str)); + hw_interface_.GetSnapshotAsync([this](const std::string & str) { + current_snapshot_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_snapshot_tree = + std::make_shared(hw_interface_.ParseJson(str)); + current_diag_tree = + std::make_shared(current_snapshot_tree->get_child("diag")); + current_status_tree = + std::make_shared(current_snapshot_tree->get_child("status")); + current_snapshot.reset(new std::string(str)); - try { - info_model_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_model); - info_serial_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_serial); - RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); - RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); - } catch (boost::bad_lexical_cast & ex) { - RCLCPP_ERROR(logger_, " Error: Can't get model and serial"); - return; + try { + info_model = GetPtreeValue(current_snapshot_tree, key_info_model); + info_serial = GetPtreeValue(current_snapshot_tree, key_info_serial); + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); + } catch (boost::bad_lexical_cast & ex) { + RCLCPP_ERROR_STREAM( + this->get_logger(), this->get_name() << " Error:" + << "Can't get model and serial"); + return; + } + + InitializeVelodyneDiagnostics(); + }); +} + +Status VelodyneHwMonitorRosWrapper::MonitorStart() { return interface_status_; } + +Status VelodyneHwMonitorRosWrapper::MonitorStop() { return Status::OK; } +Status VelodyneHwMonitorRosWrapper::Shutdown() { return Status::OK; } + +Status VelodyneHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status VelodyneHwMonitorRosWrapper::GetParameters( + drivers::VelodyneSensorConfiguration & 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 = 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()); + } + { + 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 = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "velodyne", 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 = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "range from 300 to 1200, in increments of 60"; + rcl_interfaces::msg::IntegerRange range; + range.set__from_value(300).set__to_value(1200).set__step(1); + 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 = true; + 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 = true; + 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(); + } + + 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 = "It may be safe if it is 5000 milliseconds or more..."; + this->declare_parameter("diag_span", 3000, descriptor); + diag_span_ = this->get_parameter("diag_span").as_int(); + } + + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; // because it affects initialization + descriptor.dynamic_typing = false; + descriptor.additional_constraints = "Showing advanced diagnostics"; + this->declare_parameter("advanced_diagnostics", false, descriptor); + use_advanced_diagnostics = this->get_parameter("advanced_diagnostics").as_bool(); + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - InitializeVelodyneDiagnostics(); + return Status::OK; } -void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() +void VelodyneHwMonitorRosWrapper::InitializeVelodyneDiagnostics() { - RCLCPP_INFO_STREAM(logger_, "InitializeVelodyneDiagnostics"); + RCLCPP_INFO_STREAM(get_logger(), "InitializeVelodyneDiagnostics"); using std::chrono_literals::operator""s; std::ostringstream os; - auto hardware_id = info_model_ + ": " + info_serial_; + auto hardware_id = info_model + ": " + info_serial; diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(logger_, "hardware_id" << hardware_id); + RCLCPP_INFO_STREAM(get_logger(), "hardware_id" << hardware_id); - if (show_advanced_diagnostics_) { + if (use_advanced_diagnostics) { diagnostics_updater_.add( - "velodyne_snapshot-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckSnapshot); + "velodyne_snapshot-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckSnapshot); diagnostics_updater_.add( - "velodyne_volt_temp_top_hv-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopHv); - if (sensor_configuration_->sensor_model != nebula::drivers::SensorModel::VELODYNE_VLP16) { + "velodyne_volt_temp_top_hv-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopHv); + if (sensor_configuration_.sensor_model != nebula::drivers::SensorModel::VELODYNE_VLP16) { diagnostics_updater_.add( - "velodyne_volt_temp_top_ad_temp-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp); + "velodyne_volt_temp_top_ad_temp-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopAdTemp); } diagnostics_updater_.add( - "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp); + "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopLm20Temp); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v); + "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr5v); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v); + "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr25v); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v); + "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr33v); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw); + "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrRaw); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint); + "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrVccint); diagnostics_updater_.add( - "velodyne_volt_temp_bot_i_out-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotIOut); + "velodyne_volt_temp_bot_i_out-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotIOut); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v); + "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr12v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp); + "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotLm20Temp); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v); + "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr5v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v); + "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr25v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v); + "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr33v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn); + "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwrVIn); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v); + "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr125v); diagnostics_updater_.add( - "velodyne_vhv-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckVhv); + "velodyne_vhv-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckVhv); diagnostics_updater_.add( - "velodyne_adc_nf-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckAdcNf); + "velodyne_adc_nf-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckAdcNf); diagnostics_updater_.add( - "velodyne_adc_stats-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckAdcStats); + "velodyne_adc_stats-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckAdcStats); diagnostics_updater_.add( - "velodyne_ixe-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckIxe); + "velodyne_ixe-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckIxe); diagnostics_updater_.add( - "velodyne_adctp_stat-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat); + "velodyne_adctp_stat-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckAdctpStat); diagnostics_updater_.add( - "velodyne_status_gps_pps_state-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState); + "velodyne_status_gps_pps_state-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPpsState); diagnostics_updater_.add( - "velodyne_status_gps_pps_position-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition); + "velodyne_status_gps_pps_position-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPosition); diagnostics_updater_.add( - "velodyne_status_motor_state-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorState); + "velodyne_status_motor_state-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorState); diagnostics_updater_.add( - "velodyne_status_motor_rpm-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm); + "velodyne_status_motor_rpm-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorRpm); diagnostics_updater_.add( - "velodyne_status_motor_lock-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorLock); + "velodyne_status_motor_lock-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorLock); diagnostics_updater_.add( - "velodyne_status_motor_phase-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase); + "velodyne_status_motor_phase-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorPhase); diagnostics_updater_.add( - "velodyne_status_laser_state-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckLaserState); + "velodyne_status_laser_state-" + sensor_configuration_.frame_id, this, + &VelodyneHwMonitorRosWrapper::VelodyneCheckLaserState); } diagnostics_updater_.add( - "velodyne_status", this, &VelodyneHwMonitorWrapper::VelodyneCheckStatus); - diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorWrapper::VelodyneCheckPps); + "velodyne_status", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckStatus); + diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckPps); diagnostics_updater_.add( - "velodyne_temperature", this, &VelodyneHwMonitorWrapper::VelodyneCheckTemperature); - diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorWrapper::VelodyneCheckRpm); + "velodyne_temperature", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature); + diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckRpm); diagnostics_updater_.add( - "velodyne_voltage", this, &VelodyneHwMonitorWrapper::VelodyneCheckVoltage); - - { - std::lock_guard lock(mtx_snapshot_); - current_snapshot.reset(new std::string("")); - current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); - } + "velodyne_voltage", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage); + current_snapshot.reset(new std::string("")); + current_snapshot_time.reset(new rclcpp::Time(this->get_clock()->now())); current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; auto on_timer_snapshot = [this] { OnVelodyneSnapshotTimer(); }; - diagnostics_snapshot_timer_ = parent_node_->create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot)); + diagnostics_snapshot_timer_ = std::make_shared>( + this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(diagnostics_snapshot_timer_, cbg_m_); auto on_timer_update = [this] { - auto now = parent_node_->now(); - double dif; - { - std::lock_guard lock(mtx_snapshot_); - dif = (now - *current_snapshot_time).seconds(); - } + auto now = this->get_clock()->now(); + auto dif = (now - *current_snapshot_time).seconds(); if (diag_span_ * 2.0 < dif * 1000) { current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(logger_, "STALE"); + RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); } else { current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(logger_, "OK"); + RCLCPP_DEBUG_STREAM(get_logger(), "OK"); } diagnostics_updater_.force_update(); }; - diagnostics_update_timer_ = parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_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_); +} + +std::string VelodyneHwMonitorRosWrapper::GetPtreeValue( + std::shared_ptr pt, const std::string & key) +{ + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return not_supported_message; + } +} +std::string VelodyneHwMonitorRosWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); } -void VelodyneHwMonitorWrapper::OnVelodyneSnapshotTimer() +// https://memo.appri.me/programming/cpp-curl-http-client +using namespace std; +typedef void (*CurlCallback)(string err, string body); + +class Curl { +private: + /** response body */ + string body; - auto str = hw_interface_->GetSnapshot(); - auto ptree = hw_interface_->ParseJson(str); + // see: https://curl.se/docs/faq.html#Using_C_non_static_functions_f + static size_t invoke_write_data(char * buffer, size_t size, size_t nmemb, void * f) + { + // Call non-static member function. + return static_cast(f)->write_data(buffer, size, nmemb, f); + } + /** a callback function for libcurl request */ + size_t write_data(char * buffer, size_t size, size_t nmemb, void *) { - std::lock_guard lock(mtx_snapshot_); + int dataLength = size * nmemb; + this->body.append(buffer, dataLength); + return dataLength; + } - current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); - current_snapshot_tree = - std::make_shared(ptree); +public: + /** user-agent */ + string useragent = "libcurl-agent/1.0"; + /** timeout */ + int timeout = 30L; // timeout 30 seconds + + /** + * Constructor + */ + Curl() + { + // + } + + /** + * HTTP GET + */ + void get(const string url, const CurlCallback cb) + { + CURL * curl; + CURLcode ret; + + this->body = ""; // init result body. + string err = ""; + + curl_global_init(CURL_GLOBAL_ALL); + curl = curl_easy_init(); + + if (curl == NULL) { + err = "curl_easy_init() failed on " + url; + return cb(err, ""); + } + + curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, this->invoke_write_data); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, this); + curl_easy_setopt(curl, CURLOPT_USERAGENT, this->useragent.c_str()); // UA + curl_easy_setopt(curl, CURLOPT_TIMEOUT, this->timeout); // timeout + // curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); // verbose + ret = curl_easy_perform(curl); + curl_easy_cleanup(curl); + curl_global_cleanup(); + + if (ret != CURLE_OK) { + err = "curl_easy_perform() failed on " + url + " (ret:" + to_string(ret) + ")"; + return cb(err, ""); + } + return cb(err, this->body); + } + + /** + * HTTP POST + */ + void post(const string url, const string data, const CurlCallback cb) + { + CURL * curl; + CURLcode ret; + + this->body = ""; // init result body. + string err = ""; + + curl_global_init(CURL_GLOBAL_ALL); + curl = curl_easy_init(); + + if (curl == NULL) { + err = "curl_easy_init() failed on " + url; + return cb(err, ""); + } + + curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl, CURLOPT_POST, 1); + curl_easy_setopt(curl, CURLOPT_POSTFIELDS, data.c_str()); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, this->invoke_write_data); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, this); + curl_easy_setopt(curl, CURLOPT_USERAGENT, this->useragent.c_str()); // UA + curl_easy_setopt(curl, CURLOPT_TIMEOUT, this->timeout); // timeout + // curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); // verbose + ret = curl_easy_perform(curl); + curl_easy_cleanup(curl); + curl_global_cleanup(); + + if (ret != CURLE_OK) { + err = "curl_easy_perform() failed on " + url + " (ret:" + to_string(ret) + ")"; + return cb(err, ""); + } + return cb(err, this->body); + } +}; + +void VelodyneHwMonitorRosWrapper::curl_callback(std::string err, std::string body) +{ + if (err != "") { + RCLCPP_ERROR_STREAM(get_logger(), "curl_callback:" << err); + } else { + RCLCPP_INFO_STREAM(get_logger(), body); current_diag_tree = - std::make_shared(current_snapshot_tree->get_child("diag")); - current_status_tree = - std::make_shared(current_snapshot_tree->get_child("status")); - current_snapshot.reset(new std::string(str)); + std::make_shared(hw_interface_.ParseJson(body)); + RCLCPP_DEBUG_STREAM(get_logger(), "diagnostics_updater_.force_update()"); + diagnostics_updater_.force_update(); } } -void VelodyneHwMonitorWrapper::OnVelodyneDiagnosticsTimer() +void VelodyneHwMonitorRosWrapper::OnVelodyneDiagnosticsTimer() { std::cout << "OnVelodyneDiagnosticsTimer" << std::endl; - auto str = hw_interface_->GetDiag(); - { - std::lock_guard lock(mtx_diag_); - current_diag_tree = - std::make_shared(hw_interface_->ParseJson(str)); + if (mtx_diag.try_lock() || true) { + std::cout << "mtx_diag lock" << std::endl; + hw_interface_.GetDiagAsync([this](const std::string & str) { + current_diag_tree = + std::make_shared(hw_interface_.ParseJson(str)); + diagnostics_updater_.force_update(); + mtx_diag.unlock(); + std::cout << "mtx_diag unlock" << std::endl; + }); + } else { + std::cout << "mtx_diag is locked..." << std::endl; } - diagnostics_updater_.force_update(); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopHv() +std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetTopHv() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -223,7 +628,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_hv)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_hv)); val = 101.0 * (val * 5.0 / 4096.0 - 5.0); if (val < -150.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -242,7 +647,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopAdTemp() +VelodyneHwMonitorRosWrapper::VelodyneGetTopAdTemp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -250,7 +655,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopAdTemp() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_ad_temp)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_ad_temp)); val = val * 5.0 / 4096.0; mes = GetFixedPrecisionString(val) + " V"; } catch (boost::bad_lexical_cast & ex) { @@ -262,7 +667,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopAdTemp() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() +VelodyneHwMonitorRosWrapper::VelodyneGetTopLm20Temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -271,7 +676,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_lm20_temp)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_lm20_temp)); val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); if (val < -25.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -291,7 +696,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwr5v() +VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -299,7 +704,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr5v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_5v)); val = 2.0 * val * 5.0 / 4096.0; if (val < 4.8) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -318,7 +723,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr5v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwr25v() +VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr25v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -326,7 +731,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr25v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_2_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_2_5v)); val = val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -345,7 +750,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr25v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwr33v() +VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr33v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -353,7 +758,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr33v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_3_3v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_3_3v)); val = val * 5.0 / 4096.0; if (val < 3.1) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -372,7 +777,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr33v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() +VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5vRaw() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -381,7 +786,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v_raw)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_5v_raw)); val = 2.0 * val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -400,7 +805,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwrRaw() +VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrRaw() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -408,7 +813,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwrRaw() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_raw)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_raw)); val = val * 5.0 / 4096.0; if (val < 1.6) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -427,7 +832,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwrRaw() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() +VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrVccint() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -436,7 +841,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_vccint)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_vccint)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -456,7 +861,7 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotIOut() +VelodyneHwMonitorRosWrapper::VelodyneGetBotIOut() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -464,7 +869,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotIOut() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_i_out)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_i_out)); val = 10.0 * (val * 5.0 / 4096.0 - 2.5); if (val < 0.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -483,7 +888,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotIOut() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwr12v() +VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr12v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -491,7 +896,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr12v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_2v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_1_2v)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -510,7 +915,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr12v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() +VelodyneHwMonitorRosWrapper::VelodyneGetBotLm20Temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -519,7 +924,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_lm20_temp)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_lm20_temp)); val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); if (val < -25.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -539,7 +944,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwr5v() +VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr5v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -547,7 +952,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr5v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_5v)); val = 2.0 * val * 5.0 / 4096.0; if (val < 4.8) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -566,7 +971,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr5v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwr25v() +VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr25v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -574,7 +979,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr25v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_2_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_2_5v)); val = val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -593,7 +998,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr25v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwr33v() +VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr33v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -601,7 +1006,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr33v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_3_3v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_3_3v)); val = val * 5.0 / 4096.0; if (val < 3.1) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -620,7 +1025,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr33v() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwrVIn() +VelodyneHwMonitorRosWrapper::VelodyneGetBotPwrVIn() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -628,7 +1033,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwrVIn() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_v_in)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_v_in)); val = 11.0 * val * 5.0 / 4096.0; if (val < 8.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -647,7 +1052,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwrVIn() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() +VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr125v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -656,7 +1061,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_25v)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_1_25v)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -674,7 +1079,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetVhv() +std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetVhv() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -682,7 +1087,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_vhv)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_vhv)); mes = boost::lexical_cast(val); } catch (boost::bad_lexical_cast & ex) { not_ex = false; @@ -692,7 +1097,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdcNf() +std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetAdcNf() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -720,7 +1125,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetAdcStats() +VelodyneHwMonitorRosWrapper::VelodyneGetAdcStats() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -750,14 +1155,14 @@ VelodyneHwMonitorWrapper::VelodyneGetAdcStats() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetIxe() +std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetIxe() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_diag_tree, mtx_diag_, key_ixe); + mes = GetPtreeValue(current_diag_tree, key_ixe); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -767,7 +1172,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetAdctpStat() +VelodyneHwMonitorRosWrapper::VelodyneGetAdctpStat() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -795,14 +1200,14 @@ VelodyneHwMonitorWrapper::VelodyneGetAdctpStat() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetGpsPpsState() +VelodyneHwMonitorRosWrapper::VelodyneGetGpsPpsState() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_state); + mes = GetPtreeValue(current_status_tree, key_status_gps_pps_state); if (mes == "Absent") { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; error_mes = mes; @@ -819,14 +1224,14 @@ VelodyneHwMonitorWrapper::VelodyneGetGpsPpsState() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetGpsPosition() +VelodyneHwMonitorRosWrapper::VelodyneGetGpsPosition() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_position); + mes = GetPtreeValue(current_status_tree, key_status_gps_pps_position); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -836,14 +1241,14 @@ VelodyneHwMonitorWrapper::VelodyneGetGpsPosition() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetMotorState() +VelodyneHwMonitorRosWrapper::VelodyneGetMotorState() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_state); + mes = GetPtreeValue(current_status_tree, key_status_motor_state); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -853,14 +1258,14 @@ VelodyneHwMonitorWrapper::VelodyneGetMotorState() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetMotorRpm() +VelodyneHwMonitorRosWrapper::VelodyneGetMotorRpm() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_rpm); + mes = GetPtreeValue(current_status_tree, key_status_motor_rpm); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -870,14 +1275,14 @@ VelodyneHwMonitorWrapper::VelodyneGetMotorRpm() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetMotorLock() +VelodyneHwMonitorRosWrapper::VelodyneGetMotorLock() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_lock); + mes = GetPtreeValue(current_status_tree, key_status_motor_lock); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -887,14 +1292,14 @@ VelodyneHwMonitorWrapper::VelodyneGetMotorLock() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetMotorPhase() +VelodyneHwMonitorRosWrapper::VelodyneGetMotorPhase() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_phase); + mes = GetPtreeValue(current_status_tree, key_status_motor_phase); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -904,14 +1309,14 @@ VelodyneHwMonitorWrapper::VelodyneGetMotorPhase() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetLaserState() +VelodyneHwMonitorRosWrapper::VelodyneGetLaserState() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_laser_state); + mes = GetPtreeValue(current_status_tree, key_status_laser_state); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -920,367 +1325,383 @@ VelodyneHwMonitorWrapper::VelodyneGetLaserState() return std::make_tuple(not_ex, level, mes, error_mes); } -void VelodyneHwMonitorWrapper::VelodyneCheckTopHv( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopHv( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopHv(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopAdTemp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopAdTemp(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopLm20Temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopLm20Temp(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr5v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopPwr5v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr25v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopPwr25v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr33v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopPwr33v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrRaw( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopPwrRaw(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrVccint( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetTopPwrVccint(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotIOut( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotIOut( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotIOut(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr12v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotPwr12v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotLm20Temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotLm20Temp(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr5v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotPwr5v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr25v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotPwr25v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr33v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotPwr33v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwrVIn( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotPwrVIn(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v( +void VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr125v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetBotPwr125v(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckVhv( +void VelodyneHwMonitorRosWrapper::VelodyneCheckVhv( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetVhv(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckAdcNf( +void VelodyneHwMonitorRosWrapper::VelodyneCheckAdcNf( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetAdcNf(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckAdcStats( +void VelodyneHwMonitorRosWrapper::VelodyneCheckAdcStats( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetAdcStats(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckIxe( +void VelodyneHwMonitorRosWrapper::VelodyneCheckIxe( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetIxe(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat( +void VelodyneHwMonitorRosWrapper::VelodyneCheckAdctpStat( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { auto tpl = VelodyneGetAdctpStat(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::OnVelodyneStatusTimer() +void VelodyneHwMonitorRosWrapper::OnVelodyneStatusTimer() { - auto str = hw_interface_->GetStatus(); - { - std::lock_guard lock(mtx_status_); - current_status_tree = - std::make_shared(hw_interface_->ParseJson(str)); + if (mtx_status.try_lock()) { + hw_interface_.GetStatusAsync([this](const std::string & str) { + current_status_tree = + std::make_shared(hw_interface_.ParseJson(str)); + diagnostics_updater_.force_update(); + mtx_status.unlock(); + }); } - diagnostics_updater_.force_update(); } -void VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState( +void VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPpsState( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetGpsPpsState(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition( +void VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPosition( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetGpsPosition(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorState( +void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorState( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetMotorState(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm( +void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorRpm( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetMotorRpm(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorLock( +void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorLock( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetMotorLock(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase( +void VelodyneHwMonitorRosWrapper::VelodyneCheckMotorPhase( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetMotorPhase(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckLaserState( +void VelodyneHwMonitorRosWrapper::VelodyneCheckLaserState( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { auto tpl = VelodyneGetLaserState(); - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckSnapshot( +void VelodyneHwMonitorRosWrapper::VelodyneCheckSnapshot( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { uint8_t level = current_diag_status; - diagnostics.add("sensor", sensor_configuration_->frame_id); + diagnostics.add("sensor", sensor_configuration_.frame_id); diagnostics.summary(level, *current_snapshot); + // } +} + +void VelodyneHwMonitorRosWrapper::OnVelodyneSnapshotTimer() +{ + hw_interface_.GetSnapshotAsync([this](const std::string & str) { + current_snapshot_time.reset(new rclcpp::Time(this->get_clock()->now())); + current_snapshot_tree = + std::make_shared(hw_interface_.ParseJson(str)); + current_diag_tree = + std::make_shared(current_snapshot_tree->get_child("diag")); + current_status_tree = + std::make_shared(current_snapshot_tree->get_child("status")); + current_snapshot.reset(new std::string(str)); + }); } -void VelodyneHwMonitorWrapper::VelodyneCheckStatus( +void VelodyneHwMonitorRosWrapper::VelodyneCheckStatus( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1306,12 +1727,12 @@ void VelodyneHwMonitorWrapper::VelodyneCheckStatus( } } -void VelodyneHwMonitorWrapper::VelodyneCheckPps( +void VelodyneHwMonitorRosWrapper::VelodyneCheckPps( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_status_tree && + !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1337,12 +1758,12 @@ void VelodyneHwMonitorWrapper::VelodyneCheckPps( } } -void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( +void VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1368,12 +1789,12 @@ void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( } } -void VelodyneHwMonitorWrapper::VelodyneCheckRpm( +void VelodyneHwMonitorRosWrapper::VelodyneCheckRpm( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1399,12 +1820,12 @@ void VelodyneHwMonitorWrapper::VelodyneCheckRpm( } } -void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( +void VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorRosWrapper::current_diag_tree && + !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1444,7 +1865,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_top_pwr_3_3v, std::get<2>(tpl)); - if (sensor_configuration_->sensor_model == nebula::drivers::SensorModel::VELODYNE_VLP16) { + if (sensor_configuration_.sensor_model == nebula::drivers::SensorModel::VELODYNE_VLP16) { tpl = VelodyneGetTopPwr5vRaw(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); @@ -1540,28 +1961,36 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } } -std::string VelodyneHwMonitorWrapper::GetPtreeValue( - std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key) +rcl_interfaces::msg::SetParametersResult VelodyneHwMonitorRosWrapper::paramCallback( + const std::vector & p) { - std::lock_guard lock(mtx_pt); - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; + std::scoped_lock lock(mtx_config_); + RCLCPP_INFO_STREAM(get_logger(), p); + RCLCPP_INFO_STREAM(get_logger(), sensor_configuration_); + + drivers::VelodyneSensorConfiguration new_param{sensor_configuration_}; + + std::cout << new_param << std::endl; + std::string sensor_model_str; + std::string return_mode_str; + uint16_t new_diag_span = 0; + if (get_param(p, "diag_span", new_diag_span)) { + sensor_configuration_ = new_param; + // Update sensor_configuration + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); } -} -std::string VelodyneHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; -Status VelodyneHwMonitorWrapper::Status() -{ - return Status::NOT_IMPLEMENTED; // TODO + std::cout << "add_on_set_parameters_callback success" << std::endl; + + return *result; } +RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneHwMonitorRosWrapper) } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp deleted file mode 100644 index d8246e7b5..000000000 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ /dev/null @@ -1,265 +0,0 @@ -#include "nebula_ros/velodyne/velodyne_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions& options) - : rclcpp::Node("velodyne_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(&VelodyneRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - StreamStart(); - } - else - { - packets_sub_ = create_subscription( - "velodyne_packets", rclcpp::SensorDataQoS(), - std::bind(&VelodyneRosWrapper::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(&VelodyneRosWrapper::OnParameterChange, this, std::placeholders::_1)); -} - -nebula::Status VelodyneRosWrapper::DeclareAndGetSensorConfigParams() -{ - nebula::drivers::VelodyneSensorConfiguration 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::ReturnModeFromString(_return_mode); - - config.host_ip = declare_parameter("host_ip", param_read_only()); - config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); - config.data_port = declare_parameter("data_port", param_read_only()); - config.gnss_port = declare_parameter("gnss_port", param_read_only()); - config.frame_id = declare_parameter("frame_id", 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", descriptor); - } - - config.min_range = declare_parameter("min_range", param_read_write()); - config.max_range = declare_parameter("max_range", param_read_write()); - config.packet_mtu_size = declare_parameter("packet_mtu_size", param_read_only()); - - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); - descriptor.additional_constraints = "from 300 to 1200, in increments of 60"; - descriptor.integer_range = int_range(300, 1200, 60); - config.rotation_speed = declare_parameter("rotation_speed", 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", 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", descriptor); - } - - auto new_cfg_ptr = std::make_shared(config); - return ValidateAndSetConfig(new_cfg_ptr); -} - -Status VelodyneRosWrapper::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 (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 VelodyneRosWrapper::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 VelodyneRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status VelodyneRosWrapper::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 VelodyneRosWrapper::OnParameterChange(const std::vector& p) -{ - using namespace rcl_interfaces::msg; - - if (p.empty()) - { - return rcl_interfaces::build().successful(true).reason(""); - } - - std::scoped_lock lock(mtx_config_); - - RCLCPP_INFO(get_logger(), "OnParameterChange"); - - drivers::VelodyneSensorConfiguration 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); - - // 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 VelodyneRosWrapper::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(VelodyneRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd b/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd deleted file mode 100644 index 5da07e063..000000000 Binary files a/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd and /dev/null differ diff --git a/nebula_tests/data/velodyne/vls128/1614315746471294674/1614315746471294674_0.db3 b/nebula_tests/data/velodyne/vls128/1614315746471294674/1614315746471294674_0.db3 index 2d95339b1..e9de91274 100644 Binary files a/nebula_tests/data/velodyne/vls128/1614315746471294674/1614315746471294674_0.db3 and b/nebula_tests/data/velodyne/vls128/1614315746471294674/1614315746471294674_0.db3 differ diff --git a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd new file mode 100644 index 000000000..80ed53270 Binary files /dev/null and b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd differ diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index 97046e8f8..a07a4dabd 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -46,11 +46,12 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - correction_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_), - calibration_cfg_ptr_); + std::static_pointer_cast(calibration_cfg_ptr_)); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -58,12 +59,26 @@ 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), - calibration_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)); return driver_ptr_->GetStatus(); } @@ -220,7 +235,8 @@ 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()) { @@ -273,8 +289,9 @@ void HesaiRosDecoderTest::ReadBag( storage_options.uri = params_.bag_path; storage_options.storage_id = params_.storage_id; - converter_options.output_serialization_format = params_.format; + converter_options.output_serialization_format = params_.format; //"cdr"; 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); @@ -293,21 +310,12 @@ 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); - 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); - } + scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index f12e56020..a0477a366 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -17,8 +17,6 @@ #include #include -#include -#include #ifndef _SRC_CALIBRATION_DIR_PATH #define _SRC_CALIBRATION_DIR_PATH "" @@ -50,27 +48,9 @@ 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 +class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -85,7 +65,17 @@ class HesaiRosDecoderTest final : public rclcpp::Node /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_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); /// @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 7f80f6f48..96ff00c3c 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -12,8 +12,6 @@ #include #include #include -#include -#include namespace nebula { @@ -21,15 +19,72 @@ 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) @@ -87,8 +142,6 @@ 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_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp index c513cbc2b..fa3604485 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp @@ -34,22 +34,26 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + 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_); } Status VelodyneRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(calibration_configuration)); return driver_ptr_->GetStatus(); } @@ -332,31 +336,27 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( - std::vector(pkt.data.begin(), pkt.data.end()), - pkt.stamp.sec); - auto pointcloud = std::get<0>(pointcloud_ts); - - if (!pointcloud) { - continue; - } - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); + pointcloud = std::get<0>(pointcloud_ts); + + // There are very rare cases where has_scanned_ does not become true, but it is not known + // whether it is because of decoder or deserialize_message. + if (!pointcloud) continue; + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); + ref_pointcloud.reset(new pcl::PointCloud); } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } EXPECT_GT(check_cnt, 0); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp index 4a938e388..2b889358e 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp @@ -21,21 +21,22 @@ namespace nebula namespace ros { /// @brief Testing decoder of VLP16 (Keeps VelodyneDriverRosWrapper structure as much as possible) -class VelodyneRosDecoderTest final : public rclcpp::Node +class VelodyneRosDecoderTest final : public rclcpp::Node, + NebulaDriverRosWrapperBase //, testing::Test { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @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); + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; /// @brief Get configurations (Magic numbers for VLP16 is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp index 3fa61c95a..c7623ca50 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp @@ -34,22 +34,26 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + std::make_shared(calibration_configuration); - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + 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_); } Status VelodyneRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(calibration_configuration)); return driver_ptr_->GetStatus(); } @@ -336,32 +340,28 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( - std::vector(pkt.data.begin(), pkt.data.end()), - pkt.stamp.sec); - auto pointcloud = std::get<0>(pointcloud_ts); - - if (!pointcloud) { - continue; - } - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); + pointcloud = std::get<0>(pointcloud_ts); + + // There are very rare cases where has_scanned_ does not become true, but it is not known + // whether it is because of decoder or deserialize_message. + if (!pointcloud) continue; + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); + ref_pointcloud.reset(new pcl::PointCloud); } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } EXPECT_GT(check_cnt, 0); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp index 4a938e388..2b889358e 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp @@ -21,21 +21,22 @@ namespace nebula namespace ros { /// @brief Testing decoder of VLP16 (Keeps VelodyneDriverRosWrapper structure as much as possible) -class VelodyneRosDecoderTest final : public rclcpp::Node +class VelodyneRosDecoderTest final : public rclcpp::Node, + NebulaDriverRosWrapperBase //, testing::Test { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @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); + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; /// @brief Get configurations (Magic numbers for VLP16 is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index c7bc39453..b00a9433d 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -13,9 +13,7 @@ #include #include -#include #include -#include namespace nebula { @@ -36,30 +34,30 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( 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); + std::make_shared(calibration_configuration); + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + 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_); } Status VelodyneRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration), + std::static_pointer_cast(calibration_configuration)); return driver_ptr_->GetStatus(); } -Status VelodyneRosDecoderTest::GetStatus() -{ - return wrapper_status_; -} +Status VelodyneRosDecoderTest::GetStatus() { return wrapper_status_; } Status VelodyneRosDecoderTest::GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, @@ -225,7 +223,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter( - "target_topic", "/velodyne_packets", descriptor); + "target_topic", "/sensing/lidar/top/velodyne_packets", descriptor); target_topic = this->get_parameter("target_topic").as_string(); } @@ -317,9 +315,10 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; + converter_options.output_serialization_format = format; //"cdr"; 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()); @@ -338,31 +337,27 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( - std::vector(pkt.data.begin(), pkt.data.end()), - pkt.stamp.sec); - auto pointcloud = std::get<0>(pointcloud_ts); - - if (!pointcloud) { - continue; - } - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); + pointcloud = std::get<0>(pointcloud_ts); + + // There are very rare cases where has_scanned_ does not become true, but it is not known + // whether it is because of decoder or deserialize_message. + if (!pointcloud) continue; + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); + ref_pointcloud.reset(new pcl::PointCloud); } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } } EXPECT_GT(check_cnt, 0); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp index ad29dd961..a2432838f 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp @@ -20,17 +20,18 @@ namespace nebula { namespace ros { -class VelodyneRosDecoderTest final : public rclcpp::Node +class VelodyneRosDecoderTest final : public rclcpp::Node, + NebulaDriverRosWrapperBase //, testing::Test { std::shared_ptr driver_ptr_; Status wrapper_status_; - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration); + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; Status GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, diff --git a/ros2_socketcan b/ros2_socketcan deleted file mode 160000 index 4ced5284f..000000000 --- a/ros2_socketcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/scripts/plot_instrumentation.py b/scripts/plot_instrumentation.py deleted file mode 100755 index 7540aa35f..000000000 --- a/scripts/plot_instrumentation.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/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 07dbee302..521dfd347 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -1,13 +1,11 @@ -# %% +#%% -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 = [] @@ -17,20 +15,22 @@ 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,20 +41,16 @@ 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" - ) - for col in filter(lambda col: col.startswith("d_"), df.columns): - ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".") - + ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') + d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) for j, col in enumerate(d_columns): - if i == 0: + if (i == 0): boxes[j].set_ylabel(f"{col} (ms)") boxes[j].boxplot(df[col], positions=[i], labels=[label]) @@ -62,11 +58,13 @@ 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") @@ -76,23 +74,15 @@ 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) + plot_timing_comparison(args.run_names) \ No newline at end of file diff --git a/transport_drivers b/transport_drivers deleted file mode 160000 index abf8aa8e1..000000000 --- a/transport_drivers +++ /dev/null @@ -1 +0,0 @@ -Subproject commit abf8aa8e171f33e0acd6d845c3d795192c964e9c