feat(hesai): add support for setting hires mode for Pandar128 and OT128

Signed-off-by: Max SCHMELLER <[email protected]>

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;
 }
 

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);
 

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<unsigned char> 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<unsigned char> 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

nebula_ros/launch/hesai_launch_all_hw.xml

@@ -5,6 +5,7 @@
     <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
     <arg name="frame_id"  default="hesai"/>
     <arg name="scan_phase" default="0.0" />
+    <arg name="hires_mode" default="false" description="Whether to set the sensor to high resolution mode. See readme for supported sensors." />
 
     <arg name="packet_mtu_size" default="1500" description="Packet MTU size"/>
     <arg name="dual_return_distance_threshold" default="0.1" description="Distance threshold of dual return mode"/>
@@ -70,6 +71,7 @@
             <param name="ptp_domain" value="$(var ptp_domain)"/>
             <param name="ptp_transport_type" value="$(var ptp_transport_type)"/>
             <param name="ptp_switch_type" value="$(var ptp_switch_type)"/>
+            <param name="hires_mode" value="$(var hires_mode)"/>
         </node>
 
         <node pkg="nebula_ros" exec="hesai_hw_monitor_ros_wrapper_node"

nebula_ros/launch/hesai_launch_component.launch.xml

@@ -5,6 +5,7 @@
     <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
     <arg name="frame_id"  default="hesai"/>
     <arg name="scan_phase" default="0.0" />
+    <arg name="hires_mode" default="false" description="Whether to set the sensor to high resolution mode. See readme for supported sensors." />
 
     <arg name="packet_mtu_size" default="1500" description="Packet MTU size"/>
     <arg name="dual_return_distance_threshold" default="0.1" description="Distance threshold of dual return mode"/>
@@ -82,6 +83,7 @@
                 <param name="ptp_domain" value="$(var ptp_domain)"/>
                 <param name="ptp_transport_type" value="$(var ptp_transport_type)"/>
                 <param name="ptp_switch_type" value="$(var ptp_switch_type)"/>
+                <param name="hires_mode" value="$(var hires_mode)"/>
                 <extra_arg name="use_intra_process_comms" value="true" />
             </composable_node>
         </load_composable_node>

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)]
     )

nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp

@@ -389,6 +389,11 @@ Status HesaiHwInterfaceRosWrapper::GetParameters(
     this->declare_parameter<uint8_t>("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<bool>("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())