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 index 274528172..21849fa37 100644 --- 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 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 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 index ce3bd686c..5c51adbc4 100644 --- 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(