fix(hesai_hw_interface): hesai interface (#72)

* Fix get lidar calibration (#52) * Fix(GetLidarCalibration) * Skip GetInventory if unable to connect * fix monitor * clean * remove comments Signed-off-by: amc-nu <[email protected]> * at128. remove warnings Signed-off-by: amc-nu <[email protected]> * decoder. check calibration Signed-off-by: amc-nu <[email protected]> * Saving calibration data from sensor (except AT128) * Saving correction data from sensor (AT128) * fix(AT128 path) * info for debugging * nebula_hesai_ros. at128 fallback to offline Signed-off-by: amc-nu <[email protected]> * syncGetLidarCalibrationFromSensor -> GetLidarCalibrationFromSensor (AT128) * hesai_decoder_ros. message on tcp fail Signed-off-by: amc-nu <[email protected]> * comment out da902901c4cd900bfbf906caf76ae143fe4a88a6 * Check "start of packet" in correction data * hesai_decoder. save dat file Signed-off-by: amc-nu <[email protected]> * TcpDriver startup delay for setup_sensor * delay & retry * at1282ex. return mode checks Signed-off-by: amc-nu <[email protected]> * check lock * check "isOpen" --------- Signed-off-by: amc-nu <[email protected]> Co-authored-by: Kyutoku <[email protected]> * cspell. fix typos Signed-off-by: amc-nu <[email protected]> * cspell. fix typos Signed-off-by: amc-nu <[email protected]> --------- Signed-off-by: amc-nu <[email protected]> Co-authored-by: Kyutoku <[email protected]>
tier4 · Oct 25, 2023 · 476224d · 476224d
1 parent 4010d45
commit 476224d
Show file tree

Hide file tree

Showing 10 changed files with 711 additions and 95 deletions.
diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp
@@ -65,7 +65,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
     return Status::OK;
   }
 
-  /// @brief Loading calibration data (not used)
+  /// @brief Loading calibration data
   /// @param calibration_content
   /// @return Resulting status
   inline nebula::Status LoadFromString(const std::string & calibration_content)
@@ -80,11 +80,20 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
     int laser_id;
     float elevation;
     float azimuth;
-    while (!ss.eof()) {
-      ss >> laser_id >> sep >> elevation >> sep >> azimuth;
+
+    std::string line;
+    while (std::getline(ss, line)) {
+      if (line.empty()) {
+        continue;
+      }
+      std::stringstream line_ss;
+      line_ss << line;
+      line_ss >> laser_id >> sep >> elevation >> sep >> azimuth;
       elev_angle_map[laser_id - 1] = elevation;
       azimuth_offset_map[laser_id - 1] = azimuth;
+//      std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << azimuth << std::endl;
     }
+//    std::cout << "LoadFromString fin" << std::endl;
     return Status::OK;
   }
 
@@ -108,6 +117,23 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
 
     return Status::OK;
   }
+
+  /// @brief Saving calibration data from string
+  /// @param calibration_file path
+  /// @param calibration_string calibration string
+  /// @return Resulting status
+  inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string)
+  {
+    std::ofstream ofs(calibration_file);
+    if (!ofs) {
+      return Status::CANNOT_SAVE_FILE;
+    }
+    ofs << calibration_string;
+//    std::cout << calibration_string << std::endl;
+    ofs.close();
+
+    return Status::OK;
+  }
 };
 
 /// @brief struct for Hesai correction configuration (for AT)
@@ -135,7 +161,11 @@ struct HesaiCorrection
   /// @return Resulting status
   inline nebula::Status LoadFromBinary(const std::vector<uint8_t> & buf)
   {
-    size_t index = 0;
+    size_t index;
+    for (index = 0; index < buf.size()-1; index++) {
+      if(buf[index]==0xee && buf[index+1]==0xff)
+        break;
+    }
     delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff));
     versionMajor = buf[index + 2] & 0xff;
     versionMinor = buf[index + 3] & 0xff;
@@ -275,6 +305,38 @@ struct HesaiCorrection
     return Status::OK;
   }
 
+  /// @brief Save correction data from binary buffer
+  /// @param correction_file path
+  /// @param buf correction binary
+  /// @return Resulting status
+  inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector<uint8_t> & buf)
+  {
+    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;
+  }
+
   static const int STEP3 = 200 * 256;
 
   /// @brief Get azimuth adjustment for channel and precision azimuth
@@ -395,7 +457,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode
     case SensorModel::HESAI_PANDARAT128:
       if (return_mode == ReturnMode::LAST) return 0;
       if (return_mode == ReturnMode::STRONGEST) return 1;
-      if (return_mode == ReturnMode::DUAL_LAST_STRONGEST) return 2;
+      if (return_mode == ReturnMode::DUAL_LAST_STRONGEST
+      || return_mode == ReturnMode::DUAL) return 2;
       if (return_mode == ReturnMode::FIRST) return 3;
       if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4;
       if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5;

diff --git a/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp
@@ -248,6 +248,9 @@ struct HesaiInventory
       case 17:
         return "Pandar40M";
         break;
+      case 20:
+        return "PandarMind(PM64)";
+        break;
       case 25:
         return "PandarXT32";
         break;

diff --git a/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
@@ -166,6 +166,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase
 public:
   /// @brief Constructor
   HesaiHwInterface();
+  /// @brief Destructor
+  ~HesaiHwInterface();
   /// @brief Initializing tcp_driver for TCP communication
   /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration
   /// @return Resulting status
@@ -363,6 +365,35 @@ class HesaiHwInterface : NebulaHwInterfaceBase
   /// @param with_run Automatically executes run() of TcpDriver
   /// @return Resulting status
   Status GetPtpDiagGrandmaster(bool with_run = true);
+
+  /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync)
+  /// @param target_tcp_driver TcpDriver used
+  /// @param callback Callback function for received HesaiInventory
+  /// @return Resulting status
+  Status syncGetInventory(
+    std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver,
+    std::function<void(HesaiInventory & result)> callback);
+  /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync)
+  /// @param target_tcp_driver TcpDriver used
+  /// @return Resulting status
+  Status syncGetInventory(
+    std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver);
+  /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync)
+  /// @param ctx IO Context used
+  /// @param callback Callback function for received HesaiInventory
+  /// @return Resulting status
+  Status syncGetInventory(
+    std::shared_ptr<boost::asio::io_context> ctx,
+    std::function<void(HesaiInventory & result)> callback);
+  /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync)
+  /// @param ctx IO Context used
+  /// @return Resulting status
+  Status syncGetInventory(std::shared_ptr<boost::asio::io_context> ctx);
+  /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION (sync)
+  /// @param callback Callback function for received HesaiInventory
+  /// @return Resulting status
+  Status syncGetInventory(std::function<void(HesaiInventory & result)> callback);
+
   /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO
   /// @param target_tcp_driver TcpDriver used
   /// @param callback Callback function for received HesaiInventory
@@ -935,11 +966,21 @@ class HesaiHwInterface : NebulaHwInterfaceBase
   /// @return Resulting status
   HesaiStatus CheckAndSetConfig();
 
+  /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel
+  /// @param model 
+  /// @return 
+  int NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model);
+
   /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the
   /// target model)
   /// @param model Model number
   void SetTargetModel(int model);
 
+  /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the
+  /// target model)
+  /// @param model Model
+  void SetTargetModel(nebula::drivers::SensorModel model);
+
   /// @brief Whether to use HTTP for setting SpinRate
   /// @param model Model number
   /// @return Use HTTP