diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 9c6e5d555..b778bc5e3 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -26,6 +26,7 @@ struct HesaiSensorConfiguration : LidarConfigurationBase uint8_t ptp_domain; PtpTransportType ptp_transport_type; PtpSwitchType ptp_switch_type; + bool hires_mode; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -39,7 +40,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) << ", PtpTransportType:" << arg.ptp_transport_type - << ", PtpSwitchType:" << arg.ptp_switch_type; + << ", PtpSwitchType:" << arg.ptp_switch_type + << ", HighResolutionMode:" << arg.hires_mode; return os; } diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 1b21ddae2..a1d1262de 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -61,6 +61,8 @@ const uint8_t PTC_COMMAND_SET_LIDAR_RANGE = 0x22; const uint8_t PTC_COMMAND_GET_LIDAR_RANGE = 0x23; const uint8_t PTC_COMMAND_SET_PTP_CONFIG = 0x24; const uint8_t PTC_COMMAND_GET_PTP_CONFIG = 0x26; +const uint8_t PTC_COMMAND_SET_HIGH_RESOLUTION_MODE = 0x29; +const uint8_t PTC_COMMAND_GET_HIGH_RESOLUTION_MODE = 0x28; const uint8_t PTC_COMMAND_RESET = 0x25; const uint8_t PTC_COMMAND_SET_ROTATE_DIRECTION = 0x2a; const uint8_t PTC_COMMAND_LIDAR_MONITOR = 0x27; @@ -313,6 +315,12 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE /// @return Resulting status HesaiLidarRangeAll GetLidarRange(); + /// @brief Setting values with PTC_COMMAND_GET_HIGH_RESOLUTION_MODE + /// @return Resulting status + Status SetHighResolutionMode(bool enable); + /// @brief Getting values with PTC_COMMAND_GET_HIGH_RESOLUTION_MODE + /// @return Resulting status + bool GetHighResolutionMode(); Status SetClockSource(int clock_source); 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 fccb2a622..b0c06ad91 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 @@ -660,6 +660,26 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange() return hesai_range_all; } +Status HesaiHwInterface::SetHighResolutionMode(bool enable) { + std::vector request_payload; + request_payload.emplace_back(enable ? 0x01 : 0x00); + + auto response_or_err = SendReceive(PTC_COMMAND_SET_HIGH_RESOLUTION_MODE, request_payload); + response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + return Status::OK; +} + +bool HesaiHwInterface::GetHighResolutionMode() { + auto response_or_err = SendReceive(PTC_COMMAND_GET_HIGH_RESOLUTION_MODE); + auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({}))); + + if (response.size() != 1) { + throw std::runtime_error("Unexpected payload size"); + } + + return response[0] > 0x00; +} + Status HesaiHwInterface::SetClockSource(int clock_source) { std::vector request_payload; @@ -1081,6 +1101,22 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); } + if ( + sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR128_E3X || + sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR128_E4X) { + auto hires_currently_enabled = GetHighResolutionMode(); + + if (hires_currently_enabled != sensor_configuration->hires_mode) { + + PrintInfo("current lidar hires_mode: " + std::to_string(hires_currently_enabled)); + PrintInfo( + "current configuration hires_mode: " + std::to_string(sensor_configuration->hires_mode)); + + PrintInfo("Setting hires_mode via TCP."); + SetHighResolutionMode(sensor_configuration->hires_mode); + } + } + #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index f4a1cd41c..e4e23b2d6 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -5,6 +5,7 @@ + @@ -70,6 +71,7 @@ + + @@ -82,6 +83,7 @@ + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index b394498fb..58165e596 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -90,6 +90,7 @@ def launch_setup(context, *args, **kwargs): "ptp_domain": LaunchConfiguration("ptp_domain"), "ptp_transport_type": LaunchConfiguration("ptp_transport_type"), "ptp_switch_type": LaunchConfiguration("ptp_switch_type"), + "hires_mode": LaunchConfiguration("hires_mode"), }, ], ), @@ -189,6 +190,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("ptp_domain", "0"), add_launch_arg("ptp_transport_type", "UDP"), add_launch_arg("ptp_switch_type", "TSN"), + add_launch_arg("hires_mode", "false"), ] + [OpaqueFunction(function=launch_setup)] ) diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 1ac59b55b..db000bf19 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -389,6 +389,11 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( this->declare_parameter("ptp_domain", 0, descriptor); sensor_configuration.ptp_domain = this->get_parameter("ptp_domain").as_int(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = false; + sensor_configuration.hires_mode = this->declare_parameter("hires_mode", false, descriptor); + } if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); @@ -413,8 +418,16 @@ Status HesaiHwInterfaceRosWrapper::GetParameters( if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { // || return Status::SENSOR_CONFIG_ERROR; } + if ( + sensor_configuration.hires_mode && + !(sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDAR128_E3X || + sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDAR128_E4X)) { + RCLCPP_ERROR_STREAM(get_logger(), + "Only Pandar128 and OT128 support high resolution mode."); + return Status::SENSOR_CONFIG_ERROR; + } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); return Status::OK; } @@ -452,7 +465,8 @@ rcl_interfaces::msg::SetParametersResult HesaiHwInterfaceRosWrapper::paramCallba get_param(p, "rotation_speed", new_param.rotation_speed) || get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || get_param(p, "cloud_max_angle", new_param.cloud_max_angle) || - get_param(p, "dual_return_distance_threshold", new_param.dual_return_distance_threshold)) { + get_param(p, "dual_return_distance_threshold", new_param.dual_return_distance_threshold) || + get_param(p, "hires_mode", new_param.hires_mode)) { if (0 < sensor_model_str.length()) new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); if (0 < return_mode_str.length())