diff --git a/.cspell.json b/.cspell.json index 75b26f454..a52c07b1c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -6,23 +6,39 @@ "adctp", "Adctp", "AT", + "autosar", "block_id", + "Bpearl", + "calib", + "DHAVE", + "Difop", "extrinsics", + "fout", "gprmc", + "gptp", + "Helios", "Hesai", + "Idat", + "ipaddr", + "manc", "memcpy", "mkdoxy", + "Msop", "nohup", "nproc", + "ntoa", "pandar", "PANDAR", "PANDARAT", "PANDARQT", "PANDARXT", "Pdelay", + "Piyush", + "piyushk", "QT", "rclcpp", "schedutil", + "srvs", "STD_COUT", "stds", "struct", @@ -30,23 +46,9 @@ "UDP_SEQ", "vccint", "Vccint", + "Vdat", "XT", "XTM", - "DHAVE", - "Bpearl", - "Helios", - "Msop", - "Difop", - "gptp", - "Idat", - "Vdat", - "autosar", - "srvs", - "manc", - "ipaddr", - "ntoa", - "piyushk", - "Piyush", - "fout" + "yukkysaito" ] -} +} \ No newline at end of file diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml new file mode 100644 index 000000000..00482c2b5 --- /dev/null +++ b/.github/workflows/json-schema-check.yaml @@ -0,0 +1,40 @@ +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 e966ffdfb..f068ec8be 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,6 @@ site/ # qcreator stuff CMakeLists.txt.user + +# pre-commit +node_modules/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 13c31ce62..b4c2e9f2f 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.1.0 + rev: v4.6.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.30.0 + rev: v0.40.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.5.1 + rev: v4.0.0-alpha.8 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.26.3 + rev: v1.35.1 hooks: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.4.0 + rev: v0.8.0 hooks: - id: prettier-package-xml - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.3 + rev: v0.10.0.1 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.4.2-1 + rev: v3.8.0-1 hooks: - id: shfmt args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.12.0 + rev: 5.13.2 hooks: - id: isort - repo: https://github.com/psf/black - rev: 22.1.0 + rev: 24.4.2 hooks: - id: black args: [--line-length=100] - repo: https://github.com/PyCQA/flake8 - rev: 4.0.1 + rev: 7.0.0 hooks: - id: flake8 additional_dependencies: @@ -78,12 +78,12 @@ repos: ] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v13.0.0 + rev: v18.1.5 hooks: - id: clang-format - repo: https://github.com/cpplint/cpplint - rev: 1.5.5 + rev: 1.6.1 hooks: - id: cpplint args: [--quiet] diff --git a/build_depends.repos b/build_depends.repos index e28379f1a..e9278685a 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -2,5 +2,5 @@ repositories: # TCP version of transport drivers transport_drivers: type: git - url: https://github.com/MapIV/transport_drivers - version: boost + url: https://github.com/mojomex/transport_drivers + version: mutable-buffer-in-udp-callback diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 4c146736c..154f8e453 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -40,6 +40,8 @@ ament_auto_add_library(nebula_common SHARED src/velodyne/velodyne_calibration_decoder.cpp ) +target_link_libraries(nebula_common yaml-cpp) + ament_auto_package() # Set ROS_DISTRO macros diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 9c6e5d555..8e0600999 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -8,13 +8,16 @@ #include #include #include +#include #include +#include +#include namespace nebula { namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : LidarConfigurationBase +struct HesaiSensorConfiguration : public LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -43,24 +46,38 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con return os; } +struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase +{ + virtual nebula::Status LoadFromBytes(const std::vector & buf) = 0; + virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; + virtual nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) = 0; +}; + /// @brief struct for Hesai calibration configuration -struct HesaiCalibrationConfiguration : CalibrationConfigurationBase +struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase { std::map elev_angle_map; std::map azimuth_offset_map; - inline nebula::Status LoadFromFile(const std::string & calibration_file) + inline nebula::Status LoadFromFile(const std::string & calibration_file) override { std::ifstream ifs(calibration_file); if (!ifs) { return Status::INVALID_CALIBRATION_FILE; } std::ostringstream ss; - ss << ifs.rdbuf(); // reading data + ss << ifs.rdbuf(); // reading data ifs.close(); return LoadFromString(ss.str()); } + nebula::Status LoadFromBytes(const std::vector & buf) + { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return LoadFromString(calibration_string); + } + /// @brief Loading calibration data /// @param calibration_content /// @return Resulting status @@ -70,14 +87,12 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase ss << calibration_content; std::string line; constexpr size_t expected_cols = 3; - while(std::getline(ss, line)) { + while (std::getline(ss, line)) { boost::char_separator sep(","); boost::tokenizer> tok(line, sep); std::vector actual_tokens(tok.begin(), tok.end()); - if (actual_tokens.size() < expected_cols - || actual_tokens.size() > expected_cols - ) { + if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) { std::cerr << "Ignoring line with unexpected data:" << line << std::endl; continue; } @@ -88,10 +103,9 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase float azimuth = std::stof(actual_tokens[2]); elev_angle_map[laser_id - 1] = elevation; azimuth_offset_map[laser_id - 1] = azimuth; - } catch (const std::invalid_argument& ia) { + } catch (const std::invalid_argument & ia) { continue; } - } return Status::OK; } @@ -99,7 +113,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveFile(const std::string & calibration_file) + inline nebula::Status SaveToFile(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -117,11 +131,19 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } + nebula::Status SaveToFileFromBytes( + const std::string & calibration_file, const std::vector & buf) override + { + std::string calibration_string = std::string(buf.begin(), buf.end()); + return SaveFileFromString(calibration_file, calibration_string); + } + /// @brief Saving calibration data from string /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) + inline nebula::Status SaveFileFromString( + const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -134,7 +156,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase }; /// @brief struct for Hesai correction configuration (for AT) -struct HesaiCorrection +struct HesaiCorrection : public HesaiCalibrationConfigurationBase { uint16_t delimiter; uint8_t versionMajor; @@ -156,12 +178,11 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param buf Binary buffer /// @return Resulting status - inline nebula::Status LoadFromBinary(const std::vector & buf) + inline nebula::Status LoadFromBytes(const std::vector & buf) override { size_t index; - for (index = 0; index < buf.size()-1; index++) { - if(buf[index]==0xee && buf[index+1]==0xff) - break; + for (index = 0; index < buf.size() - 1; index++) { + if (buf[index] == 0xee && buf[index + 1] == 0xff) break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -258,7 +279,7 @@ struct HesaiCorrection /// @brief Load correction data from file /// @param correction_file path /// @return Resulting status - inline nebula::Status LoadFromFile(const std::string & correction_file) + inline nebula::Status LoadFromFile(const std::string & correction_file) override { std::ifstream ifs(correction_file, std::ios::in | std::ios::binary); if (!ifs) { @@ -268,10 +289,10 @@ struct HesaiCorrection // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read((char *)&c, sizeof(unsigned char)); + ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); buf.emplace_back(c); } - LoadFromBinary(buf); + LoadFromBytes(buf); ifs.close(); return Status::OK; @@ -281,7 +302,8 @@ struct HesaiCorrection /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) + inline nebula::Status SaveToFileFromBytes( + const std::string & correction_file, const std::vector & buf) override { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -291,21 +313,20 @@ struct HesaiCorrection } std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; - for (const auto &byte : buf) { + for (const auto & byte : buf) { if (!sop_received) { if (byte == 0xEE) { std::cerr << "SOP received....\n"; sop_received = true; } } - if(sop_received) { + if (sop_received) { ofs << byte; } } std::cerr << "Closing file\n"; ofs.close(); - if(sop_received) - return Status::OK; + if (sop_received) return Status::OK; return Status::INVALID_CALIBRATION_FILE; } @@ -435,8 +456,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARQT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST - || return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL) + return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 9d04a267a..e43ab49a3 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,7 +2,10 @@ #define NEBULA_COMMON_H #include + #include + +#include #include #include #include @@ -342,24 +345,11 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { - IEEE_1588v2 = 0, - IEEE_802_1AS, - IEEE_802_1AS_AUTO, - PROFILE_UNKNOWN -}; +enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; -enum class PtpTransportType { - UDP_IP = 0, - L2, - UNKNOWN_TRANSPORT -}; +enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; -enum class PtpSwitchType { - NON_TSN = 0, - TSN, - UNKNOWN_SWITCH -}; +enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; /// @brief not used? struct PointField @@ -618,13 +608,14 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; - return PtpProfile::PROFILE_UNKNOWN; + return PtpProfile::UNKNOWN_PROFILE; } /// @brief Convert PtpProfile enum to string (Overloading the << operator) @@ -643,7 +634,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile case PtpProfile::IEEE_802_1AS_AUTO: os << "IEEE_802.1AS Automotive"; break; - case PtpProfile::PROFILE_UNKNOWN: + case PtpProfile::UNKNOWN_PROFILE: os << "UNKNOWN"; break; } @@ -657,8 +648,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -692,8 +684,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), - [](unsigned char c){ return std::tolower(c); }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { + return std::tolower(c); + }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index d3643cfb4..25cb5f83d 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -1,23 +1,22 @@ #pragma once -#include -#include #include +#include +#include namespace nebula { namespace util { -struct bad_expected_access : public std::exception { - bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} +struct bad_expected_access : public std::exception +{ + explicit bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} - const char* what() const noexcept override { - return msg_.c_str(); - } + const char * what() const noexcept override { return msg_.c_str(); } private: - const std::string msg_; + const std::string msg_; }; /// @brief A poor man's backport of C++23's std::expected. @@ -39,14 +38,16 @@ struct expected /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained. /// @return The value of type `T` - T value() { + T value() + { if (!has_value()) { throw bad_expected_access("value() called but containing error"); } - return std::get(value_); + return std::get(value_); } - /// @brief Return the contained value, or, if an error is contained, return the given `default_` instead. + /// @brief Return the contained value, or, if an error is contained, return the given `default_` + /// instead. /// @return The contained value, if any, else `default_` T value_or(const T & default_) { @@ -54,36 +55,41 @@ struct expected return default_; } - /// @brief Return the contained value, or, if an error is contained, throw `runtime_error(error_msg)`. - /// @return The contained value if no error is thrown - T value_or_throw(const std::string & error_msg) { + /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg) + /// @param error_msg The message to be included in the thrown exception + /// @return The value + T value_or_throw(const std::string & error_msg) + { if (has_value()) return value(); throw std::runtime_error(error_msg); } /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained. /// @return The error of type `E` - E error() { + E error() + { if (has_value()) { throw bad_expected_access("error() called but containing value"); } - return std::get(value_); + return std::get(value_); } - /// @brief Return the contained error, or, if a value is contained, return the given `default_` instead. + /// @brief Return the contained error, or, if a value is contained, return the given `default_` + /// instead. /// @return The contained error, if any, else `default_` - E error_or(const E & default_) { + E error_or(const E & default_) + { if (!has_value()) return error(); - return default_; - } + return default_; + } - expected(const T & value) { value_ = value; } + expected(const T & value) { value_ = value; } // NOLINT(runtime/explicit) - expected(const E & error) { value_ = error; } + expected(const E & error) { value_ = error; } // NOLINT(runtime/explicit) private: std::variant value_; }; } // namespace util -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index 42fd0a008..02d7cd535 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -23,19 +23,11 @@ struct CorrectedAngleData /// @brief Handles angle correction for given azimuth/channel combinations, as well as trigonometry /// lookup tables +template class AngleCorrector { -protected: - const std::shared_ptr sensor_calibration_; - const std::shared_ptr sensor_correction_; - public: - AngleCorrector( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : sensor_calibration_(sensor_calibration), sensor_correction_(sensor_correction) - { - } + using correction_data_t = CorrectionDataT; /// @brief Get the corrected azimuth and elevation for a given block and channel, along with their /// sin/cos values. @@ -52,8 +44,9 @@ class AngleCorrector /// @param sync_azimuth The azimuth set in the sensor configuration, for which the /// timestamp is aligned to the full second /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; + virtual bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0; }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index 3e0c999b0..da9a1b579 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -4,6 +4,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include +#include namespace nebula { @@ -11,7 +12,7 @@ namespace drivers { template -class AngleCorrectorCalibrationBased : public AngleCorrector +class AngleCorrectorCalibrationBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LEN = 360 * AngleUnit; @@ -27,9 +28,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector public: AngleCorrectorCalibrationBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction) + const std::shared_ptr & sensor_calibration) { if (sensor_calibration == nullptr) { throw std::runtime_error( @@ -37,8 +36,8 @@ class AngleCorrectorCalibrationBased : public AngleCorrector } for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { - float elevation_angle_deg = sensor_calibration->elev_angle_map[channel_id]; - float azimuth_offset_deg = sensor_calibration->azimuth_offset_map[channel_id]; + float elevation_angle_deg = sensor_calibration->elev_angle_map.at(channel_id); + float azimuth_offset_deg = sensor_calibration->azimuth_offset_map.at(channel_id); elevation_angle_rad_[channel_id] = deg2rad(elevation_angle_deg); azimuth_offset_rad_[channel_id] = deg2rad(azimuth_offset_deg); @@ -81,10 +80,10 @@ class AngleCorrectorCalibrationBased : public AngleCorrector (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; uint32_t last_diff_from_sync = (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN; - + return current_diff_from_sync < last_diff_from_sync; } }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp index be60078de..619c5a8fe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp @@ -4,8 +4,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp" #include - -#define _(x) '"' << #x << "\": " << x << ", " +#include namespace nebula { @@ -13,10 +12,11 @@ namespace drivers { template -class AngleCorrectorCorrectionBased : public AngleCorrector +class AngleCorrectorCorrectionBased : public AngleCorrector { private: static constexpr size_t MAX_AZIMUTH_LENGTH = 360 * AngleUnit; + const std::shared_ptr correction_; rclcpp::Logger logger_; std::array cos_{}; @@ -29,28 +29,26 @@ class AngleCorrectorCorrectionBased : public AngleCorrector { // Assumes that: // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) - // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg etc.) - // These assumptions hold for AT128E2X. - int field = sensor_correction_->frameNumber - 1; - for (size_t i = 0; i < sensor_correction_->frameNumber; ++i) { - if (azimuth < sensor_correction_->startFrame[i]) return field; + // * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg + // etc.) These assumptions hold for AT128E2X. + int field = correction_->frameNumber - 1; + for (size_t i = 0; i < correction_->frameNumber; ++i) { + if (azimuth < correction_->startFrame[i]) return field; field = i; } - // This is never reached if sensor_correction_ is correct + // This is never reached if correction_ is correct return field; } public: - AngleCorrectorCorrectionBased( - const std::shared_ptr & sensor_calibration, - const std::shared_ptr & sensor_correction) - : AngleCorrector(sensor_calibration, sensor_correction), - logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) + explicit AngleCorrectorCorrectionBased( + const std::shared_ptr & sensor_correction) + : correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased")) { if (sensor_correction == nullptr) { throw std::runtime_error( - "Cannot instantiate AngleCorrectorCalibrationBased without calibration data"); + "Cannot instantiate AngleCorrectorCorrectionBased without correction data"); } logger_.set_level(rclcpp::Logger::Level::Debug); @@ -64,17 +62,16 @@ class AngleCorrectorCorrectionBased : public AngleCorrector CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override { - const auto & correction = AngleCorrector::sensor_correction_; int field = findField(block_azimuth); auto elevation = - correction->elevation[channel_id] + - correction->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + correction_->elevation[channel_id] + + correction_->getElevationAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); elevation = (MAX_AZIMUTH_LENGTH + elevation) % MAX_AZIMUTH_LENGTH; - auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction->startFrame[field]) * 2 - - correction->azimuth[channel_id] + - correction->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); + auto azimuth = (block_azimuth + MAX_AZIMUTH_LENGTH - correction_->startFrame[field]) * 2 - + correction_->azimuth[channel_id] + + correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * (AngleUnit / 100); azimuth = (MAX_AZIMUTH_LENGTH + azimuth) % MAX_AZIMUTH_LENGTH; float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH_LENGTH; @@ -84,7 +81,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override + bool hasScanned( + uint32_t current_azimuth, uint32_t last_azimuth, uint32_t /*sync_azimuth*/) override { // For AT128, the scan is always cut at the beginning of the field: // If we would cut at `sync_azimuth`, the points left of it would be diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 7bdfd069e..2e2bba6ba 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -8,6 +8,11 @@ #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include + namespace nebula { namespace drivers @@ -18,7 +23,7 @@ class HesaiDecoder : public HesaiScanDecoder { protected: /// @brief Configuration for this decoder - const std::shared_ptr sensor_configuration_; + const std::shared_ptr sensor_configuration_; /// @brief The sensor definition, used for return mode and time offset handling SensorT sensor_{}; @@ -34,13 +39,13 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief The last decoded packet typename SensorT::packet_t packet_; /// @brief The last azimuth processed - int last_phase_; + int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet /// @brief The timestamp of the last completed scan in nanoseconds - uint64_t output_scan_timestamp_ns_; + uint64_t output_scan_timestamp_ns_ = 0; /// @brief The timestamp of the scan currently in progress - uint64_t decode_scan_timestamp_ns_; + uint64_t decode_scan_timestamp_ns_ = 0; /// @brief Whether a full scan has been processed - bool has_scanned_; + bool has_scanned_ = false; rclcpp::Logger logger_; @@ -52,17 +57,17 @@ class HesaiDecoder : public HesaiScanDecoder block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) + bool parsePacket(const std::vector & packet) { - if (pandar_packet.size < sizeof(typename SensorT::packet_t)) { + if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:" + logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { // FIXME(mojomex) do validation? // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); return true; @@ -174,6 +179,10 @@ class HesaiDecoder : public HesaiScanDecoder /// @return Whether the scan has completed bool checkScanCompleted(uint32_t current_phase, uint32_t sync_phase) { + if (last_phase_ == -1) { + return false; + } + return angle_corrector_.hasScanned(current_phase, last_phase_, sync_phase); } @@ -200,20 +209,17 @@ class HesaiDecoder : public HesaiScanDecoder public: /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder (can be nullptr if - /// correction_configuration is set) - /// @param correction_configuration Correction for this decoder (can be nullptr if - /// calibration_configuration is set) + /// @param correction_data Calibration data for this decoder explicit HesaiDecoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + correction_data) : sensor_configuration_(sensor_configuration), - angle_corrector_(calibration_configuration, correction_configuration), + angle_corrector_(correction_data), logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); - RCLCPP_INFO_STREAM(logger_, sensor_configuration_); + RCLCPP_INFO_STREAM(logger_, *sensor_configuration_); decode_pc_.reset(new NebulaPointCloud); output_pc_.reset(new NebulaPointCloud); @@ -222,9 +228,9 @@ class HesaiDecoder : public HesaiScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override + int unpack(const std::vector & packet) override { - if (!parsePacket(pandar_packet)) { + if (!parsePacket(packet)) { return -1; } @@ -247,7 +253,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 730888c91..aa807e567 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -190,7 +190,7 @@ struct PacketBase /// @brief Get the number of returns for a given return mode /// @param return_mode The return mode /// @return The number of returns -int get_n_returns(uint8_t return_mode) +inline int get_n_returns(uint8_t return_mode) { switch (return_mode) { case return_mode::SINGLE_FIRST: @@ -221,7 +221,8 @@ uint64_t get_timestamp_ns(const PacketT & packet) return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000; } -/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, multiplied by this value, yield the distance in meters. +/// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, +/// multiplied by this value, yield the distance in meters. /// @tparam PacketT The packet type /// @param packet The packet to get the distance unit from /// @return The distance unit in meters diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index f0b4e3f8b..153435bbe 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -8,6 +8,7 @@ #include "pandar_msgs/msg/pandar_scan.hpp" #include +#include namespace nebula { @@ -26,9 +27,9 @@ class HesaiScanDecoder HesaiScanDecoder() = default; /// @brief Parses PandarPacket and add its points to the point cloud - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return The last azimuth processed - virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + virtual int unpack(const std::vector & packet) = 0; /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index c83788965..4c211efcc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -14,15 +14,18 @@ #include #include +#include #include #include +#include +#include namespace nebula { namespace drivers { /// @brief Hesai driver -class HesaiDriver : NebulaDriverBase +class HesaiDriver { private: /// @brief Current driver status @@ -30,16 +33,22 @@ class HesaiDriver : NebulaDriverBase /// @brief Decoder according to the model std::shared_ptr scan_decoder_; + template + std::shared_ptr InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); + public: HesaiDriver() = delete; /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver (for AT) + /// @param calibration_configuration CalibrationConfiguration for this driver (either + /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128) explicit HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration = nullptr); + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); /// @brief Get current status of this driver /// @return Current status @@ -49,13 +58,13 @@ class HesaiDriver : NebulaDriverBase /// @param calibration_configuration /// @return Resulting status Status SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) override; + const HesaiCalibrationConfigurationBase & calibration_configuration); /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan); + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/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 7c319e0fe..02209c5b2 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,6 +20,7 @@ #include #include +#include namespace nebula { @@ -132,23 +133,57 @@ 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; @@ -161,7 +196,7 @@ class VelodyneScanDecoder /// @brief Virtual function for parsing and shaping VelodynePacket /// @param pandar_packet - virtual void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0; + virtual void unpack(const std::vector & packet, int32_t packet_seconds) = 0; /// @brief Virtual function for parsing VelodynePacket based on packet structure /// @param pandar_packet /// @return Resulting flag @@ -169,7 +204,10 @@ class VelodyneScanDecoder /// @brief Virtual function for getting the flag indicating whether one cycle is ready /// @return Readied - virtual bool hasScanned() = 0; + bool hasScanned() { + return has_scanned_; + } + /// @brief Calculation of points in each packet /// @return # of points virtual int pointsPerPacket() = 0; @@ -178,8 +216,7 @@ class VelodyneScanDecoder /// @return tuple of Point cloud and timestamp virtual std::tuple get_pointcloud() = 0; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0; + virtual void reset_pointcloud(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 730f7045f..be920a3e1 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,14 +25,11 @@ 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 velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - /// @brief Get the flag indicating whether one cycle is ready - /// @return Readied - bool hasScanned() override; + void unpack(const std::vector & packet, int32_t packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -40,8 +37,7 @@ class Vlp16Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - void reset_pointcloud(size_t n_pts, double time_stamp) override; + void reset_pointcloud(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 0ffa1e3ce..4d5cb938b 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,14 +24,11 @@ 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 velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - /// @brief Get the flag indicating whether one cycle is ready - /// @return Readied - bool hasScanned() override; + void unpack(const std::vector & packet, int32_t packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -39,8 +36,7 @@ class Vlp32Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - void reset_pointcloud(size_t n_pts, double time_stamp) override; + void reset_pointcloud(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 e440096e9..af84a33f4 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,14 +21,11 @@ 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 velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - /// @brief Get the flag indicating whether one cycle is ready - /// @return Readied - bool hasScanned() override; + void unpack(const std::vector & packet, int32_t packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points int pointsPerPacket() override; @@ -36,8 +33,7 @@ class Vls128Decoder : public VelodyneScanDecoder /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; /// @brief Resetting point cloud buffer - /// @param n_pts # of points - void reset_pointcloud(size_t n_pts, double time_stamp) override; + void reset_pointcloud(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 d0257b82d..c5caf00b8 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 ConvertScanToPointcloud( - const std::shared_ptr & velodyne_scan); + std::tuple ParseCloudPacket( + const std::vector & packet, int32_t packet_seconds); }; } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 49be4adac..269db2a17 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -18,91 +18,90 @@ namespace nebula namespace drivers { HesaiDriver::HesaiDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_data) { // initialize proper parser from cloud config's model and echo mode driver_status_ = nebula::Status::OK; + switch (sensor_configuration->sensor_model) { - case SensorModel::UNKNOWN: - driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; - break; case SensorModel::HESAI_PANDAR64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_.reset(new HesaiDecoder( - sensor_configuration, calibration_configuration, correction_configuration)); + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); break; + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + throw std::runtime_error("Invalid sensor model."); default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); - break; } } -std::tuple HesaiDriver::ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan) +template +std::shared_ptr HesaiDriver::InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) +{ + using CalibT = typename SensorT::angle_corrector_t::correction_data_t; + return std::make_shared>( + sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); +} + +std::tuple HesaiDriver::ParseCloudPacket( + const std::vector & packet) { std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); - if (driver_status_ != nebula::Status::OK) { + if (driver_status_ != nebula::Status::OK) + { RCLCPP_ERROR(logger, "Driver not OK."); return pointcloud; } - int cnt = 0, last_azimuth = 0; - for (auto & packet : pandar_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); - cnt++; - } + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); } - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); - } + // todo + // if (cnt == 0) { + // RCLCPP_ERROR_STREAM( + // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " + // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // } return pointcloud; } Status HesaiDriver::SetCalibrationConfiguration( - const CalibrationConfigurationBase & calibration_configuration) + const HesaiCalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( "SetCalibrationConfiguration. Not yet implemented (" + diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 338163d60..298431f7b 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,11 +56,6 @@ 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); @@ -87,11 +82,9 @@ int Vlp16Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING; } -void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp) +void Vlp16Decoder::reset_pointcloud(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 } @@ -138,12 +131,14 @@ void Vlp16Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) +void Vlp16Decoder::unpack(const std::vector & packet, int32_t packet_seconds) { - const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; + checkAndHandleScanComplete(packet, packet_seconds, phase_); + + const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; + const uint8_t return_mode = packet[RETURN_MODE_INDEX]; const bool dual_return = (return_mode == RETURN_MODE_DUAL); for (uint block = 0; block < BLOCKS_PER_PACKET; block++) { @@ -203,7 +198,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa 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 = rclcpp::Time(velodyne_packet.stamp).seconds(); + auto block_timestamp = packet_seconds; if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -216,7 +211,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa continue; } { - VelodyneLaserCorrection & corrections = + const 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 ec3e06745..3b82bf522 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,11 +56,6 @@ Vlp32Decoder::Vlp32Decoder( } } -bool Vlp32Decoder::hasScanned() -{ - return has_scanned_; -} - std::tuple Vlp32Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -85,12 +80,10 @@ int Vlp32Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp) +void Vlp32Decoder::reset_pointcloud(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 } @@ -137,10 +130,12 @@ void Vlp32Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) +void Vlp32Decoder::unpack(const std::vector & packet, int32_t packet_seconds) { - const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; - uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; + 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 bool dual_return = (return_mode == RETURN_MODE_DUAL); for (int i = 0; i < BLOCKS_PER_PACKET; i++) { @@ -170,7 +165,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa 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 = rclcpp::Time(velodyne_packet.stamp).seconds(); + auto block_timestamp = packet_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 c42f9b0b5..a3bff8fdc 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,11 +56,6 @@ Vls128Decoder::Vls128Decoder( } } -bool Vls128Decoder::hasScanned() -{ - return has_scanned_; -} - std::tuple Vls128Decoder::get_pointcloud() { double phase = angles::from_degrees(sensor_configuration_->scan_phase); @@ -87,12 +82,10 @@ int Vls128Decoder::pointsPerPacket() return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } -void Vls128Decoder::reset_pointcloud(size_t n_pts, double time_stamp) +void Vls128Decoder::reset_pointcloud(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 } @@ -139,12 +132,14 @@ void Vls128Decoder::reset_overflow(double time_stamp) overflow_pc_->points.reserve(max_pts_); } -void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) +void Vls128Decoder::unpack(const std::vector & packet, int32_t packet_seconds) { - const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; + checkAndHandleScanComplete(packet, packet_seconds, phase_); + + const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; + const uint8_t return_mode = packet[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++) { @@ -223,7 +218,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p 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 = rclcpp::Time(velodyne_packet.stamp).seconds(); + auto block_timestamp = packet_seconds; if (scan_timestamp_ < 0) { scan_timestamp_ = block_timestamp; } @@ -240,7 +235,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p 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 - VelodyneLaserCorrection & corrections = + const 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 b0f664c8c..310e75697 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,20 +46,24 @@ Status VelodyneDriver::SetCalibrationConfiguration( calibration_configuration.calibration_file + ")"); } -std::tuple VelodyneDriver::ConvertScanToPointcloud( - const std::shared_ptr & velodyne_scan) +std::tuple VelodyneDriver::ParseCloudPacket( + const std::vector & packet, int32_t packet_seconds) { std::tuple pointcloud; - 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); - } + + 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()) + { 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 a7681323c..b8bbe345c 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -15,18 +15,20 @@ #ifndef NEBULA_HesaiRosOfflineExtractBag_H #define NEBULA_HesaiRosOfflineExtractBag_H -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include +#include +#include +#include +#include +#include +#include #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include +#include #include #include @@ -35,7 +37,7 @@ namespace nebula { namespace ros { -class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractBag final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -46,12 +48,7 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node, NebulaDriverRosWrap Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); Status GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp index 711502e6a..8e26515bf 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, NebulaDriverRosWrapperBase +class HesaiRosOfflineExtractSample final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -51,17 +51,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node, NebulaDriverRosW /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp index c1910d294..33966d955 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, NebulaDriverRosWrapperBase +class VelodyneRosOfflineExtractBag final : public rclcpp::Node { 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) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration diff --git a/nebula_examples/package.xml b/nebula_examples/package.xml index 6f1163279..f63847cca 100644 --- a/nebula_examples/package.xml +++ b/nebula_examples/package.xml @@ -28,8 +28,8 @@ velodyne_msgs yaml-cpp + ament_cmake_gtest ament_lint_auto - ament_lint_common ament_cmake diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 9c1266347..649085018 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -56,12 +56,11 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,171 +68,56 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( Status HesaiRosOfflineExtractBag::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractBag::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) +Status HesaiRosOfflineExtractBag::GetStatus() { - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); + return wrapper_status_; } -Status HesaiRosOfflineExtractBag::GetStatus() {return wrapper_status_;} - Status HesaiRosOfflineExtractBag::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { + auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); + sensor_configuration.return_mode = + nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + this->declare_parameter("frame_id", "pandar", param_read_only()); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = - // nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); - nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); + descriptor.floating_point_range = float_range(0, 360, 0.01); + sensor_configuration.scan_phase = this->declare_parameter("scan_phase", 0., descriptor); + this->get_parameter("scan_phase").as_double(); } + + calibration_configuration.calibration_file = + this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("out_num", 3, descriptor); - out_num = this->get_parameter("out_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 2; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("skip_num", 3, descriptor); - skip_num = this->get_parameter("skip_num").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 1; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("only_xyz", false, descriptor); - only_xyz = this->get_parameter("only_xyz").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + correction_file_path = + this->declare_parameter("correction_file", "", param_read_only()); } + bag_path = this->declare_parameter("bag_path", "", param_read_only()); + storage_id = this->declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path = this->declare_parameter("out_path", "", param_read_only()); + format = this->declare_parameter("format", "cdr", param_read_only()); + out_num = this->declare_parameter("out_num", 3, param_read_only()); + skip_num = this->declare_parameter("skip_num", 3, param_read_only()); + only_xyz = this->declare_parameter("only_xyz", false, param_read_only()); + target_topic = this->declare_parameter("target_topic", "", param_read_only()); + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } @@ -296,75 +180,83 @@ Status HesaiRosOfflineExtractBag::ReadBag() if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } - // return Status::OK; - pcl::PCDWriter writer; + pcl::PCDWriter pcd_writer; - std::unique_ptr writer_; - bool needs_open = true; + std::unique_ptr bag_writer{}; storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; - { - rosbag2_cpp::Reader reader(std::make_unique()); - // reader.open(rosbag_directory.string()); - reader.open(storage_options, converter_options); - int cnt = 0; - int out_cnt = 0; - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - // std::cout<<"Found data in topic " << bag_message->topic_name << ": " << - // extracted_test_msg.data << std::endl; - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud( - std::make_shared(extracted_msg)); - auto pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - - if (needs_open) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( - {bag_message->topic_name, "pandar_msgs/msg/PandarScan", rmw_get_serialization_format(), - ""}); - needs_open = false; - } - writer_->write(bag_message); - cnt++; - if (skip_num < cnt) { - out_cnt++; - if (only_xyz) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); - } - // writer.writeASCII((o_dir / fn).string(), *pointcloud); - } - if (out_num <= out_cnt) { - break; + converter_options.output_serialization_format = format; + + rosbag2_cpp::Reader reader(std::make_unique()); + reader.open(storage_options, converter_options); + int cnt = 0; + int out_cnt = 0; + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name != target_topic) { + continue; + } + + std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + nebula_msgs::msg::NebulaPackets nebula_msg; + nebula_msg.header = extracted_msg.header; + for (auto & pkt : extracted_msg.packets) { + std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); + auto pointcloud_ts = driver_ptr_->ParseCloudPacket(pkt_data); + auto pointcloud = std::get<0>(pointcloud_ts); + + nebula_msgs::msg::NebulaPacket nebula_pkt; + nebula_pkt.stamp = pkt.stamp; + nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` + nebula_msg.packets.push_back(nebula_pkt); + + if (!pointcloud) { + continue; + } + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + if (!bag_writer) { + const rosbag2_storage::StorageOptions storage_options_w( + {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); + const rosbag2_cpp::ConverterOptions converter_options_w( + {rmw_get_serialization_format(), rmw_get_serialization_format()}); + bag_writer = std::make_unique(); + bag_writer->open(storage_options_w, converter_options_w); + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), + ""}); + } + + bag_writer->write(bag_message); + cnt++; + if (skip_num < cnt) { + out_cnt++; + if (only_xyz) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); } } + + if (out_num <= out_cnt) { + break; + } } - // close on scope exit } return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index a57d9b96e..8e134f8aa 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -56,12 +56,11 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -69,31 +68,20 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( Status HesaiRosOfflineExtractSample::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } -Status HesaiRosOfflineExtractSample::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) +Status HesaiRosOfflineExtractSample::GetStatus() { - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); + return wrapper_status_; } -Status HesaiRosOfflineExtractSample::GetStatus() {return wrapper_status_;} - Status HesaiRosOfflineExtractSample::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, @@ -119,7 +107,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; @@ -296,12 +284,19 @@ Status HesaiRosOfflineExtractSample::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"; - // pcl::io::savePCDFileBinary((o_dir / fn).string(), *pointcloud); - writer.writeBinary((o_dir / fn).string(), *pointcloud); + 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); + } } } // 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 913898b7c..ce3a69966 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,26 +33,22 @@ 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( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_ ,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( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } @@ -338,37 +334,46 @@ 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"; - 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); + 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 (out_num <= out_cnt) { - break; } } } diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index a6d78c45e..13ad0ed53 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -10,7 +10,7 @@ #include #include -using namespace boost::endian; +using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { @@ -100,7 +100,7 @@ struct HesaiPtpDiagTime os << ", "; os << "gmTimeBaseIndicator: " << arg.gmTimeBaseIndicator; os << ", "; - //FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect + // FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect os << "lastGmPhaseChange: " << std::string(std::begin(arg.lastGmPhaseChange), std::end(arg.lastGmPhaseChange)); os << ", "; @@ -158,7 +158,9 @@ struct HesaiInventory os << "sn: " << std::string(arg.sn, strnlen(arg.sn, sizeof(arg.sn))); os << ", "; os << "date_of_manufacture: " - << std::string(arg.date_of_manufacture, strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); + << std::string( + arg.date_of_manufacture, + strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); os << ", "; os << "mac: "; @@ -256,7 +258,7 @@ struct HesaiConfig uint8_t motor_status; uint8_t vlan_flag; big_uint16_buf_t vlan_id; - uint8_t clock_data_fmt; //FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets + uint8_t clock_data_fmt; // FIXME: labeled as gps_nmea_sentence in AT128, OT128 datasheets uint8_t noise_filtering; uint8_t reflectivity_mapping; uint8_t reserved[6]; @@ -266,29 +268,17 @@ struct HesaiConfig std::ios initial_format(nullptr); initial_format.copyfmt(os); - os << "ipaddr: " - << +arg.ipaddr[0] << "." - << +arg.ipaddr[1] << "." - << +arg.ipaddr[2] << "." + os << "ipaddr: " << +arg.ipaddr[0] << "." << +arg.ipaddr[1] << "." << +arg.ipaddr[2] << "." << +arg.ipaddr[3]; os << ", "; - os << "mask: " - << +arg.mask[0] << "." - << +arg.mask[1] << "." - << +arg.mask[2] << "." + os << "mask: " << +arg.mask[0] << "." << +arg.mask[1] << "." << +arg.mask[2] << "." << +arg.mask[3]; os << ", "; - os << "gateway: " - << +arg.gateway[0] << "." - << +arg.gateway[1] << "." - << +arg.gateway[2] << "." + os << "gateway: " << +arg.gateway[0] << "." << +arg.gateway[1] << "." << +arg.gateway[2] << "." << +arg.gateway[3]; os << ", "; - os << "dest_ipaddr: " - << +arg.dest_ipaddr[0] << "." - << +arg.dest_ipaddr[1] << "." - << +arg.dest_ipaddr[2] << "." - << +arg.dest_ipaddr[3]; + os << "dest_ipaddr: " << +arg.dest_ipaddr[0] << "." << +arg.dest_ipaddr[1] << "." + << +arg.dest_ipaddr[2] << "." << +arg.dest_ipaddr[3]; os << ", "; os << "dest_LiDAR_udp_port: " << arg.dest_LiDAR_udp_port; os << ", "; @@ -351,7 +341,7 @@ struct HesaiLidarStatus big_uint32_buf_t startup_times; big_uint32_buf_t total_operation_time; uint8_t ptp_clock_status; - uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet + uint8_t reserved[5]; // FIXME: 4 bytes labeled as humidity in OT128 datasheet friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarStatus const & arg) { @@ -463,7 +453,7 @@ struct HesaiPtpConfig int8_t logAnnounceInterval; int8_t logSyncInterval; int8_t logMinDelayReqInterval; - //FIXME: this format is not correct for OT128, or for AT128 on 802.1AS + // FIXME: this format is not correct for OT128, or for AT128 on 802.1AS friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpConfig const & arg) { @@ -489,7 +479,7 @@ struct HesaiPtpConfig /// @brief struct of PTC_COMMAND_LIDAR_MONITOR struct HesaiLidarMonitor { - //FIXME: this format is not correct for OT128 + // FIXME: this format is not correct for OT128 big_int32_buf_t input_voltage; big_int32_buf_t input_current; big_int32_buf_t input_power; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 1b21ddae2..10a1ec93f 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace nebula @@ -103,7 +104,7 @@ const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; /// @brief Hardware interface of hesai driver -class HesaiHwInterface : NebulaHwInterfaceBase +class HesaiHwInterface { private: struct ptc_error_t @@ -120,15 +121,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::shared_ptr m_owned_ctx; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; - std::shared_ptr sensor_configuration_; - std::shared_ptr calibration_configuration_; - size_t azimuth_index_{}; - size_t mtu_size_{}; - std::unique_ptr scan_cloud_ptr_; - std::function - is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function buffer)> - scan_reception_callback_; /**This function pointer is called when the scan is complete*/ + std::shared_ptr sensor_configuration_; + std::function & buffer)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ + + std::mutex mtx_inflight_tcp_request_; int prev_phase_{}; @@ -172,6 +169,12 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return A string description of all errors in this code std::string PrettyPrintPTCError(ptc_error_t error_code); + /// @brief Checks if the data size matches that of the struct to be parsed, and parses the struct. + /// If data is too small, a std::runtime_error is thrown. If data is too large, a warning is + /// printed and the struct is parsed with the first sizeof(T) bytes. + template + T CheckSizeAndParse(const std::vector & data); + /// @brief Send a PTC request with an optional payload, and return the full response payload. /// Blocking. /// @param command_id PTC command number. @@ -198,17 +201,17 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status @@ -217,12 +220,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); @@ -388,13 +390,13 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param hesai_config Current HesaiConfig /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config); + std::shared_ptr sensor_configuration, HesaiConfig hesai_config); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_lidar_range_all Current HesaiLidarRangeAll /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all); /// @brief Checking the current settings and changing the difference point /// @return Resulting status diff --git a/nebula_hw_interfaces/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 fcb907a15..858e29488 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,15 @@ #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 #include @@ -32,30 +31,20 @@ namespace nebula namespace drivers { /// @brief Hardware interface of velodyne driver -class VelodyneHwInterface : NebulaHwInterfaceBase +class VelodyneHwInterface { 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::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 sensor_configuration_; + std::function&)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ 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"}; @@ -67,6 +56,9 @@ class VelodyneHwInterface : NebulaHwInterfaceBase 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 @@ -86,7 +78,7 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @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; @@ -106,38 +98,38 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; + Status SensorInterfaceStop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status Status GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration) final; + CalibrationConfigurationBase & calibration_configuration); /// @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) final; + std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status Status RegisterScanCallback( - std::function)> scan_callback); + std::function& packet)> scan_callback); /// @brief Parsing JSON string to property_tree /// @param str JSON string @@ -217,95 +209,6 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @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 fccb2a622..29b260098 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -17,40 +17,19 @@ HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, - scan_cloud_ptr_{std::make_unique()} + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} { } HesaiHwInterface::~HesaiHwInterface() { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif - if (tcp_driver_) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................tcp_driver_ is available" << std::endl; -#endif - if (tcp_driver_ && tcp_driver_->isOpen()) { -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................st: tcp_driver_->close();" << std::endl; -#endif - tcp_driver_->close(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: tcp_driver_->close();" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: if(tcp_driver_)" << std::endl; -#endif - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << ".......................ed: HesaiHwInterface::~HesaiHwInterface()" << std::endl; -#endif + FinalizeTcpDriver(); } HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( const uint8_t command_id, const std::vector & payload) { + std::lock_guard lock(mtx_inflight_tcp_request_); + uint32_t len = payload.size(); std::vector send_buf; @@ -64,15 +43,16 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( send_buf.emplace_back(len & 0xff); send_buf.insert(send_buf.end(), payload.begin(), payload.end()); - // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can access valid memory + // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can + // access valid memory auto recv_buf = std::make_shared>(); auto response_complete = std::make_shared(false); - // Low byte is for PTC error code, the rest is nebula-specific auto error_code = std::make_shared(); std::stringstream ss; - ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; + ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) + << " (" << len << ") "; std::string log_tag = ss.str(); PrintDebug(log_tag + "Entering lock"); @@ -88,21 +68,26 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( PrintDebug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, - [this, log_tag, command_id, response_complete, error_code](const std::vector & header_bytes) { + [this, log_tag, command_id, response_complete, + error_code](const std::vector & header_bytes) { error_code->ptc_error_code = header_bytes[3]; - size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); - // If command_id in the response does not match, we got a response for another command (or rubbish), probably as a - // result of too many simultaneous TCP connections to the sensor (e.g. from GUI, Web UI, another nebula instance, etc.) + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | + (header_bytes[6] << 8) | header_bytes[7]; + PrintDebug( + log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); + // If command_id in the response does not match, we got a response for another command (or + // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. + // from GUI, Web UI, another nebula instance, etc.) if (header_bytes[2] != command_id) { error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; } - if (payload_len == 0) { - *response_complete = true; + if (payload_len == 0) { + *response_complete = true; } }, - [this, log_tag, recv_buf, response_complete, error_code](const std::vector & payload_bytes) { + [this, log_tag, recv_buf, response_complete, + error_code](const std::vector & payload_bytes) { PrintDebug(log_tag + "Received payload"); // Header had payload length 0 (thus, header callback processed request successfully already), @@ -118,7 +103,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( }, [this, log_tag, &tm]() { PrintDebug(log_tag + "Unlocking mutex"); - tm.unlock(); + tm.unlock(); PrintDebug(log_tag + "Unlocked mutex"); }); this->IOContextRun(); @@ -144,65 +129,10 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( } Status HesaiHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - HesaiStatus status = Status::OK; - mtu_size_ = MTU_SIZE; - is_solid_state = false; - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - if ( - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR40P) { - azimuth_index_ = 2; - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR40_PACKET_SIZE || packet_size == PANDAR40P_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT64) { - azimuth_index_ = 12; // 12 + 258 * [0-3] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARQT64_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT128) { - azimuth_index_ = 12; // 12 + 514 * [0-1] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARQT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { return (packet_size == PANDARXT32_PACKET_SIZE); }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARXT32M) { - azimuth_index_ = 12; // 12 + 130 * [0-7] - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARXT32M_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { - azimuth_index_ = 12; // 12 + 4 * 128 * [0-1] - is_solid_state = true; - is_valid_packet_ = [](size_t packet_size) { - return (packet_size == PANDARAT128_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { - azimuth_index_ = 8; // 8 + 192 * [0-5] - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR64_PACKET_SIZE || packet_size == PANDAR64_EXTENDED_PACKET_SIZE); - }; - } else if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { - azimuth_index_ = 12; // 12 - is_valid_packet_ = [](size_t packet_size) { - return ( - packet_size == PANDAR128_E4X_EXTENDED_PACKET_SIZE || - packet_size == PANDAR128_E4X_PACKET_SIZE); - }; - } else { - status = Status::INVALID_SENSOR_MODEL; - } - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); return Status::OK; } @@ -239,68 +169,23 @@ Status HesaiHwInterface::SensorInterfaceStart() } Status HesaiHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function &)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void HesaiHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); - if (!is_valid_packet_(buffer.size())) { - PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); - return; - } - const uint32_t buffer_size = buffer.size(); - pandar_msgs::msg::PandarPacket pandar_packet; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, pandar_packet.data.begin()); - pandar_packet.size = buffer_size; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - pandar_packet.stamp.sec = static_cast(now_secs); - pandar_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); - scan_cloud_ptr_->packets.emplace_back(pandar_packet); - - int current_phase = 0; - bool comp_flg = false; - - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_] & 0xff) + ((data[azimuth_index_ + 1] & 0xff) << 8); - if (is_solid_state) { - current_phase = (static_cast(current_phase) + 36000 - 0) % 12000; - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } else { - current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; - - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } - - if (comp_flg) { // Scan complete - if (scan_reception_callback_) { - scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; - // Callback - scan_reception_callback_(std::move(scan_cloud_ptr_)); - scan_cloud_ptr_ = std::make_unique(); - } - } + cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } -Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +Status HesaiHwInterface::GetSensorConfiguration( + const SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; @@ -344,7 +229,9 @@ Status HesaiHwInterface::InitializeTcpDriver() Status HesaiHwInterface::FinalizeTcpDriver() { try { - tcp_driver_->close(); + if (tcp_driver_) { + tcp_driver_->close(); + } } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; @@ -372,7 +259,8 @@ std::vector HesaiHwInterface::GetLidarCalibrationBytes() std::string HesaiHwInterface::GetLidarCalibrationString() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - auto calib_data = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + auto calib_data = + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); std::string calib_string(calib_data.begin(), calib_data.end()); return calib_string; } @@ -381,106 +269,67 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagStatus)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiPtpDiagStatus hesai_ptp_diag_status; - memcpy(&hesai_ptp_diag_status, response.data(), sizeof(HesaiPtpDiagStatus)); + auto diag_status = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; + ss << "HesaiHwInterface::GetPtpDiagStatus: " << diag_status; PrintInfo(ss.str()); - return hesai_ptp_diag_status; + return diag_status; } HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagPort)) { - throw std::runtime_error("Unexpected payload size"); - } - HesaiPtpDiagPort hesai_ptp_diag_port; - memcpy(&hesai_ptp_diag_port, response.data(), sizeof(HesaiPtpDiagPort)); + auto diag_port = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; + ss << "HesaiHwInterface::GetPtpDiagPort: " << diag_port; PrintInfo(ss.str()); - return hesai_ptp_diag_port; + return diag_port; } HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() { auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagTime)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiPtpDiagTime hesai_ptp_diag_time; - memcpy(&hesai_ptp_diag_time, response.data(), sizeof(HesaiPtpDiagTime)); + auto diag_time = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; + ss << "HesaiHwInterface::GetPtpDiagTime: " << diag_time; PrintInfo(ss.str()); - return hesai_ptp_diag_time; + return diag_time; } HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); + auto response_or_err = + SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiPtpDiagGrandmaster)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - memcpy(&hesai_ptp_diag_grandmaster, response.data(), sizeof(HesaiPtpDiagGrandmaster)); + auto diag_grandmaster = CheckSizeAndParse(response); std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; + ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << diag_grandmaster; PrintInfo(ss.str()); - return hesai_ptp_diag_grandmaster; + return diag_grandmaster; } HesaiInventory HesaiHwInterface::GetInventory() { auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() < sizeof(HesaiInventory)) { - throw std::runtime_error("Unexpected payload size"); - } else if (response.size() > sizeof(HesaiInventory)) { - PrintError("HesaiInventory from Sensor has unknown format. Will parse anyway."); - } - - HesaiInventory hesai_inventory; - memcpy(&hesai_inventory, response.data(), sizeof(HesaiInventory)); - - return hesai_inventory; + return CheckSizeAndParse(response); } HesaiConfig HesaiHwInterface::GetConfig() { auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiConfig)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiConfig hesai_config; - memcpy(&hesai_config, response.data(), sizeof(HesaiConfig)); - + auto hesai_config = CheckSizeAndParse(response); std::cout << "Config: " << hesai_config << std::endl; return hesai_config; } @@ -489,15 +338,7 @@ HesaiLidarStatus HesaiHwInterface::GetLidarStatus() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiLidarStatus)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiLidarStatus hesai_status; - memcpy(&hesai_status, response.data(), sizeof(HesaiLidarStatus)); - - return hesai_status; + return CheckSizeAndParse(response); } Status HesaiHwInterface::SetSpinRate(uint16_t rpm) @@ -645,15 +486,15 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() if (response.size() != 5) { throw std::runtime_error("Unexpected response payload"); } - + memcpy(&hesai_range_all.start, &response[1], 2); memcpy(&hesai_range_all.end, &response[3], 2); break; case 1: // for each channel - //TODO + // TODO(yukkysaito) break; case 2: // multi-section FOV - //TODO + // TODO(yukkysaito) break; } @@ -710,7 +551,7 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); if (response.size() < sizeof(HesaiPtpConfig)) { - throw std::runtime_error("Unexpected payload size"); + throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); } else if (response.size() > sizeof(HesaiPtpConfig)) { PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } @@ -749,15 +590,7 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - - if (response.size() != sizeof(HesaiLidarMonitor)) { - throw std::runtime_error("Unexpected payload size"); - } - - HesaiLidarMonitor hesai_lidar_monitor; - memcpy(&hesai_lidar_monitor, response.data(), sizeof(HesaiLidarMonitor)); - - return hesai_lidar_monitor; + return CheckSizeAndParse(response); } void HesaiHwInterface::IOContextRun() @@ -931,9 +764,9 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config) + std::shared_ptr sensor_configuration, HesaiConfig hesai_config) { - using namespace std::chrono_literals; + using namespace std::chrono_literals; // NOLINT(build/namespaces) #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif @@ -965,7 +798,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_rotation_speed = hesai_config.spin_rate; if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { - PrintInfo("current lidar rotation_speed: " + std::to_string(static_cast(current_rotation_speed.value()))); + PrintInfo( + "current lidar rotation_speed: " + + std::to_string(static_cast(current_rotation_speed.value()))); PrintInfo( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); @@ -983,8 +818,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( bool set_flg = false; std::stringstream ss; - ss << static_cast(hesai_config.dest_ipaddr[0]) << "." << static_cast(hesai_config.dest_ipaddr[1]) << "." - << static_cast(hesai_config.dest_ipaddr[2]) << "." << static_cast(hesai_config.dest_ipaddr[3]); + ss << static_cast(hesai_config.dest_ipaddr[0]) << "." + << static_cast(hesai_config.dest_ipaddr[1]) << "." + << static_cast(hesai_config.dest_ipaddr[2]) << "." + << static_cast(hesai_config.dest_ipaddr[3]); auto current_host_addr = ss.str(); if (sensor_configuration->host_ip != current_host_addr) { set_flg = true; @@ -995,7 +832,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_host_dport = hesai_config.dest_LiDAR_udp_port; if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo("current lidar dest_LiDAR_udp_port: " + std::to_string(static_cast(current_host_dport.value()))); + PrintInfo( + "current lidar dest_LiDAR_udp_port: " + + std::to_string(static_cast(current_host_dport.value()))); PrintInfo( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } @@ -1003,7 +842,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_host_tport = hesai_config.dest_gps_udp_port; if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo("current lidar dest_gps_udp_port: " + std::to_string(static_cast(current_host_tport.value()))); + PrintInfo( + "current lidar dest_gps_udp_port: " + + std::to_string(static_cast(current_host_tport.value()))); PrintInfo( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } @@ -1090,7 +931,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, + std::shared_ptr sensor_configuration, HesaiLidarRangeAll hesai_lidar_range_all) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1107,18 +948,26 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( set_flg = true; } else { auto current_cloud_min_angle = hesai_lidar_range_all.start; - if (static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle.value()) { + if ( + static_cast(sensor_configuration->cloud_min_angle * 10) != + current_cloud_min_angle.value()) { set_flg = true; - PrintInfo("current lidar range.start: " + std::to_string(static_cast(current_cloud_min_angle.value()))); + PrintInfo( + "current lidar range.start: " + + std::to_string(static_cast(current_cloud_min_angle.value()))); PrintInfo( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } auto current_cloud_max_angle = hesai_lidar_range_all.end; - if (static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle.value()) { + if ( + static_cast(sensor_configuration->cloud_max_angle * 10) != + current_cloud_max_angle.value()) { set_flg = true; - PrintInfo("current lidar range.end: " + std::to_string(static_cast(current_cloud_max_angle.value()))); + PrintInfo( + "current lidar range.end: " + + std::to_string(static_cast(current_cloud_max_angle.value()))); PrintInfo( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); @@ -1154,7 +1003,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t.join(); @@ -1164,7 +1013,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() ss << result; PrintInfo(ss.str()); CheckAndSetConfig( - std::static_pointer_cast(sensor_configuration_), result); + std::static_pointer_cast(sensor_configuration_), result); }); t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -1358,7 +1207,8 @@ void HesaiHwInterface::PrintDebug(const std::vector & bytes) PrintDebug(ss.str()); } -std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { +std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) +{ if (error_code.ok()) { return "No error"; } @@ -1368,10 +1218,11 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { std::stringstream ss; if (ptc_error) { - ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(ptc_error) << ' '; + ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex + << static_cast(ptc_error) << ' '; } - switch(ptc_error) { + switch (ptc_error) { case PTC_ERROR_CODE_NO_ERROR: break; case PTC_ERROR_CODE_INVALID_INPUT_PARAM: @@ -1429,5 +1280,19 @@ std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { return ss.str(); } +template +T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) +{ + if (data.size() < sizeof(T)) { + throw std::runtime_error("Attempted to parse too-small payload"); + } else if (data.size() > sizeof(T)) { + PrintError("Sensor returned longer payload than expected. Will parse anyway."); + } + + T parsed; + memcpy(&parsed, data.data(), sizeof(T)); + return parsed; +} + } // namespace drivers } // namespace nebula diff --git a/nebula_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 8a8bfefbe..fdc1051ba 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,33 +7,50 @@ 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_)} { } -Status VelodyneHwInterface::InitializeSensorConfiguration( - std::shared_ptr sensor_configuration) +std::string VelodyneHwInterface::HttpGetRequest(const std::string & endpoint) { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); + std::lock_guard lock(mtx_inflight_request_); + if (!http_client_driver_->client()->isOpen()) { + http_client_driver_->client()->open(); + } - GetDiagAsync(); - GetStatusAsync(); - Status status = Status::OK; - return status; + 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) +{ + sensor_configuration_ = sensor_configuration; + return Status::OK; } Status VelodyneHwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr sensor_configuration) { - auto velodyne_sensor_configuration = - std::static_pointer_cast(sensor_configuration); - VelodyneStatus status = CheckAndSetConfigBySnapshotAsync(velodyne_sensor_configuration); - Status rt = status; - return rt; + auto snapshot = GetSnapshot(); + auto tree = ParseJson(snapshot); + VelodyneStatus status = CheckAndSetConfig(sensor_configuration, tree); + + return status; } Status VelodyneHwInterface::SensorInterfaceStart() @@ -55,49 +72,19 @@ Status VelodyneHwInterface::SensorInterfaceStart() } Status VelodyneHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function& packet)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -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; - } +void VelodyneHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +{ + if (!cloud_packet_callback_) { + return; } - prev_packet_first_azm_phased_ = packet_first_azm_phased_; + + cloud_packet_callback_(buffer); } Status VelodyneHwInterface::SensorInterfaceStop() { @@ -112,13 +99,6 @@ 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 { @@ -133,42 +113,6 @@ 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; @@ -188,15 +132,20 @@ 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) { - SetReturnTypeAsync(sensor_configuration->return_mode); + status = SetReturnType(sensor_configuration->return_mode); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_return_mode_str << std::endl; std::cout << "current_return_mode: " << current_return_mode << std::endl; @@ -207,7 +156,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.rpm"; auto current_rotation_speed = tree.get(target_key); if (sensor_configuration->rotation_speed != current_rotation_speed) { - SetRpmAsync(sensor_configuration->rotation_speed); + status = SetRpm(sensor_configuration->rotation_speed); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_rotation_speed << std::endl; std::cout << "sensor_configuration->rotation_speed: " << sensor_configuration->rotation_speed @@ -222,7 +173,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_min_angle = 359; } if (setting_cloud_min_angle != current_cloud_min_angle) { - SetFovStartAsync(setting_cloud_min_angle); + status = SetFovStart(setting_cloud_min_angle); + if (status != OK) return status; + 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; @@ -236,7 +189,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_max_angle = 359; } if (setting_cloud_max_angle != current_cloud_max_angle) { - SetFovEndAsync(setting_cloud_max_angle); + status = SetFovEnd(setting_cloud_max_angle); + if (status != OK) return status; + 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; @@ -245,7 +200,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.addr"; auto current_host_addr = tree.get(target_key); if (sensor_configuration->host_ip != current_host_addr) { - SetHostAddrAsync(sensor_configuration->host_ip); + status = SetHostAddr(sensor_configuration->host_ip); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_addr << std::endl; std::cout << "sensor_configuration->host_ip: " << sensor_configuration->host_ip << std::endl; @@ -254,7 +211,9 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.dport"; auto current_host_dport = tree.get(target_key); if (sensor_configuration->data_port != current_host_dport) { - SetHostDportAsync(sensor_configuration->data_port); + status = SetHostDport(sensor_configuration->data_port); + if (status != OK) return status; + std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_dport << std::endl; std::cout << "sensor_configuration->data_port: " << sensor_configuration->data_port @@ -264,46 +223,35 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.tport"; auto current_host_tport = tree.get(target_key); if (sensor_configuration->gnss_port != current_host_tport) { - SetHostTportAsync(sensor_configuration->gnss_port); + status = SetHostTport(sensor_configuration->gnss_port); + if (status != OK) return status; + 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 Status::WAITING_FOR_SENSOR_RESPONSE; + return OK; } // sync std::string VelodyneHwInterface::GetStatus() { - auto rt = http_client_driver_->get(TARGET_STATUS); - http_client_driver_->client()->close(); - // str_cb(rt); - // return Status::OK; - return rt; + return HttpGetRequest(TARGET_STATUS); } std::string VelodyneHwInterface::GetDiag() { - 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); + auto rt = HttpGetRequest(TARGET_DIAG); std::cout << "read_response: " << rt << std::endl; return rt; } std::string VelodyneHwInterface::GetSnapshot() { - auto rt = http_client_driver_->get(TARGET_SNAPSHOT); - http_client_driver_->client()->close(); - return rt; + return HttpGetRequest(TARGET_SNAPSHOT); } VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) @@ -311,8 +259,7 @@ VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) if (rpm < 300 || 1200 < rpm || rpm % 60 != 0) { return VelodyneStatus::INVALID_RPM_ERROR; } - auto rt = http_client_driver_->post(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); StringCallback(rt); return Status::OK; } @@ -322,8 +269,7 @@ VelodyneStatus VelodyneHwInterface::SetFovStart(uint16_t fov_start) if (359 < fov_start) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = http_client_driver_->post(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); StringCallback(rt); return Status::OK; } @@ -333,8 +279,7 @@ VelodyneStatus VelodyneHwInterface::SetFovEnd(uint16_t fov_end) if (359 < fov_end) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = http_client_driver_->post(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); StringCallback(rt); return Status::OK; } @@ -355,8 +300,7 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re default: return VelodyneStatus::INVALID_RETURN_MODE_ERROR; } - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } @@ -364,8 +308,7 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re VelodyneStatus VelodyneHwInterface::SaveConfig() { std::string body_str = "submit"; - auto rt = http_client_driver_->post(TARGET_SAVE, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SAVE, body_str); StringCallback(rt); return Status::OK; } @@ -373,8 +316,7 @@ VelodyneStatus VelodyneHwInterface::SaveConfig() VelodyneStatus VelodyneHwInterface::ResetSystem() { std::string body_str = "reset_system"; - auto rt = http_client_driver_->post(TARGET_RESET, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_RESET, body_str); StringCallback(rt); return Status::OK; } @@ -382,8 +324,7 @@ VelodyneStatus VelodyneHwInterface::ResetSystem() VelodyneStatus VelodyneHwInterface::LaserOn() { std::string body_str = "laser=on"; - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } @@ -391,8 +332,7 @@ VelodyneStatus VelodyneHwInterface::LaserOn() VelodyneStatus VelodyneHwInterface::LaserOff() { std::string body_str = "laser=off"; - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } @@ -400,417 +340,61 @@ VelodyneStatus VelodyneHwInterface::LaserOff() VelodyneStatus VelodyneHwInterface::LaserOnOff(bool on) { std::string body_str = (boost::format("laser=%s") % (on ? "on" : "off")).str(); - auto rt = http_client_driver_->post(TARGET_SETTING, body_str); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_SETTING, body_str); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostAddr(std::string addr) { - auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("addr=%s") % addr).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_HOST, (boost::format("addr=%s") % addr).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostDport(uint16_t dport) { - auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("dport=%d") % dport).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_HOST, (boost::format("dport=%d") % dport).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetHostTport(uint16_t tport) { - auto rt = http_client_driver_->post(TARGET_HOST, (boost::format("tport=%d") % tport).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_HOST, (boost::format("tport=%d") % tport).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetAddr(std::string addr) { - auto rt = http_client_driver_->post(TARGET_NET, (boost::format("addr=%s") % addr).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_NET, (boost::format("addr=%s") % addr).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetMask(std::string mask) { - auto rt = http_client_driver_->post(TARGET_NET, (boost::format("mask=%s") % mask).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_NET, (boost::format("mask=%s") % mask).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetGateway(std::string gateway) { - auto rt = http_client_driver_->post(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); - http_client_driver_->client()->close(); + auto rt = HttpPostRequest(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); StringCallback(rt); return Status::OK; } VelodyneStatus VelodyneHwInterface::SetNetDhcp(bool use_dhcp) { - auto rt = http_client_driver_->post( + auto rt = HttpPostRequest( 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 2ffa068a5..e2b798d9d 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() +ament_package() \ No newline at end of file diff --git a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg index f6ba285d8..be5898eb1 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 +uint8[] data \ No newline at end of file diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml index cdd135c37..3eb05e603 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 dd81e1ac0..d756a3d51 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -23,6 +23,8 @@ find_package(nebula_decoders REQUIRED) find_package(nebula_hw_interfaces REQUIRED) find_package(yaml-cpp REQUIRED) find_package(robosense_msgs REQUIRED) +find_package(nebula_msgs REQUIRED) +find_package(pandar_msgs REQUIRED) include_directories( include @@ -33,63 +35,31 @@ include_directories( ) link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) ## Hesai -# Hw Interface -ament_auto_add_library(hesai_hw_ros_wrapper SHARED - src/hesai/hesai_hw_interface_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_ros_wrapper - PLUGIN "HesaiHwInterfaceRosWrapper" - EXECUTABLE hesai_hw_interface_ros_wrapper_node - ) - -# Monitor -ament_auto_add_library(hesai_hw_monitor_ros_wrapper SHARED - src/hesai/hesai_hw_monitor_ros_wrapper.cpp - ) - -rclcpp_components_register_node(hesai_hw_monitor_ros_wrapper - PLUGIN "HesaiHwMonitorRosWrapper" - EXECUTABLE hesai_hw_monitor_ros_wrapper_node - ) - -# DriverDecoder -ament_auto_add_library(hesai_driver_ros_wrapper SHARED - src/hesai/hesai_decoder_ros_wrapper.cpp +ament_auto_add_library(hesai_ros_wrapper SHARED + src/hesai/hesai_ros_wrapper.cpp + src/hesai/decoder_wrapper.cpp + src/hesai/hw_interface_wrapper.cpp + src/hesai/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp ) -rclcpp_components_register_node(hesai_driver_ros_wrapper - PLUGIN "HesaiDriverRosWrapper" - EXECUTABLE hesai_driver_ros_wrapper_node +rclcpp_components_register_node(hesai_ros_wrapper + PLUGIN "HesaiRosWrapper" + EXECUTABLE hesai_ros_wrapper_node ) ## Velodyne -# 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 +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 ) -# 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 +rclcpp_components_register_node(velodyne_ros_wrapper + PLUGIN "VelodyneRosWrapper" + EXECUTABLE velodyne_ros_wrapper_node ) ## Robosense diff --git a/nebula_ros/config/lidar/robosense/Bpearl.param.yaml b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml new file mode 100644 index 000000000..1be1f64c5 --- /dev/null +++ b/nebula_ros/config/lidar/robosense/Bpearl.param.yaml @@ -0,0 +1,16 @@ +/**: + 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 new file mode 100644 index 000000000..b745d6e93 --- /dev/null +++ b/nebula_ros/config/lidar/robosense/Helios.param.yaml @@ -0,0 +1,16 @@ +/**: + 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 new file mode 100644 index 000000000..c8b49ed78 --- /dev/null +++ b/nebula_ros/config/lidar/velodyne/VLP16.param.yaml @@ -0,0 +1,21 @@ +/**: + 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 new file mode 100644 index 000000000..d2f328476 --- /dev/null +++ b/nebula_ros/config/lidar/velodyne/VLP32.param.yaml @@ -0,0 +1,21 @@ +/**: + 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 new file mode 100644 index 000000000..19c1cfdb7 --- /dev/null +++ b/nebula_ros/config/lidar/velodyne/VLS128.param.yaml @@ -0,0 +1,21 @@ +/**: + 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 deleted file mode 100644 index 9ccb42e56..000000000 --- a/nebula_ros/config/robosense/Bpearl.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - 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 deleted file mode 100644 index 24350822d..000000000 --- a/nebula_ros/config/robosense/Helios.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - 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 deleted file mode 100644 index 1daf3a4c6..000000000 --- a/nebula_ros/config/velodyne/VLP16.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - 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 deleted file mode 100644 index ef5ae14c1..000000000 --- a/nebula_ros/config/velodyne/VLP32.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - 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 deleted file mode 100644 index db73ecc05..000000000 --- a/nebula_ros/config/velodyne/VLS128.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - 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 new file mode 100644 index 000000000..aca385ce3 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include + +template +class mt_queue +{ +private: + std::mutex mutex_; + std::condition_variable cv_not_empty_, cv_not_full_; + std::deque queue_; + size_t capacity_; + +public: + explicit mt_queue(size_t capacity) : capacity_(capacity) {} + + bool try_push(T && value) + { + std::unique_lock lock(this->mutex_); + bool can_push = queue_.size() < capacity_; + if (can_push) { + queue_.push_front(std::move(value)); + } + this->cv_not_empty_.notify_all(); + return can_push; + } + + void push(T && value) + { + std::unique_lock lock(this->mutex_); + this->cv_not_full_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); + queue_.push_front(std::move(value)); + this->cv_not_empty_.notify_all(); + } + + T pop() + { + std::unique_lock lock(this->mutex_); + this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); + T return_value(std::move(this->queue_.back())); + this->queue_.pop_back(); + this->cv_not_full_.notify_all(); + + return return_value; + } +}; diff --git a/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp new file mode 100644 index 000000000..6d4e38504 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/parameter_descriptors.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp new file mode 100644 index 000000000..e6320f91e --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +class WatchdogTimer +{ + using watchdog_cb_t = std::function; + +public: + WatchdogTimer( + rclcpp::Node & node, const std::chrono::microseconds & expected_update_interval, + const watchdog_cb_t & callback) + : node_(node), + callback_(callback), + expected_update_interval_ns_( + std::chrono::duration_cast(expected_update_interval).count()) + { + timer_ = + node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); + } + + void update() { last_update_ns_ = node_.get_clock()->now().nanoseconds(); } + +private: + void onTimer() + { + uint64_t now_ns = node_.get_clock()->now().nanoseconds(); + + bool is_late = (now_ns - last_update_ns_) > expected_update_interval_ns_; + + callback_(!is_late); + } + + rclcpp::Node & node_; + rclcpp::TimerBase::SharedPtr timer_; + std::atomic last_update_ns_; + const watchdog_cb_t callback_; + + const uint64_t expected_update_interval_ns_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp new file mode 100644 index 000000000..b2bdf856c --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "pandar_msgs/msg/pandar_packet.hpp" +#include "pandar_msgs/msg/pandar_scan.hpp" + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiDecoderWrapper +{ + using get_calibration_result_t = nebula::util::expected< + std::shared_ptr, nebula::Status>; + +public: + HesaiDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr & new_config); + + void OnCalibrationChange( + const std::shared_ptr & + new_calibration); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status Status(); + +private: + /// @brief Load calibration data from the best available source: + /// 1. If sensor connected, download and save from sensor + /// 2. If downloaded file available, load that file + /// 3. Load the file given by `calibration_file_path` + /// @param calibration_file_path The file to use if no better option is available + /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration + /// options + /// @return The calibration data if successful, or an error code if not + get_calibration_result_t GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others = false); + + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + const std::shared_ptr hw_interface_; + std::shared_ptr sensor_cfg_; + + std::string calibration_file_path_{}; + std::shared_ptr calibration_cfg_ptr_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_{}; + rclcpp::Publisher::SharedPtr aw_points_base_pub_{}; + + std::shared_ptr cloud_watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp deleted file mode 100644 index 52518fcd0..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef NEBULA_HesaiDriverRosWrapper_H -#define NEBULA_HesaiDriverRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include -#include - -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of hesai driver -class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - rclcpp::Subscription::SharedPtr pandar_scan_sub_; - rclcpp::Publisher::SharedPtr nebula_points_pub_; - rclcpp::Publisher::SharedPtr aw_points_ex_pub_; - rclcpp::Publisher::SharedPtr aw_points_base_pub_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - drivers::HesaiHwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - - /*** - * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher - * @param pointcloud unique pointer containing the point cloud to publish - * @param publisher - */ - void PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher); - -public: - explicit HesaiDriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for PandarScan subscriber - /// @param scan_msg Received PandarScan - void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - -private: - /// @brief File path of Correction data (Only required only for AT) - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 1802f87b2..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef NEBULA_HesaiHwInterfaceRosWrapper_H -#define NEBULA_HesaiHwInterfaceRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of hesai driver -class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Received Hesai message publisher - rclcpp::Publisher::SharedPtr pandar_scan_pub_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving PandarScan - /// @param scan_buffer Received PandarScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); - -public: - explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwInterfaceRosWrapper() noexcept override; - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - uint16_t delay_hw_ms_; - bool retry_hw_; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp deleted file mode 100644 index cf8739279..000000000 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ /dev/null @@ -1,166 +0,0 @@ -#ifndef NEBULA_HesaiHwMonitorRosWrapper_H -#define NEBULA_HesaiHwMonitorRosWrapper_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware monitor ros wrapper of hesai driver -class HesaiHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase -{ - drivers::HesaiHwInterface hw_interface_; - Status interface_status_; - - drivers::HesaiSensorConfiguration sensor_configuration_; - - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; - -public: - explicit HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiHwMonitorRosWrapper() noexcept override; - /// @brief Not used - /// @return Current status - Status MonitorStart() override; - /// @brief Not used - /// @return Status::OK - Status MonitorStop() override; - /// @brief Not used - /// @return Status::OK - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration); - -private: - diagnostic_updater::Updater diagnostics_updater_; - /// @brief Initializing diagnostics - void InitializeHesaiDiagnostics(); - /// @brief Callback of the timer for getting the current lidar status - void OnHesaiStatusTimer(); - /// @brief Callback of the timer for getting the current lidar monitor via http - void OnHesaiLidarMonitorTimerHttp(); - /// @brief Callback of the timer for getting the current lidar monitor via tcp - void OnHesaiLidarMonitorTimer(); - // void OnHesaiDiagnosticsTimer(); - // void OnHesaiStatusTimer(); - - /// @brief Check status information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check ptp information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check temperature information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check rpm information from HesaiLidarStatus for diagnostic_updater - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via http - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Check voltage information from HesaiLidarStatus for diagnostic_updater via tcp - /// @param diagnostics DiagnosticStatusWrapper - void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - - rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; - rclcpp::TimerBase::SharedPtr diagnostics_update_monitor_timer_; - rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_; - std::unique_ptr current_status; - std::unique_ptr current_monitor; - std::unique_ptr current_config; - std::unique_ptr current_inventory; - std::unique_ptr current_lidar_monitor_tree; - std::unique_ptr current_status_time; - std::unique_ptr current_config_time; - std::unique_ptr current_inventory_time; - std::unique_ptr current_lidar_monitor_time; - uint8_t current_diag_status; - uint8_t current_monitor_status; - - uint16_t diag_span_; - uint16_t delay_monitor_ms_; - std::mutex mtx_diag; - std::mutex mtx_status; - std::mutex mtx_lidar_monitor; - // std::timed_mutex mtx_lidar_monitor; - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - /// @brief Get value from property_tree - /// @param pt property_tree - /// @param key Pey string - /// @return Value - std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); - /// @brief Making fixed precision string - /// @param val Target value - /// @param pre Precision - /// @return Created string - std::string GetFixedPrecisionString(double val, int pre = 2); - - std::string info_model; - std::string info_serial; - rclcpp::CallbackGroup::SharedPtr cbg_r_; - rclcpp::CallbackGroup::SharedPtr cbg_m_; - rclcpp::CallbackGroup::SharedPtr cbg_m2_; - - const char * not_supported_message; - const char * error_message; - std::string message_sep; - - std::vector temperature_names; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiHwMonitorRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp new file mode 100644 index 000000000..0a60d3e8e --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include "boost_tcp_driver/tcp_driver.hpp" +#include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/hesai/decoder_wrapper.hpp" +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" + +#include +#include +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of hesai driver +class HesaiRosWrapper final : public rclcpp::Node +{ +public: + explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); + ~HesaiRosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + void ReceiveCloudPacketCallback(std::vector & packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & new_config); + + Status wrapper_status_; + + std::shared_ptr sensor_cfg_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_; + + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp new file mode 100644 index 000000000..0791af5e2 --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwInterfaceWrapper +{ +public: + HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & new_config); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + bool setup_sensor_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..0b270623f --- /dev/null +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class HesaiHwMonitorWrapper +{ +public: + HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void OnConfigChange( + const std::shared_ptr & /* new_config */) + { + } + + nebula::Status Status(); + +private: + void InitializeHesaiDiagnostics(); + + std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); + + std::string GetFixedPrecisionString(double val, int pre); + + void OnHesaiStatusTimer(); + + void OnHesaiLidarMonitorTimerHttp(); + + void OnHesaiLidarMonitorTimer(); + + void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + rclcpp::Logger logger_; + diagnostic_updater::Updater diagnostics_updater_; + nebula::Status status_; + + const std::shared_ptr hw_interface_; + rclcpp::Node * const parent_node_; + + uint16_t diag_span_; + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; + rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; + + std::unique_ptr current_status_{}; + std::unique_ptr current_monitor_{}; + std::unique_ptr current_config_{}; + std::unique_ptr current_inventory_{}; + std::unique_ptr current_lidar_monitor_tree_{}; + + std::unique_ptr current_status_time_{}; + std::unique_ptr current_config_time_{}; + std::unique_ptr current_inventory_time_{}; + std::unique_ptr current_lidar_monitor_time_{}; + + uint8_t current_diag_status_; + uint8_t current_monitor_status_; + + std::mutex mtx_lidar_status_; + std::mutex mtx_lidar_monitor_; + + std::string info_model_; + std::string info_serial_; + + std::vector temperature_names_; + + bool setup_sensor; + const std::string MSG_NOT_SUPPORTED = "Not supported"; + const std::string MSG_ERROR = "Error"; + const std::string MSG_SEP = ": "; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp new file mode 100644 index 000000000..283b68368 --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp @@ -0,0 +1,91 @@ +#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 new file mode 100644 index 000000000..5c857fa12 --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp @@ -0,0 +1,35 @@ +#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_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp similarity index 68% rename from nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp index 3f589bbbf..e695f3114 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp @@ -1,92 +1,64 @@ -#ifndef NEBULA_VelodyneHwMonitorRosWrapper_H -#define NEBULA_VelodyneHwMonitorRosWrapper_H +#pragma once -#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 "nebula_ros/common/parameter_descriptors.hpp" -#include #include +#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) +class VelodyneHwMonitorWrapper { - 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; +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 */) + { } - return false; -} -/// @brief Hardware monitor ros wrapper of velodyne driver -class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWrapperBase -{ - drivers::VelodyneHwInterface hw_interface_; - Status interface_status_; + nebula::Status Status(); - drivers::VelodyneSensorConfiguration sensor_configuration_; - drivers::VelodyneCalibrationConfiguration calibration_configuration_; +private: + void InitializeVelodyneDiagnostics(); - /// @brief Initializing hardware monitor ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwMonitor( - const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback of the timer for getting the current lidar status + void OnVelodyneStatusTimer(); + + /// @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(); -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, const std::string & key); + std::shared_ptr pt, std::mutex & mtx_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 @@ -242,8 +214,6 @@ class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWr 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); @@ -286,150 +256,111 @@ class VelodyneHwMonitorRosWrapper final : public rclcpp::Node, NebulaHwMonitorWr /// @param diagnostics DiagnosticStatusWrapper void VelodyneCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief Callback of the timer for getting the current lidar snapshot - void OnVelodyneSnapshotTimer(); + 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_; + + 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; - 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_; -}; + std::mutex mtx_snapshot_; + std::mutex mtx_status_; + std::mutex mtx_diag_; -} // namespace ros -} // namespace nebula + 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"; -#endif // NEBULA_VelodyneHwMonitorRosWrapper_H + 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"; +}; +} // 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 deleted file mode 100644 index 5bc79b6c8..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,83 +0,0 @@ -#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 deleted file mode 100644 index 5a0e094c3..000000000 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,202 +0,0 @@ -#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/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp new file mode 100644 index 000000000..a7110bd4a --- /dev/null +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -0,0 +1,90 @@ +#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 f4a1cd41c..10c7aa218 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,34 +27,27 @@ - - - - - - - - - - - - - - + + + + - + + + + @@ -64,33 +57,13 @@ - - - - - - - - - - - - - - - - - - - - - + + diff --git a/nebula_ros/launch/hesai_launch_component.launch.xml b/nebula_ros/launch/hesai_launch_component.launch.xml deleted file mode 100644 index 75c7414da..000000000 --- a/nebula_ros/launch/hesai_launch_component.launch.xml +++ /dev/null @@ -1,115 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index b394498fb..6eee53443 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -26,6 +26,7 @@ def get_sensor_make(sensor_name): return "Continental", None return "unrecognized_sensor_model", None + def get_plugin_name(sensor_make, sensor_model): if sensor_make.lower() != "continental": return sensor_make @@ -34,15 +35,22 @@ def get_plugin_name(sensor_make, sensor_model): else: return "invalid_plugin" + def is_hw_monitor_available(sensor_make): return sensor_make.lower() != "continental" + def launch_setup(context, *args, **kwargs): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) calibration_file = LaunchConfiguration("calibration_file").perform(context) - correction_file = LaunchConfiguration("correction_file").perform(context) sensor_make, sensor_extension = get_sensor_make(sensor_model) + + if sensor_make.lower() == "hesai": + raise ValueError( + "\n `nebula_launch.py` is deprecated. For Hesai sensors, use `hesai_launch_all_hw.xml` instead." + ) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") nebula_ros_share_dir = get_package_share_directory("nebula_ros") @@ -50,23 +58,36 @@ def launch_setup(context, *args, **kwargs): sensor_params_fp = LaunchConfiguration("config_file").perform(context) if sensor_params_fp == "": warnings.warn("No config file provided, using sensor model default", RuntimeWarning) - sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml") + sensor_params_fp = os.path.join( + nebula_ros_share_dir, "config", sensor_make.lower(), sensor_model + ".yaml" + ) if not os.path.exists(sensor_params_fp): sensor_params_fp = os.path.join(nebula_ros_share_dir, "config", "BaseParams.yaml") - assert os.path.exists(sensor_params_fp), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) + assert os.path.exists( + sensor_params_fp + ), "Sensor params yaml file under config/ was not found: {}".format(sensor_params_fp) sensor_calib_fp = sensor_corr_fp = "" if sensor_extension is not None: # Velodyne and Hesai - sensor_calib_fp = os.path.join(nebula_decoders_share_dir, "calibration", sensor_make.lower(), sensor_model + sensor_extension) - assert os.path.exists(sensor_calib_fp), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) if sensor_model.lower() == "pandarat128": sensor_corr_fp = os.path.splitext(sensor_calib_fp)[0] + ".dat" - assert os.path.exists(sensor_corr_fp), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) + assert os.path.exists( + sensor_corr_fp + ), "Sensor corr file under calibration/ was not found: {}".format(sensor_corr_fp) with open(sensor_params_fp, "r") as f: - sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] + sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] nodes = [] launch_hw = LaunchConfiguration("launch_hw").perform(context) == "true" @@ -75,8 +96,8 @@ def launch_setup(context, *args, **kwargs): # HwInterface ComposableNode( package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"HwInterfaceRosWrapper", - name=sensor_make.lower()+"_hw_interface_ros_wrapper_node", + plugin=get_plugin_name(sensor_make, sensor_model) + "HwInterfaceRosWrapper", + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", parameters=[ sensor_params, { @@ -84,7 +105,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, "setup_sensor": LaunchConfiguration("setup_sensor"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), @@ -95,14 +115,13 @@ def launch_setup(context, *args, **kwargs): ), ) - if launch_hw and is_hw_monitor_available(sensor_make): nodes.append( # HwMonitor ComposableNode( package="nebula_ros", - plugin=sensor_make+"HwMonitorRosWrapper", - name=sensor_make.lower()+"_hw_monitor_ros_wrapper_node", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", parameters=[ sensor_params, { @@ -110,7 +129,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, }, ], ) @@ -118,8 +136,8 @@ def launch_setup(context, *args, **kwargs): nodes.append( ComposableNode( package="nebula_ros", - plugin=get_plugin_name(sensor_make, sensor_model)+"DriverRosWrapper", - name=sensor_make.lower()+"_driver_ros_wrapper_node", + plugin=get_plugin_name(sensor_make, sensor_model) + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", parameters=[ sensor_params, { @@ -127,7 +145,6 @@ def launch_setup(context, *args, **kwargs): "sensor_ip": LaunchConfiguration("sensor_ip"), "return_mode": LaunchConfiguration("return_mode"), "calibration_file": calibration_file or sensor_calib_fp, - "correction_file": correction_file or sensor_corr_fp, "launch_hw": LaunchConfiguration("launch_hw"), "ptp_profile": LaunchConfiguration("ptp_profile"), "ptp_domain": LaunchConfiguration("ptp_domain"), @@ -146,7 +163,7 @@ def launch_setup(context, *args, **kwargs): container_kwargs = {} if LaunchConfiguration("debug_logging").perform(context) == "true": - container_kwargs["ros_arguments"] = ['--log-level', 'debug'] + container_kwargs["ros_arguments"] = ["--log-level", "debug"] container = ComposableNodeContainer( name="nebula_ros_node", diff --git a/nebula_ros/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml index 6e2ab6dda..52a013523 100644 --- a/nebula_ros/launch/robosense_launch.all_hw.xml +++ b/nebula_ros/launch/robosense_launch.all_hw.xml @@ -1,53 +1,10 @@ - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + \ 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 c39437039..d2f030b29 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -1,67 +1,10 @@ - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 87bc000c9..a0bdec51f 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -21,6 +21,8 @@ nebula_common nebula_decoders nebula_hw_interfaces + nebula_msgs + pandar_msgs pcl_conversions rclcpp rclcpp_components diff --git a/nebula_ros/schema/Bpearl.schema.json b/nebula_ros/schema/Bpearl.schema.json new file mode 100644 index 000000000..0622e2089 --- /dev/null +++ b/nebula_ros/schema/Bpearl.schema.json @@ -0,0 +1,37 @@ +{ + "$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 new file mode 100644 index 000000000..4485f4aad --- /dev/null +++ b/nebula_ros/schema/Helios.schema.json @@ -0,0 +1,37 @@ +{ + "$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 new file mode 100644 index 000000000..4aadb40a4 --- /dev/null +++ b/nebula_ros/schema/VLP16.schema.json @@ -0,0 +1,37 @@ +{ + "$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 new file mode 100644 index 000000000..9adb5d98b --- /dev/null +++ b/nebula_ros/schema/VLP32.schema.json @@ -0,0 +1,37 @@ +{ + "$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 new file mode 100644 index 000000000..8b5d6375b --- /dev/null +++ b/nebula_ros/schema/VLS128.schema.json @@ -0,0 +1,37 @@ +{ + "$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 new file mode 100644 index 000000000..b12e242fd --- /dev/null +++ b/nebula_ros/schema/sub/lidar.json @@ -0,0 +1,35 @@ +{ + "$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 new file mode 100644 index 000000000..6f977cf41 --- /dev/null +++ b/nebula_ros/schema/sub/nebula.json @@ -0,0 +1,44 @@ +{ + "$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 new file mode 100644 index 000000000..0e23e8433 --- /dev/null +++ b/nebula_ros/schema/sub/robosense.json @@ -0,0 +1,87 @@ +{ + "$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 new file mode 100644 index 000000000..1c2e186b8 --- /dev/null +++ b/nebula_ros/schema/sub/velodyne.json @@ -0,0 +1,123 @@ +{ + "$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 new file mode 100644 index 000000000..b62cb8a9e --- /dev/null +++ b/nebula_ros/src/common/parameter_descriptors.cpp @@ -0,0 +1,34 @@ +#include "nebula_ros/common/parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp new file mode 100644 index 000000000..f51378e47 --- /dev/null +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -0,0 +1,316 @@ +#include "nebula_ros/hesai/decoder_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +using namespace std::chrono_literals; // NOLINT(build/namespaces) + +HesaiDecoderWrapper::HesaiDecoderWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("HesaiDecoder")), + hw_interface_(hw_interface), + sensor_cfg_(config) +{ + if (!config) { + throw std::runtime_error("HesaiDecoderWrapper cannot be instantiated without a valid config!"); + } + + if (config->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calibration_file_path_ = + parent_node->declare_parameter("correction_file", "", param_read_write()); + } else { + calibration_file_path_ = + parent_node->declare_parameter("calibration_file", "", param_read_write()); + } + + auto calibration_result = GetCalibrationData(calibration_file_path_, false); + + if (!calibration_result.has_value()) { + throw std::runtime_error( + (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); + } + + calibration_cfg_ptr_ = calibration_result.value(); + RCLCPP_INFO_STREAM( + logger_, "Using calibration data from " << calibration_cfg_ptr_->calibration_file); + + RCLCPP_INFO(logger_, "Starting Decoder"); + + driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_) { + current_scan_msg_ = std::make_unique(); + packets_pub_ = parent_node->create_publisher( + "pandar_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = + parent_node->create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node->create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node->create_publisher("aw_points_ex", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); +} + +void HesaiDecoderWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +void HesaiDecoderWrapper::OnCalibrationChange( + const std::shared_ptr & new_calibration) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(sensor_cfg_, new_calibration); + driver_ptr_ = new_driver; + calibration_cfg_ptr_ = new_calibration; + calibration_file_path_ = calibration_cfg_ptr_->calibration_file; +} + +rcl_interfaces::msg::SetParametersResult HesaiDecoderWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + std::string calibration_path = ""; + + // Only one of the two parameters is defined, so not checking for sensor model explicitly here is + // fine + bool got_any = get_param(p, "calibration_file", calibration_path) | + get_param(p, "correction_file", calibration_path); + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (!std::filesystem::exists(calibration_path)) { + auto result = SetParametersResult(); + result.successful = false; + result.reason = + "The given calibration path does not exist, ignoring: '" + calibration_path + "'"; + return result; + } + + auto get_calibration_result = GetCalibrationData(calibration_path, true); + if (!get_calibration_result.has_value()) { + auto result = SetParametersResult(); + result.successful = false; + result.reason = + (std::stringstream() << "Could not change calibration file to '" << calibration_path + << "': " << get_calibration_result.error()) + .str(); + return result; + } + + OnCalibrationChange(get_calibration_result.value()); + RCLCPP_INFO_STREAM( + logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); + return rcl_interfaces::build().successful(true).reason(""); +} + +HesaiDecoderWrapper::get_calibration_result_t HesaiDecoderWrapper::GetCalibrationData( + const std::string & calibration_file_path, bool ignore_others) +{ + std::shared_ptr calib; + + if (sensor_cfg_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calib = std::make_shared(); + } else { + calib = std::make_shared(); + } + + bool hw_connected = hw_interface_ != nullptr; + std::string calibration_file_path_from_sensor; + + { + int ext_pos = calibration_file_path.find_last_of('.'); + calibration_file_path_from_sensor = calibration_file_path.substr(0, ext_pos); + // TODO(mojomex): if multiple different sensors of the same type are used, this will mix up + // their calibration data + calibration_file_path_from_sensor += "_from_sensor"; + calibration_file_path_from_sensor += + calibration_file_path.substr(ext_pos, calibration_file_path.size() - ext_pos); + } + + // If a sensor is connected, try to download and save its calibration data + if (!ignore_others && hw_connected) { + try { + auto raw_data = hw_interface_->GetLidarCalibrationBytes(); + RCLCPP_INFO(logger_, "Downloaded calibration data from sensor."); + auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM(logger_, "Could not save calibration data: " << status); + } else { + RCLCPP_INFO_STREAM( + logger_, "Saved downloaded data to " << calibration_file_path_from_sensor); + } + } catch (std::runtime_error & e) { + RCLCPP_ERROR_STREAM(logger_, "Could not download calibration data: " << e.what()); + } + } + + // If saved calibration data from a sensor exists (either just downloaded above, or previously), + // try to load it + if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { + auto status = calib->LoadFromFile(calibration_file_path_from_sensor); + if (status == Status::OK) { + calib->calibration_file = calibration_file_path_from_sensor; + return calib; + } + + RCLCPP_ERROR_STREAM(logger_, "Could not load downloaded calibration data: " << status); + } else if (!ignore_others) { + RCLCPP_ERROR(logger_, "No downloaded calibration data found."); + } + + if (!ignore_others) { + RCLCPP_WARN(logger_, "Falling back to generic calibration file."); + } + + // If downloaded data did not exist or could not be loaded, fall back to a generic file. + // If that file does not exist either, return an error code + if (!std::filesystem::exists(calibration_file_path)) { + RCLCPP_ERROR(logger_, "No calibration data found."); + return nebula::Status(Status::INVALID_CALIBRATION_FILE); + } + + // Try to load the existing fallback calibration file. Return an error if this fails + auto status = calib->LoadFromFile(calibration_file_path); + if (status != Status::OK) { + RCLCPP_ERROR_STREAM( + logger_, "Could not load calibration file at '" << calibration_file_path << "'"); + return status; + } + + // Return the fallback calibration file + calib->calibration_file = calibration_file_path; + return calib; +} + +void HesaiDecoderWrapper::ProcessCloudPacket( + std::unique_ptr packet_msg) +{ + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if ( + hw_interface_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + if (current_scan_msg_->packets.size() == 0) { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + pandar_msgs::msg::PandarPacket pandar_packet_msg{}; + pandar_packet_msg.stamp = packet_msg->stamp; + pandar_packet_msg.size = packet_msg->data.size(); + std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); + current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); + } + + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud = std::get<0>(pointcloud_ts); + } + + // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th + // emits one) + if (pointcloud == nullptr) { + // Since this ends the function early, the `cloud_watchdog_` will not be updated. + // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no + // packets come in), the watchdog will log a warning automatically + return; + } + + cloud_watchdog_->update(); + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + if ( + nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_subscription_count() > 0) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } + if ( + aw_points_base_pub_->get_subscription_count() > 0 || + aw_points_base_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_cloud_xyzi = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + } + if ( + aw_points_ex_pub_->get_subscription_count() > 0 || + aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.stamp = + rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); + PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + } +} + +void HesaiDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +nebula::Status HesaiDecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp deleted file mode 100644 index 6f27d547e..000000000 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,488 +0,0 @@ -#include "nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_driver_ros_wrapper", options), hw_interface_() -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = - GetParameters(sensor_configuration, calibration_configuration, correction_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); - pandar_scan_sub_ = create_subscription( - "pandar_packets", qos, - std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - nebula_points_pub_ = - this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); - aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); -} - -void HesaiDriverRosWrapper::ReceiveScanMsgCallback( - const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) -{ - auto t_start = std::chrono::high_resolution_clock::now(); - std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); - nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - - if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); - return; - }; - if ( - nebula_points_pub_->get_subscription_count() > 0 || - nebula_points_pub_->get_intra_process_subscription_count() > 0) { - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); - } - if ( - aw_points_base_pub_->get_subscription_count() > 0 || - aw_points_base_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); - } - if ( - aw_points_ex_pub_->get_subscription_count() > 0 || - aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); - } - - auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); -} - -void HesaiDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::InitializeDriver( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration), - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } - -Status HesaiDriverRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("calibration_file", "", descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("min_range", 0.3, descriptor); - sensor_configuration.min_range = this->get_parameter("min_range").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("max_range", 300., descriptor); - sensor_configuration.max_range = this->get_parameter("max_range").as_double(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("correction_file", "", descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - bool launch_hw; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("launch_hw", "", descriptor); - launch_hw = this->get_parameter("launch_hw").as_bool(); - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - bool run_local = !launch_hw; - if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - std::string calibration_file_path_from_sensor; - if (launch_hw && !calibration_configuration.calibration_file.empty()) { - int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); - calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); - } - if(launch_hw) { - run_local = false; - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver() == Status::OK) { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - } else { - run_local = true; - } - }); - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(5000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - RCLCPP_ERROR_STREAM(get_logger(), "GetCalibration Timeout"); - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_local) { - RCLCPP_WARN_STREAM(get_logger(), "Running locally"); - bool run_org = false; - if (calibration_file_path_from_sensor.empty()) { - run_org = true; - } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); - } - } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); - if (calibration_configuration.calibration_file.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); - } - } - } - } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 - std::string correction_file_path_from_sensor; - if (launch_hw && !correction_file_path.empty()) { - int ext_pos = correction_file_path.find_last_of('.'); - correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); - correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); - } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local, &launch_hw]() { - if (launch_hw && hw_interface_.InitializeTcpDriver() == Status::OK) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); - if (!run_local) { - std::future_status status; - status = future.wait_for(std::chrono::milliseconds(8000)); - if (status == std::future_status::timeout) { - std::cerr << "# std::future_status::timeout\n"; - run_local = true; - } else if (status == std::future_status::ready && !run_local) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_local) { - bool run_org = false; - if (correction_file_path_from_sensor.empty()) { - run_org = true; - } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); - - if (cal_status != Status::OK) { - run_org = true; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); - } - } - if(run_org) { - if (correction_file_path.empty()) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - }else{ - RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); - } - } - } - } - } // end AT128 - // Do not use outside of this location - hw_interface_.~HesaiHwInterface(); - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 1ac59b55b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); -#if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) - { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); - } - } - - if(rt != Status::ERROR_1){ - try{ - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - } - catch (...) - { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); - } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); - } -#endif - - hw_interface_.RegisterScanCallback( - std::bind(&HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); - pandar_scan_pub_ = - this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - -#if not defined(TEST_PCAP) - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - } -#endif - -#ifdef WITH_DEBUG_STDOUT_HesaiHwInterfaceRosWrapper - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(0); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarCalib(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagStatus(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagPort(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagTime(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpDiagGrandmaster(ios); - }); - // thread_pool.emplace_back([&hw_interface_]{ - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetInventory(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarStatus(ios); - }); - thread_pool.emplace_back([this] { hw_interface_.SetStandbyMode(1); }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetPtpConfig(ios); - }); - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - hw_interface_.GetLidarRange(ios); - }); - for (std::thread & th : thread_pool) { - // hw_interface_.IOServiceRun(); - th.join(); - } - } - if (false) { - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto ios = std::make_shared(); - std::cout << "GetLidarCalib" << std::endl; - hw_interface_.GetLidarCalib(ios); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - } -#endif - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); -} - -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); -} - -Status HesaiHwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - return interface_status_; -} - -Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwInterfaceRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("setup_sensor", true, descriptor); - this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_hw_ms", 0, descriptor); - this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("retry_hw", true, descriptor); - this->retry_hw_ = this->get_parameter("retry_hw").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("ptp_profile", ""); - sensor_configuration.ptp_profile = - nebula::drivers::PtpProfileFromString(this->get_parameter("ptp_profile").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { - sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; - } - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("ptp_switch_type", ""); - sensor_configuration.ptp_switch_type = - nebula::drivers::PtpSwitchTypeFromString(this->get_parameter("ptp_switch_type").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(127).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("ptp_domain", 0, descriptor); - sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); - } - - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when using the '1588v2' PTP Profile"); - return Status::SENSOR_CONFIG_ERROR; - } - if(sensor_configuration.ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { - RCLCPP_ERROR_STREAM(get_logger(), - "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); - return Status::SENSOR_CONFIG_ERROR; - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwInterfaceRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_configuration_.frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - std::string return_mode_str; - if ( - get_param(p, "sensor_model", sensor_model_str) || - get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || - get_param(p, "sensor_ip", new_param.sensor_ip) || - get_param(p, "frame_id", new_param.frame_id) || - get_param(p, "data_port", new_param.data_port) || - get_param(p, "gnss_port", new_param.gnss_port) || - get_param(p, "scan_phase", new_param.scan_phase) || - get_param(p, "packet_mtu_size", new_param.packet_mtu_size) || - get_param(p, "rotation_speed", new_param.rotation_speed) || - get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || - get_param(p, "cloud_max_angle", new_param.cloud_max_angle) || - get_param(p, "dual_return_distance_threshold", new_param.dual_return_distance_threshold)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - if (0 < return_mode_str.length()) - new_param.return_mode = nebula::drivers::ReturnModeFromString(return_mode_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -std::vector HesaiHwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - std::ostringstream os_return_mode; - os_return_mode << sensor_configuration_.return_mode; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("return_mode", os_return_mode.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("gnss_port", sensor_configuration_.gnss_port), - rclcpp::Parameter("scan_phase", sensor_configuration_.scan_phase), - rclcpp::Parameter("packet_mtu_size", sensor_configuration_.packet_mtu_size), - rclcpp::Parameter("rotation_speed", sensor_configuration_.rotation_speed), - rclcpp::Parameter("cloud_min_angle", sensor_configuration_.cloud_min_angle), - rclcpp::Parameter("cloud_max_angle", sensor_configuration_.cloud_max_angle), - rclcpp::Parameter( - "dual_return_distance_threshold", sensor_configuration_.dual_return_distance_threshold)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp deleted file mode 100644 index e64bf443b..000000000 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ /dev/null @@ -1,620 +0,0 @@ -#include "nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp" - -namespace nebula -{ -namespace ros -{ -HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_monitor_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) -{ - cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; - - switch (sensor_cfg_ptr->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - while(hw_interface_.InitializeTcpDriver() == Status::ERROR_1) - { - RCLCPP_WARN(this->get_logger(), "Could not initialize TCP driver, retrying in 8s..."); - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - } - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); - RCLCPP_INFO_STREAM(this->get_logger(), result); - info_model = result.get_str_model(); - info_serial = std::string(std::begin(result.sn), std::end(result.sn)); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); -} - -Status HesaiHwMonitorRosWrapper::MonitorStart() { return interface_status_; } - -Status HesaiHwMonitorRosWrapper::MonitorStop() { return Status::OK; } -Status HesaiHwMonitorRosWrapper::Shutdown() { return Status::OK; } - -Status HesaiHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status HesaiHwMonitorRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "pandar", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("gnss_port", 2369, descriptor); - sensor_configuration.gnss_port = this->get_parameter("gnss_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("packet_mtu_size", 1500, descriptor); - sensor_configuration.packet_mtu_size = this->get_parameter("packet_mtu_size").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - rcl_interfaces::msg::IntegerRange range; - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { - descriptor.additional_constraints = "200, 300, 400, 500"; - range.set__from_value(200).set__to_value(500).set__step(100); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 200, descriptor); - } else { - descriptor.additional_constraints = "300, 600, 1200"; - range.set__from_value(300).set__to_value(1200).set__step(300); - descriptor.integer_range = {range}; - this->declare_parameter("rotation_speed", 600, descriptor); - } - sensor_configuration.rotation_speed = this->get_parameter("rotation_speed").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_min_angle", 0, descriptor); - sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(360).set__step(1); - descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 360, descriptor); - sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.01).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || - return Status::SENSOR_CONFIG_ERROR; - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("diag_span", 1000, descriptor); - this->diag_span_ = this->get_parameter("diag_span").as_int(); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_monitor_ms", 0, descriptor); - this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void HesaiHwMonitorRosWrapper::InitializeHesaiDiagnostics() -{ - RCLCPP_INFO_STREAM(this->get_logger(), "InitializeHesaiDiagnostics"); - using std::chrono_literals::operator""s; - std::ostringstream os; - auto hardware_id = info_model + ": " + info_serial; - diagnostics_updater_.setHardwareID(hardware_id); - RCLCPP_INFO_STREAM(this->get_logger(), "hardware_id: " + hardware_id); - - diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorRosWrapper::HesaiCheckStatus); - diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add( - "hesai_temperature", this, &HesaiHwMonitorRosWrapper::HesaiCheckTemperature); - diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorRosWrapper::HesaiCheckRpm); - - current_status.reset(); - current_monitor.reset(); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - - auto fetch_diag_from_sensor = [this](){ - OnHesaiStatusTimer(); - if (hw_interface_.UseHttpGetLidarMonitor()) { - OnHesaiLidarMonitorTimerHttp(); - } else { - OnHesaiLidarMonitorTimer(); - } - }; - - fetch_diagnostics_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(fetch_diagnostics_timer_, cbg_m_); - - if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorRosWrapper::HesaiCheckVoltage); - } - - auto on_timer_update = [this] { - RCLCPP_DEBUG_STREAM(get_logger(), "OnUpdateTimer"); - auto now = this->get_clock()->now(); - auto dif = (now - *current_status_time).seconds(); - - RCLCPP_DEBUG_STREAM(get_logger(), "dif(status): " << dif); - - if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - dif = (now - *current_lidar_monitor_time).seconds(); - RCLCPP_DEBUG_STREAM(get_logger(), "dif(monitor): " << dif); - if (diag_span_ * 2.0 < dif * 1000) { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); - } else { - current_monitor_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); - } - diagnostics_updater_.force_update(); - }; - diagnostics_update_timer_ = std::make_shared>( - this->get_clock(), std::chrono::milliseconds(1000), std::move(on_timer_update), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(diagnostics_update_timer_, cbg_r_); - - RCLCPP_DEBUG_STREAM(get_logger(), "add_timer"); -} - -std::string HesaiHwMonitorRosWrapper::GetPtreeValue( - boost::property_tree::ptree * pt, const std::string & key) -{ - boost::optional value = pt->get_optional(key); - if (value) { - return value.get(); - } else { - return not_supported_message; - } -} -std::string HesaiHwMonitorRosWrapper::GetFixedPrecisionString(double val, int pre) -{ - std::stringstream ss; - ss << std::fixed << std::setprecision(pre) << val; - return ss.str(); -} - -rcl_interfaces::msg::SetParametersResult HesaiHwMonitorRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(get_logger(), p); - RCLCPP_DEBUG_STREAM(get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::HesaiSensorConfiguration new_param{sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - uint16_t new_diag_span = 0; - if (get_param(p, "diag_span", new_diag_span)) { - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - return *result; -} - -void HesaiHwMonitorRosWrapper::OnHesaiStatusTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer" << std::endl); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarStatus(); - std::scoped_lock lock(mtx_status); - current_status_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_status.reset(new HesaiLidarStatus(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), - error.what()); - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiStatusTimer END" << std::endl); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp"); - try { - hw_interface_.GetLidarMonitorAsyncHttp([this](const std::string & str) { - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_lidar_monitor_tree = - std::make_unique(hw_interface_.ParseJson(str)); - }); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimerHttp END"); -} - -void HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer() -{ - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer"); - try { - auto ios = std::make_shared(); - auto result = hw_interface_.GetLidarMonitor(); - std::scoped_lock lock(mtx_lidar_monitor); - current_lidar_monitor_time.reset(new rclcpp::Time(this->get_clock()->now())); - current_monitor.reset(new HesaiLidarMonitor(result)); - } catch (const std::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), - error.what()); - } catch (const boost::system::system_error & error) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiHwMonitorRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), - error.what()); - } - - RCLCPP_DEBUG_STREAM(this->get_logger(), "OnHesaiLidarMonitorTimer END"); -} - -void HesaiHwMonitorRosWrapper::HesaiCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - - diagnostics.add("system_uptime", std::to_string(current_status->system_uptime.value())); - diagnostics.add("startup_times", std::to_string(current_status->startup_times.value())); - diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time.value())); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckPtp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - auto gps_status = current_status->get_str_gps_pps_lock(); - auto gprmc_status = current_status->get_str_gps_gprmc_status(); - auto ptp_status = current_status->get_str_ptp_clock_status(); - std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); - std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); - std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); - diagnostics.add("gps_pps_lock", gps_status); - diagnostics.add("gps_gprmc_status", gprmc_status); - diagnostics.add("ptp_clock_status", ptp_status); - if (gps_status != "UNKNOWN") { - msg.emplace_back("gps_pps_lock: " + gps_status); - } - if (gprmc_status != "UNKNOWN") { - msg.emplace_back("gprmc_status: " + gprmc_status); - } - if (ptp_status != "UNKNOWN") { - msg.emplace_back("ptp_status: " + ptp_status); - } - if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckTemperature( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - for (size_t i = 0; i < std::size(current_status->temperature); i++) { - diagnostics.add( - temperature_names[i], GetFixedPrecisionString(current_status->temperature[i].value() * 0.01)); - } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_status); - if (current_status) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add("motor_speed", std::to_string(current_status->motor_speed.value())); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltageHttp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_lidar_monitor_tree) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - std::string key = ""; - - std::string mes; - key = "lidarInCur"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - key = "lidarInVol"; - try { - mes = GetPtreeValue(current_lidar_monitor_tree.get(), "Body." + key); - } catch (boost::bad_lexical_cast & ex) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = error_message + std::string(ex.what()); - } - diagnostics.add(key, mes); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - -void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - std::scoped_lock lock(mtx_lidar_monitor); - if (current_monitor) { - uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::vector msg; - diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage.value() * 0.01) + " V"); - diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor->input_current.value() * 0.01) + " mA"); - diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor->input_power.value() * 0.01) + " W"); - - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); - } else { - diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); - } -} - - HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); - } - - RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp new file mode 100644 index 000000000..8449bdfe5 --- /dev/null +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -0,0 +1,315 @@ +#include "nebula_ros/hesai/hesai_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + sensor_cfg_ptr_(nullptr), + packet_queue_(3000), + hw_interface_wrapper_(), + hw_monitor_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); + } + + decoder_wrapper_.emplace( + this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessCloudPacket(std::move(packet_queue_.pop())); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "pandar_packets", rclcpp::SensorDataQoS(), + std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::HesaiSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", "", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", "", param_read_write()); + config.return_mode = drivers::ReturnModeFromStringHesai(_return_mode, config.sensor_model); + + config.host_ip = declare_parameter("host_ip", "255.255.255.255", param_read_only()); + config.sensor_ip = + declare_parameter("sensor_ip", "192.168.1.201", param_read_only()); + config.data_port = declare_parameter("data_port", 2368, param_read_only()); + config.gnss_port = declare_parameter("gnss_port", 2369, param_read_only()); + config.frame_id = declare_parameter("frame_id", "pandar", param_read_write()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.])"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + config.scan_phase = declare_parameter("scan_phase", 0., descriptor); + } + + config.min_range = declare_parameter("min_range", 0.3, param_read_write()); + config.max_range = declare_parameter("max_range", 300., param_read_write()); + config.packet_mtu_size = declare_parameter("packet_mtu_size", 1500, param_read_only()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + uint16_t default_value; + RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model); + if (config.sensor_model == nebula::drivers::SensorModel::HESAI_PANDARAT128) { + descriptor.additional_constraints = "200, 400"; + descriptor.integer_range = int_range(200, 400, 200); + default_value = 200; + } else { + descriptor.additional_constraints = "300, 600, 1200"; + descriptor.integer_range = int_range(300, 1200, 300); + default_value = 600; + } + config.rotation_speed = + declare_parameter("rotation_speed", default_value, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_min_angle = declare_parameter("cloud_min_angle", 0, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_max_angle = declare_parameter("cloud_max_angle", 360, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; + descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); + config.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + } + + auto _ptp_profile = declare_parameter("ptp_profile", "", param_read_only()); + config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); + + auto _ptp_transport = declare_parameter("ptp_transport_type", "", param_read_only()); + config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); + + if ( + config.ptp_transport_type != drivers::PtpTransportType::L2 && + config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && + config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_WARN_STREAM( + get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" + << _ptp_profile + << "' only supports 'L2'. Setting it to 'L2'."); + config.ptp_transport_type = drivers::PtpTransportType::L2; + set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); + } + + auto _ptp_switch = declare_parameter("ptp_switch_type", "", param_read_only()); + config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(0, 127, 1); + config.ptp_domain = declare_parameter("ptp_domain", 0, descriptor); + } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status HesaiRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { + return Status::INVALID_ECHO_MODE; + } + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (hw_monitor_wrapper_) { + hw_monitor_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void HesaiRosWrapper::ReceiveScanMessageCallback( + std::unique_ptr scan_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + return; + } + + for (auto & pkt : scan_msg->packets) { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + +Status HesaiRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status HesaiRosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); +} + +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::HesaiSensorConfiguration new_cfg(*sensor_cfg_ptr_); + + std::string _return_mode = ""; + bool got_any = + get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | + get_param(p, "max_range", new_cfg.max_range) | + get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); + + // Currently, HW interface and monitor wrappers have only read-only parameters, so their update + // logic is not implemented + if (decoder_wrapper_) { + auto result = decoder_wrapper_->OnParameterChange(p); + if (!result.successful) { + return result; + } + } + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (_return_mode.length() > 0) + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + + auto new_cfg_ptr = std::make_shared(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(packet); + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp new file mode 100644 index 000000000..18da9da83 --- /dev/null +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -0,0 +1,82 @@ +#include "nebula_ros/hesai/hw_interface_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & config) +: hw_interface_(new nebula::drivers::HesaiHwInterface()), + logger_(parent_node->get_logger().get_child("HwInterface")), + status_(Status::NOT_INITIALIZED) +{ + setup_sensor_ = parent_node->declare_parameter("setup_sensor", true, param_read_only()); + bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); + + status_ = hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(config)); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); + hw_interface_->SetTargetModel(config->sensor_model); + + int retry_count = 0; + + while (true) { + status_ = hw_interface_->InitializeTcpDriver(); + if (status_ == Status::OK || !retry_connect) { + break; + } + + retry_count++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count); + } + + if (status_ == Status::OK) { + try { + auto inventory = hw_interface_->GetInventory(); + RCLCPP_INFO_STREAM(logger_, inventory); + hw_interface_->SetTargetModel(inventory.model); + } catch (...) { + RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); + } + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } + } else { + RCLCPP_ERROR_STREAM( + logger_, "Failed to get model from sensor... Set from config: " << config->sensor_model); + } + + status_ = Status::OK; +} + +void HesaiHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr & new_config) +{ + hw_interface_->SetSensorConfiguration( + std::static_pointer_cast(new_config)); + if (setup_sensor_) { + hw_interface_->CheckAndSetConfig(); + } +} + +Status HesaiHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr HesaiHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..da1f78794 --- /dev/null +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -0,0 +1,362 @@ +#include "nebula_ros/hesai/hw_monitor_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( + rclcpp::Node * const parent_node, + const std::shared_ptr & hw_interface, + std::shared_ptr & config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + diagnostics_updater_(parent_node), + status_(Status::OK), + hw_interface_(hw_interface), + parent_node_(parent_node) +{ + diag_span_ = parent_node->declare_parameter("diag_span", 1000, param_read_only()); + + switch (config->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names_.emplace_back("Bottom circuit board T1"); + temperature_names_.emplace_back("Bottom circuit board T2"); + temperature_names_.emplace_back("Laser emitting board RT_L1 (Internal)"); + temperature_names_.emplace_back("Laser emitting board RT_L2"); + temperature_names_.emplace_back("Receiving board RT_R"); + temperature_names_.emplace_back("Receiving board RT2"); + temperature_names_.emplace_back("Top circuit RT3"); + temperature_names_.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names_.emplace_back("Bottom circuit RT1"); + temperature_names_.emplace_back("Bottom circuit RT2"); + temperature_names_.emplace_back("Internal Temperature"); + temperature_names_.emplace_back("Laser emitting board RT1"); + temperature_names_.emplace_back("Laser emitting board RT2"); + temperature_names_.emplace_back("Receiving board RT1"); + temperature_names_.emplace_back("Top circuit RT1"); + temperature_names_.emplace_back("Top circuit RT2"); + break; + } + + auto result = hw_interface->GetInventory(); + current_inventory_.reset(new HesaiInventory(result)); + current_inventory_time_.reset(new rclcpp::Time(parent_node->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model_ = result.get_str_model(); + info_serial_ = std::string(std::begin(result.sn), std::end(result.sn)); + RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); + InitializeHesaiDiagnostics(); +} + +void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() +{ + RCLCPP_INFO_STREAM(logger_, "InitializeHesaiDiagnostics"); + using std::chrono_literals::operator""s; + std::ostringstream os; + auto hardware_id = info_model_ + ": " + info_serial_; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hardware_id); + + diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::HesaiCheckStatus); + diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::HesaiCheckPtp); + diagnostics_updater_.add( + "hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::HesaiCheckRpm); + + current_status_.reset(); + current_monitor_.reset(); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + auto fetch_diag_from_sensor = [this]() { + OnHesaiStatusTimer(); + if (hw_interface_->UseHttpGetLidarMonitor()) { + OnHesaiLidarMonitorTimerHttp(); + } else { + OnHesaiLidarMonitorTimer(); + } + }; + + fetch_diagnostics_timer_ = parent_node_->create_wall_timer( + std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); + + if (hw_interface_->UseHttpGetLidarMonitor()) { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltageHttp); + } else { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltage); + } + + auto on_timer_update = [this] { + RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); + auto now = parent_node_->get_clock()->now(); + auto dif = (now - *current_status_time_).seconds(); + + RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + dif = (now - *current_lidar_monitor_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "dif(monitor): " << dif); + if (diag_span_ * 2.0 < dif * 1000) { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } else { + current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + diagnostics_updater_.force_update(); + }; + diagnostics_update_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); + + RCLCPP_DEBUG_STREAM(logger_, "add_timer"); +} + +std::string HesaiHwMonitorWrapper::GetPtreeValue( + boost::property_tree::ptree * pt, const std::string & key) +{ + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return MSG_NOT_SUPPORTED; + } +} +std::string HesaiHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} + +void HesaiHwMonitorWrapper::OnHesaiStatusTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer" << std::endl); + try { + auto result = hw_interface_->GetLidarStatus(); + std::scoped_lock lock(mtx_lidar_status_); + current_status_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_status_.reset(new HesaiLidarStatus(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer END" << std::endl); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp"); + try { + hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string & str) { + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_lidar_monitor_tree_ = + std::make_unique(hw_interface_->ParseJson(str)); + }); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp END"); +} + +void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer"); + try { + auto result = hw_interface_->GetLidarMonitor(); + std::scoped_lock lock(mtx_lidar_monitor_); + current_lidar_monitor_time_.reset(new rclcpp::Time(parent_node_->get_clock()->now())); + current_monitor_.reset(new HesaiLidarMonitor(result)); + } catch (const std::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + error.what()); + } catch (const boost::system::system_error & error) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" + "error)"), + error.what()); + } + RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer END"); +} + +void HesaiHwMonitorWrapper::HesaiCheckStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + + diagnostics.add("system_uptime", std::to_string(current_status_->system_uptime.value())); + diagnostics.add("startup_times", std::to_string(current_status_->startup_times.value())); + diagnostics.add( + "total_operation_time", std::to_string(current_status_->total_operation_time.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + auto gps_status = current_status_->get_str_gps_pps_lock(); + auto gprmc_status = current_status_->get_str_gps_gprmc_status(); + auto ptp_status = current_status_->get_str_ptp_clock_status(); + std::transform(gps_status.cbegin(), gps_status.cend(), gps_status.begin(), toupper); + std::transform(gprmc_status.cbegin(), gprmc_status.cend(), gprmc_status.begin(), toupper); + std::transform(ptp_status.cbegin(), ptp_status.cend(), ptp_status.begin(), toupper); + diagnostics.add("gps_pps_lock", gps_status); + diagnostics.add("gps_gprmc_status", gprmc_status); + diagnostics.add("ptp_clock_status", ptp_status); + if (gps_status != "UNKNOWN") { + msg.emplace_back("gps_pps_lock: " + gps_status); + } + if (gprmc_status != "UNKNOWN") { + msg.emplace_back("gprmc_status: " + gprmc_status); + } + if (ptp_status != "UNKNOWN") { + msg.emplace_back("ptp_status: " + ptp_status); + } + if (ptp_status == "FREE RUN" && gps_status == "UNKNOWN") { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckTemperature( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + for (size_t i = 0; i < std::size(current_status_->temperature); i++) { + diagnostics.add( + temperature_names_[i], + GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); + } + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_status_); + if (current_status_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add("motor_speed", std::to_string(current_status_->motor_speed.value())); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_lidar_monitor_tree_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + std::string key = ""; + + std::string mes; + key = "lidarInCur"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + key = "lidarInVol"; + try { + mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + } catch (boost::bad_lexical_cast & ex) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mes = MSG_ERROR + std::string(ex.what()); + } + diagnostics.add(key, mes); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +void HesaiHwMonitorWrapper::HesaiCheckVoltage( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::scoped_lock lock(mtx_lidar_monitor_); + if (current_monitor_) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; + diagnostics.add( + "input_voltage", + GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); + diagnostics.add( + "input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + + " m" + "A"); + diagnostics.add( + "input_power", + GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); + + diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + } else { + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); + } +} + +Status HesaiHwMonitorWrapper::Status() +{ + return Status::OK; +} +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp new file mode 100644 index 000000000..d07383fbe --- /dev/null +++ b/nebula_ros/src/velodyne/decoder_wrapper.cpp @@ -0,0 +1,242 @@ +#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 new file mode 100644 index 000000000..38091f89b --- /dev/null +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -0,0 +1,62 @@ +#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_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp similarity index 52% rename from nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp rename to nebula_ros/src/velodyne/hw_monitor_wrapper.cpp index 172e61c83..11bbff4c4 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp @@ -1,626 +1,221 @@ -#include "nebula_ros/velodyne/velodyne_hw_monitor_ros_wrapper.hpp" - -#include -#include - -#include -#include - -#include +#include "nebula_ros/velodyne/hw_monitor_wrapper.hpp" namespace nebula { namespace ros { -VelodyneHwMonitorRosWrapper::VelodyneHwMonitorRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("velodyne_hw_monitor_ros_wrapper", options), - hw_interface_(), - diagnostics_updater_(this) +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) { - 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"; + diag_span_ = parent_node->declare_parameter("diag_span", param_read_only()); + show_advanced_diagnostics_ = parent_node->declare_parameter("advanced_diagnostics", param_read_only()); std::cout << "Get model name and serial." << std::endl; - 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, 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; - } + 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)); - { - 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(); + 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; } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - - return Status::OK; + InitializeVelodyneDiagnostics(); } -void VelodyneHwMonitorRosWrapper::InitializeVelodyneDiagnostics() +void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() { - RCLCPP_INFO_STREAM(get_logger(), "InitializeVelodyneDiagnostics"); + RCLCPP_INFO_STREAM(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(get_logger(), "hardware_id" << hardware_id); + RCLCPP_INFO_STREAM(logger_, "hardware_id" << hardware_id); - if (use_advanced_diagnostics) { + if (show_advanced_diagnostics_) { diagnostics_updater_.add( - "velodyne_snapshot-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckSnapshot); + "velodyne_snapshot-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckSnapshot); diagnostics_updater_.add( - "velodyne_volt_temp_top_hv-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopHv); - if (sensor_configuration_.sensor_model != nebula::drivers::SensorModel::VELODYNE_VLP16) { + "velodyne_volt_temp_top_hv-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::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, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopAdTemp); + "velodyne_volt_temp_top_ad_temp-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp); } diagnostics_updater_.add( - "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopLm20Temp); + "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr5v); + "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr25v); + "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr33v); + "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrRaw); + "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw); diagnostics_updater_.add( - "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrVccint); + "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint); diagnostics_updater_.add( - "velodyne_volt_temp_bot_i_out-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotIOut); + "velodyne_volt_temp_bot_i_out-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotIOut); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr12v); + "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotLm20Temp); + "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr5v); + "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr25v); + "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr33v); + "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwrVIn); + "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn); diagnostics_updater_.add( - "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr125v); + "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v); diagnostics_updater_.add( - "velodyne_vhv-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckVhv); + "velodyne_vhv-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckVhv); diagnostics_updater_.add( - "velodyne_adc_nf-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckAdcNf); + "velodyne_adc_nf-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckAdcNf); diagnostics_updater_.add( - "velodyne_adc_stats-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckAdcStats); + "velodyne_adc_stats-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckAdcStats); diagnostics_updater_.add( - "velodyne_ixe-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckIxe); + "velodyne_ixe-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckIxe); diagnostics_updater_.add( - "velodyne_adctp_stat-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckAdctpStat); + "velodyne_adctp_stat-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat); diagnostics_updater_.add( - "velodyne_status_gps_pps_state-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPpsState); + "velodyne_status_gps_pps_state-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState); diagnostics_updater_.add( - "velodyne_status_gps_pps_position-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPosition); + "velodyne_status_gps_pps_position-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition); diagnostics_updater_.add( - "velodyne_status_motor_state-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorState); + "velodyne_status_motor_state-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorState); diagnostics_updater_.add( - "velodyne_status_motor_rpm-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorRpm); + "velodyne_status_motor_rpm-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm); diagnostics_updater_.add( - "velodyne_status_motor_lock-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorLock); + "velodyne_status_motor_lock-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorLock); diagnostics_updater_.add( - "velodyne_status_motor_phase-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckMotorPhase); + "velodyne_status_motor_phase-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase); diagnostics_updater_.add( - "velodyne_status_laser_state-" + sensor_configuration_.frame_id, this, - &VelodyneHwMonitorRosWrapper::VelodyneCheckLaserState); + "velodyne_status_laser_state-" + sensor_configuration_->frame_id, this, + &VelodyneHwMonitorWrapper::VelodyneCheckLaserState); } diagnostics_updater_.add( - "velodyne_status", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckStatus); - diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckPps); + "velodyne_status", this, &VelodyneHwMonitorWrapper::VelodyneCheckStatus); + diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorWrapper::VelodyneCheckPps); diagnostics_updater_.add( - "velodyne_temperature", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature); - diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckRpm); + "velodyne_temperature", this, &VelodyneHwMonitorWrapper::VelodyneCheckTemperature); + diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorWrapper::VelodyneCheckRpm); diagnostics_updater_.add( - "velodyne_voltage", this, &VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage); + "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())); + } - 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_ = 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_); + diagnostics_snapshot_timer_ = parent_node_->create_wall_timer(std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot)); auto on_timer_update = [this] { - auto now = this->get_clock()->now(); - auto dif = (now - *current_snapshot_time).seconds(); + auto now = parent_node_->now(); + double dif; + { + std::lock_guard lock(mtx_snapshot_); + dif = (now - *current_snapshot_time).seconds(); + } if (diag_span_ * 2.0 < dif * 1000) { current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; - RCLCPP_DEBUG_STREAM(get_logger(), "STALE"); + RCLCPP_DEBUG_STREAM(logger_, "STALE"); } else { current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; - RCLCPP_DEBUG_STREAM(get_logger(), "OK"); + RCLCPP_DEBUG_STREAM(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_); -} - -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(); + diagnostics_update_timer_ = parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); } -// https://memo.appri.me/programming/cpp-curl-http-client -using namespace std; -typedef void (*CurlCallback)(string err, string body); - -class Curl +void VelodyneHwMonitorWrapper::OnVelodyneSnapshotTimer() { -private: - /** response body */ - string body; - - // 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 *) - { - int dataLength = size * nmemb; - this->body.append(buffer, dataLength); - return dataLength; - } - -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); - } + auto str = hw_interface_->GetSnapshot(); + auto ptree = hw_interface_->ParseJson(str); - /** - * 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, ""); - } + std::lock_guard lock(mtx_snapshot_); - 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_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); + current_snapshot_tree = + std::make_shared(ptree); current_diag_tree = - std::make_shared(hw_interface_.ParseJson(body)); - RCLCPP_DEBUG_STREAM(get_logger(), "diagnostics_updater_.force_update()"); - diagnostics_updater_.force_update(); + 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 VelodyneHwMonitorRosWrapper::OnVelodyneDiagnosticsTimer() +void VelodyneHwMonitorWrapper::OnVelodyneDiagnosticsTimer() { std::cout << "OnVelodyneDiagnosticsTimer" << std::endl; - 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; + auto str = hw_interface_->GetDiag(); + { + std::lock_guard lock(mtx_diag_); + current_diag_tree = + std::make_shared(hw_interface_->ParseJson(str)); } + diagnostics_updater_.force_update(); } -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetTopHv() +std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopHv() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -628,7 +223,7 @@ std::tuple VelodyneHwMonitorRosWrapper: std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_hv)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, 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; @@ -647,7 +242,7 @@ std::tuple VelodyneHwMonitorRosWrapper: } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopAdTemp() +VelodyneHwMonitorWrapper::VelodyneGetTopAdTemp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -655,7 +250,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopAdTemp() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_ad_temp)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_ad_temp)); val = val * 5.0 / 4096.0; mes = GetFixedPrecisionString(val) + " V"; } catch (boost::bad_lexical_cast & ex) { @@ -667,7 +262,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopAdTemp() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopLm20Temp() +VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -676,7 +271,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopLm20Temp() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_lm20_temp)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, 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; @@ -696,7 +291,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopLm20Temp() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5v() +VelodyneHwMonitorWrapper::VelodyneGetTopPwr5v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -704,7 +299,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v)); val = 2.0 * val * 5.0 / 4096.0; if (val < 4.8) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -723,7 +318,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr25v() +VelodyneHwMonitorWrapper::VelodyneGetTopPwr25v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -731,7 +326,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr25v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_2_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_2_5v)); val = val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -750,7 +345,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr25v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr33v() +VelodyneHwMonitorWrapper::VelodyneGetTopPwr33v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -758,7 +353,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr33v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_3_3v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_3_3v)); val = val * 5.0 / 4096.0; if (val < 3.1) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -777,7 +372,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr33v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5vRaw() +VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -786,7 +381,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5vRaw() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_5v_raw)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, 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; @@ -805,7 +400,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwr5vRaw() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrRaw() +VelodyneHwMonitorWrapper::VelodyneGetTopPwrRaw() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -813,7 +408,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrRaw() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_raw)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_raw)); val = val * 5.0 / 4096.0; if (val < 1.6) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -832,7 +427,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrRaw() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrVccint() +VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -841,7 +436,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrVccint() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_top_pwr_vccint)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_vccint)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -861,7 +456,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetTopPwrVccint() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotIOut() +VelodyneHwMonitorWrapper::VelodyneGetBotIOut() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -869,7 +464,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotIOut() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_i_out)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, 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; @@ -888,7 +483,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotIOut() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr12v() +VelodyneHwMonitorWrapper::VelodyneGetBotPwr12v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -896,7 +491,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr12v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_1_2v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_2v)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -915,7 +510,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr12v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotLm20Temp() +VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -924,7 +519,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotLm20Temp() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_lm20_temp)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, 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; @@ -944,7 +539,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotLm20Temp() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr5v() +VelodyneHwMonitorWrapper::VelodyneGetBotPwr5v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -952,7 +547,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr5v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_5v)); val = 2.0 * val * 5.0 / 4096.0; if (val < 4.8) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -971,7 +566,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr5v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr25v() +VelodyneHwMonitorWrapper::VelodyneGetBotPwr25v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -979,7 +574,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr25v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_2_5v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_2_5v)); val = val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -998,7 +593,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr25v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr33v() +VelodyneHwMonitorWrapper::VelodyneGetBotPwr33v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1006,7 +601,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr33v() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_3_3v)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_3_3v)); val = val * 5.0 / 4096.0; if (val < 3.1) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -1025,7 +620,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr33v() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwrVIn() +VelodyneHwMonitorWrapper::VelodyneGetBotPwrVIn() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1033,7 +628,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwrVIn() std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_v_in)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, 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; @@ -1052,7 +647,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwrVIn() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr125v() +VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1061,7 +656,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr125v() try { double val = 0.0; val = - boost::lexical_cast(GetPtreeValue(current_diag_tree, key_volt_temp_bot_pwr_1_25v)); + boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_25v)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; @@ -1079,7 +674,7 @@ VelodyneHwMonitorRosWrapper::VelodyneGetBotPwr125v() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetVhv() +std::tuple VelodyneHwMonitorWrapper::VelodyneGetVhv() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1087,7 +682,7 @@ std::tuple VelodyneHwMonitorRosWrapper: std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, key_vhv)); + val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_vhv)); mes = boost::lexical_cast(val); } catch (boost::bad_lexical_cast & ex) { not_ex = false; @@ -1097,7 +692,7 @@ std::tuple VelodyneHwMonitorRosWrapper: return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetAdcNf() +std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdcNf() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1125,7 +720,7 @@ std::tuple VelodyneHwMonitorRosWrapper: } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetAdcStats() +VelodyneHwMonitorWrapper::VelodyneGetAdcStats() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1155,14 +750,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetAdcStats() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorRosWrapper::VelodyneGetIxe() +std::tuple VelodyneHwMonitorWrapper::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, key_ixe); + mes = GetPtreeValue(current_diag_tree, mtx_diag_, key_ixe); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1172,7 +767,7 @@ std::tuple VelodyneHwMonitorRosWrapper: } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetAdctpStat() +VelodyneHwMonitorWrapper::VelodyneGetAdctpStat() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -1200,14 +795,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetAdctpStat() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetGpsPpsState() +VelodyneHwMonitorWrapper::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, key_status_gps_pps_state); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_state); if (mes == "Absent") { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; error_mes = mes; @@ -1224,14 +819,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetGpsPpsState() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetGpsPosition() +VelodyneHwMonitorWrapper::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, key_status_gps_pps_position); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_position); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1241,14 +836,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetGpsPosition() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorState() +VelodyneHwMonitorWrapper::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, key_status_motor_state); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_state); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1258,14 +853,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetMotorState() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorRpm() +VelodyneHwMonitorWrapper::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, key_status_motor_rpm); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_rpm); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1275,14 +870,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetMotorRpm() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorLock() +VelodyneHwMonitorWrapper::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, key_status_motor_lock); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_lock); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1292,14 +887,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetMotorLock() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetMotorPhase() +VelodyneHwMonitorWrapper::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, key_status_motor_phase); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_phase); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1309,14 +904,14 @@ VelodyneHwMonitorRosWrapper::VelodyneGetMotorPhase() } std::tuple -VelodyneHwMonitorRosWrapper::VelodyneGetLaserState() +VelodyneHwMonitorWrapper::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, key_status_laser_state); + mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_laser_state); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -1325,383 +920,367 @@ VelodyneHwMonitorRosWrapper::VelodyneGetLaserState() return std::make_tuple(not_ex, level, mes, error_mes); } -void VelodyneHwMonitorRosWrapper::VelodyneCheckTopHv( +void VelodyneHwMonitorWrapper::VelodyneCheckTopHv( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopAdTemp( +void VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopLm20Temp( +void VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr5v( +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr25v( +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwr33v( +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrRaw( +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckTopPwrVccint( +void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotIOut( +void VelodyneHwMonitorWrapper::VelodyneCheckBotIOut( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr12v( +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotLm20Temp( +void VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr5v( +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr25v( +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr33v( +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwrVIn( +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckBotPwr125v( +void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckVhv( +void VelodyneHwMonitorWrapper::VelodyneCheckVhv( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckAdcNf( +void VelodyneHwMonitorWrapper::VelodyneCheckAdcNf( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckAdcStats( +void VelodyneHwMonitorWrapper::VelodyneCheckAdcStats( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckIxe( +void VelodyneHwMonitorWrapper::VelodyneCheckIxe( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckAdctpStat( +void VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::OnVelodyneStatusTimer() +void VelodyneHwMonitorWrapper::OnVelodyneStatusTimer() { - 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(); - }); + auto str = hw_interface_->GetStatus(); + { + std::lock_guard lock(mtx_status_); + current_status_tree = + std::make_shared(hw_interface_->ParseJson(str)); } + diagnostics_updater_.force_update(); } -void VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPpsState( +void VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckGpsPosition( +void VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckMotorState( +void VelodyneHwMonitorWrapper::VelodyneCheckMotorState( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckMotorRpm( +void VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckMotorLock( +void VelodyneHwMonitorWrapper::VelodyneCheckMotorLock( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckMotorPhase( +void VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckLaserState( +void VelodyneHwMonitorWrapper::VelodyneCheckLaserState( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckSnapshot( +void VelodyneHwMonitorWrapper::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 VelodyneHwMonitorRosWrapper::VelodyneCheckStatus( +void VelodyneHwMonitorWrapper::VelodyneCheckStatus( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1727,12 +1306,12 @@ void VelodyneHwMonitorRosWrapper::VelodyneCheckStatus( } } -void VelodyneHwMonitorRosWrapper::VelodyneCheckPps( +void VelodyneHwMonitorWrapper::VelodyneCheckPps( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_status_tree && - !VelodyneHwMonitorRosWrapper::current_status_tree->empty()) { + VelodyneHwMonitorWrapper::current_status_tree && + !VelodyneHwMonitorWrapper::current_status_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1758,12 +1337,12 @@ void VelodyneHwMonitorRosWrapper::VelodyneCheckPps( } } -void VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature( +void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1789,12 +1368,12 @@ void VelodyneHwMonitorRosWrapper::VelodyneCheckTemperature( } } -void VelodyneHwMonitorRosWrapper::VelodyneCheckRpm( +void VelodyneHwMonitorWrapper::VelodyneCheckRpm( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1820,12 +1399,12 @@ void VelodyneHwMonitorRosWrapper::VelodyneCheckRpm( } } -void VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage( +void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { if ( - VelodyneHwMonitorRosWrapper::current_diag_tree && - !VelodyneHwMonitorRosWrapper::current_diag_tree->empty()) { + VelodyneHwMonitorWrapper::current_diag_tree && + !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; @@ -1865,7 +1444,7 @@ void VelodyneHwMonitorRosWrapper::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)); @@ -1961,36 +1540,28 @@ void VelodyneHwMonitorRosWrapper::VelodyneCheckVoltage( } } -rcl_interfaces::msg::SetParametersResult VelodyneHwMonitorRosWrapper::paramCallback( - const std::vector & p) +std::string VelodyneHwMonitorWrapper::GetPtreeValue( + std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key) { - 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::lock_guard lock(mtx_pt); + boost::optional value = pt->get_optional(key); + if (value) { + return value.get(); + } else { + return not_supported_message; } +} - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - std::cout << "add_on_set_parameters_callback success" << std::endl; +std::string VelodyneHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(pre) << val; + return ss.str(); +} - return *result; +Status VelodyneHwMonitorWrapper::Status() +{ + return Status::NOT_IMPLEMENTED; // TODO } -RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneHwMonitorRosWrapper) } // namespace ros -} // namespace nebula +} // 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 deleted file mode 100644 index df484a27a..000000000 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,281 +0,0 @@ -#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 deleted file mode 100644 index cbbc19e05..000000000 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,326 +0,0 @@ -#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/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp new file mode 100644 index 000000000..d8246e7b5 --- /dev/null +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -0,0 +1,265 @@ +#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 new file mode 100644 index 000000000..5da07e063 Binary files /dev/null and b/nebula_tests/data/velodyne/vls128/1585897255376374245.pcd differ diff --git a/nebula_tests/data/velodyne/vls128/1614315746471294674/1614315746471294674_0.db3 b/nebula_tests/data/velodyne/vls128/1614315746471294674/1614315746471294674_0.db3 index e9de91274..2d95339b1 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 deleted file mode 100644 index 80ed53270..000000000 Binary files a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd and /dev/null differ diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index a07a4dabd..97046e8f8 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -46,12 +46,11 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( correction_cfg_ptr_ = std::make_shared(correction_configuration); wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); + correction_cfg_ptr_); } else { wrapper_status_ = InitializeDriver( std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + calibration_cfg_ptr_); } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); @@ -59,26 +58,12 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( Status HesaiRosDecoderTest::InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) + std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); + calibration_configuration); return driver_ptr_->GetStatus(); } @@ -235,8 +220,7 @@ Status HesaiRosDecoderTest::GetParameters( if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if ( - sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { @@ -289,9 +273,8 @@ void HesaiRosDecoderTest::ReadBag( storage_options.uri = params_.bag_path; storage_options.storage_id = params_.storage_id; - converter_options.output_serialization_format = params_.format; //"cdr"; + converter_options.output_serialization_format = params_.format; rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); rosbag2_cpp::Reader bag_reader(std::make_unique()); bag_reader.open(storage_options, converter_options); @@ -310,12 +293,21 @@ void HesaiRosDecoderTest::ReadBag( "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp); auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - auto scan_timestamp = std::get<1>(pointcloud_ts); - pointcloud = std::get<0>(pointcloud_ts); - scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); - pointcloud.reset(new nebula::drivers::NebulaPointCloud); + for (auto & pkt : extracted_msg_ptr->packets) { + auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); + auto pointcloud = std::get<0>(pointcloud_ts); + auto scan_timestamp = std::get<1>(pointcloud_ts); + + if (!pointcloud) { + continue; + } + + std::cerr << "Pointcloud size: " << pointcloud->size() << std::endl; + + scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); + } } } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index a0477a366..f12e56020 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #ifndef _SRC_CALIBRATION_DIR_PATH #define _SRC_CALIBRATION_DIR_PATH "" @@ -48,9 +50,27 @@ struct HesaiRosDecoderTestParams double dual_return_distance_threshold = 0.1; }; +inline std::ostream & operator<<( + std::ostream & os, nebula::ros::HesaiRosDecoderTestParams const & arg) +{ + return os << "sensor_model=" << arg.sensor_model << ", " + << "return_mode=" << arg.return_mode << ", " + << "calibration_file=" << arg.calibration_file << ", " + << "bag_path=" << arg.bag_path << ", " + << "correction_file=" << arg.correction_file << ", " + << "frame_id=" << arg.frame_id << ", " + << "scan_phase=" << arg.scan_phase << ", " + << "min_range=" << arg.min_range << ", " + << "max_range=" << arg.max_range << ", " + << "storage_id=" << arg.storage_id << ", " + << "format=" << arg.format << ", " + << "target_topic=" << arg.target_topic << ", " + << "dual_return_distance_threshold=" << arg.dual_return_distance_threshold << ", "; +} + /// @brief Testing decoder of pandar 40p (Keeps HesaiDriverRosWrapper structure as much as /// possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test +class HesaiRosDecoderTest final : public rclcpp::Node { std::shared_ptr driver_ptr_; Status wrapper_status_; @@ -65,17 +85,7 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas /// @return Resulting status Status InitializeDriver( std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); + std::shared_ptr calibration_configuration); /// @brief Get configurations (Magic numbers for Pandar40P is described, each function can be /// integrated if the ros parameter can be passed to Google Test) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 96ff00c3c..7f80f6f48 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -12,6 +12,8 @@ #include #include #include +#include +#include namespace nebula { @@ -19,72 +21,15 @@ namespace test { const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { - { - "Pandar40P", - "Dual", - "Pandar40P.csv", - "40p/1673400149412331409", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "Pandar64", - "Dual", - "Pandar64.csv", - "64/1673403880599376836", - "", - "hesai", - 0, - 0.3f, - 200.f - }, - { - "PandarAT128", - "LastStrongest", - "PandarAT128.csv", - "at128/1679653308406038376", - "PandarAT128.dat", - "hesai", - 0, - 1.f, - 180.f - }, - { - "PandarQT64", - "Dual", - "PandarQT64.csv", - "qt64/1673401195788312575", - "", - "hesai", - 0, - 0.1f, - 60.f - }, - { - "PandarXT32", - "Dual", - "PandarXT32.csv", - "xt32/1673400677802009732", - "", - "hesai", - 0, - 0.05f, - 120.f - }, - { - "PandarXT32M", - "LastStrongest", - "PandarXT32M.csv", - "xt32m/1660893203042895158", - "", - "hesai", - 0, - 0.5f, - 300.f - }}; + {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "", "hesai", 0, 0.3f, 200.f}, + {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "", "hesai", 0, 0.3f, 200.f}, + {"PandarAT128", "LastStrongest", "PandarAT128.csv", "at128/1679653308406038376", + "PandarAT128.dat", "hesai", 0, 1.f, 180.f}, + {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "", "hesai", 0, 0.1f, 60.f}, + {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "", "hesai", 0, 0.05f, + 120.f}, + {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "", "hesai", 0, + 0.5f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd) @@ -142,6 +87,8 @@ TEST_P(DecoderTest, TestTimezone) auto gmt = timezone; hesai_driver_->ReadBag(scan_callback); + ASSERT_GT(decoded_timestamps.size(), 0U); + // Then, reset driver and timestamps vector for the next decode run TearDown(); SetUp(); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp index fa3604485..c513cbc2b 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp @@ -34,26 +34,22 @@ 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( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, 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( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } @@ -336,27 +332,31 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - 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); + 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); } - 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 2b889358e..4a938e388 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp @@ -21,22 +21,21 @@ namespace nebula namespace ros { /// @brief Testing decoder of VLP16 (Keeps VelodyneDriverRosWrapper structure as much as possible) -class VelodyneRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class VelodyneRosDecoderTest final : public rclcpp::Node { 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) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); /// @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 c7623ca50..3fa61c95a 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp @@ -34,26 +34,22 @@ 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( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, 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( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); return driver_ptr_->GetStatus(); } @@ -340,28 +336,32 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - 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); + 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); } - 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 2b889358e..4a938e388 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp @@ -21,22 +21,21 @@ namespace nebula namespace ros { /// @brief Testing decoder of VLP16 (Keeps VelodyneDriverRosWrapper structure as much as possible) -class VelodyneRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class VelodyneRosDecoderTest final : public rclcpp::Node { 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) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); /// @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 b00a9433d..c7bc39453 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -13,7 +13,9 @@ #include #include +#include #include +#include namespace nebula { @@ -34,30 +36,30 @@ 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( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); + wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, 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( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); + driver_ptr_ = std::make_shared(sensor_configuration, 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, @@ -223,7 +225,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter( - "target_topic", "/sensing/lidar/top/velodyne_packets", descriptor); + "target_topic", "/velodyne_packets", descriptor); target_topic = this->get_parameter("target_topic").as_string(); } @@ -315,10 +317,9 @@ void VelodyneRosDecoderTest::ReadBag() storage_options.uri = bag_path; storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; + converter_options.output_serialization_format = format; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); { rosbag2_cpp::Reader bag_reader(std::make_unique()); @@ -337,27 +338,31 @@ void VelodyneRosDecoderTest::ReadBag() << bag_message->time_stamp << std::endl; auto extracted_msg_ptr = std::make_shared(extracted_msg); - 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); + 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); } - 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 a2432838f..ad29dd961 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp @@ -20,18 +20,17 @@ namespace nebula { namespace ros { -class VelodyneRosDecoderTest final : public rclcpp::Node, - NebulaDriverRosWrapperBase //, testing::Test +class VelodyneRosDecoderTest final : public rclcpp::Node { 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) override; + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration); Status GetParameters( drivers::VelodyneSensorConfiguration & sensor_configuration, diff --git a/ros2_socketcan b/ros2_socketcan new file mode 160000 index 000000000..4ced5284f --- /dev/null +++ b/ros2_socketcan @@ -0,0 +1 @@ +Subproject commit 4ced5284fdd080274b04530406d887e8330a0cdb diff --git a/scripts/plot_instrumentation.py b/scripts/plot_instrumentation.py new file mode 100755 index 000000000..7540aa35f --- /dev/null +++ b/scripts/plot_instrumentation.py @@ -0,0 +1,79 @@ +#!/usr/bin/python3 + +import argparse +from collections import defaultdict +import json +import os +import re + +from matplotlib import pyplot as plt +import pandas as pd + + +def condition_data(log_file: str): + with open(log_file, "r") as f: + lines = f.readlines() + lines = [re.search(r'(\{"type":.*?"tag":.*?\})', line) for line in lines] + lines = [re.sub(r'([0-9])"', r"\1", line[1]) for line in lines if line] + lines = [json.loads(line) for line in lines if line] + + cols = defaultdict(list) + + for record in lines: + for key in record.keys(): + if key in ["tag", "type"]: + continue + + colname = f"{record['type']}.{key}.{record['tag']}" + cols[colname] += [num for num in record[key] if num is not None] + + def quantile_filter(series, quantile): + q = series.quantile(quantile) + return series[series <= q] + + cols = {k: pd.Series(v, name=k) / 1e3 for k, v in cols.items()} + cols = {k: quantile_filter(v, 0.999) for k, v in cols.items()} + + for v in cols.values(): + v: pd.Series + v.attrs["file"] = os.path.basename(os.path.splitext(log_file)[0]) + + return cols + + +def plot(conditioned_logs): + + conditioned_logs = {k: [dic[k] for dic in conditioned_logs] for k in conditioned_logs[0]} + + fig, axs = plt.subplots(1, len(conditioned_logs), figsize=(15, 8), dpi=120) + + handles, labels = [], [] + + for i, k in enumerate(sorted(conditioned_logs.keys())): + k: str + v = conditioned_logs[k] + ax: plt.Axes = axs[i] + art = ax.boxplot(v) + handles.append(art) + labels.append(k) + ax.set_ylabel("dt [µs]") + ax.set_title(k.replace(".", "\n")) + ax.set_xticks([1 + i for i in range(len(v))], [ser.attrs["file"] for ser in v]) + ax.tick_params(axis="x", labelrotation=90) + + fig.tight_layout() + plt.savefig("instrumentation.png") + plt.show() + + +def main(args): + conditioned_logs = [condition_data(f) for f in args.log_files] + plot(conditioned_logs) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("log_files", nargs="+") + args = parser.parse_args() + + main(args) diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 521dfd347..07dbee302 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -1,11 +1,13 @@ -#%% +# %% +import argparse +import glob import os -import pandas as pd import re -import glob -import argparse + from matplotlib import pyplot as plt +import pandas as pd + def parse_logs(run_name): dfs = [] @@ -15,22 +17,20 @@ def parse_logs(run_name): with open(path) as f: lines = f.readlines() lines = filter(lambda line: "PROFILING" in line, lines) - lines = [re.sub(r'.*PROFILING (\{.*?\}).*', r'\1', line) for line in lines] + lines = [re.sub(r".*PROFILING (\{.*?\}).*", r"\1", line) for line in lines] records = [eval(line) for line in lines] dfs.append(pd.DataFrame(records)) df = pd.concat(dfs) - + for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms - df['d_total'] = sum([df[c] for c in df.columns if c.startswith("d_")]) # type: ignore return df + def plot_timing_comparison(run_names): - scenario_dfs = { - run_name: parse_logs(run_name) for run_name in run_names - } + scenario_dfs = {run_name: parse_logs(run_name) for run_name in run_names} n_cols = sum(1 for c in next(iter(scenario_dfs.values())).columns if c.startswith("d_")) @@ -41,16 +41,20 @@ def plot_timing_comparison(run_names): boxes = axs[1:] for i, (label, df) in enumerate(scenario_dfs.items()): - durations = df['d_total'] + # durations = df['d_total'] - ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') + # ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): - ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') - + ax_n.plot( + df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".", color="black" + ) + for col in filter(lambda col: col.startswith("d_"), df.columns): + ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle="", marker=".") + d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns) for j, col in enumerate(d_columns): - if (i == 0): + if i == 0: boxes[j].set_ylabel(f"{col} (ms)") boxes[j].boxplot(df[col], positions=[i], labels=[label]) @@ -58,13 +62,11 @@ def plot_timing_comparison(run_names): ax_d.set_ylabel("time (ms)") ax_d.set_xlabel("iteration") - df_means = pd.DataFrame({ - label: df.describe().loc["mean"] for label, df in scenario_dfs.items() - }) + df_means = pd.DataFrame( + {label: df.describe().loc["mean"] for label, df in scenario_dfs.items()} + ) - df_stds = pd.DataFrame({ - label: df.describe().loc["std"] for label, df in scenario_dfs.items() - }) + df_stds = pd.DataFrame({label: df.describe().loc["std"] for label, df in scenario_dfs.items()}) df_means = df_means.rename(index=lambda label: f"{label} AVG") df_stds = df_stds.rename(index=lambda label: f"{label} STD") @@ -74,15 +76,23 @@ def plot_timing_comparison(run_names): baseline_name = run_names[0] df_base = df_stat[baseline_name] for label in df_base.index: - df_stat.loc[f"{label} % rel to {baseline_name}", :] = df_stat.loc[label, :] / df_base.loc[label] * 100 + df_stat.loc[f"{label} % rel to {baseline_name}", :] = ( + df_stat.loc[label, :] / df_base.loc[label] * 100 + ) print(df_stat.to_markdown()) plt.show() + + # %% if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("run_names", nargs='+', help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.") + parser.add_argument( + "run_names", + nargs="+", + help="The names of the runs to plot, as entered in test_runner.bash (e.g. 'baseline', 'optimized')\nAll files of the form -[0-9].log will be plotted.", + ) args = parser.parse_args() - plot_timing_comparison(args.run_names) \ No newline at end of file + plot_timing_comparison(args.run_names) diff --git a/transport_drivers b/transport_drivers new file mode 160000 index 000000000..abf8aa8e1 --- /dev/null +++ b/transport_drivers @@ -0,0 +1 @@ +Subproject commit abf8aa8e171f33e0acd6d845c3d795192c964e9c