From e6350b00128098e80f4d510e0792a7355b5403be Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 13 Aug 2024 18:54:02 +0900 Subject: [PATCH 01/15] chore: add abstract point filter class and add to Hesai sensor config --- .../nebula_common/hesai/hesai_common.hpp | 3 ++ .../point_filters/point_filter.hpp | 30 +++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 nebula_common/include/nebula_common/point_filters/point_filter.hpp diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 950b2710d..d2f3724dd 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -18,12 +18,14 @@ #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_common/util/string_conversions.hpp" +#include "nebula_common/point_filters/point_filter.hpp" #include #include #include #include #include +#include #include #include #include @@ -49,6 +51,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase uint8_t ptp_domain; PtpTransportType ptp_transport_type; PtpSwitchType ptp_switch_type; + std::vector> point_filters; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os diff --git a/nebula_common/include/nebula_common/point_filters/point_filter.hpp b/nebula_common/include/nebula_common/point_filters/point_filter.hpp new file mode 100644 index 000000000..b3eab5735 --- /dev/null +++ b/nebula_common/include/nebula_common/point_filters/point_filter.hpp @@ -0,0 +1,30 @@ +// 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 + +namespace nebula::drivers +{ + +class PointFilter +{ +public: + virtual ~PointFilter() = default; + + virtual bool excluded(const NebulaPoint & point) = 0; +}; + +} // namespace nebula::drivers From c9f90b819db955d7e196274097c71c8d451babe7 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 13 Aug 2024 18:57:15 +0900 Subject: [PATCH 02/15] chore: add abstract point filter class and add to Hesai sensor config --- nebula_decoders/CMakeLists.txt | 4 + .../point_filters/parser.hpp | 66 +++++++++ .../point_filters/ring_section_filter.hpp | 128 ++++++++++++++++++ nebula_decoders/package.xml | 2 + 4 files changed, 200 insertions(+) create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index e94afcf2a..5adbc3fcb 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(robosense_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(velodyne_msgs REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(nlohmann_json REQUIRED) include_directories(PUBLIC include @@ -35,6 +36,7 @@ include_directories(PUBLIC ${pcl_conversions_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS} + ${NLOHMANN_JSON_INCLUDE_DIRS} ) link_libraries( @@ -43,6 +45,7 @@ link_libraries( ${pcl_conversions_LIBRARIES} ${rclcpp_TARGETS} ${sensor_msgs_TARGETS} + ${NLOHMANN_JSON_LIBRARIES} ) # Lidar Decoders @@ -153,6 +156,7 @@ ament_export_dependencies( sensor_msgs velodyne_msgs yaml-cpp + nlohmann_json ) ament_package() diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp new file mode 100644 index 000000000..433f2a195 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp @@ -0,0 +1,66 @@ +// 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 "nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace nebula::drivers +{ + +using nlohmann::json; +using namespace std::string_literals; // NOLINT + +nebula::util::expected>, std::string> parse_point_filters( + const std::string & s) +{ + std::vector> parsed_filters; + + json j; + try { + j = json::parse(s); + } catch (json::parse_error & e) { + return "Could not parse JSON: "s + e.what(); + } + + if (!j.is_object()) { + return "expected a JSON object"s; + } + + for (const auto & [key, value] : j.items()) { + if (key == "ring_section_filter") { + auto parsed = RingSectionFilter::fromJson(value); + if (!parsed.has_value()) { + return "Could not parse " + key + ": " + parsed.error(); + } + + auto filter = std::make_shared(parsed.value()); + parsed_filters.emplace_back(filter); + } else { + return "unknown filter type: " + key; + } + } + + return parsed_filters; +} + +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp new file mode 100644 index 000000000..0be3729f7 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp @@ -0,0 +1,128 @@ +// 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 "nebula_decoders/nebula_decoders_common/angles.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +using namespace std::string_literals; // NOLINT + +class RingSectionFilter : public PointFilter +{ +public: + explicit RingSectionFilter( + const std::vector> & excluded_sections_deg) + { + uint32_t max_channel = 0; + for (const auto & [channel, _1, _2] : excluded_sections_deg) { + max_channel = std::max(max_channel, channel); + } + + excluded_sections_rad_.resize(max_channel + 1); + + for (auto [channel, section_start_deg, section_end_deg] : excluded_sections_deg) { + section_start_deg = normalize_angle(section_start_deg, 360.f); + section_end_deg = normalize_angle(section_end_deg, 360.f); + if (section_end_deg == section_start_deg) { + section_end_deg += 360; + } + + excluded_sections_rad_.at(channel).emplace_back( + deg2rad(section_start_deg), deg2rad(section_end_deg)); + } + } + + bool excluded(const NebulaPoint & point) override + { + if (point.channel >= excluded_sections_rad_.size()) return false; + const auto & ring_sections = excluded_sections_rad_[point.channel]; + for (const auto & [section_start_rad, section_end_rad] : ring_sections) { + bool in_excluded_region = angle_is_between(section_start_rad, section_end_rad, point.azimuth); + if (in_excluded_region) return true; + } + + return false; + } + + /** + * @brief Generate a ring section filter from a given JSON array. The array is expected to be in + * the format [[channel_id, start_deg, end_deg], [channel_id, start_deg, end_deg], ...]. Not all + * channels need to be specified, and channels can be specified multiple times. In that case, + * multiple sections in the channel can be filtered. + * + * @param s The string to be parsed. + * @return nebula::util::expected, std::string> A ring section + * filter if parsed successfully, an error message otherwise. + */ + static nebula::util::expected fromJson( + const nlohmann::json & json) + { + std::vector> parsed_sections; + + if (!json.is_array()) { + return "expected JSON string to represent an array"s; + } + + for (const auto & sector : json) { + if (!sector.is_array()) { + return "expected sector to be an array"s; + } + + if (sector.size() != 3) { + return "expected sector to be of length 3 (channel_id, start_deg, end_deg)"s; + } + + if (!sector[0].is_number_unsigned()) { + return "expected unsigned integer as channel ID"s; + } + + if (!sector[1].is_number() || !sector[2].is_number()) { + return "expected section start and end to be numbers"s; + } + + uint32_t channel_id = sector[0]; + float start_deg = sector[1]; + float end_deg = sector[2]; + + parsed_sections.emplace_back(channel_id, start_deg, end_deg); + } + + if (parsed_sections.empty()) { + return "expected at least one filtered-out section"s; + } + + return RingSectionFilter(parsed_sections); + } + +private: + std::vector>> excluded_sections_rad_; +}; + +} // namespace nebula::drivers diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index 85c7a9a87..ec6ccae0c 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -18,6 +18,8 @@ libpcl-all-dev nebula_common nebula_msgs + nlohmann-json-dev + nlohmann-json-dev pandar_msgs pcl_conversions rclcpp From a656880ff1f7dfba36f3db89a43a9c092b38234c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 13 Aug 2024 19:00:59 +0900 Subject: [PATCH 03/15] chore(hesai_decoder): add support for point filters --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 314c40473..a07154af3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -214,6 +214,13 @@ class HesaiDecoder : public HesaiScanDecoder // The driver wrapper converts to degrees, expects radians point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; + for (const auto & filter : sensor_configuration_->point_filters) { + if (filter->excluded(point)) { + pc->points.pop_back(); + perf_cnt_.n_filt++; + break; + } + } } } } From 89620bad0332d6579dba3d819f47e788ce311938 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 13 Aug 2024 19:02:18 +0900 Subject: [PATCH 04/15] feat(nebula_ros): add support for point filters as parameters --- nebula_ros/CMakeLists.txt | 3 +++ .../lidar/hesai/Pandar128E4X.param.yaml | 1 + .../config/lidar/hesai/Pandar40P.param.yaml | 1 + .../config/lidar/hesai/Pandar64.param.yaml | 1 + .../config/lidar/hesai/PandarAT128.param.yaml | 1 + .../config/lidar/hesai/PandarQT128.param.yaml | 1 + .../config/lidar/hesai/PandarQT64.param.yaml | 1 + .../config/lidar/hesai/PandarXT32.param.yaml | 1 + .../config/lidar/hesai/PandarXT32M.param.yaml | 1 + nebula_ros/package.xml | 1 + nebula_ros/schema/Pandar128E4X.schema.json | 6 +++++- nebula_ros/schema/Pandar40P.schema.json | 6 +++++- nebula_ros/schema/Pandar64.schema.json | 6 +++++- nebula_ros/schema/PandarAT128.schema.json | 6 +++++- nebula_ros/schema/PandarQT128.schema.json | 6 +++++- nebula_ros/schema/PandarQT64.schema.json | 6 +++++- nebula_ros/schema/PandarXT32.schema.json | 6 +++++- nebula_ros/schema/PandarXT32M.schema.json | 6 +++++- nebula_ros/schema/sub/misc.json | 6 ++++++ nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 21 +++++++++++++++++++ 20 files changed, 79 insertions(+), 8 deletions(-) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 709de2025..5efff3f89 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(robosense_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(nlohmann_json REQUIRED) include_directories( include @@ -36,12 +37,14 @@ include_directories( ${YAML_CPP_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${rclcpp_components_INCLUDE_DIRS} + ${NLOHMANN_JSON_INCLUDE_DIRS} ) link_libraries( ${nebula_common_TARGETS} ${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES} + ${NLOHMANN_JSON_LIBRARIES} ) ## Hesai diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index da32fc0eb..33ac9cfb4 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index 332b2a1c0..411500013 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml index f731049fa..a5b748eb8 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml index dff7789dd..af6e15aa2 100644 --- a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -28,3 +28,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml index 17779ee02..9f99ee5d5 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml index f11d30076..f75d982da 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml index 7288ec371..13243819c 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml index cc1fffbc9..0ab76740a 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -27,3 +27,4 @@ ptp_switch_type: TSN retry_hw: true dual_return_distance_threshold: 0.1 + point_filters: "{}" diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 3a97db1f6..605b9eea7 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -22,6 +22,7 @@ nebula_decoders nebula_hw_interfaces nebula_msgs + nlohmann-json-dev pandar_msgs radar_msgs rclcpp diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 3dbb34f93..752dfcc55 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -104,6 +104,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -131,7 +134,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index abfa77699..a7d95ce0d 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -95,6 +95,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -122,7 +125,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index e006afa77..77f70c726 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -95,6 +95,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -122,7 +125,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index f9780f76c..3c95d0abf 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -115,6 +115,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -143,7 +146,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index 29af8d712..e6675ec22 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -98,6 +98,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -125,7 +128,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index 811bad884..949e8d236 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -95,6 +95,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -122,7 +125,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index 776967546..9ccfb3e5c 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -98,6 +98,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -125,7 +128,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index cbd1d2aee..3b96af81b 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -98,6 +98,9 @@ }, "dual_return_distance_threshold": { "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" } }, "required": [ @@ -125,7 +128,8 @@ "ptp_transport_type", "ptp_switch_type", "retry_hw", - "dual_return_distance_threshold" + "dual_return_distance_threshold", + "point_filters" ], "additionalProperties": false } diff --git a/nebula_ros/schema/sub/misc.json b/nebula_ros/schema/sub/misc.json index 8aebfbc65..3c366996b 100644 --- a/nebula_ros/schema/sub/misc.json +++ b/nebula_ros/schema/sub/misc.json @@ -85,6 +85,12 @@ "minimum": 0.0, "maximum": 360.0, "description": "Sensor scan phase." + }, + "point_filters": { + "type": "string", + "default": "\"{}\"", + "pattern": "^\\{.*?\\}$", + "description": "A map of filter definitions {\"filter_type1\": filter_def1, \"filter_type2\": filter_def2} to filter points while decoding" } } } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index dbf5fe099..493f98c78 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -201,6 +202,13 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() config.ptp_domain = declare_parameter("ptp_domain", descriptor); } + auto point_filters_raw = declare_parameter("point_filters", param_read_write()); + auto point_filters = drivers::parse_point_filters(point_filters_raw); + if (!point_filters.has_value()) { + throw std::runtime_error("Could not parse point filters: " + point_filters.error()); + } + config.point_filters = point_filters.value(); + auto new_cfg_ptr = std::make_shared(config); return validate_and_set_config(new_cfg_ptr); } @@ -322,6 +330,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( std::string _return_mode{}; std::string calibration_parameter_name = get_calibration_parameter_name(sensor_cfg_ptr_->sensor_model); + std::string point_filters_raw; bool got_any = get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | @@ -331,6 +340,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( 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, "point_filters", point_filters_raw) | get_param(p, calibration_parameter_name, new_cfg.calibration_path); // Currently, all of the sub-wrappers read-only parameters, so they do not be queried for updates @@ -376,6 +386,17 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( new_calibration_ptr = get_calibration_result.value(); } + if (!point_filters_raw.empty()) { + auto point_filters = drivers::parse_point_filters(point_filters_raw); + if (!point_filters.has_value()) { + SetParametersResult result{}; + result.successful = false; + result.reason = "Could not parse point filters: " + point_filters.error(); + return result; + } + new_cfg.point_filters = point_filters.value(); + } + auto new_cfg_ptr = std::make_shared(new_cfg); auto status = validate_and_set_config(new_cfg_ptr); if (status != Status::OK) { From af5650c22e223dcceedeb128c31e51ad7e15ca0a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 13 Aug 2024 19:25:17 +0900 Subject: [PATCH 05/15] fix: remove duplicate nlohmann-json-dev dependency --- nebula_decoders/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index ec6ccae0c..3fc0bdc80 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -19,7 +19,6 @@ nebula_common nebula_msgs nlohmann-json-dev - nlohmann-json-dev pandar_msgs pcl_conversions rclcpp From 67d4e73bf86cfe8516afbe5e068ee84a37e890b5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 13 Aug 2024 19:48:46 +0900 Subject: [PATCH 06/15] docs: add point filter docs --- docs/filters.md | 98 +++++++++++++++++++++++++++++++++++++++++++++++++ docs/index.md | 1 + 2 files changed, 99 insertions(+) create mode 100644 docs/filters.md diff --git a/docs/filters.md b/docs/filters.md new file mode 100644 index 000000000..1f0171092 --- /dev/null +++ b/docs/filters.md @@ -0,0 +1,98 @@ +# Point Filters + +Filters run for every point, as soon as it is fully decoded. This can speed up the later parts of pointcloud processing pipelines by reducing the number of points sent and copied between modules. + +## Configuration + +Filters are configured via the ROS parameter `point_filters`. The parameter is a stringified JSOn object of the form: + +```json +{ + "filter_type1": configuration1, + "filter_type2": configuration2 +} +``` + +Where the `filter_type` field has to be one of the supported filters below, and where the `configuration` format depends on the filter type. + +Filters can also be set during runtime, e.g. via: + +```shell +ros2 param set /hesai_ros_wrapper_node point_filters '"{\"filter_type1\": configuration1, ...}"' +``` + +## Supported Filters + +The following filter types are supported: + +| Filter Name | Filter Type Field | +| ------------------- | --------------------- | +| Ring Section Filter | `ring_section_filter` | + +Below, each filter type is documented in detail. + +### Ring Section Filter + +This filter can remove zero or more contiguous sections per ring. + +Configuration is done in the following format: + +```json + [ + [channel_id1, start_deg1, end_deg1], + [channel_id2, start_deg2, end_deg2], + ... + ] +``` + +Things to keep in mind: + +- Not all channels have to be configured (unconfigured channels are not filtered). +- A channel can be configured multiple times, in which case all of the configured sections are filtered (like a logical `OR` operation) +- Sections are allowed to overlap +- Sections can wrap around the `360/0` boundary (e.g. `[270, 90] deg`) +- `start_deg` and `end_deg` are floating-point, so precise angles can be specified + +Examples: + +- `[50.25, 100.114] deg` on rings `[2, 4]`: + + ```json + [ + [2, 50.25, 100, 114], + [3, 50.25, 100, 114], + [4, 50.25, 100, 114] + ] + ``` + +- complete rings `1, 3, 5`: + + ```json + [ + [1, 0, 360], + [3, 0, 360], + [5, 0, 360] + ] + ``` + +- `[10, 20] deg`, `[30, 40] deg` and `[50, 60] deg` on ring `5`: + + ```json + [ + [5, 10, 20], + [5, 30, 40], + [5, 50, 60] + ] + ``` + +## Compatibility Chart + +| | Ring Section Filter | +| --------- | ------------------- | +| Hesai | ✅ | +| Robosense | ❌ | +| Velodyne | ❌ | + +Compatibility: +✅: compatible +❌: incompatible diff --git a/docs/index.md b/docs/index.md index 602333bf0..4975bbfab 100644 --- a/docs/index.md +++ b/docs/index.md @@ -25,6 +25,7 @@ Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware]( - [Design](design.md) - [Parameters](parameters.md) - [Point cloud types](point_types.md) +- [Point filters](filters.md) ## Supported sensors From 9fb50044cb27cefead8d789b2b79ef4d0ba1b2a9 Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Fri, 30 Aug 2024 16:07:12 +0900 Subject: [PATCH 07/15] fix(hesai_decoder): remove use of undefined perf_cnt_ --- .../nebula_decoders_hesai/decoders/hesai_decoder.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index a07154af3..759303ae9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -217,7 +217,6 @@ class HesaiDecoder : public HesaiScanDecoder for (const auto & filter : sensor_configuration_->point_filters) { if (filter->excluded(point)) { pc->points.pop_back(); - perf_cnt_.n_filt++; break; } } From 35cbb1949d3d54d970fbf899d1f9928964eb072e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 3 Sep 2024 18:46:00 +0900 Subject: [PATCH 08/15] chore(nebula_common): add function to get number of channels for a given sensor model --- .../include/nebula_common/nebula_common.hpp | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 36f7f60c3..892c13595 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -15,17 +15,23 @@ #ifndef NEBULA_COMMON_H #define NEBULA_COMMON_H +#include "nebula_common/util/expected.hpp" + #include #include #include +#include #include #include #include namespace nebula::drivers { + +using std::string_literals::operator""s; + /// @brief Coordinate mode for Velodyne's setting (need to check) enum class CoordinateMode { UNKNOWN = 0, CARTESIAN, SPHERICAL, CYLINDRICAL }; @@ -634,6 +640,47 @@ inline std::string sensor_model_to_string(const SensorModel & sensor_model) } } +/** + * @brief Get the number of channels a specific sensor model has. A string error message is returned + * for sensors that do not have a meaningful channel number. + * + * @param sensor_model The sensor model of interest + * @return nebula::util::expected The number of channels on success, or a + * string error message. + */ +inline nebula::util::expected get_n_channels( + const SensorModel & sensor_model) +{ + switch (sensor_model) { + case SensorModel::VELODYNE_VLP16: + return 16; + case SensorModel::VELODYNE_VLP32: + case SensorModel::VELODYNE_VLP32MR: + case SensorModel::VELODYNE_HDL32: + case SensorModel::HESAI_PANDARXT32: + case SensorModel::HESAI_PANDARXT32M: + case SensorModel::ROBOSENSE_HELIOS: + case SensorModel::ROBOSENSE_BPEARL_V3: + case SensorModel::ROBOSENSE_BPEARL_V4: + return 32; + case SensorModel::HESAI_PANDAR40P: + case SensorModel::HESAI_PANDAR40M: + return 40; + case SensorModel::VELODYNE_HDL64: + case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDARQT64: + return 64; + case SensorModel::VELODYNE_VLS128: + case SensorModel::HESAI_PANDARQT128: + case SensorModel::HESAI_PANDARAT128: + case SensorModel::HESAI_PANDAR128_E3X: + case SensorModel::HESAI_PANDAR128_E4X: + return 128; + default: + return "unsupported sensor model"s; + } +} + /// @brief Convert return mode name to ReturnMode enum /// @param return_mode Return mode name (Upper and lower case letters must match) /// @return Corresponding ReturnMode From 405a3706829ae090366be21fb52832043e22600d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 3 Sep 2024 18:47:05 +0900 Subject: [PATCH 09/15] feat(ring_section_filter): check if the provided filters exceed the sensor's number of channels --- .../point_filters/parser.hpp | 5 +++-- .../point_filters/ring_section_filter.hpp | 20 ++++++++++++++++++- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 5 +++-- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp index 433f2a195..778d8105a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp @@ -16,6 +16,7 @@ #include "nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp" +#include #include #include #include @@ -31,7 +32,7 @@ using nlohmann::json; using namespace std::string_literals; // NOLINT nebula::util::expected>, std::string> parse_point_filters( - const std::string & s) + const std::string & s, SensorModel sensor_model) { std::vector> parsed_filters; @@ -48,7 +49,7 @@ nebula::util::expected>, std::string> p for (const auto & [key, value] : j.items()) { if (key == "ring_section_filter") { - auto parsed = RingSectionFilter::fromJson(value); + auto parsed = RingSectionFilter::fromJson(value, sensor_model); if (!parsed.has_value()) { return "Could not parse " + key + ": " + parsed.error(); } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp index 0be3729f7..9d342532b 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp @@ -34,9 +34,19 @@ namespace nebula::drivers using namespace std::string_literals; // NOLINT +/** + * @brief Filters out angular sections on the configured rings. + */ class RingSectionFilter : public PointFilter { public: + /** + * @brief Construct a new ring section filter given a list of (channel_id, start_deg, end_deg) + * tuples. + * + * @param excluded_sections_deg A list of (channel_id, start_deg, end_deg) tuples which should be + * excluded. Sections wrapping around the 360/0 deg boundary are handled correctly. + */ explicit RingSectionFilter( const std::vector> & excluded_sections_deg) { @@ -82,8 +92,12 @@ class RingSectionFilter : public PointFilter * filter if parsed successfully, an error message otherwise. */ static nebula::util::expected fromJson( - const nlohmann::json & json) + const nlohmann::json & json, SensorModel sensor_model) { + auto sensor_n_channels_result = get_n_channels(sensor_model); + if (!sensor_n_channels_result.has_value()) return sensor_n_channels_result.error(); + uint32_t sensor_n_channels = sensor_n_channels_result.value(); + std::vector> parsed_sections; if (!json.is_array()) { @@ -111,6 +125,10 @@ class RingSectionFilter : public PointFilter float start_deg = sector[1]; float end_deg = sector[2]; + if (channel_id >= sensor_n_channels) { + return "sensor supports at most " + std::to_string(sensor_n_channels) + " channels"; + } + parsed_sections.emplace_back(channel_id, start_deg, end_deg); } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 493f98c78..6b5a74ba0 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -203,7 +203,7 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() } auto point_filters_raw = declare_parameter("point_filters", param_read_write()); - auto point_filters = drivers::parse_point_filters(point_filters_raw); + auto point_filters = drivers::parse_point_filters(point_filters_raw, config.sensor_model); if (!point_filters.has_value()) { throw std::runtime_error("Could not parse point filters: " + point_filters.error()); } @@ -387,7 +387,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( } if (!point_filters_raw.empty()) { - auto point_filters = drivers::parse_point_filters(point_filters_raw); + auto point_filters = + drivers::parse_point_filters(point_filters_raw, sensor_cfg_ptr_->sensor_model); if (!point_filters.has_value()) { SetParametersResult result{}; result.successful = false; From a4779ebe484e5ec22d0a3bcd3b549cd3b034d9d2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Sep 2024 07:38:30 +0000 Subject: [PATCH 10/15] ci(pre-commit): autofix --- nebula_common/include/nebula_common/hesai/hesai_common.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index d2f3724dd..1487047fb 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -17,8 +17,8 @@ #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_common/util/string_conversions.hpp" #include "nebula_common/point_filters/point_filter.hpp" +#include "nebula_common/util/string_conversions.hpp" #include #include From d05213fd9f09f3e25f5f4d1c4e43baab97340180 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 10 Sep 2024 14:42:10 +0900 Subject: [PATCH 11/15] chore(point_filters): allow empty string (= no filters) in parser --- .../nebula_decoders_common/point_filters/parser.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp index 778d8105a..38d3d6f44 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp @@ -34,6 +34,10 @@ using namespace std::string_literals; // NOLINT nebula::util::expected>, std::string> parse_point_filters( const std::string & s, SensorModel sensor_model) { + if (s.empty()) { + return std::vector>{}; + } + std::vector> parsed_filters; json j; From 580e7138c24b19d73500dbcd8dde7a60b897d886 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 10 Sep 2024 14:42:57 +0900 Subject: [PATCH 12/15] chore(point_filters): add `inline` to function in header file --- .../nebula_decoders_common/point_filters/parser.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp index 38d3d6f44..2665e0f55 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp @@ -31,8 +31,8 @@ namespace nebula::drivers using nlohmann::json; using namespace std::string_literals; // NOLINT -nebula::util::expected>, std::string> parse_point_filters( - const std::string & s, SensorModel sensor_model) +inline nebula::util::expected>, std::string> +parse_point_filters(const std::string & s, SensorModel sensor_model) { if (s.empty()) { return std::vector>{}; From ee790f0d7a95b52552a0efcd145cedbbc08a5a4d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 21 Oct 2024 15:56:11 +0900 Subject: [PATCH 13/15] fix(hesai): workaround for PCL bug to fix pointcloud size fields when filters are enabled Signed-off-by: Max SCHMELLER --- .../decoders/hesai_decoder.hpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 759303ae9..9676fa40b 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -214,12 +214,17 @@ class HesaiDecoder : public HesaiScanDecoder // The driver wrapper converts to degrees, expects radians point.azimuth = corrected_angle_data.azimuth_rad; point.elevation = corrected_angle_data.elevation_rad; - for (const auto & filter : sensor_configuration_->point_filters) { - if (filter->excluded(point)) { - pc->points.pop_back(); - break; - } + + const auto & filters = sensor_configuration_->point_filters; + bool excluded = std::any_of(filters.begin(), filters.end(), [&](const auto & filter) { + return filter->excluded(point); + }); + + if (excluded) { + continue; } + + pc->emplace_back(point); } } } From 73228ef6d13d285a0da83a3eaec54dae9e4f4ff3 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 18:02:50 +0900 Subject: [PATCH 14/15] chore(expected): make move-constructible Signed-off-by: Max SCHMELLER --- nebula_common/include/nebula_common/util/expected.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index 1d4333443..f4d8b2921 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -15,6 +15,7 @@ #pragma once #include +#include #include #include @@ -97,9 +98,10 @@ struct expected return default_; } - expected(const T & value) { value_ = value; } // NOLINT(runtime/explicit) - - expected(const E & error) { value_ = error; } // NOLINT(runtime/explicit) + expected(const T & value) : value_(value) {} // NOLINT(runtime/explicit) + expected(T && value) : value_(value) {} // NOLINT(runtime/explicit) + expected(const E & error) : value_(error) {} // NOLINT(runtime/explicit) + expected(E && error) : value_(error) {} // NOLINT(runtime/explicit) private: std::variant value_; From 1c0897f71324be2bbfe3be9d611ee1a67fd5cd57 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 11 Dec 2024 07:55:53 +0000 Subject: [PATCH 15/15] ci(pre-commit): autofix --- nebula_common/include/nebula_common/util/expected.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index f4d8b2921..425aa06dd 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -99,9 +99,9 @@ struct expected } expected(const T & value) : value_(value) {} // NOLINT(runtime/explicit) - expected(T && value) : value_(value) {} // NOLINT(runtime/explicit) + expected(T && value) : value_(value) {} // NOLINT(runtime/explicit) expected(const E & error) : value_(error) {} // NOLINT(runtime/explicit) - expected(E && error) : value_(error) {} // NOLINT(runtime/explicit) + expected(E && error) : value_(error) {} // NOLINT(runtime/explicit) private: std::variant value_;