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 <variant>
+
+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 <typename T, typename E>
+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<T>(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<T>(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<E>(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<T, E> 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 <boost/algorithm/string/join.hpp>
+#include <boost/endian/buffers.hpp>
 #include <boost/format.hpp>
 
+#include <array>
+#include <cstdint>
 #include <ostream>
+#include <string>
+
+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<char> portIdentity = std::vector<char>(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<char> lastGmPhaseChange = std::vector<char>(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<char> sn = std::vector<char>(18);
-  std::vector<char> date_of_manufacture = std::vector<char>(16);
-  std::vector<char> mac = std::vector<char>(6);
-  std::vector<char> sw_ver = std::vector<char>(16);
-  std::vector<char> hw_ver = std::vector<char>(16);
-  std::vector<char> control_fw_ver = std::vector<char>(16);
-  std::vector<char> sensor_fw_ver = std::vector<char>(16);
-  int angle_offset;
-  int model;
-  int motor_type;
-  int num_of_lines;
-  std::vector<unsigned char> reserved = std::vector<unsigned char>(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<int>(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<int>(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<int>(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<int> temperature = std::vector<int>(8);
-  int gps_pps_lock;
-  int gps_gprmc_status;
-  int startup_times;
-  int total_operation_time;
-  int ptp_clock_status;
-  std::vector<unsigned char> reserved = std::vector<unsigned char>(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<int>(arg.gps_pps_lock);
     os << ", ";
-    os << "gps_gprmc_status: " << arg.gps_gprmc_status;
+    os << "gps_gprmc_status: " << static_cast<int>(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<int>(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<int>(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<int>(arg.method);
     os << ", ";
-    os << "start: " << arg.start;
+    os << "start: " << static_cast<int>(arg.start.value());
     os << ", ";
-    os << "end: " << arg.end;
+    os << "end: " << static_cast<int>(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<int>(arg.status);
     os << ", ";
-    os << "profile: " << arg.profile;
+    os << "profile: " << static_cast<int>(arg.profile);
     os << ", ";
-    os << "domain: " << arg.domain;
+    os << "domain: " << static_cast<int>(arg.domain);
     os << ", ";
-    os << "network: " << arg.network;
+    os << "network: " << static_cast<int>(arg.network);
     if (arg.status == 0) {
       os << ", ";
-      os << "logAnnounceInterval: " << arg.logAnnounceInterval;
+      os << "logAnnounceInterval: " << static_cast<int>(arg.logAnnounceInterval);
       os << ", ";
-      os << "logSyncInterval: " << arg.logSyncInterval;
+      os << "logSyncInterval: " << static_cast<int>(arg.logSyncInterval);
       os << ", ";
-      os << "logMinDelayReqInterval: " << arg.logMinDelayReqInterval;
+      os << "logMinDelayReqInterval: " << static_cast<int>(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<unsigned char> reserved = std::vector<unsigned char>(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<int>(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 <memory>
 #include <mutex>
+#include <vector>
 
 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<std::vector<uint8_t>, ptc_error_t> ptc_cmd_result_t;
+
   std::unique_ptr<::drivers::common::IoContext> cloud_io_context_;
   std::shared_ptr<boost::asio::io_context> 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<uint8_t> & 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<std::vector<uint8_t>> SendReceive(
-    const uint8_t command_id, const std::vector<uint8_t> & payload = {});
+  ptc_cmd_result_t SendReceive(const uint8_t command_id, const std::vector<uint8_t> & 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<std::vector<uint8_t>> HesaiHwInterface::SendReceive(
+HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive(
   const uint8_t command_id, const std::vector<uint8_t> & payload)
 {
   uint32_t len = payload.size();
@@ -64,8 +64,12 @@ std::shared_ptr<std::vector<uint8_t>> HesaiHwInterface::SendReceive(
   send_buf.emplace_back(len & 0xff);
   send_buf.insert(send_buf.end(), payload.begin(), payload.end());
 
+  // These are shared_ptrs so that in case of request timeout, the callback (if ever called) can access valid memory
   auto recv_buf = std::make_shared<std::vector<uint8_t>>();
-  bool success = false;
+  auto response_complete = std::make_shared<bool>(false);
+
+  // Low byte is for PTC error code, the rest is nebula-specific
+  auto error_code = std::make_shared<ptc_error_t>();
 
   std::stringstream ss;
   ss << "0x" << std::setfill('0') << std::setw(2) << std::hex << static_cast<int>(command_id) << " (" << len << ") ";
@@ -84,23 +88,33 @@ std::shared_ptr<std::vector<uint8_t>> HesaiHwInterface::SendReceive(
   PrintDebug(log_tag + "Sending payload");
   tcp_driver_->asyncSendReceiveHeaderPayload(
     send_buf,
-    [this, log_tag, &success](const std::vector<uint8_t> & header_bytes) {
+    [this, log_tag, command_id, response_complete, error_code](const std::vector<uint8_t> & 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<uint8_t> & payload_bytes) {
+    [this, log_tag, recv_buf, response_complete, error_code](const std::vector<uint8_t> & 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<std::vector<uint8_t>> 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<uint8_t> HesaiHwInterface::GetLidarCalibrationBytes()
 {
-  auto response_ptr = SendReceive(PTC_COMMAND_GET_LIDAR_CALIBRATION);
-  return std::vector<uint8_t>(*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<long long>(response[payload_pos++]) << 56;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(response[payload_pos++]) << 48;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(response[payload_pos++]) << 40;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(response[payload_pos++]) << 32;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(response[payload_pos++]) << 24;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(response[payload_pos++]) << 16;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(response[payload_pos++]) << 8;
-  hesai_ptp_diag_status.master_offset =
-    hesai_ptp_diag_status.master_offset | static_cast<long long>(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<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.logMinDelayReqInterval = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.peerMeanPathDelay = static_cast<long long>(response[payload_pos++]) << 56;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]) << 48;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]) << 40;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]) << 32;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]) << 24;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]) << 16;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]) << 8;
-  hesai_ptp_diag_port.peerMeanPathDelay =
-    hesai_ptp_diag_port.peerMeanPathDelay | static_cast<long long>(response[payload_pos++]);
-  hesai_ptp_diag_port.logAnnounceInterval = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.announceReceiptTimeout = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.logSyncInterval = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.delayMechanism = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.logMinPdelayReqInterval = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_diag_port.versionNumber = static_cast<int>(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<long long>(response[payload_pos++]) << 56;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]) << 48;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]) << 40;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]) << 32;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]) << 24;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]) << 16;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]) << 8;
-  hesai_ptp_diag_time.master_offset =
-    hesai_ptp_diag_time.master_offset | static_cast<long long>(response[payload_pos++]);
-  hesai_ptp_diag_time.ingress_time = static_cast<long long>(response[payload_pos++]) << 56;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(response[payload_pos++]) << 48;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(response[payload_pos++]) << 40;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(response[payload_pos++]) << 32;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(response[payload_pos++]) << 24;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(response[payload_pos++]) << 16;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(response[payload_pos++]) << 8;
-  hesai_ptp_diag_time.ingress_time =
-    hesai_ptp_diag_time.ingress_time | static_cast<long long>(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<long long>(response[payload_pos++]) << 56;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(response[payload_pos++]) << 48;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(response[payload_pos++]) << 40;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(response[payload_pos++]) << 32;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(response[payload_pos++]) << 24;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(response[payload_pos++]) << 16;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(response[payload_pos++]) << 8;
-  hesai_ptp_diag_time.gmIdentity =
-    hesai_ptp_diag_time.gmIdentity | static_cast<long long>(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<int>(response[payload_pos++]);
-  hesai_ptp_diag_grandmaster.time_source = static_cast<int>(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<int>(response[payload_pos++]);
-  hesai_inventory.motor_type = static_cast<int>(response[payload_pos++]);
-  hesai_inventory.num_of_lines = static_cast<int>(response[payload_pos++]);
-  for (size_t i = 0; i < hesai_inventory.reserved.size(); i++) {
-    hesai_inventory.reserved[i] = static_cast<unsigned char>(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<int>(response[payload_pos++]);
-  hesai_config.ipaddr[1] = static_cast<int>(response[payload_pos++]);
-  hesai_config.ipaddr[2] = static_cast<int>(response[payload_pos++]);
-  hesai_config.ipaddr[3] = static_cast<int>(response[payload_pos++]);
-  hesai_config.mask[0] = static_cast<int>(response[payload_pos++]);
-  hesai_config.mask[1] = static_cast<int>(response[payload_pos++]);
-  hesai_config.mask[2] = static_cast<int>(response[payload_pos++]);
-  hesai_config.mask[3] = static_cast<int>(response[payload_pos++]);
-  hesai_config.gateway[0] = static_cast<int>(response[payload_pos++]);
-  hesai_config.gateway[1] = static_cast<int>(response[payload_pos++]);
-  hesai_config.gateway[2] = static_cast<int>(response[payload_pos++]);
-  hesai_config.gateway[3] = static_cast<int>(response[payload_pos++]);
-  hesai_config.dest_ipaddr[0] = static_cast<int>(response[payload_pos++]);
-  hesai_config.dest_ipaddr[1] = static_cast<int>(response[payload_pos++]);
-  hesai_config.dest_ipaddr[2] = static_cast<int>(response[payload_pos++]);
-  hesai_config.dest_ipaddr[3] = static_cast<int>(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<int>(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<int>(response[payload_pos++]);
-  hesai_config.udp_seq = static_cast<int>(response[payload_pos++]);
-  hesai_config.trigger_method = static_cast<int>(response[payload_pos++]);
-  hesai_config.return_mode = static_cast<int>(response[payload_pos++]);
-  hesai_config.standby_mode = static_cast<int>(response[payload_pos++]);
-  hesai_config.motor_status = static_cast<int>(response[payload_pos++]);
-  hesai_config.vlan_flag = static_cast<int>(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<int>(response[payload_pos++]);
-  hesai_config.noise_filtering = static_cast<int>(response[payload_pos++]);
-  hesai_config.reflectivity_mapping = static_cast<int>(response[payload_pos++]);
-  hesai_config.reserved[0] = static_cast<unsigned char>(response[payload_pos++]);
-  hesai_config.reserved[1] = static_cast<unsigned char>(response[payload_pos++]);
-  hesai_config.reserved[2] = static_cast<unsigned char>(response[payload_pos++]);
-  hesai_config.reserved[3] = static_cast<unsigned char>(response[payload_pos++]);
-  hesai_config.reserved[4] = static_cast<unsigned char>(response[payload_pos++]);
-  hesai_config.reserved[5] = static_cast<unsigned char>(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<int>(response[payload_pos++]);
-  hesai_status.gps_gprmc_status = static_cast<int>(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<int>(response[payload_pos++]);
-  for (size_t i = 0; i < hesai_status.reserved.size(); i++) {
-    hesai_status.reserved[i] = static_cast<unsigned char>(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<unsigned char> 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<unsigned char> 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<unsigned char> 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<unsigned char> 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<int>(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<unsigned char> 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<int>(response[payload_pos++]);
-  hesai_ptp_config.profile = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_config.domain = static_cast<int>(response[payload_pos++]);
-  hesai_ptp_config.network = static_cast<int>(response[payload_pos++]);
-  if (hesai_ptp_config.status == 0) {
-    hesai_ptp_config.logAnnounceInterval = static_cast<int>(response[payload_pos++]);
-    hesai_ptp_config.logSyncInterval = static_cast<int>(response[payload_pos++]);
-    hesai_ptp_config.logMinDelayReqInterval = static_cast<int>(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<unsigned char> 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<unsigned char>(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<int>(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<int>(hesai_config.dest_ipaddr[0]) << "." << static_cast<int>(hesai_config.dest_ipaddr[1]) << "."
+     << static_cast<int>(hesai_config.dest_ipaddr[2]) << "." << static_cast<int>(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<int>(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<int>(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<int>(hesai_config.sync_angle / 100);
+    auto sync_angle = static_cast<int>(hesai_config.sync_angle.value() / 100);
     auto scan_phase = static_cast<int>(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<int>(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle) {
+    if (static_cast<int>(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<int>(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<int>(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle) {
+    if (static_cast<int>(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<int>(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<uint8_t> & 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<int>(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<std::string> 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 <boost/lexical_cast.hpp>
 
 #include <thread>
+#include <array>
 
 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<std::string> 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<std::string> 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<std::string> 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<std::string> 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 {