refactor(hesai): per-sensor diagnostic struct definitions (#208)

* fix hesaiconfig ptp * chagnge struct name * refactor lidaStatus * fix lidarstatus tcp * fix lidarmonitor and some issues * fix lidarstatus binary issue * refactor lidarConfig * fix any codes * fix as review * fix reviewd points * fix reviewd point * chore(hesai): reduce log spam Signed-off-by: Max SCHMELLER <[email protected]> * chore(hesai_cmd_response): make all inheritances public Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai): correct sensor get command return logic after merge Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai): add missing diagnostics summary, display strings without quotes Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai): fix diagnostics keys being output in the wrong categories Signed-off-by: Max SCHMELLER <[email protected]> * feat(hesai): change PTP status to ERROR when not synchronized Signed-off-by: Max SCHMELLER <[email protected]> * chore(hesai): remove duplicate config printing Signed-off-by: Max SCHMELLER <[email protected]> * chore(hesai): remove unnecessary print statements Signed-off-by: Max SCHMELLER <[email protected]> * chore(hesai): lessen log spam, error messages Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai): add back support for pandar64 Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai): add back support for QT128 Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai): disable voltage monitor output for sensors that don't support it Signed-off-by: Max SCHMELLER <[email protected]> * chore(hesai_cmd_response): reduce compiler warnings Signed-off-by: Max SCHMELLER <[email protected]> * fix(hesai_hw_interface): for sensors we couldn't test, fall back to using the simplest available structs to parse Signed-off-by: Max SCHMELLER <[email protected]> * chore(hesai_hw_interface): remove temporary cxxabi usage Signed-off-by: Max SCHMELLER <[email protected]> * fix(nebula_common): properly add nlohmann_json dependency Signed-off-by: Max SCHMELLER <[email protected]> * fix(pandar64): add calibration_file to schema Signed-off-by: Max SCHMELLER <[email protected]> --------- Signed-off-by: Max SCHMELLER <[email protected]> Co-authored-by: Max SCHMELLER <[email protected]> Co-authored-by: Max Schmeller <[email protected]>
tier4 · Nov 8, 2024 · a5fb803 · a5fb803
1 parent b48d713
commit a5fb803
Show file tree

Hide file tree

Showing 14 changed files with 888 additions and 433 deletions.
diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt
@@ -4,6 +4,7 @@ project(nebula_common)
 find_package(ament_cmake_auto REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common)
 find_package(yaml-cpp REQUIRED)
+find_package(nlohmann_json REQUIRED)
 
 # Default to C++17
 if (NOT CMAKE_CXX_STANDARD)
@@ -28,11 +29,13 @@ include_directories(
     SYSTEM
     ${YAML_CPP_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
+    ${NLOHMANN_JSON_INCLUDE_DIRS}
 )
 
 link_libraries(
     ${PCL_LIBRARIES}
     ${YAML_CPP_LIBRARIES}
+    ${NLOHMANN_JSON_LIBRARIES}
 )
 
 add_library(nebula_common SHARED

diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp
@@ -349,26 +349,22 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   inline nebula::Status save_to_file_from_bytes(
     const std::string & correction_file, const std::vector<uint8_t> & buf) override
   {
-    std::cerr << "Saving in: " << correction_file << "\n";
     std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary);
     if (!ofs) {
       std::cerr << "Could not create file: " << correction_file << "\n";
       return Status::CANNOT_SAVE_FILE;
     }
-    std::cerr << "Writing start...." << buf.size() << "\n";
     bool sop_received = false;
     for (const auto & byte : buf) {
       if (!sop_received) {
         if (byte == 0xEE) {
-          std::cerr << "SOP received....\n";
           sop_received = true;
         }
       }
       if (sop_received) {
         ofs << byte;
       }
     }
-    std::cerr << "Closing file\n";
     ofs.close();
     if (sop_received) return Status::OK;
     return Status::INVALID_CALIBRATION_FILE;

diff --git a/nebula_common/include/nebula_common/util/string_conversions.hpp b/nebula_common/include/nebula_common/util/string_conversions.hpp
@@ -14,6 +14,8 @@
 
 #pragma once
 
+#include <nlohmann/json.hpp>
+
 #include <ostream>
 #include <sstream>
 #include <string>
@@ -41,4 +43,20 @@ std::enable_if_t<IsStreamable<T>::value, std::string> to_string(const T & value)
   return ss.str();
 }
 
+template <size_t N>
+std::string to_string(const char value[N])
+{
+  return std::string(value, strnlen(value, N));
+}
+
+inline std::string to_string(const nlohmann::ordered_json & j)
+{
+  return j.is_string() ? j.template get<std::string>() : j.dump();
+}
+
+inline std::string to_string(const nlohmann::json & j)
+{
+  return to_string(nlohmann::ordered_json(j));
+}
+
 }  // namespace nebula::util
diff --git a/nebula_common/package.xml b/nebula_common/package.xml
@@ -13,6 +13,7 @@
   <buildtool_depend>ros_environment</buildtool_depend>
 
   <depend>libpcl-all-dev</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>yaml-cpp</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp
@@ -254,7 +254,6 @@ class HesaiDecoder : public HesaiScanDecoder
     logger_(rclcpp::get_logger("HesaiDecoder"))
   {
     logger_.set_level(rclcpp::Logger::Level::Debug);
-    RCLCPP_INFO_STREAM(logger_, *sensor_configuration_);
 
     decode_pc_ = std::make_shared<NebulaPointCloud>();
     output_pc_ = std::make_shared<NebulaPointCloud>();