WIP

Author: mojomex

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp

@@ -354,9 +354,9 @@ struct HesaiLidarStatus
   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;
+  uint8_t gps_pps_lock;//wrong
+  uint8_t gps_gprmc_status;//wrong
+  big_uint32_buf_t startup_times;//wrong
   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
@@ -528,22 +528,22 @@ struct HesaiLidarMonitor
 
 struct HesaiFaultModeInfo
 {
-  char work_mode;
-  char fault_code[4];
+  uint8_t work_mode;
+  uint8_t fault_code;
 
   [[nodiscard]] std::string describeWorkMode() const
   {
-    std::string prefix = std::string(&work_mode, 1) + " - ";
+    std::string prefix = std::to_string(work_mode) + " - ";
     switch (work_mode) {
-      case '0':
+      case 0:
         return prefix + "Energy-Saving";
-      case '1':
+      case 1:
         return prefix + "Standard";
-      case '2':
+      case 2:
         return prefix + "Standby";
-      case '3':
+      case 3:
         return prefix + "High-Temp-Shutdown";
-      case '4':
+      case 4:
         return prefix + "Other-Shutdown";
     }
 
@@ -552,9 +552,6 @@ struct HesaiFaultModeInfo
 
   [[nodiscard]] std::string describeFaultCode() const
   {
-    std::string hex = std::string(fault_code, sizeof(fault_code));
-    uint8_t fault_code = std::stoul(hex, nullptr, 16);
-
     std::bitset<8> bits{fault_code};
     std::vector<std::string> fault_messages;
 
@@ -582,13 +579,16 @@ struct HesaiFaultModeInfo
       return "OK";
     }
 
-    return "Fault code " + hex + ": " + boost::join(fault_messages, ", ");
+    std::stringstream ss;
+    ss << "0x" << std::hex << std::setfill('0') << std::setw(2) << fault_code;
+
+    return "Fault code " + ss.str() + ": " + boost::join(fault_messages, ", ");
   }
 
   bool ok()
   {
-    bool work_mode_ok = work_mode != '3' && work_mode != '4';
-    bool fault_code_ok = std::strncmp(fault_code, "0x00", sizeof(fault_code));
+    bool work_mode_ok = work_mode < 3;
+    bool fault_code_ok = fault_code == 0;
     return work_mode_ok && fault_code_ok;
   }
 

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -451,6 +451,10 @@ Status HesaiHwInterface::SetControlPort(
 
 Status HesaiHwInterface::SetLidarRange(int method, std::vector<unsigned char> data)
 {
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    return Status::OK;
+  }
+
   // 0 - for all channels : 5-1 bytes
   // 1 - for each channel : 323-1 bytes
   // 2 - multi-section FOV : 1347-1 bytes
@@ -465,6 +469,10 @@ Status HesaiHwInterface::SetLidarRange(int method, std::vector<unsigned char> da
 
 Status HesaiHwInterface::SetLidarRange(int start, int end)
 {
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    return Status::OK;
+  }
+
   // 0 - for all channels : 5-1 bytes
   // 1 - for each channel : 323-1 bytes
   // 2 - multi-section FOV : 1347-1 bytes
@@ -483,6 +491,10 @@ Status HesaiHwInterface::SetLidarRange(int start, int end)
 
 HesaiLidarRangeAll HesaiHwInterface::GetLidarRange()
 {
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    return HesaiLidarRangeAll();
+  }
+
   auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE);
   auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
 
@@ -1295,9 +1307,14 @@ template <typename T>
 T HesaiHwInterface::CheckSizeAndParse(const std::vector<uint8_t> & data)
 {
   if (data.size() < sizeof(T)) {
-    throw std::runtime_error("Attempted to parse too-small payload");
+    std::stringstream ss{};
+    ss << "Attempted to parse too-small payload: expected " << sizeof(T) << ", got " << data.size();
+    throw std::runtime_error(ss.str());
   } else if (data.size() > sizeof(T)) {
-    PrintError("Sensor returned longer payload than expected. Will parse anyway.");
+    std::stringstream ss{};
+    ss << "Sensor returned too-large payload: expected " << sizeof(T) << ", got " << data.size()
+       << ". Will parse anyway.";
+    PrintError(ss.str());
   }
 
   T parsed;

nebula_ros/include/nebula_ros/hesai/hw_monitor/tcp_fault_mode_info_provider.hpp

@@ -62,7 +62,12 @@ class TcpFaultModeInfoProvider : public DiagnosticProvider
     diagnostics.add("operation_mode", current_data_->describeWorkMode());
     diagnostics.add("faults", current_data_->describeFaultCode());
 
-    diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "");
+    if (current_data_->ok()) {
+      diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "");
+      return;
+    }
+
+    diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "");
   }
 
   std::unique_ptr<HesaiFaultModeInfo> current_data_;

nebula_ros/src/hesai/hw_monitor_wrapper.cpp

@@ -7,6 +7,7 @@
 #include "nebula_ros/hesai/hw_monitor/http_lidar_monitpr_provider.hpp"
 #include "nebula_ros/hesai/hw_monitor/tcp_lidar_monitor_provider.hpp"
 #include "nebula_ros/hesai/hw_monitor/tcp_lidar_status_provider.hpp"
+#include "nebula_ros/hesai/hw_monitor/tcp_fault_mode_info_provider.hpp"
 
 #include <nebula_common/nebula_common.hpp>
 
@@ -83,6 +84,9 @@ HesaiHwMonitorWrapper::getDiagnosticProviders(drivers::SensorModel sensor_model)
   result.push_back(status_provider);
 
   if (sensor_model == drivers::SensorModel::HESAI_PANDARAT128) {
+    auto fault_info_provider = std::make_shared<hw_monitor::TcpFaultModeInfoProvider>();
+    result.push_back(fault_info_provider);
+
     return result;
   }