From b8b25a6e858cc999df525dc23dba43c4d8f6fc5a Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Tue, 21 May 2024 16:52:02 +0900 Subject: [PATCH] fix(hesai_hw_interface): add error handling (#131) * fix(hesai_hw_interface): add error handling * check PTC command error codes, throw exception if necessary * perform size checks before parsing responses * emit errors on too-large payload size * fix(hesai_hw_monitor_ros_wrapper): fixed wrong range given for S/N copy * fix(hesai): print uint8, uint16 as numbers * chore: update cspell ignore * chore(velodyne_calibration_decoder): fix spelling * chore(velodyne_calibration_decoder): fix spelling once and for all * fix(hesai_hw_interface): add missing check for PTC error, make error type more readable * fix(expected): make error behavior clearer, add doc comments --------- Co-authored-by: Abraham Monrroy Cano --- .cspell.json | 5 +- .../include/nebula_common/util/expected.hpp | 87 +++ .../hesai_cmd_response.hpp | 440 +++++++------ .../hesai_hw_interface.hpp | 40 +- .../hesai_hw_interface.cpp | 592 +++++++----------- .../hesai/hesai_hw_monitor_ros_wrapper.hpp | 1 + .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 20 +- 7 files changed, 587 insertions(+), 598 deletions(-) create mode 100644 nebula_common/include/nebula_common/util/expected.hpp diff --git a/.cspell.json b/.cspell.json index 4f17f7ddd..75b26f454 100644 --- a/.cspell.json +++ b/.cspell.json @@ -44,6 +44,9 @@ "srvs", "manc", "ipaddr", - "ntoa" + "ntoa", + "piyushk", + "Piyush", + "fout" ] } diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp new file mode 100644 index 000000000..6a441db70 --- /dev/null +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include + +namespace nebula +{ +namespace util +{ + +struct bad_expected_access : public std::exception { + bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {} + + const char* what() const noexcept override { + return msg_.c_str(); + } + +private: + const std::string msg_; +}; + +/// @brief A poor man's backport of C++23's std::expected. +/// +/// At any given time, holds exactly one of: expected value, or error value. +/// Provides functions for easy exception handling. +/// +/// More info here: https://en.cppreference.com/w/cpp/utility/expected +/// +/// @tparam T Type of the expected value +/// @tparam E Error type +template +struct expected +{ + /// @brief Whether the expected instance holds a value (as opposed to an error). + /// Call this before trying to access values via `value()`. + /// @return True if a value is contained, false if an error is contained + bool has_value() { return std::holds_alternative(value_); } + + /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained. + /// @return The value of type `T` + T value() { + if (!has_value()) { + throw bad_expected_access("value() called but containing error"); + } + return std::get(value_); + } + + /// @brief Return the contained value, or, if an error is contained, return the given `default_` instead. + /// @return The contained value, if any, else `default_` + T value_or(const T & default_) + { + if (has_value()) return value(); + return default_; + } + + /// @brief Return the contained value, or, if an error is contained, throw `runtime_error(error_msg)`. + /// @return The contained value if no error is thrown + T value_or_throw(const std::string & error_msg) { + if (has_value()) return value(); + throw std::runtime_error(error_msg); + } + + /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained. + /// @return The error of type `E` + E error() { + if (has_value()) { + throw bad_expected_access("error() called but containing value"); + } + return std::get(value_); + } + + /// @brief Return the contained error, or, if a value is contained, return the given `default_` instead. + /// @return The contained error, if any, else `default_` + E error_or(const E & default_) { + if (!has_value()) return error(); + return default_; + } + + expected(const T & value) { value_ = value; } + + expected(const E & error) { value_ = error; } + +private: + std::variant value_; +}; + +} // namespace util +} // namespace nebula \ No newline at end of file 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 21989bf9b..a6d78c45e 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp @@ -2,18 +2,27 @@ #define HESAI_CMD_RESPONSE_HPP #include +#include #include +#include +#include #include +#include + +using namespace boost::endian; namespace nebula { + +#pragma pack(push, 1) + /// @brief PTP STATUS struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagStatus { - long long master_offset; - int ptp_state; - int elapsed_millisec; + big_int64_buf_t master_offset; + big_int32_buf_t ptp_state; + big_int32_buf_t elapsed_millisec; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagStatus const & arg) { @@ -30,35 +39,20 @@ struct HesaiPtpDiagStatus /// @brief PTP TLV PORT_DATA_SET struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagPort { - std::vector portIdentity = std::vector(10); - int portState; - int logMinDelayReqInterval; - long long peerMeanPathDelay; - int logAnnounceInterval; - int announceReceiptTimeout; - int logSyncInterval; - int delayMechanism; - int logMinPdelayReqInterval; - int versionNumber; - - HesaiPtpDiagPort() {} - HesaiPtpDiagPort(const HesaiPtpDiagPort & arg) - { - std::copy(arg.portIdentity.begin(), arg.portIdentity.end(), portIdentity.begin()); - portState = arg.portState; - logMinDelayReqInterval = arg.logMinDelayReqInterval; - peerMeanPathDelay = arg.peerMeanPathDelay; - logAnnounceInterval = arg.logAnnounceInterval; - announceReceiptTimeout = arg.announceReceiptTimeout; - logSyncInterval = arg.logSyncInterval; - delayMechanism = arg.delayMechanism; - logMinPdelayReqInterval = arg.logMinPdelayReqInterval; - versionNumber = arg.versionNumber; - } + uint8_t portIdentity[10]; + big_int32_buf_t portState; + big_int32_buf_t logMinDelayReqInterval; + big_int64_buf_t peerMeanPathDelay; + big_int32_buf_t logAnnounceInterval; + big_int32_buf_t announceReceiptTimeout; + big_int32_buf_t logSyncInterval; + big_int32_buf_t delayMechanism; + big_int32_buf_t logMinPdelayReqInterval; + big_int32_buf_t versionNumber; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagPort const & arg) { - os << "portIdentity: " << std::string(arg.portIdentity.begin(), arg.portIdentity.end()); + os << "portIdentity: " << std::string(std::begin(arg.portIdentity), std::end(arg.portIdentity)); os << ", "; os << "portState: " << arg.portState; os << ", "; @@ -85,28 +79,14 @@ struct HesaiPtpDiagPort /// @brief LinuxPTP TLV TIME_STATUS_NP struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagTime { - long long master_offset; - long long ingress_time; - int cumulativeScaledRateOffset; - int scaledLastGmPhaseChange; - int gmTimeBaseIndicator; - std::vector lastGmPhaseChange = std::vector(12); - int gmPresent; - long long gmIdentity; - - HesaiPtpDiagTime() {} - HesaiPtpDiagTime(const HesaiPtpDiagTime & arg) - { - master_offset = arg.master_offset; - ingress_time = arg.ingress_time; - cumulativeScaledRateOffset = arg.cumulativeScaledRateOffset; - scaledLastGmPhaseChange = arg.scaledLastGmPhaseChange; - gmTimeBaseIndicator = arg.gmTimeBaseIndicator; - std::copy( - arg.lastGmPhaseChange.begin(), arg.lastGmPhaseChange.end(), lastGmPhaseChange.begin()); - gmPresent = arg.gmPresent; - gmIdentity = arg.gmIdentity; - } + big_int64_buf_t master_offset; + big_int64_buf_t ingress_time; + big_int32_buf_t cumulativeScaledRateOffset; + big_int32_buf_t scaledLastGmPhaseChange; + big_int16_buf_t gmTimeBaseIndicator; + uint8_t lastGmPhaseChange[12]; + big_int32_buf_t gmPresent; + big_int64_buf_t gmIdentity; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagTime const & arg) { @@ -120,8 +100,9 @@ struct HesaiPtpDiagTime os << ", "; os << "gmTimeBaseIndicator: " << arg.gmTimeBaseIndicator; os << ", "; + //FIXME: lastGmPhaseChange is a binary number, displaying it as string is incorrect os << "lastGmPhaseChange: " - << std::string(arg.lastGmPhaseChange.begin(), arg.lastGmPhaseChange.end()); + << std::string(std::begin(arg.lastGmPhaseChange), std::end(arg.lastGmPhaseChange)); os << ", "; os << "gmPresent: " << arg.gmPresent; os << ", "; @@ -134,10 +115,10 @@ struct HesaiPtpDiagTime /// @brief LinuxPTP TLV GRANDMASTER_SETTINGS_NP struct of PTC_COMMAND_PTP_DIAGNOSTICS struct HesaiPtpDiagGrandmaster { - int clockQuality; - int utc_offset; - int time_flags; - int time_source; + big_int32_buf_t clockQuality; + big_int16_buf_t utc_offset; + int8_t time_flags; + int8_t time_source; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpDiagGrandmaster const & arg) { @@ -145,9 +126,9 @@ struct HesaiPtpDiagGrandmaster os << ", "; os << "utc_offset: " << arg.utc_offset; os << ", "; - os << "time_flags: " << arg.time_flags; + os << "time_flags: " << +arg.time_flags; os << ", "; - os << "time_source: " << arg.time_source; + os << "time_source: " << +arg.time_source; return os; } @@ -156,76 +137,66 @@ struct HesaiPtpDiagGrandmaster /// @brief struct of PTC_COMMAND_GET_INVENTORY_INFO struct HesaiInventory { - std::vector sn = std::vector(18); - std::vector date_of_manufacture = std::vector(16); - std::vector mac = std::vector(6); - std::vector sw_ver = std::vector(16); - std::vector hw_ver = std::vector(16); - std::vector control_fw_ver = std::vector(16); - std::vector sensor_fw_ver = std::vector(16); - int angle_offset; - int model; - int motor_type; - int num_of_lines; - std::vector reserved = std::vector(11); - - HesaiInventory() {} - HesaiInventory(const HesaiInventory & arg) - { - std::copy(arg.sn.begin(), arg.sn.end(), sn.begin()); - std::copy( - arg.date_of_manufacture.begin(), arg.date_of_manufacture.end(), date_of_manufacture.begin()); - std::copy(arg.mac.begin(), arg.mac.end(), mac.begin()); - std::copy(arg.sw_ver.begin(), arg.sw_ver.end(), sw_ver.begin()); - std::copy(arg.hw_ver.begin(), arg.hw_ver.end(), hw_ver.begin()); - std::copy(arg.control_fw_ver.begin(), arg.control_fw_ver.end(), control_fw_ver.begin()); - std::copy(arg.sensor_fw_ver.begin(), arg.sensor_fw_ver.end(), sensor_fw_ver.begin()); - angle_offset = arg.angle_offset; - model = arg.model; - motor_type = arg.motor_type; - num_of_lines = arg.num_of_lines; - std::copy(arg.reserved.begin(), arg.reserved.end(), reserved.begin()); - } + 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) { - os << "sn: " << std::string(arg.sn.begin(), arg.sn.end()); + 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.begin(), arg.date_of_manufacture.end()); + << std::string(arg.date_of_manufacture, strnlen(arg.date_of_manufacture, sizeof(arg.date_of_manufacture))); os << ", "; os << "mac: "; - std::stringstream ss; - for (long unsigned int i = 0; i < arg.mac.size() - 1; i++) { - ss << std::hex << std::setfill('0') << std::setw(2) << (static_cast(arg.mac[i]) & 0xff) - << ":"; + + for (size_t i = 0; i < sizeof(arg.mac); i++) { + if (i != 0) { + os << ':'; + } + os << std::hex << std::setfill('0') << std::setw(2) << (+arg.mac[i]); } - ss << std::hex << std::setfill('0') << std::setw(2) - << (static_cast(arg.mac[arg.mac.size() - 1]) & 0xff); - os << ss.str(); + os.copyfmt(initial_format); + os << ", "; - os << "sw_ver: " << std::string(arg.sw_ver.begin(), arg.sw_ver.end()); + 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.begin(), arg.hw_ver.end()); + 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.begin(), arg.control_fw_ver.end()); + 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.begin(), arg.sensor_fw_ver.end()); + 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 << "model: " << +arg.model; os << ", "; - os << "motor_type: " << arg.motor_type; + os << "motor_type: " << +arg.motor_type; os << ", "; - os << "num_of_lines: " << arg.num_of_lines; + os << "num_of_lines: " << +arg.num_of_lines; os << ", "; - // os << "reserved: " << boost::algorithm::join(arg.reserved, ","); - os << "reserved: "; - for (long unsigned int i = 0; i < arg.reserved.size() - 1; i++) { - os << arg.reserved[i] << ","; + + 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 << arg.reserved[arg.reserved.size() - 1]; + os.copyfmt(initial_format); return os; } @@ -258,7 +229,7 @@ struct HesaiInventory case 48: return "PandarAT128"; default: - return "Unknown(" + std::to_string(model) + ")"; + return "Unknown(" + std::to_string(static_cast(model)) + ")"; } } }; @@ -266,42 +237,58 @@ struct HesaiInventory /// @brief struct of PTC_COMMAND_GET_CONFIG_INFO struct HesaiConfig { - int ipaddr[4]; - int mask[4]; - int gateway[4]; - int dest_ipaddr[4]; - int dest_LiDAR_udp_port; - int dest_gps_udp_port; - int spin_rate; - int sync; - int sync_angle; - int start_angle; - int stop_angle; - int clock_source; - int udp_seq; - int trigger_method; - int return_mode; - int standby_mode; - int motor_status; - int vlan_flag; - int vlan_id; - int clock_data_fmt; - int noise_filtering; - int reflectivity_mapping; - unsigned char reserved[6]; + 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) { - 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]; + 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 << ", "; @@ -309,7 +296,7 @@ struct HesaiConfig os << ", "; os << "spin_rate: " << arg.spin_rate; os << ", "; - os << "sync: " << arg.sync; + os << "sync: " << +arg.sync; os << ", "; os << "sync_angle: " << arg.sync_angle; os << ", "; @@ -317,30 +304,37 @@ struct HesaiConfig os << ", "; os << "stop_angle: " << arg.stop_angle; os << ", "; - os << "clock_source: " << arg.clock_source; + os << "clock_source: " << +arg.clock_source; os << ", "; - os << "udp_seq: " << arg.udp_seq; + os << "udp_seq: " << +arg.udp_seq; os << ", "; - os << "trigger_method: " << arg.trigger_method; + os << "trigger_method: " << +arg.trigger_method; os << ", "; - os << "return_mode: " << arg.return_mode; + os << "return_mode: " << +arg.return_mode; os << ", "; - os << "standby_mode: " << arg.standby_mode; + os << "standby_mode: " << +arg.standby_mode; os << ", "; - os << "motor_status: " << arg.motor_status; + os << "motor_status: " << +arg.motor_status; os << ", "; - os << "vlan_flag: " << arg.vlan_flag; + os << "vlan_flag: " << +arg.vlan_flag; os << ", "; os << "vlan_id: " << arg.vlan_id; os << ", "; - os << "clock_data_fmt: " << arg.clock_data_fmt; + os << "clock_data_fmt: " << +arg.clock_data_fmt; os << ", "; - os << "noise_filtering: " << arg.noise_filtering; + os << "noise_filtering: " << +arg.noise_filtering; os << ", "; - os << "reflectivity_mapping: " << arg.reflectivity_mapping; + os << "reflectivity_mapping: " << +arg.reflectivity_mapping; os << ", "; - os << "reserved: " << arg.reserved[0] << "," << arg.reserved[1] << "," << arg.reserved[2] << "," - << arg.reserved[3] << "," << arg.reserved[4] << "," << arg.reserved[5]; + 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) << (+arg.reserved[i]); + } + os.copyfmt(initial_format); return os; } @@ -349,58 +343,54 @@ struct HesaiConfig /// @brief struct of PTC_COMMAND_GET_LIDAR_STATUS struct HesaiLidarStatus { - int system_uptime; - int motor_speed; - // int temperature[8]; - std::vector temperature = std::vector(8); - int gps_pps_lock; - int gps_gprmc_status; - int startup_times; - int total_operation_time; - int ptp_clock_status; - std::vector reserved = std::vector(5); - - HesaiLidarStatus() {} - HesaiLidarStatus(const HesaiLidarStatus & arg) - { - system_uptime = arg.system_uptime; - motor_speed = arg.motor_speed; - std::copy(arg.temperature.begin(), arg.temperature.end(), temperature.begin()); - gps_pps_lock = arg.gps_pps_lock; - gps_gprmc_status = arg.gps_gprmc_status; - startup_times = arg.startup_times; - total_operation_time = arg.total_operation_time; - ptp_clock_status = arg.ptp_clock_status; - std::copy(arg.reserved.begin(), arg.reserved.end(), reserved.begin()); - } + 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: "; - for (long unsigned int i = 0; i < arg.temperature.size() - 1; i++) { - os << arg.temperature[i] << ","; + + for (size_t i = 0; i < sizeof(arg.temperature); i++) { + if (i != 0) { + os << ','; + } + os << arg.temperature[i]; } - os << arg.temperature[arg.temperature.size() - 1]; + os << ", "; - os << "gps_pps_lock: " << arg.gps_pps_lock; + os << "gps_pps_lock: " << static_cast(arg.gps_pps_lock); os << ", "; - os << "gps_gprmc_status: " << arg.gps_gprmc_status; + 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: " << arg.ptp_clock_status; + os << "ptp_clock_status: " << static_cast(arg.ptp_clock_status); os << ", "; os << "reserved: "; - for (long unsigned int i = 0; i < arg.reserved.size() - 1; i++) { - os << arg.reserved[i] << ","; + + 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])); } - os << arg.reserved[arg.reserved.size() - 1]; + os.copyfmt(initial_format); return os; } @@ -410,13 +400,10 @@ struct HesaiLidarStatus switch (gps_pps_lock) { case 1: return "Lock"; - break; case 0: return "Unlock"; - break; default: return "Unknown"; - break; } } std::string get_str_gps_gprmc_status() @@ -424,13 +411,10 @@ struct HesaiLidarStatus switch (gps_gprmc_status) { case 1: return "Lock"; - break; case 0: return "Unlock"; - break; default: return "Unknown"; - break; } } std::string get_str_ptp_clock_status() @@ -438,19 +422,14 @@ struct HesaiLidarStatus switch (ptp_clock_status) { case 0: return "free run"; - break; case 1: return "tracking"; - break; case 2: return "locked"; - break; case 3: return "frozen"; - break; default: return "Unknown"; - break; } } }; @@ -458,17 +437,17 @@ struct HesaiLidarStatus /// @brief struct of PTC_COMMAND_GET_LIDAR_RANGE struct HesaiLidarRangeAll { - int method; - int start; - int end; + uint8_t method; + big_uint16_buf_t start; + big_uint16_buf_t end; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarRangeAll const & arg) { - os << "method: " << arg.method; + os << "method: " << static_cast(arg.method); os << ", "; - os << "start: " << arg.start; + os << "start: " << static_cast(arg.start.value()); os << ", "; - os << "end: " << arg.end; + os << "end: " << static_cast(arg.end.value()); return os; } @@ -477,30 +456,31 @@ struct HesaiLidarRangeAll /// @brief struct of PTC_COMMAND_GET_PTP_CONFIG struct HesaiPtpConfig { - int status; - int profile; - int domain; - int network; - int logAnnounceInterval; - int logSyncInterval; - int logMinDelayReqInterval; + int8_t status; + int8_t profile; + int8_t domain; + int8_t network; + int8_t logAnnounceInterval; + int8_t logSyncInterval; + int8_t logMinDelayReqInterval; + //FIXME: this format is not correct for OT128, or for AT128 on 802.1AS friend std::ostream & operator<<(std::ostream & os, nebula::HesaiPtpConfig const & arg) { - os << "status: " << arg.status; + os << "status: " << static_cast(arg.status); os << ", "; - os << "profile: " << arg.profile; + os << "profile: " << static_cast(arg.profile); os << ", "; - os << "domain: " << arg.domain; + os << "domain: " << static_cast(arg.domain); os << ", "; - os << "network: " << arg.network; + os << "network: " << static_cast(arg.network); if (arg.status == 0) { os << ", "; - os << "logAnnounceInterval: " << arg.logAnnounceInterval; + os << "logAnnounceInterval: " << static_cast(arg.logAnnounceInterval); os << ", "; - os << "logSyncInterval: " << arg.logSyncInterval; + os << "logSyncInterval: " << static_cast(arg.logSyncInterval); os << ", "; - os << "logMinDelayReqInterval: " << arg.logMinDelayReqInterval; + os << "logMinDelayReqInterval: " << static_cast(arg.logMinDelayReqInterval); } return os; } @@ -509,10 +489,11 @@ struct HesaiPtpConfig /// @brief struct of PTC_COMMAND_LIDAR_MONITOR struct HesaiLidarMonitor { - int input_voltage; - int input_current; - int input_power; - std::vector reserved = std::vector(52); + //FIXME: this format is not correct for OT128 + big_int32_buf_t input_voltage; + big_int32_buf_t input_current; + big_int32_buf_t input_power; + uint8_t reserved[52]; friend std::ostream & operator<<(std::ostream & os, nebula::HesaiLidarMonitor const & arg) { @@ -523,14 +504,19 @@ struct HesaiLidarMonitor os << "input_power: " << arg.input_power; os << ", "; os << "reserved: "; - for (long unsigned int i = 0; i < arg.reserved.size() - 1; i++) { - os << arg.reserved[i] << ","; + + 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])); } - os << arg.reserved[arg.reserved.size() - 1]; return os; } }; +#pragma pack(pop) + } // namespace nebula #endif // HESAI_CMD_RESPONSE_HPP 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 2f5639373..1b21ddae2 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -15,6 +15,7 @@ #include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" +#include "nebula_common/util/expected.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" @@ -29,6 +30,7 @@ #include #include +#include namespace nebula { @@ -63,6 +65,20 @@ const uint8_t PTC_COMMAND_RESET = 0x25; const uint8_t PTC_COMMAND_SET_ROTATE_DIRECTION = 0x2a; const uint8_t PTC_COMMAND_LIDAR_MONITOR = 0x27; +const uint8_t PTC_ERROR_CODE_NO_ERROR = 0x00; +const uint8_t PTC_ERROR_CODE_INVALID_INPUT_PARAM = 0x01; +const uint8_t PTC_ERROR_CODE_SERVER_CONN_FAILED = 0x02; +const uint8_t PTC_ERROR_CODE_INVALID_DATA = 0x03; +const uint8_t PTC_ERROR_CODE_OUT_OF_MEMORY = 0x04; +const uint8_t PTC_ERROR_CODE_UNSUPPORTED_CMD = 0x05; +const uint8_t PTC_ERROR_CODE_FPGA_COMM_FAILED = 0x06; +const uint8_t PTC_ERROR_CODE_OTHER = 0x07; + +const uint8_t TCP_ERROR_UNRELATED_RESPONSE = 1; +const uint8_t TCP_ERROR_UNEXPECTED_PAYLOAD = 2; +const uint8_t TCP_ERROR_TIMEOUT = 4; +const uint8_t TCP_ERROR_INCOMPLETE_RESPONSE = 8; + const uint16_t PANDARQT64_PACKET_SIZE = 1072; const uint16_t PANDARQT128_PACKET_SIZE = 1127; const uint16_t PANDARXT32_PACKET_SIZE = 1080; @@ -74,13 +90,12 @@ const uint16_t PANDAR40_PACKET_SIZE = 1262; const uint16_t PANDAR40P_EXTENDED_PACKET_SIZE = 1266; const uint16_t PANDAR128_E4X_PACKET_SIZE = 861; const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; - const uint16_t MTU_SIZE = 1500; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; +const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; +const int PTP_SYNC_INTERVAL = 1; // Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) const int PTP_LOG_MIN_DELAY_INTERVAL = 0; @@ -91,6 +106,16 @@ const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; class HesaiHwInterface : NebulaHwInterfaceBase { private: + struct ptc_error_t + { + uint8_t error_flags = 0; + uint8_t ptc_error_code = 0; + + bool ok() { return !error_flags && !ptc_error_code; } + }; + + typedef nebula::util::expected, ptc_error_t> ptc_cmd_result_t; + std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::shared_ptr m_owned_ctx; std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; @@ -141,13 +166,18 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param bytes Target byte vector void PrintDebug(const std::vector & bytes); + /// @brief Convert an error code to a human-readable string + /// @param error_code The error code, containing the sensor's error code (if any), along with + /// flags such as TCP_ERROR_UNRELATED_RESPONSE etc. + /// @return A string description of all errors in this code + std::string PrettyPrintPTCError(ptc_error_t error_code); + /// @brief Send a PTC request with an optional payload, and return the full response payload. /// Blocking. /// @param command_id PTC command number. /// @param payload Payload bytes of the PTC command. Not including the 8-byte PTC header. /// @return The returned payload, if successful, or nullptr. - std::shared_ptr> SendReceive( - const uint8_t command_id, const std::vector & payload = {}); + ptc_cmd_result_t SendReceive(const uint8_t command_id, const std::vector & payload = {}); public: /// @brief Constructor 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 a5e5bd068..fccb2a622 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -48,7 +48,7 @@ HesaiHwInterface::~HesaiHwInterface() #endif } -std::shared_ptr> HesaiHwInterface::SendReceive( +HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( const uint8_t command_id, const std::vector & payload) { uint32_t len = payload.size(); @@ -64,8 +64,12 @@ std::shared_ptr> HesaiHwInterface::SendReceive( send_buf.emplace_back(len & 0xff); send_buf.insert(send_buf.end(), payload.begin(), payload.end()); + // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can access valid memory auto recv_buf = std::make_shared>(); - bool success = false; + auto response_complete = std::make_shared(false); + + // Low byte is for PTC error code, the rest is nebula-specific + auto error_code = std::make_shared(); std::stringstream ss; ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(command_id) << " (" << len << ") "; @@ -84,23 +88,33 @@ std::shared_ptr> HesaiHwInterface::SendReceive( PrintDebug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, - [this, log_tag, &success](const std::vector & header_bytes) { + [this, log_tag, command_id, response_complete, error_code](const std::vector & header_bytes) { + error_code->ptc_error_code = header_bytes[3]; + size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; PrintDebug(log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); - if (payload_len == 0) { success = true; } + // If command_id in the response does not match, we got a response for another command (or rubbish), probably as a + // result of too many simultaneous TCP connections to the sensor (e.g. from GUI, Web UI, another nebula instance, etc.) + if (header_bytes[2] != command_id) { + error_code->error_flags |= TCP_ERROR_UNRELATED_RESPONSE; + } + if (payload_len == 0) { + *response_complete = true; + } }, - [this, log_tag, &recv_buf, &success](const std::vector & payload_bytes) { + [this, log_tag, recv_buf, response_complete, error_code](const std::vector & payload_bytes) { PrintDebug(log_tag + "Received payload"); // Header had payload length 0 (thus, header callback processed request successfully already), // but we still received a payload: invalid state - if (success == true) { - throw std::runtime_error("Received payload despite payload length 0 in header"); + if (*response_complete == true) { + error_code->error_flags |= TCP_ERROR_UNEXPECTED_PAYLOAD; + return; } // Skip 8 header bytes recv_buf->insert(recv_buf->end(), std::next(payload_bytes.begin(), 8), payload_bytes.end()); - success = true; + *response_complete = true; }, [this, log_tag, &tm]() { PrintDebug(log_tag + "Unlocking mutex"); @@ -110,16 +124,23 @@ std::shared_ptr> HesaiHwInterface::SendReceive( this->IOContextRun(); if (!tm.try_lock_for(std::chrono::seconds(1))) { PrintError(log_tag + "Request did not finish within 1s"); - return nullptr; + error_code->error_flags |= TCP_ERROR_TIMEOUT; + return *error_code; } - if (!success) { + if (!response_complete) { PrintError(log_tag + "Did not receive response"); - return nullptr; + error_code->error_flags |= TCP_ERROR_INCOMPLETE_RESPONSE; + return *error_code; + } + + if (!error_code->ok()) { + return *error_code; } PrintDebug(log_tag + "Received response"); - return recv_buf; + + return *recv_buf; } Status HesaiHwInterface::SetSensorConfiguration( @@ -344,50 +365,29 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str) std::vector HesaiHwInterface::GetLidarCalibrationBytes() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - return std::vector(*response_ptr); + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + return response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); } std::string HesaiHwInterface::GetLidarCalibrationString() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); - std::string calib_string(response_ptr->begin(), response_ptr->end()); + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION); + auto calib_data = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + std::string calib_string(calib_data.begin(), calib_data.end()); return calib_string; } HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() { - auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); - auto & response = *response_ptr; - - HesaiPtpDiagStatus hesai_ptp_diag_status{}; - int payload_pos = 0; - hesai_ptp_diag_status.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_status.master_offset = - hesai_ptp_diag_status.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_status.ptp_state = response[payload_pos++] << 24; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 16; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++] << 8; - hesai_ptp_diag_status.ptp_state = hesai_ptp_diag_status.ptp_state | response[payload_pos++]; - hesai_ptp_diag_status.elapsed_millisec = response[payload_pos++] << 24; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 16; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++] << 8; - hesai_ptp_diag_status.elapsed_millisec = - hesai_ptp_diag_status.elapsed_millisec | response[payload_pos++]; + auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_STATUS}); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + + if (response.size() != sizeof(HesaiPtpDiagStatus)) { + throw std::runtime_error("Unexpected payload size"); + } + + HesaiPtpDiagStatus hesai_ptp_diag_status; + memcpy(&hesai_ptp_diag_status, response.data(), sizeof(HesaiPtpDiagStatus)); std::stringstream ss; ss << "HesaiHwInterface::GetPtpDiagStatus: " << hesai_ptp_diag_status; @@ -398,38 +398,14 @@ HesaiPtpDiagStatus HesaiHwInterface::GetPtpDiagStatus() HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() { - auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_PORT_DATA_SET}); - auto & response = *response_ptr; + 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({}))); - HesaiPtpDiagPort hesai_ptp_diag_port; - int payload_pos = 0; - - for (size_t i = 0; i < hesai_ptp_diag_port.portIdentity.size(); i++) { - hesai_ptp_diag_port.portIdentity[i] = response[payload_pos++]; + if (response.size() != sizeof(HesaiPtpDiagPort)) { + throw std::runtime_error("Unexpected payload size"); } - hesai_ptp_diag_port.portState = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinDelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.peerMeanPathDelay = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_port.peerMeanPathDelay = - hesai_ptp_diag_port.peerMeanPathDelay | static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.announceReceiptTimeout = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.delayMechanism = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast(response[payload_pos++]); - hesai_ptp_diag_port.versionNumber = static_cast(response[payload_pos++]); + HesaiPtpDiagPort hesai_ptp_diag_port; + memcpy(&hesai_ptp_diag_port, response.data(), sizeof(HesaiPtpDiagPort)); std::stringstream ss; ss << "HesaiHwInterface::GetPtpDiagPort: " << hesai_ptp_diag_port; @@ -440,80 +416,15 @@ HesaiPtpDiagPort HesaiHwInterface::GetPtpDiagPort() HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() { - auto response_ptr = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_TIME_STATUS_NP}); - auto & response = *response_ptr; + 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({}))); - HesaiPtpDiagTime hesai_ptp_diag_time; - int payload_pos = 0; - hesai_ptp_diag_time.master_offset = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.master_offset = - hesai_ptp_diag_time.master_offset | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.ingress_time = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.ingress_time = - hesai_ptp_diag_time.ingress_time | static_cast(response[payload_pos++]); - hesai_ptp_diag_time.cumulativeScaledRateOffset = response[payload_pos++] << 24; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 16; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++] << 8; - hesai_ptp_diag_time.cumulativeScaledRateOffset = - hesai_ptp_diag_time.cumulativeScaledRateOffset | response[payload_pos++]; - hesai_ptp_diag_time.scaledLastGmPhaseChange = response[payload_pos++] << 24; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 16; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++] << 8; - hesai_ptp_diag_time.scaledLastGmPhaseChange = - hesai_ptp_diag_time.scaledLastGmPhaseChange | response[payload_pos++]; - hesai_ptp_diag_time.gmTimeBaseIndicator = response[payload_pos++] << 8; - hesai_ptp_diag_time.gmTimeBaseIndicator = - hesai_ptp_diag_time.gmTimeBaseIndicator | response[payload_pos++]; - for (size_t i = 0; i < hesai_ptp_diag_time.lastGmPhaseChange.size(); i++) { - hesai_ptp_diag_time.lastGmPhaseChange[i] = response[payload_pos++]; + if (response.size() != sizeof(HesaiPtpDiagTime)) { + throw std::runtime_error("Unexpected payload size"); } - hesai_ptp_diag_time.gmPresent = response[payload_pos++] << 24; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 16; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++] << 8; - hesai_ptp_diag_time.gmPresent = hesai_ptp_diag_time.gmPresent | response[payload_pos++]; - hesai_ptp_diag_time.gmIdentity = static_cast(response[payload_pos++]) << 56; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 48; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 40; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 32; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 24; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 16; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]) << 8; - hesai_ptp_diag_time.gmIdentity = - hesai_ptp_diag_time.gmIdentity | static_cast(response[payload_pos++]); + + HesaiPtpDiagTime hesai_ptp_diag_time; + memcpy(&hesai_ptp_diag_time, response.data(), sizeof(HesaiPtpDiagTime)); std::stringstream ss; ss << "HesaiHwInterface::GetPtpDiagTime: " << hesai_ptp_diag_time; @@ -524,25 +435,15 @@ HesaiPtpDiagTime HesaiHwInterface::GetPtpDiagTime() HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() { - auto response_ptr = - SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_PTP_DIAGNOSTICS, {PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP}); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + + if (response.size() != sizeof(HesaiPtpDiagGrandmaster)) { + throw std::runtime_error("Unexpected payload size"); + } HesaiPtpDiagGrandmaster hesai_ptp_diag_grandmaster; - int payload_pos = 0; - - hesai_ptp_diag_grandmaster.clockQuality = response[payload_pos++] << 24; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 16; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.clockQuality = - hesai_ptp_diag_grandmaster.clockQuality | response[payload_pos++]; - hesai_ptp_diag_grandmaster.utc_offset = response[payload_pos++] << 8; - hesai_ptp_diag_grandmaster.utc_offset = - hesai_ptp_diag_grandmaster.utc_offset | response[payload_pos++]; - hesai_ptp_diag_grandmaster.time_flags = static_cast(response[payload_pos++]); - hesai_ptp_diag_grandmaster.time_source = static_cast(response[payload_pos++]); + memcpy(&hesai_ptp_diag_grandmaster, response.data(), sizeof(HesaiPtpDiagGrandmaster)); std::stringstream ss; ss << "HesaiHwInterface::GetPtpDiagGrandmaster: " << hesai_ptp_diag_grandmaster; @@ -553,98 +454,32 @@ HesaiPtpDiagGrandmaster HesaiHwInterface::GetPtpDiagGrandmaster() HesaiInventory HesaiHwInterface::GetInventory() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_GET_INVENTORY_INFO); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - HesaiInventory hesai_inventory; - int payload_pos = 0; - for (size_t i = 0; i < hesai_inventory.sn.size(); i++) { - hesai_inventory.sn[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.date_of_manufacture.size(); i++) { - hesai_inventory.date_of_manufacture[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.mac.size(); i++) { - hesai_inventory.mac[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sw_ver.size(); i++) { - hesai_inventory.sw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.hw_ver.size(); i++) { - hesai_inventory.hw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.control_fw_ver.size(); i++) { - hesai_inventory.control_fw_ver[i] = response[payload_pos++]; - } - for (size_t i = 0; i < hesai_inventory.sensor_fw_ver.size(); i++) { - hesai_inventory.sensor_fw_ver[i] = response[payload_pos++]; - } - hesai_inventory.angle_offset = response[payload_pos++] << 8; - hesai_inventory.angle_offset = hesai_inventory.angle_offset | response[payload_pos++]; - hesai_inventory.model = static_cast(response[payload_pos++]); - hesai_inventory.motor_type = static_cast(response[payload_pos++]); - hesai_inventory.num_of_lines = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) { - hesai_inventory.reserved[i] = static_cast(response[payload_pos++]); + if (response.size() < sizeof(HesaiInventory)) { + throw std::runtime_error("Unexpected payload size"); + } else if (response.size() > sizeof(HesaiInventory)) { + PrintError("HesaiInventory from Sensor has unknown format. Will parse anyway."); } + HesaiInventory hesai_inventory; + memcpy(&hesai_inventory, response.data(), sizeof(HesaiInventory)); + return hesai_inventory; } HesaiConfig HesaiHwInterface::GetConfig() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); - auto & response = *response_ptr; - - HesaiConfig hesai_config{}; - int payload_pos = 0; - hesai_config.ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.mask[0] = static_cast(response[payload_pos++]); - hesai_config.mask[1] = static_cast(response[payload_pos++]); - hesai_config.mask[2] = static_cast(response[payload_pos++]); - hesai_config.mask[3] = static_cast(response[payload_pos++]); - hesai_config.gateway[0] = static_cast(response[payload_pos++]); - hesai_config.gateway[1] = static_cast(response[payload_pos++]); - hesai_config.gateway[2] = static_cast(response[payload_pos++]); - hesai_config.gateway[3] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[0] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[1] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[2] = static_cast(response[payload_pos++]); - hesai_config.dest_ipaddr[3] = static_cast(response[payload_pos++]); - hesai_config.dest_LiDAR_udp_port = response[payload_pos++] << 8; - hesai_config.dest_LiDAR_udp_port = hesai_config.dest_LiDAR_udp_port | response[payload_pos++]; - hesai_config.dest_gps_udp_port = response[payload_pos++] << 8; - hesai_config.dest_gps_udp_port = hesai_config.dest_gps_udp_port | response[payload_pos++]; - hesai_config.spin_rate = response[payload_pos++] << 8; - hesai_config.spin_rate = hesai_config.spin_rate | response[payload_pos++]; - hesai_config.sync = static_cast(response[payload_pos++]); - hesai_config.sync_angle = response[payload_pos++] << 8; - hesai_config.sync_angle = hesai_config.sync_angle | response[payload_pos++]; - hesai_config.start_angle = response[payload_pos++] << 8; - hesai_config.start_angle = hesai_config.start_angle | response[payload_pos++]; - hesai_config.stop_angle = response[payload_pos++] << 8; - hesai_config.stop_angle = hesai_config.stop_angle | response[payload_pos++]; - hesai_config.clock_source = static_cast(response[payload_pos++]); - hesai_config.udp_seq = static_cast(response[payload_pos++]); - hesai_config.trigger_method = static_cast(response[payload_pos++]); - hesai_config.return_mode = static_cast(response[payload_pos++]); - hesai_config.standby_mode = static_cast(response[payload_pos++]); - hesai_config.motor_status = static_cast(response[payload_pos++]); - hesai_config.vlan_flag = static_cast(response[payload_pos++]); - hesai_config.vlan_id = response[payload_pos++] << 8; - hesai_config.vlan_id = hesai_config.vlan_id | response[payload_pos++]; - hesai_config.clock_data_fmt = static_cast(response[payload_pos++]); - hesai_config.noise_filtering = static_cast(response[payload_pos++]); - hesai_config.reflectivity_mapping = static_cast(response[payload_pos++]); - hesai_config.reserved[0] = static_cast(response[payload_pos++]); - hesai_config.reserved[1] = static_cast(response[payload_pos++]); - hesai_config.reserved[2] = static_cast(response[payload_pos++]); - hesai_config.reserved[3] = static_cast(response[payload_pos++]); - hesai_config.reserved[4] = static_cast(response[payload_pos++]); - hesai_config.reserved[5] = static_cast(response[payload_pos++]); + auto response_or_err = SendReceive(PTC_COMMAND_GET_CONFIG_INFO); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + + if (response.size() != sizeof(HesaiConfig)) { + throw std::runtime_error("Unexpected payload size"); + } + + HesaiConfig hesai_config; + memcpy(&hesai_config, response.data(), sizeof(HesaiConfig)); std::cout << "Config: " << hesai_config << std::endl; return hesai_config; @@ -652,40 +487,16 @@ HesaiConfig HesaiHwInterface::GetConfig() HesaiLidarStatus HesaiHwInterface::GetLidarStatus() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_STATUS); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - HesaiLidarStatus hesai_status; - int payload_pos = 0; - hesai_status.system_uptime = response[payload_pos++] << 24; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 16; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++] << 8; - hesai_status.system_uptime = hesai_status.system_uptime | response[payload_pos++]; - hesai_status.motor_speed = response[payload_pos++] << 8; - hesai_status.motor_speed = hesai_status.motor_speed | response[payload_pos++]; - for (size_t i = 0; i < hesai_status.temperature.size(); i++) { - hesai_status.temperature[i] = response[payload_pos++] << 24; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 16; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++] << 8; - hesai_status.temperature[i] = hesai_status.temperature[i] | response[payload_pos++]; - } - hesai_status.gps_pps_lock = static_cast(response[payload_pos++]); - hesai_status.gps_gprmc_status = static_cast(response[payload_pos++]); - hesai_status.startup_times = response[payload_pos++] << 24; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 16; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++] << 8; - hesai_status.startup_times = hesai_status.startup_times | response[payload_pos++]; - hesai_status.total_operation_time = response[payload_pos++] << 24; - hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] - << 16; - hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++] - << 8; - hesai_status.total_operation_time = hesai_status.total_operation_time | response[payload_pos++]; - hesai_status.ptp_clock_status = static_cast(response[payload_pos++]); - for (size_t i = 0; i < hesai_status.reserved.size(); i++) { - hesai_status.reserved[i] = static_cast(response[payload_pos++]); + if (response.size() != sizeof(HesaiLidarStatus)) { + throw std::runtime_error("Unexpected payload size"); } + HesaiLidarStatus hesai_status; + memcpy(&hesai_status, response.data(), sizeof(HesaiLidarStatus)); + return hesai_status; } @@ -695,7 +506,8 @@ Status HesaiHwInterface::SetSpinRate(uint16_t rpm) request_payload.emplace_back((rpm >> 8) & 0xff); request_payload.emplace_back(rpm & 0xff); - SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_SPIN_RATE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -706,7 +518,8 @@ Status HesaiHwInterface::SetSyncAngle(int sync_angle, int angle) request_payload.emplace_back((angle >> 8) & 0xff); request_payload.emplace_back(angle & 0xff); - SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_SYNC_ANGLE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -715,7 +528,8 @@ Status HesaiHwInterface::SetTriggerMethod(int trigger_method) std::vector request_payload; request_payload.emplace_back(trigger_method & 0xff); - SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_TRIGGER_METHOD, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -724,7 +538,8 @@ Status HesaiHwInterface::SetStandbyMode(int standby_mode) std::vector request_payload; request_payload.emplace_back(standby_mode & 0xff); - SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_STANDBY_MODE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -733,7 +548,8 @@ Status HesaiHwInterface::SetReturnMode(int return_mode) std::vector request_payload; request_payload.emplace_back(return_mode & 0xff); - SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_RETURN_MODE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -750,7 +566,8 @@ Status HesaiHwInterface::SetDestinationIp( request_payload.emplace_back((gps_port >> 8) & 0xff); request_payload.emplace_back(gps_port & 0xff); - SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_DESTINATION_IP, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -775,7 +592,8 @@ Status HesaiHwInterface::SetControlPort( request_payload.emplace_back((vlan_id >> 8) & 0xff); request_payload.emplace_back(vlan_id & 0xff); - SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_CONTROL_PORT, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -788,7 +606,8 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector da request_payload.emplace_back(method & 0xff); request_payload.insert(request_payload.end(), data.begin(), data.end()); - SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -805,31 +624,36 @@ Status HesaiHwInterface::SetLidarRange(int start, int end) request_payload.emplace_back((end >> 8) & 0xff); request_payload.emplace_back(end & 0xff); - SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_LIDAR_RANGE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + + if (response.size() < 1) { + throw std::runtime_error("Response payload too short"); + } HesaiLidarRangeAll hesai_range_all; - int payload_pos = 0; - int method = static_cast(response[payload_pos++]); - switch (method) { + hesai_range_all.method = response[0]; + switch (hesai_range_all.method) { case 0: // for all channels - hesai_range_all.method = method; - hesai_range_all.start = response[payload_pos++] << 8; - hesai_range_all.start = hesai_range_all.start | response[payload_pos++]; - hesai_range_all.end = response[payload_pos++] << 8; - hesai_range_all.end = hesai_range_all.end | response[payload_pos++]; + if (response.size() != 5) { + throw std::runtime_error("Unexpected response payload"); + } + + memcpy(&hesai_range_all.start, &response[1], 2); + memcpy(&hesai_range_all.end, &response[3], 2); break; case 1: // for each channel - hesai_range_all.method = method; + //TODO break; case 2: // multi-section FOV - hesai_range_all.method = method; + //TODO break; } @@ -841,7 +665,8 @@ Status HesaiHwInterface::SetClockSource(int clock_source) std::vector request_payload; request_payload.emplace_back(clock_source & 0xff); - SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_CLOCK_SOURCE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -874,28 +699,28 @@ Status HesaiHwInterface::SetPtpConfig( request_payload.emplace_back(switch_type & 0xff); } - SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_PTP_CONFIG, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } HesaiPtpConfig HesaiHwInterface::GetPtpConfig() { - auto response_ptr = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_GET_PTP_CONFIG); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - HesaiPtpConfig hesai_ptp_config{}; - - int payload_pos = 0; - hesai_ptp_config.status = static_cast(response[payload_pos++]); - hesai_ptp_config.profile = static_cast(response[payload_pos++]); - hesai_ptp_config.domain = static_cast(response[payload_pos++]); - hesai_ptp_config.network = static_cast(response[payload_pos++]); - if (hesai_ptp_config.status == 0) { - hesai_ptp_config.logAnnounceInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logSyncInterval = static_cast(response[payload_pos++]); - hesai_ptp_config.logMinDelayReqInterval = static_cast(response[payload_pos++]); + if (response.size() < sizeof(HesaiPtpConfig)) { + throw std::runtime_error("Unexpected payload size"); + } else if (response.size() > sizeof(HesaiPtpConfig)) { + PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } + HesaiPtpConfig hesai_ptp_config; + memcpy(&hesai_ptp_config.status, response.data(), 1); + + 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()); @@ -905,7 +730,8 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() Status HesaiHwInterface::SendReset() { - SendReceive(PTC_COMMAND_RESET); + auto response_or_err = SendReceive(PTC_COMMAND_RESET); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } @@ -914,38 +740,23 @@ Status HesaiHwInterface::SetRotDir(int mode) std::vector request_payload; request_payload.emplace_back(mode & 0xff); - SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); + auto response_or_err = SendReceive(PTC_COMMAND_SET_ROTATE_DIRECTION, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); return Status::OK; } HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor() { - auto response_ptr = SendReceive(PTC_COMMAND_LIDAR_MONITOR); - auto & response = *response_ptr; + auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); - HesaiLidarMonitor hesai_lidar_monitor; - int payload_pos = 0; - hesai_lidar_monitor.input_voltage = response[payload_pos++] << 24; - hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] - << 16; - hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++] - << 8; - hesai_lidar_monitor.input_voltage = hesai_lidar_monitor.input_voltage | response[payload_pos++]; - hesai_lidar_monitor.input_current = response[payload_pos++] << 24; - hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] - << 16; - hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++] - << 8; - hesai_lidar_monitor.input_current = hesai_lidar_monitor.input_current | response[payload_pos++]; - hesai_lidar_monitor.input_power = response[payload_pos++] << 24; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 16; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++] << 8; - hesai_lidar_monitor.input_power = hesai_lidar_monitor.input_power | response[payload_pos++]; - - for (size_t i = 0; i < hesai_lidar_monitor.reserved.size(); i++) { - hesai_lidar_monitor.reserved[i] = static_cast(response[payload_pos++]); + if (response.size() != sizeof(HesaiLidarMonitor)) { + throw std::runtime_error("Unexpected payload size"); } + HesaiLidarMonitor hesai_lidar_monitor; + memcpy(&hesai_lidar_monitor, response.data(), sizeof(HesaiLidarMonitor)); + return hesai_lidar_monitor; } @@ -1153,8 +964,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } auto current_rotation_speed = hesai_config.spin_rate; - if (sensor_configuration->rotation_speed != current_rotation_speed) { - PrintInfo("current lidar rotation_speed: " + std::to_string(current_rotation_speed)); + if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { + PrintInfo("current lidar rotation_speed: " + std::to_string(static_cast(current_rotation_speed.value()))); PrintInfo( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); @@ -1172,8 +983,8 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( bool set_flg = false; std::stringstream ss; - ss << hesai_config.dest_ipaddr[0] << "." << hesai_config.dest_ipaddr[1] << "." - << hesai_config.dest_ipaddr[2] << "." << hesai_config.dest_ipaddr[3]; + ss << static_cast(hesai_config.dest_ipaddr[0]) << "." << static_cast(hesai_config.dest_ipaddr[1]) << "." + << static_cast(hesai_config.dest_ipaddr[2]) << "." << static_cast(hesai_config.dest_ipaddr[3]); auto current_host_addr = ss.str(); if (sensor_configuration->host_ip != current_host_addr) { set_flg = true; @@ -1182,17 +993,17 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( } auto current_host_dport = hesai_config.dest_LiDAR_udp_port; - if (sensor_configuration->data_port != current_host_dport) { + if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo("current lidar dest_LiDAR_udp_port: " + std::to_string(current_host_dport)); + PrintInfo("current lidar dest_LiDAR_udp_port: " + std::to_string(static_cast(current_host_dport.value()))); PrintInfo( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } auto current_host_tport = hesai_config.dest_gps_udp_port; - if (sensor_configuration->gnss_port != current_host_tport) { + if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo("current lidar dest_gps_udp_port: " + std::to_string(current_host_tport)); + PrintInfo("current lidar dest_gps_udp_port: " + std::to_string(static_cast(current_host_tport.value()))); PrintInfo( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } @@ -1212,7 +1023,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; - auto sync_angle = static_cast(hesai_config.sync_angle / 100); + auto sync_angle = static_cast(hesai_config.sync_angle.value() / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); int sync_flg = 1; if (scan_phase != sync_angle) { @@ -1296,18 +1107,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( set_flg = true; } else { auto current_cloud_min_angle = hesai_lidar_range_all.start; - if (static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle) { + if (static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle.value()) { set_flg = true; - PrintInfo("current lidar range.start: " + std::to_string(current_cloud_min_angle)); + PrintInfo("current lidar range.start: " + std::to_string(static_cast(current_cloud_min_angle.value()))); PrintInfo( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } auto current_cloud_max_angle = hesai_lidar_range_all.end; - if (static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle) { + if (static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle.value()) { set_flg = true; - PrintInfo("current lidar range.end: " + std::to_string(current_cloud_max_angle)); + PrintInfo("current lidar range.end: " + std::to_string(static_cast(current_cloud_max_angle.value()))); PrintInfo( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); @@ -1547,5 +1358,76 @@ void HesaiHwInterface::PrintDebug(const std::vector & bytes) PrintDebug(ss.str()); } +std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { + if (error_code.ok()) { + return "No error"; + } + + auto ptc_error = error_code.ptc_error_code; + auto error_flags = error_code.error_flags; + std::stringstream ss; + + if (ptc_error) { + ss << "Sensor error: 0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast(ptc_error) << ' '; + } + + switch(ptc_error) { + case PTC_ERROR_CODE_NO_ERROR: + break; + case PTC_ERROR_CODE_INVALID_INPUT_PARAM: + ss << "Invalid input parameter"; + break; + case PTC_ERROR_CODE_SERVER_CONN_FAILED: + ss << "Failure to connect to server"; + break; + case PTC_ERROR_CODE_INVALID_DATA: + ss << "No valid data returned"; + break; + case PTC_ERROR_CODE_OUT_OF_MEMORY: + ss << "Server does not have enough memory"; + break; + case PTC_ERROR_CODE_UNSUPPORTED_CMD: + ss << "Server does not support this command yet"; + break; + case PTC_ERROR_CODE_FPGA_COMM_FAILED: + ss << "Server failed to communicate with FPGA"; + break; + case PTC_ERROR_CODE_OTHER: + ss << "Unspecified internal error"; + break; + default: + ss << "Unknown error"; + break; + } + + if (!error_flags) { + return ss.str(); + } + + if (ptc_error) { + ss << ", "; + } + + ss << "Communication error: "; + std::vector nebula_errors; + + if (error_flags & TCP_ERROR_INCOMPLETE_RESPONSE) { + nebula_errors.push_back("Incomplete response payload"); + } + if (error_flags & TCP_ERROR_TIMEOUT) { + nebula_errors.push_back("Request timeout"); + } + if (error_flags & TCP_ERROR_UNEXPECTED_PAYLOAD) { + nebula_errors.push_back("Received payload but expected payload length 0"); + } + if (error_flags & TCP_ERROR_UNRELATED_RESPONSE) { + nebula_errors.push_back("Received unrelated response"); + } + + ss << boost::algorithm::join(nebula_errors, ", "); + + return ss.str(); +} + } // namespace drivers } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp index c23b76c55..cf8739279 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -20,6 +20,7 @@ #include #include +#include namespace nebula { diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index 85476075e..e64bf443b 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -74,7 +74,7 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory"); RCLCPP_INFO_STREAM(this->get_logger(), result); info_model = result.get_str_model(); - info_serial = std::string(result.sn.begin(), result.sn.end()); + info_serial = std::string(std::begin(result.sn), std::end(result.sn)); hw_interface_.SetTargetModel(result.model); RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); @@ -481,9 +481,9 @@ void HesaiHwMonitorRosWrapper::HesaiCheckStatus( uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - diagnostics.add("system_uptime", std::to_string(current_status->system_uptime)); - diagnostics.add("startup_times", std::to_string(current_status->startup_times)); - diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time)); + diagnostics.add("system_uptime", std::to_string(current_status->system_uptime.value())); + diagnostics.add("startup_times", std::to_string(current_status->startup_times.value())); + diagnostics.add("total_operation_time", std::to_string(current_status->total_operation_time.value())); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -532,9 +532,9 @@ void HesaiHwMonitorRosWrapper::HesaiCheckTemperature( if (current_status) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - for (size_t i = 0; i < current_status->temperature.size(); i++) { + for (size_t i = 0; i < std::size(current_status->temperature); i++) { diagnostics.add( - temperature_names[i], GetFixedPrecisionString(current_status->temperature[i] * 0.01)); + temperature_names[i], GetFixedPrecisionString(current_status->temperature[i].value() * 0.01)); } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -549,7 +549,7 @@ void HesaiHwMonitorRosWrapper::HesaiCheckRpm( 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)); + diagnostics.add("motor_speed", std::to_string(current_status->motor_speed.value())); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -598,11 +598,11 @@ void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; diagnostics.add( - "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage * 0.01) + " V"); + "input_voltage", GetFixedPrecisionString(current_monitor->input_voltage.value() * 0.01) + " V"); diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor->input_current * 0.01) + " mA"); + "input_current", GetFixedPrecisionString(current_monitor->input_current.value() * 0.01) + " mA"); diagnostics.add( - "input_power", GetFixedPrecisionString(current_monitor->input_power * 0.01) + " W"); + "input_power", GetFixedPrecisionString(current_monitor->input_power.value() * 0.01) + " W"); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else {