wip

Author: mojomex

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp

@@ -20,9 +20,13 @@
 #include <boost/format.hpp>
 
 #include <array>
+#include <bitset>
 #include <cstdint>
+#include <cstring>
+#include <iomanip>
 #include <ostream>
 #include <string>
+#include <vector>
 
 using namespace boost::endian;  // NOLINT(build/namespaces)
 
@@ -217,7 +221,7 @@ struct HesaiInventory
     return os;
   }
 
-  std::string get_str_model()
+  [[nodiscard]] std::string get_str_model() const
   {
     switch (model) {
       case 0:
@@ -399,7 +403,7 @@ struct HesaiLidarStatus
     return os;
   }
 
-  std::string get_str_gps_pps_lock()
+  [[nodiscard]] std::string get_str_gps_pps_lock() const
   {
     switch (gps_pps_lock) {
       case 1:
@@ -410,7 +414,8 @@ struct HesaiLidarStatus
         return "Unknown";
     }
   }
-  std::string get_str_gps_gprmc_status()
+
+  [[nodiscard]] std::string get_str_gps_gprmc_status() const
   {
     switch (gps_gprmc_status) {
       case 1:
@@ -421,7 +426,8 @@ struct HesaiLidarStatus
         return "Unknown";
     }
   }
-  std::string get_str_ptp_clock_status()
+
+  [[nodiscard]] std::string get_str_ptp_clock_status() const
   {
     switch (ptp_clock_status) {
       case 0:
@@ -520,6 +526,82 @@ struct HesaiLidarMonitor
   }
 };
 
+struct HesaiFaultModeInfo
+{
+  char work_mode;
+  char fault_code[4];
+
+  [[nodiscard]] std::string describeWorkMode() const
+  {
+    std::string prefix = std::string(&work_mode, 1) + " - ";
+    switch (work_mode) {
+      case '0':
+        return prefix + "Energy-Saving";
+      case '1':
+        return prefix + "Standard";
+      case '2':
+        return prefix + "Standby";
+      case '3':
+        return prefix + "High-Temp-Shutdown";
+      case '4':
+        return prefix + "Other-Shutdown";
+    }
+
+    return prefix + "Unknown";
+  }
+
+  [[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;
+
+    if (bits[0]) {
+      fault_messages.emplace_back("energy-saving fault");
+    }
+
+    if (bits[4]) {
+      fault_messages.emplace_back("high-temp shutdown fault");
+    }
+
+    if (bits[5]) {
+      fault_messages.emplace_back("other shutdown fault");
+    }
+
+    // If any other fault bits are set, there are undocumented faults
+    bits.reset(0);
+    bits.reset(4);
+    bits.reset(5);
+    if (bits.any()) {
+      fault_messages.emplace_back(std::to_string(bits.count()) + " unknown faults");
+    }
+
+    if (fault_messages.empty()) {
+      return "OK";
+    }
+
+    return "Fault code " + hex + ": " + 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));
+    return work_mode_ok && fault_code_ok;
+  }
+
+  friend std::ostream & operator<<(std::ostream & os, nebula::HesaiFaultModeInfo const & arg)
+  {
+    os << "work_mode: " << arg.describeWorkMode();
+    os << ", ";
+    os << "fault_code: " << arg.describeFaultCode();
+
+    return os;
+  }
+};
+
 #pragma pack(pop)
 
 }  // namespace nebula

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -75,6 +75,7 @@ const uint8_t PTC_COMMAND_GET_PTP_CONFIG = 0x26;
 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_COMMAND_GET_FAULT_MODE_INFO = 0x8d;
 
 const uint8_t PTC_ERROR_CODE_NO_ERROR = 0x00;
 const uint8_t PTC_ERROR_CODE_INVALID_INPUT_PARAM = 0x01;
@@ -259,6 +260,9 @@ class HesaiHwInterface
   /// @brief Getting data with PTC_COMMAND_GET_LIDAR_STATUS
   /// @return Resulting status
   HesaiLidarStatus GetLidarStatus();
+  /// @brief Getting data with PTC_COMMAND_GET_FAULT_MODE_INFO
+  /// @return Resulting status
+  HesaiFaultModeInfo GetFaultModeInfo();
   /// @brief Setting value with PTC_COMMAND_SET_SPIN_RATE
   /// @param rpm Spin rate
   /// @return Resulting status

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -2,6 +2,8 @@
 
 #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
 
+#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp"
+
 // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
 
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
@@ -343,6 +345,13 @@ HesaiLidarStatus HesaiHwInterface::GetLidarStatus()
   return CheckSizeAndParse<HesaiLidarStatus>(response);
 }
 
+HesaiFaultModeInfo HesaiHwInterface::GetFaultModeInfo()
+{
+  auto response_or_err = SendReceive(PTC_COMMAND_GET_FAULT_MODE_INFO);
+  auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
+  return CheckSizeAndParse<HesaiFaultModeInfo>(response);
+}
+
 Status HesaiHwInterface::SetSpinRate(uint16_t rpm)
 {
   std::vector<unsigned char> request_payload;

nebula_ros/include/nebula_ros/hesai/hw_monitor/diagnostic_provider.hpp

@@ -0,0 +1,41 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <diagnostic_updater/diagnostic_status_wrapper.hpp>
+#include <nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp>
+#include <rclcpp/time.hpp>
+
+#include <functional>
+#include <map>
+#include <string>
+
+namespace nebula::ros::hw_monitor
+{
+
+class DiagnosticProvider
+{
+public:
+  using hook_t = std::function<void(diagnostic_updater::DiagnosticStatusWrapper &)>;
+
+  virtual std::map<std::string, hook_t> getHooks() = 0;
+  virtual void fetch(drivers::HesaiHwInterface & hw_interface, rclcpp::Clock & clock) = 0;
+
+  [[nodiscard]] virtual std::optional<rclcpp::Time> getLastUpdate() const = 0;
+
+  [[nodiscard]] virtual std::string getName() const = 0;
+};
+
+}  // namespace nebula::ros::hw_monitor

nebula_ros/include/nebula_ros/hesai/hw_monitor/http_lidar_monitpr_provider.hpp

@@ -0,0 +1,101 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "nebula_ros/hesai/hw_monitor/diagnostic_provider.hpp"
+
+#include <diagnostic_updater/diagnostic_status_wrapper.hpp>
+
+#include <diagnostic_msgs/msg/detail/diagnostic_status__struct.hpp>
+
+#include <boost/lexical_cast/bad_lexical_cast.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nebula::ros::hw_monitor
+{
+class HttpLidarMonitorProvider : public DiagnosticProvider
+{
+public:
+  std::map<std::string, hook_t> getHooks() override
+  {
+    return {{"hesai_voltage", [this](auto & diagnostics) { updateVoltage(diagnostics); }}};
+  }
+
+  void fetch(drivers::HesaiHwInterface & hw_interface, rclcpp::Clock & clock) override
+  {
+    hw_interface.GetLidarMonitorAsyncHttp([&](const std::string & str) {
+      std::scoped_lock lock(mtx_lidar_monitor_);
+      current_lidar_monitor_time_ = std::make_unique<rclcpp::Time>(clock.now());
+      current_data_ = std::make_unique<boost::property_tree::ptree>(hw_interface.ParseJson(str));
+    });
+  }
+
+  [[nodiscard]] std::optional<rclcpp::Time> getLastUpdate() const override
+  {
+    if (!current_lidar_monitor_time_) return {};
+    return *current_lidar_monitor_time_;
+  }
+
+  [[nodiscard]] std::string getName() const override { return "LidarMonitor"; }
+
+private:
+  void updateVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics)
+  {
+    std::scoped_lock lock(mtx_lidar_monitor_);
+    if (!current_data_) {
+      diagnostics.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "No data available");
+      return;
+    }
+
+    uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    std::vector<std::string> msgs;
+
+    std::vector<std::string> keys = {"lidarInCur", "lidarInVol"};
+
+    for (const auto & key : keys) {
+      std::optional<std::string> value_opt{};
+      std::string full_key = "Body." + key;
+
+      try {
+        auto result = current_data_->get_optional<std::string>(full_key);
+        if (result.has_value()) {
+          value_opt.emplace(result.value());
+        } else {
+          level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+          msgs.emplace_back("field '" + full_key + "' not present");
+        }
+      } catch (boost::bad_lexical_cast & ex) {
+        level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+        msgs.emplace_back("field '" + full_key + "' not parsed: " + std::string(ex.what()));
+      }
+
+      if (value_opt.has_value()) {
+        diagnostics.add(key, value_opt.value());
+      }
+    }
+
+    diagnostics.summary(level, boost::algorithm::join(msgs, ", "));
+  }
+
+  std::unique_ptr<boost::property_tree::ptree> current_data_;
+  std::unique_ptr<rclcpp::Time> current_lidar_monitor_time_;
+  std::mutex mtx_lidar_monitor_;
+};
+}  // namespace nebula::ros::hw_monitor

nebula_ros/include/nebula_ros/hesai/hw_monitor/string_utils.hpp

@@ -0,0 +1,31 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <iomanip>
+#include <sstream>
+#include <string>
+
+namespace nebula::ros::hw_monitor
+{
+
+inline std::string to_fixed(double value, int decimals)
+{
+  std::stringstream ss;
+  ss << std::fixed << std::setprecision(decimals) << value;
+  return ss.str();
+}
+
+}  // namespace nebula::ros::hw_monitor