From 5cbbe6b5bd9fbac71d1445222896bac574cc956e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 29 Oct 2024 22:41:11 +0900 Subject: [PATCH] chore(hesai): lessen log spam, error messages Signed-off-by: Max SCHMELLER --- .../nebula_common/hesai/hesai_common.hpp | 4 - .../nebula_common/util/string_conversions.hpp | 19 + .../hesai_cmd_response.hpp | 432 +++++++++++------- .../hesai_hw_interface.hpp | 9 +- .../hesai_hw_interface.cpp | 76 ++- .../nebula_ros/hesai/hw_monitor_wrapper.hpp | 9 +- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 2 +- nebula_ros/src/hesai/hw_monitor_wrapper.cpp | 60 +-- 8 files changed, 379 insertions(+), 232 deletions(-) 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..55d74b3fc 100644 --- a/nebula_common/include/nebula_common/util/string_conversions.hpp +++ b/nebula_common/include/nebula_common/util/string_conversions.hpp @@ -14,6 +14,9 @@ #pragma once +#include +#include + #include #include #include @@ -41,4 +44,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_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 9c2b737f5..a2c97bcd0 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,23 +15,28 @@ #ifndef HESAI_CMD_RESPONSE_HPP #define HESAI_CMD_RESPONSE_HPP +#include #include #include #include #include +#include #include #include #include #include #include #include +#include using namespace boost::endian; // NOLINT(build/namespaces) namespace nebula { -using nlohmann::json; + +using nebula::util::to_string; +using nlohmann::ordered_json; #pragma pack(push, 1) @@ -57,7 +62,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; @@ -70,7 +75,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 << ", "; @@ -153,75 +158,64 @@ 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: "; + 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]; + }; + + [[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); - for (size_t i = 0; i < sizeof(arg.mac); i++) { - if (i != 0) { - os << ':'; + { + 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]] std::string get_str_model(uint8_t model) const { switch (model) { case 0: @@ -246,12 +240,174 @@ struct HesaiInventory return "PandarXT32M"; case 42: return "OT128"; + case 40: case 48: return "PandarAT128"; default: return "Unknown(" + std::to_string(static_cast(model)) + ")"; } } + + [[nodiscard]] std::string get_motor_type(uint8_t motor_type) const + { + switch (motor_type) { + case 0: + return "unidirectional"; + case 1: + return "bidirectional"; + default: + return "unknown"; + } + } +}; + +struct HesaiInventory_OT128 : public HesaiInventoryBase +{ + 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]; + }; + + 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); + } + + return j; + } + +private: + Internal value; +}; + +struct HesaiInventory_XT32_40P : public HesaiInventoryBase +{ + 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]; + }; + + explicit HesaiInventory_XT32_40P(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; + 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 @@ -280,9 +436,11 @@ struct HesaiConfigBase big_uint16_buf_t vlan_id; }; - json to_json() + virtual ~HesaiConfigBase() = default; + + [[nodiscard]] ordered_json to_json() const { - json j; + ordered_json j; { std::stringstream ss; ss << static_cast(get().ipaddr[0]) << "." << static_cast(get().ipaddr[1]) << "." @@ -331,13 +489,14 @@ struct HesaiConfigBase [[nodiscard]] virtual const Internal & get() const = 0; protected: - virtual json sensor_specifics_to_json() = 0; + [[nodiscard]] virtual ordered_json sensor_specifics_to_json() const = 0; }; -inline std::ostream & operator<<(std::ostream & os, HesaiConfigBase & arg) +inline std::ostream & operator<<(std::ostream & os, const HesaiConfigBase & arg) { - for (auto & [key, value] : arg.to_json().items()) { - os << key << " : " << value << std::endl; + ordered_json j = arg.to_json(); + for (const auto & [key, value] : j.items()) { + os << key << ": " << to_string(value) << '\n'; } return os; } @@ -356,9 +515,9 @@ struct HesaiConfig_OT128_AT128 : public HesaiConfigBase [[nodiscard]] const HesaiConfigBase::Internal & get() const override { return value; } - json sensor_specifics_to_json() override + [[nodiscard]] ordered_json sensor_specifics_to_json() const override { - json j; + ordered_json j; j["gps_nmea_sentence"] = value.gps_nmea_sentence; j["noise_filtering"] = value.noise_filtering; j["reflectivity_mapping"] = value.reflectivity_mapping; @@ -370,14 +529,6 @@ struct HesaiConfig_OT128_AT128 : public HesaiConfigBase Internal value; }; -inline std::ostream & operator<<(std::ostream & os, HesaiConfig_OT128_AT128 & arg) -{ - for (auto & [key, value] : arg.to_json().items()) { - os << key << " : " << value << std::endl; - } - return os; -} - struct HesaiConfig_XT_40p : public HesaiConfigBase { struct Internal : public HesaiConfigBase::Internal @@ -392,9 +543,9 @@ struct HesaiConfig_XT_40p : public HesaiConfigBase [[nodiscard]] const HesaiConfigBase::Internal & get() const override { return value; } - json sensor_specifics_to_json() override + [[nodiscard]] ordered_json sensor_specifics_to_json() const override { - json j; + ordered_json j; j["clock_data_fmt"] = value.clock_data_fmt; j["noise_filtering"] = value.noise_filtering; j["reflectivity_mapping"] = value.reflectivity_mapping; @@ -406,14 +557,6 @@ struct HesaiConfig_XT_40p : public HesaiConfigBase Internal value; }; -inline std::ostream & operator<<(std::ostream & os, HesaiConfig_XT_40p & arg) -{ - for (auto & [key, value] : arg.to_json().items()) { - os << key << " : " << value << std::endl; - } - return os; -} - /// @brief struct of PTC_COMMAND_GET_LIDAR_STATUS struct HesaiLidarStatusBase { @@ -423,9 +566,11 @@ struct HesaiLidarStatusBase big_uint16_buf_t motor_speed; }; - json to_json() + virtual ~HesaiLidarStatusBase() = default; + + [[nodiscard]] ordered_json to_json() const { - json j; + 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()); @@ -436,28 +581,28 @@ struct HesaiLidarStatusBase [[nodiscard]] virtual const Internal & get() const = 0; protected: - virtual json sensor_specifics_to_json() = 0; + [[nodiscard]] virtual ordered_json sensor_specifics_to_json() const = 0; [[nodiscard]] std::string get_str_gps_pps_lock(uint8_t value) const { 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(uint8_t value) const { 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(uint8_t value) const @@ -472,16 +617,17 @@ struct HesaiLidarStatusBase case 3: return "frozen"; default: - return "Unknown"; + return "unknown"; } } }; -inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatusBase const & arg) +inline std::ostream & operator<<(std::ostream & os, const HesaiLidarStatusBase & arg) { - os << "system_uptime: " << arg.get().system_uptime << "\n"; - os << "motor_speed: " << arg.get().motor_speed; - + ordered_json j = arg.to_json(); + for (const auto & [key, value] : j.items()) { + os << key << ": " << to_string(value) << '\n'; + } return os; } @@ -490,20 +636,20 @@ struct HesaiLidarStatusAT128 : public HesaiLidarStatusBase struct Internal : public HesaiLidarStatusBase::Internal { big_int32_buf_t temperature[9]; - uint8_t gps_pps_lock; - uint8_t gps_gprmc_status; + 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[1]; + unsigned char reserved[5]; }; explicit HesaiLidarStatusAT128(Internal value) : value(value) {} - json sensor_specifics_to_json() override + [[nodiscard]] ordered_json sensor_specifics_to_json() const override { - json j; - json temperature; + ordered_json j; + ordered_json temperature; temperature["tx1 temperature"] = std::to_string(value.temperature[0].value() * 0.01) + " deg"; temperature["tx2 temperature"] = std::to_string(value.temperature[1].value() * 0.01) + " deg"; temperature["fpga temperature"] = std::to_string(value.temperature[2].value() * 0.01) + " deg"; @@ -514,12 +660,9 @@ struct HesaiLidarStatusAT128 : public HesaiLidarStatusBase temperature["pb temperature"] = std::to_string(value.temperature[7].value() * 0.01) + " deg"; temperature["hot temperature"] = std::to_string(value.temperature[8].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["reserved"] = value.reserved; return j; } @@ -529,14 +672,6 @@ struct HesaiLidarStatusAT128 : public HesaiLidarStatusBase Internal value; }; -inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatusAT128 & arg) -{ - for (auto & [key, value] : arg.to_json().items()) { - os << key << " : " << value << std::endl; - } - return os; -} - struct HesaiLidarStatusOT128 : public HesaiLidarStatusBase { struct Internal : public HesaiLidarStatusBase::Internal @@ -554,10 +689,10 @@ struct HesaiLidarStatusOT128 : public HesaiLidarStatusBase [[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override { return value; } - json sensor_specifics_to_json() override + [[nodiscard]] ordered_json sensor_specifics_to_json() const override { - json j; - json temperature; + 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"] = @@ -581,7 +716,6 @@ struct HesaiLidarStatusOT128 : public HesaiLidarStatusBase 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) + " %"; - j["reserved"] = value.reserved; return j; } @@ -590,14 +724,6 @@ struct HesaiLidarStatusOT128 : public HesaiLidarStatusBase Internal value; }; -inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatusOT128 & arg) -{ - for (auto & [key, value] : arg.to_json().items()) { - os << key << " : " << value << std::endl; - } - return os; -} - struct HesaiLidarStatus_XT_40p : public HesaiLidarStatusBase { struct Internal : public HesaiLidarStatusBase::Internal @@ -614,10 +740,10 @@ struct HesaiLidarStatus_XT_40p : public HesaiLidarStatusBase [[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override { return value; } - json sensor_specifics_to_json() override + [[nodiscard]] ordered_json sensor_specifics_to_json() const override { - json j; - json temperature; + 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"] = @@ -640,7 +766,6 @@ struct HesaiLidarStatus_XT_40p : public HesaiLidarStatusBase 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["reserved"] = value.reserved; return j; } @@ -649,14 +774,6 @@ struct HesaiLidarStatus_XT_40p : public HesaiLidarStatusBase Internal value; }; -inline std::ostream & operator<<(std::ostream & os, HesaiLidarStatus_XT_40p & arg) -{ - for (auto & [key, value] : arg.to_json().items()) { - os << key << " : " << value << std::endl; - } - return os; -} - /// @brief struct of PTC_COMMAND_GET_LIDAR_RANGE struct HesaiLidarRangeAll { @@ -710,44 +827,31 @@ struct HesaiPtpConfig }; /// @brief struct of PTC_COMMAND_LIDAR_MONITOR -struct HesaiLidarMonitor_OT128 +struct HesaiLidarMonitor { - // FIXME: this format is not correct for OT128 big_int32_buf_t input_current; big_int32_buf_t input_voltage; big_int32_buf_t input_power; big_int32_buf_t phase_offset; uint8_t reserved[48]; - json to_json() + [[nodiscard]] ordered_json to_json() const { - json j; + 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"; - j["reserved"] = reserved; return j; } }; -inline std::ostream & operator<<(std::ostream & os, HesaiLidarMonitor_OT128 const & arg) +inline std::ostream & operator<<(std::ostream & os, const HesaiLidarMonitor & arg) { - os << "input_current: " << arg.input_current; - os << ", "; - os << "input_voltage: " << arg.input_voltage; - os << ", "; - os << "input_power: " << arg.input_power; - os << ", "; - os << "phase_offset: " << arg.phase_offset; - 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 = arg.to_json(); + for (const auto & [key, value] : j.items()) { + os << key << ": " << to_string(value) << '\n'; } return os; } 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 2e7ead339..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,7 +263,7 @@ 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 std::shared_ptr GetConfig(); @@ -368,7 +373,7 @@ class HesaiHwInterface Status SetRotDir(int mode); /// @brief Getting data with PTC_COMMAND_LIDAR_MONITOR /// @return Resulting status - HesaiLidarMonitor_OT128 GetLidarMonitor(); + HesaiLidarMonitor GetLidarMonitor(); /// @brief Call run() of IO Context void IOContextRun(); 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 5e30c44ad..e13b9d222 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,7 +3,12 @@ #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_status.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" + +#include +#include #include @@ -23,6 +28,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)}, @@ -327,11 +336,33 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() 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) { + 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); + } + default: { + throw std::runtime_error("Not supported for this LiDAR model"); + } + } } std::shared_ptr HesaiHwInterface::GetConfig() @@ -662,7 +693,7 @@ Status HesaiHwInterface::SetRotDir(int mode) return Status::OK; } -HesaiLidarMonitor_OT128 HesaiHwInterface::GetLidarMonitor() +HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { throw std::runtime_error("Not supported on this sensor"); @@ -670,7 +701,7 @@ HesaiLidarMonitor_OT128 HesaiHwInterface::GetLidarMonitor() auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - return CheckSizeAndParse(response); + return CheckSizeAndParse(response); } void HesaiHwInterface::IOContextRun() @@ -716,6 +747,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) { @@ -776,8 +830,7 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( logMinDelayReqInterval % 0) .str()); ctx->run(); - PrintInfo(response); - return Status::OK; + return unwrap_http_response(response).first; } HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( @@ -803,11 +856,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) @@ -1366,8 +1417,11 @@ 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. Will parse anyway."); } T parsed; 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 847a313e0..7a52a219b 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 @@ -31,6 +32,8 @@ namespace nebula::ros { +using nlohmann::json; + class HesaiHwMonitorWrapper { public: @@ -87,14 +90,12 @@ class HesaiHwMonitorWrapper rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; std::shared_ptr current_status_{}; - std::shared_ptr current_monitor_{}; + std::shared_ptr current_monitor_{}; std::shared_ptr current_config_{}; - std::shared_ptr current_inventory_{}; 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_; @@ -103,8 +104,6 @@ class HesaiHwMonitorWrapper std::mutex mtx_lidar_status_; std::mutex mtx_lidar_monitor_; - std::vector temperature_names_; - bool supports_monitor_; const std::string MSG_NOT_SUPPORTED_ = "Not supported"; diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index 3b1b7d80e..b5f991090 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -50,7 +50,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( if (status_ == Status::OK) { try { auto inventory = hw_interface_->GetInventory(); - 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 85c57d47b..b9dc2ce3f 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -2,6 +2,7 @@ #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 @@ -9,6 +10,7 @@ #include +#include #include namespace nebula::ros @@ -43,47 +45,14 @@ 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; - } - 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())); - RCLCPP_INFO_STREAM(logger_, "Inventory info: " << result); + std::shared_ptr inventory = hw_interface->GetInventory(); + RCLCPP_INFO_STREAM(logger_, "Inventory info: " << *inventory); + json inventory_json = inventory->to_json(); - std::string model = result.get_str_model(); - std::string serial = std::string(std::begin(result.sn), std::end(result.sn)); + 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); @@ -103,8 +72,8 @@ void HesaiHwMonitorWrapper::initialize_hesai_diagnostics() 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_lidar_monitor_time_ = std::make_unique(parent_node_->get_clock()->now()); current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; @@ -184,7 +153,7 @@ 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_time_ = std::make_unique(parent_node_->get_clock()->now()); current_status_ = result; } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( @@ -205,7 +174,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)); }); @@ -230,8 +200,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_OT128(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)"), @@ -282,7 +252,7 @@ void HesaiHwMonitorWrapper::hesai_check_ptp( if (value.type() == json::value_t::string) { auto str = value.template get(); - if (str == "Lock" || str == "locked") { + if (str == "locked") { level = diagnostic_msgs::msg::DiagnosticStatus::OK; msg = "synchronized"; }