diff --git a/build_depends.repos b/build_depends.repos index 8e7351482..6b7e58ff7 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -8,4 +8,4 @@ repositories: ros2_socketcan: type: git url: https://github.com/knzo25/ros2_socketcan - version: feat/continental_fd \ No newline at end of file + version: feat/continental_fd diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 2017bbf4f..9ef149fa9 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/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; } 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 668469b2e..e2edfe406 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 @@ -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); 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 4a976f9e6..ae3e862e0 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 @@ -503,6 +503,28 @@ 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; @@ -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 diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 9e9ffe421..0255e89f9 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -17,7 +17,7 @@ - + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index 6eee53443..2bb234512 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/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)] ) diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 913d58be0..5e17306cd 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/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": [ diff --git a/nebula_ros/schema/SRR520.schema.json b/nebula_ros/schema/SRR520.schema.json index 19356adff..0c3004a64 100644 --- a/nebula_ros/schema/SRR520.schema.json +++ b/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 } diff --git a/nebula_ros/schema/sub/communication.json b/nebula_ros/schema/sub/communication.json index b01c22e06..f7f3b6d86 100644 --- a/nebula_ros/schema/sub/communication.json +++ b/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": { diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index ef374da50..5e8de6cad 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/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 } } } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index f7b93cb81..12fb3db27 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/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("max_range", param_read_write()); config.packet_mtu_size = declare_parameter("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("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 diff --git a/nebula_tests/continental/parameter_descriptors.cpp b/nebula_tests/continental/parameter_descriptors.cpp index d2d265780..3f26adf10 100644 --- a/nebula_tests/continental/parameter_descriptors.cpp +++ b/nebula_tests/continental/parameter_descriptors.cpp @@ -1,3 +1,5 @@ +// Copyright 2024 TIER IV, Inc. + #include "parameter_descriptors.hpp" namespace nebula diff --git a/nebula_tests/continental/parameter_descriptors.hpp b/nebula_tests/continental/parameter_descriptors.hpp index 6d4e38504..db37cdb10 100644 --- a/nebula_tests/continental/parameter_descriptors.hpp +++ b/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