fix(at128): disable unsupported lidar range feature for AT128

tier4 · Jul 2, 2024 · 0245df6 · 0245df6
1 parent 89dbb41
commit 0245df6
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 2 deletions.
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
@@ -441,6 +441,9 @@ Status HesaiHwInterface::SetControlPort(
 
 Status HesaiHwInterface::SetLidarRange(int method, std::vector<unsigned char> data)
 {
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    return Status::SENSOR_CONFIG_ERROR;
+  }
   // 0 - for all channels : 5-1 bytes
   // 1 - for each channel : 323-1 bytes
   // 2 - multi-section FOV : 1347-1 bytes
@@ -455,6 +458,9 @@ 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::SENSOR_CONFIG_ERROR;
+  }
   // 0 - for all channels : 5-1 bytes
   // 1 - for each channel : 323-1 bytes
   // 2 - multi-section FOV : 1347-1 bytes
@@ -473,6 +479,9 @@ Status HesaiHwInterface::SetLidarRange(int start, int end)
 
 HesaiLidarRangeAll HesaiHwInterface::GetLidarRange()
 {
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    throw std::runtime_error("Not supported on this sensor");
+  }
   auto response_or_err = SendReceive(PTC_COMMAND_GET_LIDAR_RANGE);
   auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
 
@@ -505,6 +514,10 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange()
 Status HesaiHwInterface::checkAndSetLidarRange(
   const HesaiCalibrationConfigurationBase & calibration)
 {
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    return Status::SENSOR_CONFIG_ERROR;
+  }
+
   int cloud_min = sensor_configuration_->cloud_min_angle * 10;
   int cloud_max = sensor_configuration_->cloud_max_angle * 10;
 
@@ -1034,6 +1047,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig()
   });
   t.join();
 
+  if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) {
+    return Status::OK;
+  }
+
   std::thread t2([this] {
     auto result = GetLidarRange();
     std::stringstream ss;

diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp
@@ -52,7 +52,9 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
       (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str());
   }
 
-  if (hw_interface_wrapper_) {
+  if (
+    hw_interface_wrapper_ &&
+    sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) {
     auto status =
       hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*calibration_result.value());
     if (status != Status::OK) {
@@ -350,7 +352,9 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(
     RCLCPP_INFO_STREAM(get_logger(), "Changed calibration to '" << new_cfg.calibration_path << "'");
   }
 
-  if (new_calibration_ptr && hw_interface_wrapper_) {
+  if (
+    new_calibration_ptr && hw_interface_wrapper_ &&
+    sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) {
     auto status = hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*new_calibration_ptr);
     if (status != Status::OK) {
       RCLCPP_ERROR_STREAM(