Merge branch 'ot-128' into hesai_ptp_params

tier4 · Dec 25, 2023 · c402492 · c402492
2 parents fbab6ef + 05a3b24
commit c402492
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 4 deletions.
diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp
@@ -397,6 +397,8 @@ inline ReturnMode ReturnModeFromStringHesai(
   switch (sensor_model) {
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
       if (return_mode == "Last") return ReturnMode::LAST;
       if (return_mode == "Strongest") return ReturnMode::STRONGEST;
       if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST;
@@ -429,6 +431,8 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode
   switch (sensor_model) {
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
       if (return_mode == 0) return ReturnMode::LAST;
       if (return_mode == 1) return ReturnMode::STRONGEST;
       if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST;
@@ -460,6 +464,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode
   switch (sensor_model) {
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
       if (return_mode == ReturnMode::LAST) return 0;
       if (return_mode == ReturnMode::STRONGEST) return 1;
       if (return_mode == ReturnMode::DUAL_LAST_STRONGEST

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
@@ -113,7 +113,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase
   std::timed_mutex tms_;
   int tms_fail_cnt = 0;
   int tms_fail_cnt_max = 3;
-  bool wl = true;
+  bool wl = false;
   bool is_solid_state = false;
   int target_model_no;
 

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
@@ -2900,6 +2900,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp(
 HesaiStatus HesaiHwInterface::CheckAndSetConfig(
   std::shared_ptr<HesaiSensorConfiguration> sensor_configuration, HesaiConfig hesai_config)
 {
+  using namespace std::chrono_literals;
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
   std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl;
 #endif
@@ -2922,7 +2923,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
       SetReturnMode(return_mode_int);
     });
     t.join();
+    std::this_thread::sleep_for(100ms);
   }
+
   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));
@@ -2932,10 +2935,12 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
     if (UseHttpSetSpinRate()) {
       SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed);
     } else {
+      PrintInfo("Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed) );
       std::thread t(
         [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); });
       t.join();
     }
+    std::this_thread::sleep_for(100ms);
   }
 
   bool set_flg = false;
@@ -2975,9 +2980,11 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
         sensor_configuration->gnss_port);
     });
     t.join();
+    std::this_thread::sleep_for(100ms);
   }
 
-  if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){
+  if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128
+    && sensor_configuration->sensor_model != SensorModel::HESAI_PANDARQT128) {
     set_flg = true;
     auto sync_angle = static_cast<int>(hesai_config.sync_angle / 100);
     auto scan_phase = static_cast<int>(sensor_configuration->scan_phase);
@@ -2993,6 +3000,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
         SetSyncAngle(sync_flg, scan_phase);
       });
       t.join();
+      std::this_thread::sleep_for(100ms);
     }
 
     std::thread t([this, sensor_configuration] {
@@ -3018,6 +3026,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
       );
     });
     t.join();
+    std::this_thread::sleep_for(100ms);
   }
   else { //AT128 only supports PTP setup via HTTP
     PrintInfo("Trying to set SyncAngle via HTTP");
@@ -3209,8 +3218,8 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod
       return 38;
     case SensorModel::HESAI_PANDAR128_E3X://check required
       return 40;
-    case SensorModel::HESAI_PANDAR128_E4X://check required
-      return 40;
+    case SensorModel::HESAI_PANDAR128_E4X://OT128
+      return 42;
     case SensorModel::HESAI_PANDARAT128:
       return 48;
     // All other vendors and unknown sensors
@@ -3251,6 +3260,9 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model)
     case 38:
       return false;
       break;
+    case 42:
+      return false;
+      break;
     case 48:
       return false;
       break;
@@ -3290,6 +3302,9 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model)
     case 38:
       return false;
       break;
+    case 42:
+      return false;
+      break;
     case 48:
       return false;
       break;