diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 2e4be4f14..9a17fb217 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -4,6 +4,7 @@ project(nebula_common) find_package(ament_cmake_auto REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(yaml-cpp REQUIRED) +find_package(nlohmann_json REQUIRED) # Default to C++17 if (NOT CMAKE_CXX_STANDARD) @@ -28,11 +29,13 @@ include_directories( SYSTEM ${YAML_CPP_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} + ${NLOHMANN_JSON_INCLUDE_DIRS} ) link_libraries( ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} + ${NLOHMANN_JSON_LIBRARIES} ) add_library(nebula_common SHARED diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 6f70b8ec6..950b2710d 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -349,18 +349,15 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase inline nebula::Status save_to_file_from_bytes( 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); if (!ofs) { std::cerr << "Could not create file: " << correction_file << "\n"; return Status::CANNOT_SAVE_FILE; } - std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; for (const auto & byte : buf) { if (!sop_received) { if (byte == 0xEE) { - std::cerr << "SOP received....\n"; sop_received = true; } } @@ -368,7 +365,6 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase ofs << byte; } } - std::cerr << "Closing file\n"; ofs.close(); if (sop_received) return Status::OK; return Status::INVALID_CALIBRATION_FILE; diff --git a/nebula_common/include/nebula_common/util/string_conversions.hpp b/nebula_common/include/nebula_common/util/string_conversions.hpp index df5cb2b48..917fee51d 100644 --- a/nebula_common/include/nebula_common/util/string_conversions.hpp +++ b/nebula_common/include/nebula_common/util/string_conversions.hpp @@ -14,6 +14,8 @@ #pragma once +#include + #include #include #include @@ -41,4 +43,20 @@ std::enable_if_t::value, std::string> to_string(const T & value) return ss.str(); } +template +std::string to_string(const char value[N]) +{ + return std::string(value, strnlen(value, N)); +} + +inline std::string to_string(const nlohmann::ordered_json & j) +{ + return j.is_string() ? j.template get() : j.dump(); +} + +inline std::string to_string(const nlohmann::json & j) +{ + return to_string(nlohmann::ordered_json(j)); +} + } // namespace nebula::util diff --git a/nebula_common/package.xml b/nebula_common/package.xml index 6ca9d47c9..092453ee2 100644 --- a/nebula_common/package.xml +++ b/nebula_common/package.xml @@ -13,6 +13,7 @@ ros_environment libpcl-all-dev + nlohmann-json-dev yaml-cpp ament_cmake_gtest 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 f7d3d29bb..314c40473 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 @@ -254,7 +254,6 @@ class HesaiDecoder : public HesaiScanDecoder logger_(rclcpp::get_logger("HesaiDecoder")) { logger_.set_level(rclcpp::Logger::Level::Debug); - RCLCPP_INFO_STREAM(logger_, *sensor_configuration_); decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); 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 f3bb7b073..e036ac5c3 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 @@ -15,21 +15,31 @@ #ifndef HESAI_CMD_RESPONSE_HPP #define HESAI_CMD_RESPONSE_HPP +#include +#include +#include + #include #include #include +#include #include #include #include #include +#include #include +#include +#include using namespace boost::endian; // NOLINT(build/namespaces) - namespace nebula { +using nebula::util::to_string; +using nlohmann::ordered_json; + #pragma pack(push, 1) /// @brief PTP STATUS struct of PTC_COMMAND_PTP_DIAGNOSTICS @@ -54,7 +64,7 @@ struct HesaiPtpDiagStatus /// @brief PTP TLV PORT_DATA_SET struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagPort { - uint8_t portIdentity[10]; + char portIdentity[10]; big_int32_buf_t portState; big_int32_buf_t logMinDelayReqInterval; big_int64_buf_t peerMeanPathDelay; @@ -67,7 +77,7 @@ struct HesaiPtpDiagPort friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagPort const & arg) { - os << "portIdentity: " << std::string(std::begin(arg.portIdentity), std::end(arg.portIdentity)); + os << "portIdentity: " << to_string(arg.portIdentity); os << ", "; os << "portState: " << arg.portState; os << ", "; @@ -150,75 +160,66 @@ struct HesaiPtpDiagGrandmaster }; /// @brief struct of PTC_COMMAND_GET_INVENTORY_INFO -struct HesaiInventory +struct HesaiInventoryBase { - char sn[18]; - char date_of_manufacture[16]; - uint8_t mac[6]; - char sw_ver[16]; - char hw_ver[16]; - char control_fw_ver[16]; - char sensor_fw_ver[16]; - big_uint16_buf_t angle_offset; - uint8_t model; - uint8_t motor_type; - uint8_t num_of_lines; - uint8_t reserved[11]; - - friend std::ostream & operator<<(std::ostream & os, nebula::HesaiInventory const & arg) - { - std::ios initial_format(nullptr); - initial_format.copyfmt(os); - - 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))); - os << ", "; - os << "mac: "; - - for (size_t i = 0; i < sizeof(arg.mac); i++) { - if (i != 0) { - os << ':'; + struct Internal + { + char sn[18]; + char date_of_manufacture[16]; + uint8_t mac[6]; + char sw_ver[16]; + char hw_ver[16]; + char control_fw_ver[16]; + char sensor_fw_ver[16]; + }; + + virtual ~HesaiInventoryBase() = default; + + [[nodiscard]] ordered_json to_json() const + { + const Internal & val = get(); + ordered_json j; + j["sn"] = to_string(val.sn); + j["date_of_manufacture"] = to_string(val.date_of_manufacture); + + { + std::stringstream ss; + for (size_t i = 0; i < sizeof(val.mac); i++) { + if (i != 0) { + ss << ':'; + } + ss << std::hex << std::setfill('0') << std::setw(2) << (+val.mac[i]); } - os << std::hex << std::setfill('0') << std::setw(2) << (+arg.mac[i]); + j["mac"] = ss.str(); } - os.copyfmt(initial_format); - os << ", "; - os << "sw_ver: " << std::string(arg.sw_ver, strnlen(arg.sw_ver, sizeof(arg.sw_ver))); - os << ", "; - os << "hw_ver: " << std::string(arg.hw_ver, strnlen(arg.hw_ver, sizeof(arg.hw_ver))); - os << ", "; - os << "control_fw_ver: " - << std::string(arg.control_fw_ver, strnlen(arg.control_fw_ver, sizeof(arg.control_fw_ver))); - os << ", "; - os << "sensor_fw_ver: " - << std::string(arg.sensor_fw_ver, strnlen(arg.sensor_fw_ver, sizeof(arg.sensor_fw_ver))); - os << ", "; - os << "angle_offset: " << arg.angle_offset; - os << ", "; - os << "model: " << +arg.model; - os << ", "; - os << "motor_type: " << +arg.motor_type; - os << ", "; - os << "num_of_lines: " << +arg.num_of_lines; - os << ", "; + j["sw_ver"] = to_string(val.sw_ver); + j["hw_ver"] = to_string(val.hw_ver); + j["control_fw_ver"] = to_string(val.control_fw_ver); + j["sensor_fw_ver"] = to_string(val.sensor_fw_ver); + j.update(sensor_specifics_to_json()); - for (size_t i = 0; i < sizeof(arg.reserved); i++) { - if (i != 0) { - os << ' '; - } - os << std::hex << std::setfill('0') << std::setw(2) << (+arg.reserved[i]); - } - os.copyfmt(initial_format); + return j; + } - return os; + [[nodiscard]] virtual uint8_t model_number() const = 0; + + [[nodiscard]] virtual const Internal & get() const = 0; + +protected: + [[nodiscard]] virtual ordered_json sensor_specifics_to_json() const = 0; + + friend std::ostream & operator<<(std::ostream & os, const HesaiInventoryBase & arg) + { + ordered_json j = arg.to_json(); + std::vector kv_pairs; + for (const auto & [key, value] : j.items()) { + kv_pairs.emplace_back(key + ": " + to_string(value)); + } + return os << boost::algorithm::join(kv_pairs, ", "); } - std::string get_str_model() const + [[nodiscard]] static std::string get_str_model(uint8_t model) { switch (model) { case 0: @@ -243,188 +244,374 @@ struct HesaiInventory return "PandarXT32M"; case 42: return "OT128"; + case 40: case 48: return "PandarAT128"; default: return "Unknown(" + std::to_string(static_cast(model)) + ")"; } } + + [[nodiscard]] static std::string get_motor_type(uint8_t motor_type) + { + switch (motor_type) { + case 0: + return "unidirectional"; + case 1: + return "bidirectional"; + default: + return "unknown"; + } + } }; -/// @brief struct of PTC_COMMAND_GET_CONFIG_INFO -struct HesaiConfig +struct HesaiInventory_OT128 : public HesaiInventoryBase { - uint8_t ipaddr[4]; - uint8_t mask[4]; - uint8_t gateway[4]; - uint8_t dest_ipaddr[4]; - big_uint16_buf_t dest_LiDAR_udp_port; - big_uint16_buf_t dest_gps_udp_port; - big_uint16_buf_t spin_rate; - uint8_t sync; - big_uint16_buf_t sync_angle; - big_uint16_buf_t start_angle; - big_uint16_buf_t stop_angle; - uint8_t clock_source; - uint8_t udp_seq; - uint8_t trigger_method; - uint8_t return_mode; - uint8_t standby_mode; - 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 noise_filtering; - uint8_t reflectivity_mapping; - uint8_t reserved[6]; - - friend std::ostream & operator<<(std::ostream & os, nebula::HesaiConfig const & arg) - { - std::ios initial_format(nullptr); - initial_format.copyfmt(os); - - 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] << "." - << +arg.mask[3]; - os << ", "; - 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 << ", "; - os << "dest_LiDAR_udp_port: " << arg.dest_LiDAR_udp_port; - os << ", "; - os << "dest_gps_udp_port: " << arg.dest_gps_udp_port; - os << ", "; - os << "spin_rate: " << arg.spin_rate; - os << ", "; - os << "sync: " << +arg.sync; - os << ", "; - os << "sync_angle: " << arg.sync_angle; - os << ", "; - os << "start_angle: " << arg.start_angle; - os << ", "; - os << "stop_angle: " << arg.stop_angle; - os << ", "; - os << "clock_source: " << +arg.clock_source; - os << ", "; - os << "udp_seq: " << +arg.udp_seq; - os << ", "; - os << "trigger_method: " << +arg.trigger_method; - os << ", "; - os << "return_mode: " << +arg.return_mode; - os << ", "; - os << "standby_mode: " << +arg.standby_mode; - os << ", "; - os << "motor_status: " << +arg.motor_status; - os << ", "; - os << "vlan_flag: " << +arg.vlan_flag; - os << ", "; - os << "vlan_id: " << arg.vlan_id; - os << ", "; - os << "clock_data_fmt: " << +arg.clock_data_fmt; - os << ", "; - os << "noise_filtering: " << +arg.noise_filtering; - os << ", "; - os << "reflectivity_mapping: " << +arg.reflectivity_mapping; - os << ", "; - os << "reserved: "; + struct Internal : public HesaiInventoryBase::Internal + { + big_int16_buf_t zero_angle_offset; + uint8_t product_model; + uint8_t motor_type; + uint8_t num_of_lines; + char pn[32]; + uint8_t customer_pn_enable; + char customer_pn[20]; + }; - for (size_t i = 0; i < sizeof(arg.reserved); i++) { - if (i != 0) { - os << ' '; - } - os << std::hex << std::setfill('0') << std::setw(2) << (+arg.reserved[i]); + explicit HesaiInventory_OT128(Internal value) : value(value) {} + + [[nodiscard]] uint8_t model_number() const override { return value.product_model; } + + [[nodiscard]] const HesaiInventoryBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["zero_angle_offset"] = value.zero_angle_offset.value(); + j["model"] = get_str_model(value.product_model); + j["motor_type"] = get_motor_type(value.motor_type); + j["num_of_lines"] = value.num_of_lines; + j["pn"] = to_string(value.pn); + + if (value.customer_pn_enable) { + j["customer_pn"] = to_string(value.customer_pn); } - os.copyfmt(initial_format); - return os; + return j; } + +private: + Internal value; }; -/// @brief struct of PTC_COMMAND_GET_LIDAR_STATUS -struct HesaiLidarStatus +struct HesaiInventory_XT32_40P : public HesaiInventoryBase { - big_uint32_buf_t system_uptime; - big_uint16_buf_t motor_speed; - big_int32_buf_t temperature[8]; - uint8_t gps_pps_lock; - uint8_t gps_gprmc_status; - 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 - - friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarStatus const & arg) - { - std::ios initial_format(nullptr); - initial_format.copyfmt(os); - - os << "system_uptime: " << arg.system_uptime; - os << ", "; - os << "motor_speed: " << arg.motor_speed; - os << ", "; - os << "temperature: "; + struct Internal : public HesaiInventoryBase::Internal + { + big_int16_buf_t zero_angle_offset; + uint8_t product_model; + uint8_t motor_type; + uint8_t num_of_lines; + uint8_t reserved[11]; + }; - for (size_t i = 0; i < sizeof(arg.temperature); i++) { - if (i != 0) { - os << ','; - } - os << arg.temperature[i]; - } + explicit HesaiInventory_XT32_40P(Internal value) : value(value) {} - os << ", "; - os << "gps_pps_lock: " << static_cast(arg.gps_pps_lock); - os << ", "; - os << "gps_gprmc_status: " << static_cast(arg.gps_gprmc_status); - os << ", "; - os << "startup_times: " << arg.startup_times; - os << ", "; - os << "total_operation_time: " << arg.total_operation_time; - os << ", "; - os << "ptp_clock_status: " << static_cast(arg.ptp_clock_status); - os << ", "; - os << "reserved: "; + [[nodiscard]] uint8_t model_number() const override { return value.product_model; } - for (size_t i = 0; i < sizeof(arg.reserved); i++) { - if (i != 0) { - os << ' '; - } - os << std::hex << std::setfill('0') << std::setw(2) << (static_cast(arg.reserved[i])); + [[nodiscard]] const HesaiInventoryBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["zero_angle_offset"] = value.zero_angle_offset.value(); + j["model"] = get_str_model(value.product_model); + j["motor_type"] = get_motor_type(value.motor_type); + j["num_of_lines"] = value.num_of_lines; + return j; + } + +private: + Internal value; +}; + +struct HesaiInventory_QT128 : public HesaiInventoryBase +{ + struct Internal : public HesaiInventoryBase::Internal + { + big_int16_buf_t zero_angle_offset; + big_int16_buf_t angle_offset_cc; + uint8_t product_model; + uint8_t motor_type; + uint8_t num_of_lines; + char pn[32]; + uint8_t reserved; + }; + + explicit HesaiInventory_QT128(Internal value) : value(value) {} + + [[nodiscard]] uint8_t model_number() const override { return value.product_model; } + + [[nodiscard]] const HesaiInventoryBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["zero_angle_offset"] = value.zero_angle_offset.value(); + j["zero_angle_offset_cc"] = value.angle_offset_cc.value(); + j["model"] = get_str_model(value.product_model); + j["motor_type"] = get_motor_type(value.motor_type); + j["num_of_lines"] = value.num_of_lines; + j["pn"] = to_string(value.pn); + return j; + } + +private: + Internal value; +}; + +struct HesaiInventory_AT128 : public HesaiInventoryBase +{ + struct Internal : public HesaiInventoryBase::Internal + { + // zero_angle_offset from datasheet does not exist + char fpga_para_ver[16]; + char fpga_cfg_ver[16]; + char fpga_para_sha[16]; + char fpga_cfg_sha[16]; + big_int16_buf_t unused_angle_offset; + uint8_t product_model; + uint8_t motor_type; + uint8_t num_of_lines; // this is also not in the datasheet but definitely exists + uint8_t motor_correction_flag; + uint8_t encoder_disk_correction_flag; + uint8_t reserved[9]; + }; + + explicit HesaiInventory_AT128(Internal value) : value(value) {} + + [[nodiscard]] uint8_t model_number() const override { return value.product_model; } + + [[nodiscard]] const HesaiInventoryBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["fpga_para_ver"] = to_string(value.fpga_para_ver); + j["fpga_cfg_ver"] = to_string(value.fpga_cfg_ver); + j["fpga_para_sha"] = to_string(value.fpga_para_sha); + j["fpga_cfg_sha"] = to_string(value.fpga_cfg_sha); + j["model"] = get_str_model(value.product_model); + j["motor_type"] = get_motor_type(value.motor_type); + j["num_of_lines"] = value.num_of_lines; + j["motor_correction_flag"] = value.motor_correction_flag ? "finished" : "not finished"; + j["encoder_disk_correction_flag"] = + value.encoder_disk_correction_flag ? "finished" : "not finished"; + return j; + } + +private: + Internal value; +}; + +/// @brief struct of PTC_COMMAND_GET_CONFIG_INFO +struct HesaiConfigBase +{ + struct Internal + { + uint8_t ipaddr[4]; + uint8_t mask[4]; + uint8_t gateway[4]; + uint8_t dest_ipaddr[4]; + big_uint16_buf_t dest_LiDAR_udp_port; + big_uint16_buf_t dest_gps_udp_port; + big_uint16_buf_t spin_rate; + uint8_t sync; + big_uint16_buf_t sync_angle; + big_uint16_buf_t start_angle; + big_uint16_buf_t stop_angle; + uint8_t clock_source; + uint8_t udp_seq; + uint8_t trigger_method; + uint8_t return_mode; + uint8_t standby_mode; + uint8_t motor_status; + uint8_t vlan_flag; + big_uint16_buf_t vlan_id; + }; + + virtual ~HesaiConfigBase() = default; + + [[nodiscard]] ordered_json to_json() const + { + ordered_json j; + { + std::stringstream ss; + ss << static_cast(get().ipaddr[0]) << "." << static_cast(get().ipaddr[1]) << "." + << static_cast(get().ipaddr[2]) << "." << static_cast(get().ipaddr[3]); + j["ipaddr"] = ss.str(); + } + { + std::stringstream ss; + ss << static_cast(get().mask[0]) << "." << static_cast(get().mask[1]) << "." + << static_cast(get().mask[2]) << "." << static_cast(get().mask[3]); + j["mask"] = ss.str(); + } + { + std::stringstream ss; + ss << static_cast(get().gateway[0]) << "." << static_cast(get().gateway[1]) << "." + << static_cast(get().gateway[2]) << "." << static_cast(get().gateway[3]); + j["gateway"] = ss.str(); + } + { + std::stringstream ss; + ss << static_cast(get().dest_ipaddr[0]) << "." << static_cast(get().dest_ipaddr[1]) + << "." << static_cast(get().dest_ipaddr[2]) << "." + << static_cast(get().dest_ipaddr[3]); + j["dest_ipaddr"] = ss.str(); } - os.copyfmt(initial_format); + j["dest_LiDAR_udp_port"] = get().dest_LiDAR_udp_port.value(); + j["dest_gps_udp_port"] = get().dest_gps_udp_port.value(); + j["spin_rate"] = std::to_string(get().spin_rate.value()) + " RPM"; + j["sync"] = get().sync; + j["sync_angle"] = std::to_string(get().sync * 0.01) + " deg"; + j["start_angle"] = get().start_angle.value(); + j["stop_angle"] = get().stop_angle.value(); + j["clock_source"] = get().clock_source; + j["udp_seq"] = get().udp_seq; + j["trigger_method"] = get().trigger_method; + j["return_mode"] = get().return_mode; + j["standby_mode"] = get().standby_mode; + j["motor_status"] = get().motor_status; + j["vlan_flag"] = get().vlan_flag; + j["vlan_id"] = get().vlan_id.value(); + j.update(sensor_specifics_to_json()); + + return j; + } - return os; + [[nodiscard]] virtual const Internal & get() const = 0; + +protected: + [[nodiscard]] virtual ordered_json sensor_specifics_to_json() const = 0; +}; + +inline std::ostream & operator<<(std::ostream & os, const HesaiConfigBase & arg) +{ + ordered_json j = arg.to_json(); + for (const auto & [key, value] : j.items()) { + os << key << ": " << to_string(value) << '\n'; + } + return os; +} + +struct HesaiConfig_OT128_AT128 : public HesaiConfigBase +{ + struct Internal : public HesaiConfigBase::Internal + { + uint8_t gps_nmea_sentence; + uint8_t noise_filtering; + uint8_t reflectivity_mapping; + unsigned char reserved[6]; + }; + + explicit HesaiConfig_OT128_AT128(Internal value) : value(value) {} + + [[nodiscard]] const HesaiConfigBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["gps_nmea_sentence"] = value.gps_nmea_sentence; + j["noise_filtering"] = value.noise_filtering; + j["reflectivity_mapping"] = value.reflectivity_mapping; + + return j; + } + +private: + Internal value; +}; + +struct HesaiConfig_XT_40P_64_QT128 : public HesaiConfigBase +{ + struct Internal : public HesaiConfigBase::Internal + { + uint8_t clock_data_fmt; + uint8_t noise_filtering; + uint8_t reflectivity_mapping; + unsigned char reserved[6]; + }; + + explicit HesaiConfig_XT_40P_64_QT128(Internal value) : value(value) {} + + [[nodiscard]] const HesaiConfigBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["clock_data_fmt"] = value.clock_data_fmt; + j["noise_filtering"] = value.noise_filtering; + j["reflectivity_mapping"] = value.reflectivity_mapping; + + return j; } - [[nodiscard]] std::string get_str_gps_pps_lock() const +private: + Internal value; +}; + +/// @brief struct of PTC_COMMAND_GET_LIDAR_STATUS +struct HesaiLidarStatusBase +{ + struct Internal { - switch (gps_pps_lock) { + big_uint32_buf_t system_uptime; + big_uint16_buf_t motor_speed; + }; + + virtual ~HesaiLidarStatusBase() = default; + + [[nodiscard]] ordered_json to_json() const + { + ordered_json j; + j["system_uptime"] = std::to_string(get().system_uptime.value()) + " s"; + j["motor_speed"] = std::to_string(get().motor_speed.value()) + " RPM"; + j.update(sensor_specifics_to_json()); + + return j; + } + + [[nodiscard]] virtual const Internal & get() const = 0; + +protected: + [[nodiscard]] virtual ordered_json sensor_specifics_to_json() const = 0; + + [[nodiscard]] static std::string get_str_gps_pps_lock(uint8_t value) + { + switch (value) { case 1: - return "Lock"; + return "locked"; case 0: - return "Unlock"; + return "not locked"; default: - return "Unknown"; + return "unknown"; } } - [[nodiscard]] std::string get_str_gps_gprmc_status() const + [[nodiscard]] static std::string get_str_gps_gprmc_status(uint8_t value) { - switch (gps_gprmc_status) { + switch (value) { case 1: - return "Lock"; + return "locked"; case 0: - return "Unlock"; + return "not locked"; default: - return "Unknown"; + return "unknown"; } } - [[nodiscard]] std::string get_str_ptp_clock_status() const + [[nodiscard]] static std::string get_str_ptp_clock_status(uint8_t value) { - switch (ptp_clock_status) { + switch (value) { case 0: return "free run"; case 1: @@ -434,9 +621,196 @@ struct HesaiLidarStatus case 3: return "frozen"; default: - return "Unknown"; + return "unknown"; + } + } +}; + +inline std::ostream & operator<<(std::ostream & os, const HesaiLidarStatusBase & arg) +{ + ordered_json j = arg.to_json(); + for (const auto & [key, value] : j.items()) { + os << key << ": " << to_string(value) << '\n'; + } + return os; +} + +struct HesaiLidarStatus_AT128_QT128 : public HesaiLidarStatusBase +{ + struct Internal : public HesaiLidarStatusBase::Internal + { + big_int32_buf_t temperature[9]; + uint8_t unused_gps_pps_lock; + uint8_t unused_gps_gprmc_status; + big_int32_buf_t startup_times; + big_int32_buf_t total_operation_time; + uint8_t ptp_status; + unsigned char reserved[5]; + }; + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + ordered_json temperature; + const auto temperature_names = get_temperature_names(); + for (size_t i = 0; i < temperature_names.size(); ++i) { + temperature[temperature_names[i]] = + std::to_string(value.temperature[i].value() * 0.01) + " deg"; } + j["temperature"] = temperature; + j["startup_times"] = value.startup_times.value(); + j["total_operation_time"] = std::to_string(value.total_operation_time.value()) + " min"; + j["ptp_status"] = get_str_ptp_clock_status(value.ptp_status); + + return j; + } + [[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override { return value; } + +protected: + explicit HesaiLidarStatus_AT128_QT128(Internal value) : value(value) {} + + [[nodiscard]] virtual std::array get_temperature_names() const = 0; + +private: + Internal value; +}; + +struct HesaiLidarStatusAT128 : public HesaiLidarStatus_AT128_QT128 +{ + explicit HesaiLidarStatusAT128(Internal value) : HesaiLidarStatus_AT128_QT128(value) {} + +protected: + [[nodiscard]] std::array get_temperature_names() const override + { + return { + "tx1 temperature", "tx2 temperature", "fpga temperature", + "rx1 temperature", "rx2 temperature", "mb1 temperature", + "mb2 temperature", "pb temperature", "hot temperature", + }; + } +}; + +struct HesaiLidarStatusQT128 : public HesaiLidarStatus_AT128_QT128 +{ + explicit HesaiLidarStatusQT128(Internal value) : HesaiLidarStatus_AT128_QT128(value) {} + +protected: + [[nodiscard]] std::array get_temperature_names() const override + { + return { + "Bottom circuit board T1 temperature", + "Bottom circuit board T2 temperature", + "Internal temperature", + "Laser emitting board RT1 temperature", + "Laser emitting board RT2 temperature", + "Receiving board RT1 temperature", + "Receiving board RT2 temperature", + "Top circuit RT1 temperature", + "Top circuit RT2 temperature", + }; + } +}; + +struct HesaiLidarStatusOT128 : public HesaiLidarStatusBase +{ + struct Internal : public HesaiLidarStatusBase::Internal + { + big_int32_buf_t temperature[8]; + uint8_t gps_pps_lock; + uint8_t gps_gprmc_status; + big_int32_buf_t startup_times; + big_int32_buf_t total_operation_time; + uint8_t ptp_status; + big_uint32_buf_t humidity; + unsigned char reserved[1]; + }; + explicit HesaiLidarStatusOT128(Internal value) : value(value) {} + + [[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + ordered_json temperature; + temperature["Bottom circuit board T1 temperature"] = + std::to_string(value.temperature[0].value() * 0.01) + " deg"; + temperature["Bottom circuit board T2 temperature"] = + std::to_string(value.temperature[1].value() * 0.01) + " deg"; + temperature["Laser emitting board RT_L1 temperature"] = + std::to_string(value.temperature[2].value() * 0.01) + " deg"; + temperature["Laser emitting board RT_L2 temperature"] = + std::to_string(value.temperature[3].value() * 0.01) + " deg"; + temperature["Laser Receiving board RT_R temperature"] = + std::to_string(value.temperature[4].value() * 0.01) + " deg"; + temperature["Laser Receiving board RT2 temperature"] = + std::to_string(value.temperature[5].value() * 0.01) + " deg"; + temperature["Top circuit RT3 temperature"] = + std::to_string(value.temperature[6].value() * 0.01) + " deg"; + temperature["Top circuit RT4 temperature"] = + std::to_string(value.temperature[7].value() * 0.01) + " deg"; + j["temperature"] = temperature; + j["gps_pps_lock"] = get_str_gps_pps_lock(value.gps_pps_lock); + j["gps_gprmc_status"] = get_str_gps_gprmc_status(value.gps_gprmc_status); + j["startup_times"] = value.startup_times.value(); + j["total_operation_time"] = std::to_string(value.total_operation_time.value()) + " min"; + j["ptp_status"] = get_str_ptp_clock_status(value.ptp_status); + j["humidity"] = std::to_string(value.humidity.value() * 0.1) + " %"; + + return j; } + +private: + Internal value; +}; + +struct HesaiLidarStatus_XT_40p : public HesaiLidarStatusBase +{ + struct Internal : public HesaiLidarStatusBase::Internal + { + big_int32_buf_t temperature[8]; + uint8_t gps_pps_lock; + uint8_t gps_gprmc_status; + big_int32_buf_t startup_times; + big_int32_buf_t total_operation_time; + uint8_t ptp_status; + unsigned char reserved[5]; + }; + explicit HesaiLidarStatus_XT_40p(Internal value) : value(value) {} + + [[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override { return value; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + ordered_json temperature; + temperature["Bottom circuit board T1 temperature"] = + std::to_string(value.temperature[0].value() * 0.01) + " deg"; + temperature["Bottom circuit board T2 temperature"] = + std::to_string(value.temperature[1].value() * 0.01) + " deg"; + temperature["Laser emitting board RT_L temperature"] = + std::to_string(value.temperature[2].value() * 0.01) + " deg"; + temperature["Laser emitting board RT_R temperature"] = + std::to_string(value.temperature[3].value() * 0.01) + " deg"; + temperature["Laser Receiving board RT2 temperature"] = + std::to_string(value.temperature[4].value() * 0.01) + " deg"; + temperature["Top circult RT3 temperature"] = + std::to_string(value.temperature[5].value() * 0.01) + " deg"; + temperature["Top circuit RT4 temperature"] = + std::to_string(value.temperature[6].value() * 0.01) + " deg"; + temperature["Top circuit RT5 temperature"] = + std::to_string(value.temperature[7].value() * 0.01) + " deg"; + j["temperature"] = temperature; + j["gps_pps_lock"] = get_str_gps_pps_lock(value.gps_pps_lock); + j["gps_gprmc_status"] = get_str_gps_gprmc_status(value.gps_gprmc_status); + j["startup_times"] = value.startup_times.value(); + j["total_operation_time"] = std::to_string(value.total_operation_time.value()) + " min"; + j["ptp_status"] = get_str_ptp_clock_status(value.ptp_status); + + return j; + } + +private: + Internal value; }; /// @brief struct of PTC_COMMAND_GET_LIDAR_RANGE @@ -494,33 +868,33 @@ struct HesaiPtpConfig /// @brief struct of PTC_COMMAND_LIDAR_MONITOR struct HesaiLidarMonitor { - // 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_voltage; big_int32_buf_t input_power; - uint8_t reserved[52]; + big_int32_buf_t phase_offset; + uint8_t reserved[48]; - friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarMonitor const & arg) + [[nodiscard]] ordered_json to_json() const { - os << "input_voltage: " << arg.input_voltage; - os << ", "; - os << "input_current: " << arg.input_current; - os << ", "; - os << "input_power: " << arg.input_power; - os << ", "; - os << "reserved: "; - - for (size_t i = 0; i < sizeof(arg.reserved); i++) { - if (i != 0) { - os << ' '; - } - os << std::hex << std::setfill('0') << std::setw(2) << (static_cast(arg.reserved[i])); - } + ordered_json j; + j["input_current"] = std::to_string(input_current.value() * 0.01) + " mA"; + j["input_voltage"] = std::to_string(input_voltage.value() * 0.01) + " V"; + j["input_power"] = std::to_string(input_power.value() * 0.01) + " W"; + j["phase_offset"] = std::to_string(phase_offset.value() * 0.01) + " deg"; - return os; + return j; } }; +inline std::ostream & operator<<(std::ostream & os, const HesaiLidarMonitor & arg) +{ + ordered_json j = arg.to_json(); + for (const auto & [key, value] : j.items()) { + os << key << ": " << to_string(value) << '\n'; + } + return os; +} + #pragma pack(pop) } // namespace nebula diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index e3031fcf4..d37a33e02 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 @@ -17,6 +17,8 @@ // Have to define macros to silence warnings about deprecated headers being used by // boost/property_tree/ in some versions of boost. // See: https://github.com/boostorg/property_tree/issues/51 +#include "nebula_common/nebula_status.hpp" + #include #include @@ -43,6 +45,7 @@ #include #include #include +#include #include namespace nebula::drivers @@ -195,6 +198,8 @@ class HesaiHwInterface /// @return The returned payload, if successful, or nullptr. ptc_cmd_result_t SendReceive(const uint8_t command_id, const std::vector & payload = {}); + static std::pair unwrap_http_response(const std::string & response); + public: /// @brief Constructor HesaiHwInterface(); @@ -258,13 +263,13 @@ class HesaiHwInterface HesaiPtpDiagGrandmaster GetPtpDiagGrandmaster(); /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO /// @return Resulting status - HesaiInventory GetInventory(); + std::shared_ptr GetInventory(); /// @brief Getting data with PTC_COMMAND_GET_CONFIG_INFO /// @return Resulting status - HesaiConfig GetConfig(); + std::shared_ptr GetConfig(); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS /// @return Resulting status - HesaiLidarStatus GetLidarStatus(); + std::shared_ptr GetLidarStatus(); /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE /// @param rpm Spin rate /// @return Resulting status @@ -413,7 +418,8 @@ class HesaiHwInterface /// @param hesai_config Current HesaiConfig /// @return Resulting status HesaiStatus CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config); + std::shared_ptr sensor_configuration, + std::shared_ptr hesai_config); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param hesai_lidar_range_all Current HesaiLidarRangeAll 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 432cb3de4..0a08dd93e 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 @@ -3,10 +3,17 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_common/hesai/hesai_status.hpp" +#include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" + +#include +#include #include +#include #include #include #include @@ -22,6 +29,10 @@ namespace nebula::drivers { + +using std::string_literals::operator""s; +using nlohmann::json; + HesaiHwInterface::HesaiHwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx{new boost::asio::io_context(1)}, @@ -149,7 +160,7 @@ Status HesaiHwInterface::SetSensorConfiguration( Status HesaiHwInterface::SensorInterfaceStart() { try { - std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl; + PrintInfo("Starting UDP receiver"); if (sensor_configuration_->multicast_ip.empty()) { cloud_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->data_port); @@ -298,11 +309,6 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); auto diag_status = CheckSizeAndParse(response); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagStatus: " << diag_status; - PrintInfo(ss.str()); - return diag_status; } @@ -311,11 +317,6 @@ HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); auto diag_port = CheckSizeAndParse(response); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagPort: " << diag_port; - PrintInfo(ss.str()); - return diag_port; } @@ -324,11 +325,6 @@ HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); auto diag_time = CheckSizeAndParse(response); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagTime: " << diag_time; - PrintInfo(ss.str()); - return diag_time; } @@ -338,35 +334,84 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); auto diag_grandmaster = CheckSizeAndParse(response); - - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << diag_grandmaster; - PrintInfo(ss.str()); - return diag_grandmaster; } -HesaiInventory HesaiHwInterface::GetInventory() +std::shared_ptr HesaiHwInterface::GetInventory() { auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - return CheckSizeAndParse(response); + + switch (sensor_configuration_->sensor_model) { + default: + case SensorModel::HESAI_PANDARXT32: + case SensorModel::HESAI_PANDAR40P: { + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); + } + case SensorModel::HESAI_PANDARQT128: { + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); + } + case SensorModel::HESAI_PANDARAT128: { + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); + } + case SensorModel::HESAI_PANDAR128_E4X: { + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); + } + } } -HesaiConfig HesaiHwInterface::GetConfig() +std::shared_ptr HesaiHwInterface::GetConfig() { auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - auto hesai_config = CheckSizeAndParse(response); - std::cout << "Config: " << hesai_config << std::endl; - return hesai_config; + + switch (sensor_configuration_->sensor_model) { + default: + case SensorModel::HESAI_PANDAR40P: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDARQT128: + case SensorModel::HESAI_PANDARXT32: { + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); + } + case SensorModel::HESAI_PANDAR128_E4X: + case SensorModel::HESAI_PANDARAT128: { + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); + } + } } -HesaiLidarStatus HesaiHwInterface::GetLidarStatus() +std::shared_ptr HesaiHwInterface::GetLidarStatus() { auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - return CheckSizeAndParse(response); + + switch (sensor_configuration_->sensor_model) { + default: + case SensorModel::HESAI_PANDAR40P: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDARXT32: { + auto hesai_lidarstatus = CheckSizeAndParse(response); + return std::make_shared(hesai_lidarstatus); + } + case SensorModel::HESAI_PANDAR128_E4X: { + auto hesai_lidarstatus = CheckSizeAndParse(response); + return std::make_shared(hesai_lidarstatus); + } + case SensorModel::HESAI_PANDARAT128: { + auto hesai_lidarstatus = CheckSizeAndParse(response); + return std::make_shared(hesai_lidarstatus); + } + case SensorModel::HESAI_PANDARQT128: { + auto hesai_lidarstatus = CheckSizeAndParse(response); + return std::make_shared(hesai_lidarstatus); + } + } } Status HesaiHwInterface::SetSpinRate(uint16_t rpm) @@ -490,9 +535,12 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector da Status HesaiHwInterface::SetLidarRange(int start_ddeg, int end_ddeg) { - if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { return Status::SENSOR_CONFIG_ERROR; } + // 0 - for all channels : 5-1 bytes // 1 - for each channel : 323-1 bytes // 2 - multi-section FOV : 1347-1 bytes @@ -511,9 +559,12 @@ Status HesaiHwInterface::SetLidarRange(int start_ddeg, int end_ddeg) HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() { - if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { throw std::runtime_error("Not supported on this sensor"); } + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); @@ -630,10 +681,6 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() size_t bytes_to_parse = (hesai_ptp_config.status == 0) ? sizeof(HesaiPtpConfig) : 4; memcpy(&hesai_ptp_config, response.data(), bytes_to_parse); - std::stringstream ss; - ss << "HesaiHwInterface::GetPtpConfig: " << hesai_ptp_config; - PrintInfo(ss.str()); - return hesai_ptp_config; } @@ -661,12 +708,6 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() } auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); - - // FIXME(mojomex): this is a hotfix for sensors that do not support this command - if (!response_or_err.has_value()) { - return HesaiLidarMonitor{}; - } - auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return CheckSizeAndParse(response); } @@ -714,6 +755,29 @@ void HesaiHwInterface::str_cb(const std::string & str) PrintInfo(str); } +std::pair HesaiHwInterface::unwrap_http_response( + const std::string & response) +{ + json j; + try { + j = json::parse(response); + } catch (const json::parse_error & e) { + return {Status::ERROR_1, "JSON response malformed: "s + e.what()}; + } + + if (!j.contains("Head") || !j["Head"].contains("ErrorCode") || !j["Head"].contains("Message")) { + return {Status::ERROR_1, "Unexpected JSON structure"}; + } + + json error_code = j["Head"]["ErrorCode"]; + json message = j["Head"]["Message"]; + if (error_code == "0") { + return {Status::OK, message}; + } + + return {Status::ERROR_1, message}; +} + HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( std::shared_ptr ctx, uint16_t rpm) { @@ -774,8 +838,7 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( logMinDelayReqInterval % 0) .str()); ctx->run(); - PrintInfo(response); - return Status::OK; + return unwrap_http_response(response).first; } HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( @@ -801,11 +864,9 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( "}") % enable % angle) .str(); - PrintInfo(tmp_str); auto response = hcd->get(tmp_str); ctx->run(); - PrintInfo(response); - return Status::OK; + return unwrap_http_response(response).first; } HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) @@ -842,12 +903,14 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( } HesaiStatus HesaiHwInterface::CheckAndSetConfig( - std::shared_ptr sensor_configuration, HesaiConfig hesai_config) + std::shared_ptr sensor_configuration, + std::shared_ptr hesai_config_ptr) { using namespace std::chrono_literals; // NOLINT(build/namespaces) #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif + const auto hesai_config = hesai_config_ptr->get(); auto current_return_mode = nebula::drivers::return_mode_from_int_hesai( hesai_config.return_mode, sensor_configuration->sensor_model); // Avoids spamming the sensor, which leads to failure when configuring it. @@ -1078,24 +1141,19 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() #endif std::thread t([this] { auto result = GetConfig(); - - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); CheckAndSetConfig( std::static_pointer_cast(sensor_configuration_), result); }); t.join(); - if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { return Status::OK; } std::thread t2([this] { auto result = GetLidarRange(); - std::stringstream ss; - ss << result; - PrintInfo(ss.str()); CheckAndSetConfig( std::static_pointer_cast(sensor_configuration_), result); }); @@ -1369,8 +1427,12 @@ 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."); + } + + if (data.size() > sizeof(T)) { + RCLCPP_WARN_ONCE( + *parent_node_logger, + "Sensor returned longer payload than expected. Truncating and parsing anyway."); } T parsed; diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml index 5e78f796a..f731049fa 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -18,6 +18,7 @@ sync_angle: 0 cut_angle: 0.0 sensor_model: Pandar64 + calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv rotation_speed: 600 return_mode: Dual ptp_profile: 1588v2 diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp index 5ae1370c4..5872f877d 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,9 @@ namespace nebula::ros { + +using nlohmann::json; + class HesaiHwMonitorWrapper { public: @@ -46,7 +50,11 @@ class HesaiHwMonitorWrapper nebula::Status status(); private: - void initialize_hesai_diagnostics(); + static void add_json_item_to_diagnostics( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics, const std::string & key, + const json & value); + + void initialize_hesai_diagnostics(bool monitor_enabled); std::string get_ptree_value(boost::property_tree::ptree * pt, const std::string & key); @@ -81,15 +89,13 @@ class HesaiHwMonitorWrapper 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::shared_ptr current_status_{}; + std::shared_ptr current_monitor_{}; + std::shared_ptr current_config_{}; + std::shared_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_; @@ -98,13 +104,6 @@ class HesaiHwMonitorWrapper std::mutex mtx_lidar_status_; std::mutex mtx_lidar_monitor_; - std::string info_model_; - std::string info_serial_; - - std::vector temperature_names_; - - bool supports_monitor_; - const std::string MSG_NOT_SUPPORTED_ = "Not supported"; const std::string MSG_ERROR_ = "Error"; const std::string MSG_SEP_ = ": "; diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index 666f1253d..d05d001df 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -60,6 +60,9 @@ "sensor_model": { "$ref": "sub/lidar_hesai.json#/definitions/sensor_model" }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, "rotation_speed": { "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" }, @@ -108,6 +111,7 @@ "sync_angle", "cut_angle", "sensor_model", + "calibration_file", "rotation_speed", "return_mode", "ptp_profile", diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 77267c4a6..73869d1fd 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -65,9 +65,11 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); } - if ( - hw_interface_wrapper_ && !use_udp_only && - sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { + bool lidar_range_supported = + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128 && + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDAR64; + + if (hw_interface_wrapper_ && !use_udp_only && lidar_range_supported) { auto status = hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*calibration_result.value()); if (status != Status::OK) { diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index bc746c523..b5f991090 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -50,8 +50,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( if (status_ == Status::OK) { try { auto inventory = hw_interface_->GetInventory(); - RCLCPP_INFO_STREAM(logger_, inventory); - hw_interface_->SetTargetModel(inventory.model); + hw_interface_->SetTargetModel(inventory->model_number()); } catch (...) { RCLCPP_ERROR_STREAM(logger_, "Failed to get model from sensor..."); } diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 7f5fa8f0c..de65bc23b 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -2,12 +2,37 @@ #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include +#include + +#include + +#include +#include namespace nebula::ros { + +using nlohmann::json; + +void HesaiHwMonitorWrapper::add_json_item_to_diagnostics( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics, const std::string & key, + const json & value) +{ + if (key.find("reserved") != std::string::npos) return; + + switch (value.type()) { + case nlohmann::detail::value_t::string: + diagnostics.add(key, value.template get()); + break; + default: + diagnostics.add(key, value.dump()); + } +} + HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( rclcpp::Node * const parent_node, const std::shared_ptr & hw_interface, @@ -20,61 +45,27 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( { diag_span_ = parent_node->declare_parameter("diag_span", 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; - } + bool monitor_enabled = config->sensor_model != drivers::SensorModel::HESAI_PANDARAT128 && + config->sensor_model != drivers::SensorModel::HESAI_PANDAR40P && + config->sensor_model != drivers::SensorModel::HESAI_PANDAR64; + + std::shared_ptr inventory = hw_interface->GetInventory(); + RCLCPP_INFO_STREAM(logger_, "Inventory info: " << *inventory); + json inventory_json = inventory->to_json(); + + std::string model = inventory_json.at("model"); + std::string serial = inventory_json.at("sn"); + auto hardware_id = model + ": " + serial; + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "Hardware ID: " + hardware_id); - supports_monitor_ = config->sensor_model != drivers::SensorModel::HESAI_PANDARAT128; - - auto result = hw_interface->GetInventory(); - current_inventory_.reset(new HesaiInventory(result)); - current_inventory_time_.reset(new rclcpp::Time(parent_node->get_clock()->now())); - 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_); - initialize_hesai_diagnostics(); + initialize_hesai_diagnostics(monitor_enabled); } -void HesaiHwMonitorWrapper::initialize_hesai_diagnostics() +void HesaiHwMonitorWrapper::initialize_hesai_diagnostics(bool monitor_enabled) { - RCLCPP_INFO_STREAM(logger_, "initialize_hesai_diagnostics"); 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::hesai_check_status); diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::hesai_check_ptp); diagnostics_updater_.add( @@ -82,16 +73,17 @@ void HesaiHwMonitorWrapper::initialize_hesai_diagnostics() diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::hesai_check_rpm); 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_status_time_ = std::make_unique(parent_node_->get_clock()->now()); current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; + + current_monitor_.reset(); + current_lidar_monitor_time_ = std::make_unique(parent_node_->get_clock()->now()); current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; - auto fetch_diag_from_sensor = [this]() { + auto fetch_diag_from_sensor = [this, monitor_enabled]() { on_hesai_status_timer(); - if (!supports_monitor_) return; + if (!monitor_enabled) return; if (hw_interface_->UseHttpGetLidarMonitor()) { on_hesai_lidar_monitor_timer_http(); @@ -103,14 +95,16 @@ void HesaiHwMonitorWrapper::initialize_hesai_diagnostics() 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::hesai_check_voltage_http); - } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::hesai_check_voltage); + if (monitor_enabled) { + if (hw_interface_->UseHttpGetLidarMonitor()) { + diagnostics_updater_.add( + "hesai_voltage", this, &HesaiHwMonitorWrapper::hesai_check_voltage_http); + } else { + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::hesai_check_voltage); + } } - auto on_timer_update = [this] { + auto on_timer_update = [this, monitor_enabled] { RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); auto now = parent_node_->get_clock()->now(); auto dif = (now - *current_status_time_).seconds(); @@ -124,6 +118,9 @@ void HesaiHwMonitorWrapper::initialize_hesai_diagnostics() current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; RCLCPP_DEBUG_STREAM(logger_, "OK"); } + + if (!monitor_enabled) return; + dif = (now - *current_lidar_monitor_time_).seconds(); RCLCPP_DEBUG_STREAM(logger_, "dif(monitor): " << dif); if (diag_span_ * 2.0 < dif * 1000) { @@ -164,8 +161,8 @@ void HesaiHwMonitorWrapper::on_hesai_status_timer() 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)); + current_status_time_ = std::make_unique(parent_node_->get_clock()->now()); + current_status_ = result; } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("HesaiHwMonitorWrapper::on_hesai_status_timer(std::system_error)"), @@ -185,7 +182,8 @@ void HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer_http() 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_time_ = + std::make_unique(parent_node_->get_clock()->now()); current_lidar_monitor_tree_ = std::make_unique(hw_interface_->ParseJson(str)); }); @@ -210,8 +208,8 @@ void HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer() 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)); + current_lidar_monitor_time_ = std::make_unique(parent_node_->get_clock()->now()); + current_monitor_ = std::make_shared(result); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer(std::system_error)"), @@ -231,15 +229,17 @@ void HesaiHwMonitorWrapper::hesai_check_status( { 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, ", ")); + json data = current_status_->to_json(); + for (const auto & [key, value] : data.items()) { + if ( + key == "motor_speed" || key == "temperature" || + (key.find("ptp") != std::string::npos || key.find("gps") != std::string::npos)) { + continue; + } + + add_json_item_to_diagnostics(diagnostics, key, value); + } + diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, ""); } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } @@ -248,32 +248,27 @@ void HesaiHwMonitorWrapper::hesai_check_status( void HesaiHwMonitorWrapper::hesai_check_ptp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + std::string msg = "not synchronized"; 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; + json data = current_status_->to_json(); + for (const auto & [key, value] : data.items()) { + if (key.find("ptp") == std::string::npos && key.find("gps") == std::string::npos) { + continue; + } + + if (value.type() == json::value_t::string) { + auto str = value.template get(); + if (str == "locked") { + level = diagnostic_msgs::msg::DiagnosticStatus::OK; + msg = "synchronized"; + } + } + + add_json_item_to_diagnostics(diagnostics, key, value); } - diagnostics.summary(level, boost::algorithm::join(msg, ", ")); + diagnostics.summary(level, msg); } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); } @@ -282,14 +277,15 @@ void HesaiHwMonitorWrapper::hesai_check_ptp( void HesaiHwMonitorWrapper::hesai_check_temperature( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::vector msg; 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], - get_fixed_precision_string(current_status_->temperature[i].value() * 0.01, 3)); + json data = current_status_->to_json(); + if (data.contains("temperature")) { + for (const auto & [key, value] : data["temperature"].items()) { + add_json_item_to_diagnostics(diagnostics, key, value); + } } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -304,8 +300,10 @@ void HesaiHwMonitorWrapper::hesai_check_rpm( 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())); - + json data = current_status_->to_json(); + if (data.contains("motor_speed")) { + add_json_item_to_diagnostics(diagnostics, "motor_speed", data["motor_speed"]); + } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available"); @@ -329,7 +327,7 @@ void HesaiHwMonitorWrapper::hesai_check_voltage_http( level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; mes = MSG_ERROR_ + std::string(ex.what()); } - diagnostics.add(key, mes); + add_json_item_to_diagnostics(diagnostics, key, mes); key = "lidarInVol"; try { mes = get_ptree_value(current_lidar_monitor_tree_.get(), "Body." + key); @@ -337,7 +335,7 @@ void HesaiHwMonitorWrapper::hesai_check_voltage_http( level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; mes = MSG_ERROR_ + std::string(ex.what()); } - diagnostics.add(key, mes); + add_json_item_to_diagnostics(diagnostics, key, mes); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -352,17 +350,10 @@ void HesaiHwMonitorWrapper::hesai_check_voltage( if (current_monitor_) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - diagnostics.add( - "input_voltage", - get_fixed_precision_string(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); - diagnostics.add( - "input_current", - get_fixed_precision_string(current_monitor_->input_current.value() * 0.01, 3) + - " m" - "A"); - diagnostics.add( - "input_power", - get_fixed_precision_string(current_monitor_->input_power.value() * 0.01, 3) + " W"); + json data = current_monitor_->to_json(); + for (const auto & [key, value] : data.items()) { + add_json_item_to_diagnostics(diagnostics, key, value); + } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else {