feat(hesai): add high resolution mode setting for Pandar128 and OT128

build_depends.repos

@@ -8,4 +8,4 @@ repositories:
   ros2_socketcan:
     type: git
     url: https://github.com/knzo25/ros2_socketcan
-    version: feat/continental_fd
+    version: feat/continental_fd

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -42,6 +42,7 @@ struct HesaiSensorConfiguration : public 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
@@ -54,8 +55,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
      << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle
      << ", 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;
+     << ", PtpTransportType:" << arg.ptp_transport_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

@@ -72,6 +72,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;
@@ -322,6 +324,12 @@ class HesaiHwInterface
   /// @brief Getting values with PTC_COMMAND_GET_LIDAR_RANGE
   /// @return Resulting status
   HesaiLidarRangeAll GetLidarRange();
+  /// @brief Setting values with PTC_COMMAND_SET_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

@@ -503,6 +503,28 @@ 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;
@@ -924,6 +946,21 @@ 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/continental_launch_all_hw.xml

@@ -17,7 +17,7 @@
             <remap from="steering_angle_input" to="$(var steering_angle_topic)"/>
         </node>
     </group>
-    
+
     <group if="$(eval &quot;'$(var sensor_model)' == 'SRR520' &quot;)" >
 
         <node pkg="nebula_ros" exec="continental_srr520_ros_wrapper_node" name="nebula_continental_srr520" output="screen">

nebula_ros/launch/nebula_launch.py

@@ -110,6 +110,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"),
                     },
                 ],
             ),
@@ -206,6 +207,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/schema/Pandar128E4X.schema.json

@@ -92,6 +92,9 @@
         },
         "dual_return_distance_threshold": {
           "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold"
+        },
+        "hires_mode": {
+          "$ref": "sub/lidar_hesai.json#/definitions/hires_mode"
         }
       },
       "required": [

nebula_ros/schema/SRR520.schema.json

@@ -60,10 +60,14 @@
           "$ref": "#/definitions/SRR520"
         }
       },
-      "required": ["ros__parameters"],
+      "required": [
+        "ros__parameters"
+      ],
       "additionalProperties": false
     }
   },
-  "required": ["/**"],
+  "required": [
+    "/**"
+  ],
   "additionalProperties": false
 }

nebula_ros/schema/sub/communication.json

@@ -72,7 +72,11 @@
     "ptp_profile": {
       "type": "string",
       "default": "1588v2",
-      "enum": ["1588v2", "802.1as", "automotive"],
+      "enum": [
+        "1588v2",
+        "802.1as",
+        "automotive"
+      ],
       "description": "PTP profile."
     },
     "ptp_domain": {
@@ -85,13 +89,19 @@
     "ptp_transport_type": {
       "type": "string",
       "default": "UDP",
-      "enum": ["UDP", "L2"],
+      "enum": [
+        "UDP",
+        "L2"
+      ],
       "description": "1588v2 supports 'UDP' or 'L2', other profiles only L2 (HW)."
     },
     "ptp_switch_type": {
       "type": "string",
       "default": "TSN",
-      "enum": ["TSN", "NON_TSN"],
+      "enum": [
+        "TSN",
+        "NON_TSN"
+      ],
       "description": "For automotive profile,'TSN' or 'NON_TSN'."
     },
     "receiver_timeout_sec": {

nebula_ros/schema/sub/lidar_hesai.json

@@ -31,6 +31,11 @@
       "minimum": 300,
       "maximum": 1200,
       "multipleOf": 60
+    },
+    "hires_mode": {
+      "type": "boolean",
+      "description": "Whether to turn on or off the sensor's high resolution mode",
+      "default": false
     }
   }
 }

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -2,6 +2,8 @@
 
 #include "nebula_ros/hesai/hesai_ros_wrapper.hpp"
 
+#include "nebula_ros/common/parameter_descriptors.hpp"
+
 #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical"
 
 namespace nebula
@@ -92,6 +94,13 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams()
   config.max_range = declare_parameter<double>("max_range", param_read_write());
   config.packet_mtu_size = declare_parameter<uint16_t>("packet_mtu_size", param_read_only());
 
+  config.hires_mode = false;
+  if (
+    config.sensor_model == drivers::SensorModel::HESAI_PANDAR128_E4X ||
+    config.sensor_model == drivers::SensorModel::HESAI_PANDAR128_E3X) {
+    config.hires_mode = this->declare_parameter<bool>("hires_mode", param_read_write());
+  }
+
   {
     rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write();
     RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model);
@@ -257,7 +266,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(
     get_param(p, "rotation_speed", new_cfg.rotation_speed) |
     get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) |
     get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) |
-    get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold);
+    get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold) |
+    get_param(p, "hires_mode", new_cfg.hires_mode);
 
   // Currently, HW interface and monitor wrappers have only read-only parameters, so their update
   // logic is not implemented

nebula_tests/continental/parameter_descriptors.cpp

@@ -1,3 +1,5 @@
+// Copyright 2024 TIER IV, Inc.
+
 #include "parameter_descriptors.hpp"
 
 namespace nebula

nebula_tests/continental/parameter_descriptors.hpp

@@ -1,3 +1,17 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
 #pragma once
 
 #include <rcl_interfaces/msg/parameter.hpp>