diff --git a/sensing/radar_threshold_filter/CMakeLists.txt b/sensing/radar_threshold_filter/CMakeLists.txt new file mode 100644 index 0000000000000..642edef9371e1 --- /dev/null +++ b/sensing/radar_threshold_filter/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_threshold_filter) + +## Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +## Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +## Targets +ament_auto_add_library(radar_threshold_filter_node_component SHARED + src/radar_threshold_filter_node/radar_threshold_filter_node.cpp +) + +rclcpp_components_register_node(radar_threshold_filter_node_component + PLUGIN "radar_threshold_filter::RadarThresholdFilterNode" + EXECUTABLE radar_threshold_filter_node +) + +## Tests +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +## Package +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/sensing/radar_threshold_filter/README.md b/sensing/radar_threshold_filter/README.md new file mode 100644 index 0000000000000..1273ae3ebe935 --- /dev/null +++ b/sensing/radar_threshold_filter/README.md @@ -0,0 +1,48 @@ +# radar_threshold_filter + +## radar_threshold_filter_node + +Remove noise from radar return by threshold. + +- Amplitude filter: Low amplitude consider noise +- FOV filter: Pointcloud from radar's FOV edge occur perturbation +- Range filter: Too near pointcloud often occur noise + +Calculation cost is O(n). `n` is the number of radar return. + +### Input topics + +| Name | Type | Description | +| ----------- | ---------------------------- | --------------------- | +| input/radar | radar_msgs/msg/RadarScan.msg | Radar pointcloud data | + +### Output topics + +| Name | Type | Description | +| ------------ | ---------------------------- | ------------------------- | +| output/radar | radar_msgs/msg/RadarScan.msg | Filtered radar pointcloud | + +### Parameters + +- For node parameter + +| Name | Type | Description | +| ------------------- | ------ | ----------------------------------------------------------------------------------------------------- | +| is_amplitude_filter | bool | if this parameter is true, apply amplitude filter (publish amplitude_min < amplitude < amplitude_max) | +| amplitude_min | double | [dBm^2] | +| amplitude_max | double | [dBm^2] | +| is_range_filter | bool | if this parameter is true, apply range filter (publish range_min < range < range_max) | +| range_min | double | [m] | +| range_max | double | [m] | +| is_azimuth_filter | bool | if this parameter is true, apply angle filter (publish azimuth_min < range < azimuth_max) | +| azimuth_min | double | [rad] | +| azimuth_max | double | [rad] | +| is_z_filter | bool | if this parameter is true, apply z position filter (publish z_min < z < z_max) | +| z_min | double | [m] | +| z_max | double | [m] | + +### How to launch + +```sh +ros2 launch radar_threshold_filter radar_threshold_filter.launch.xml +``` diff --git a/sensing/radar_threshold_filter/config/radar_threshold_filter.param.yaml b/sensing/radar_threshold_filter/config/radar_threshold_filter.param.yaml new file mode 100644 index 0000000000000..ee9ad1f564d74 --- /dev/null +++ b/sensing/radar_threshold_filter/config/radar_threshold_filter.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + node_params: + is_amplitude_filter: true + amplitude_min: -10.0 + amplitude_max: 100.0 + + is_range_filter: false + range_min: 20.0 + range_max: 300.0 + + is_azimuth_filter: true + azimuth_min: -1.2 + azimuth_max: 1.2 + + is_z_filter: false + z_min: -2.0 + z_max: 5.0 diff --git a/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp b/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp new file mode 100644 index 0000000000000..a6709b721b1d6 --- /dev/null +++ b/sensing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp @@ -0,0 +1,77 @@ +// Copyright 2022 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. + +#ifndef RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ +#define RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ + +#include + +#include + +#include +#include +#include +#include + +namespace radar_threshold_filter +{ +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; + +class RadarThresholdFilterNode : public rclcpp::Node +{ +public: + explicit RadarThresholdFilterNode(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + bool is_amplitude_filter{}; + double amplitude_min{}; + double amplitude_max{}; + bool is_range_filter{}; + double range_min{}; + double range_max{}; + bool is_azimuth_filter{}; + double azimuth_min{}; + double azimuth_max{}; + bool is_z_filter{}; + double z_min{}; + double z_max{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_radar_{}; + + // Callback + void onData(const RadarScan::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_radar_{}; + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Parameter + NodeParam node_param_{}; + + // Function + bool isWithinThreshold(const RadarReturn & radar_return); +}; + +} // namespace radar_threshold_filter + +#endif // RADAR_THRESHOLD_FILTER__RADAR_THRESHOLD_FILTER_NODE_HPP_ diff --git a/sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml b/sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml new file mode 100644 index 0000000000000..62a42f02d814b --- /dev/null +++ b/sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/radar_threshold_filter/package.xml new file mode 100644 index 0000000000000..ba6816e5c788d --- /dev/null +++ b/sensing/radar_threshold_filter/package.xml @@ -0,0 +1,23 @@ + + + radar_threshold_filter + 0.1.0 + radar_threshold_filter + Satoshi Tanaka + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + + radar_msgs + rclcpp + rclcpp_components + + ament_clang_format + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp b/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp new file mode 100644 index 0000000000000..ebcb3896f4e58 --- /dev/null +++ b/sensing/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp @@ -0,0 +1,157 @@ +// Copyright 2022 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. + +#include "radar_threshold_filter/radar_threshold_filter_node.hpp" + +#include + +#include +#include +#include + +using std::placeholders::_1; + +namespace +{ +template +bool update_param( + const std::vector & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; + } + + value = itr->template get_value(); + return true; +} + +bool isWithin(double value, double max, double min) +{ + if (min < value && value < max) { + return true; + } else { + return false; + } +} +} // namespace + +namespace radar_threshold_filter +{ +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; + +RadarThresholdFilterNode::RadarThresholdFilterNode(const rclcpp::NodeOptions & node_options) +: Node("radar_threshold_filter", node_options) +{ + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarThresholdFilterNode::onSetParam, this, _1)); + + // Node Parameter + node_param_.is_amplitude_filter = + declare_parameter("node_params.is_amplitude_filter", false); + node_param_.amplitude_min = declare_parameter("node_params.amplitude_min", 0.0); + node_param_.amplitude_max = declare_parameter("node_params.amplitude_max", 0.0); + node_param_.is_range_filter = declare_parameter("node_params.is_range_filter", false); + node_param_.range_min = declare_parameter("node_params.range_min", 0.0); + node_param_.range_max = declare_parameter("node_params.range_max", 0.0); + node_param_.is_azimuth_filter = declare_parameter("node_params.is_azimuth_filter", false); + node_param_.azimuth_min = declare_parameter("node_params.azimuth_min", 0.0); + node_param_.azimuth_max = declare_parameter("node_params.azimuth_max", 0.0); + + // Subscriber + sub_radar_ = create_subscription( + "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarThresholdFilterNode::onData, this, _1)); + + // Publisher + pub_radar_ = create_publisher("~/output/radar", 1); +} + +rcl_interfaces::msg::SetParametersResult RadarThresholdFilterNode::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + try { + { + auto & p = node_param_; + update_param(params, "node_params.is_amplitude_filter", p.is_amplitude_filter); + update_param(params, "node_params.amplitude_min", p.amplitude_min); + update_param(params, "node_params.amplitude_max", p.amplitude_max); + update_param(params, "node_params.is_range_filter", p.is_range_filter); + update_param(params, "node_params.range_min", p.range_min); + update_param(params, "node_params.range_max", p.range_max); + update_param(params, "node_params.is_azimuth_filter", p.is_azimuth_filter); + update_param(params, "node_params.azimuth_min", p.azimuth_min); + update_param(params, "node_params.azimuth_max", p.azimuth_max); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + result.successful = true; + result.reason = "success"; + return result; +} + +void RadarThresholdFilterNode::onData(const RadarScan::ConstSharedPtr radar_msg) +{ + RadarScan output; + output.header = radar_msg->header; + for (const auto & radar_return : radar_msg->returns) { + if (isWithinThreshold(radar_return)) { + output.returns.push_back(radar_return); + } + } + pub_radar_->publish(output); +} + +bool RadarThresholdFilterNode::isWithinThreshold(const RadarReturn & radar_return) +{ + if ( + node_param_.is_amplitude_filter && + !isWithin(radar_return.amplitude, node_param_.amplitude_max, node_param_.amplitude_min)) { + return false; + } + + if ( + node_param_.is_range_filter && + !isWithin(radar_return.range, node_param_.range_max, node_param_.range_min)) { + return false; + } + + if ( + node_param_.is_azimuth_filter && + !isWithin(radar_return.azimuth, node_param_.azimuth_max, node_param_.azimuth_min)) { + return false; + } + + if (node_param_.is_z_filter) { + const auto z = radar_return.range * std::sin(radar_return.elevation); + if (!isWithin(z, node_param_.z_max, node_param_.z_min)) { + return false; + } + } + return true; +} + +} // namespace radar_threshold_filter + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_threshold_filter::RadarThresholdFilterNode)