feat(radar_threshold_filter): add radar_threshold_filter package (#2300)

* feat(radar_threshold_filter): add radar_threshold_filter

Signed-off-by: scepter914 <scepter914@gmail.com>

* apply pre-commit

Signed-off-by: scepter914 <scepter914@gmail.com>

* update README

Signed-off-by: scepter914 <scepter914@gmail.com>

* update package.xml

Signed-off-by: scepter914 <scepter914@gmail.com>

* change onTimer to onData

Signed-off-by: scepter914 <scepter914@gmail.com>

* refactor

Signed-off-by: scepter914 <scepter914@gmail.com>

* delete update hz param

Signed-off-by: scepter914 <scepter914@gmail.com>

* refactor

Signed-off-by: scepter914 <scepter914@gmail.com>

* delete xml model

Signed-off-by: scepter914 <scepter914@gmail.com>

* delete function

Signed-off-by: scepter914 <scepter914@gmail.com>

Signed-off-by: scepter914 <scepter914@gmail.com>

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

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
+```

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

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 <rclcpp/rclcpp.hpp>
+
+#include <radar_msgs/msg/radar_scan.hpp>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<RadarScan>::SharedPtr sub_radar_{};
+
+  // Callback
+  void onData(const RadarScan::ConstSharedPtr msg);
+
+  // Publisher
+  rclcpp::Publisher<RadarScan>::SharedPtr pub_radar_{};
+
+  // Parameter Server
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & 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_

sensing/radar_threshold_filter/launch/radar_threshold_filter.launch.xml

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="input/radar" default="input/radar"/>
+  <arg name="output/radar" default="output/threshold_filtered_radar"/>
+  <arg name="config_file" default="$(find-pkg-share radar_threshold_filter)/config/radar_threshold_filter.param.yaml"/>
+
+  <!-- Node -->
+  <node pkg="radar_threshold_filter" exec="radar_threshold_filter_node" name="radar_threshold_filter" output="screen">
+    <remap from="~/input/radar" to="$(var input/radar)"/>
+    <remap from="~/output/radar" to="$(var output/radar)"/>
+    <param from="$(var config_file)"/>
+  </node>
+</launch>

sensing/radar_threshold_filter/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>radar_threshold_filter</name>
+  <version>0.1.0</version>
+  <description>radar_threshold_filter</description>
+  <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
+  <author email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</author>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>radar_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <radar_msgs/msg/radar_scan.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+using std::placeholders::_1;
+
+namespace
+{
+template <class T>
+bool update_param(
+  const std::vector<rclcpp::Parameter> & 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<T>();
+  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<bool>("node_params.is_amplitude_filter", false);
+  node_param_.amplitude_min = declare_parameter<double>("node_params.amplitude_min", 0.0);
+  node_param_.amplitude_max = declare_parameter<double>("node_params.amplitude_max", 0.0);
+  node_param_.is_range_filter = declare_parameter<bool>("node_params.is_range_filter", false);
+  node_param_.range_min = declare_parameter<double>("node_params.range_min", 0.0);
+  node_param_.range_max = declare_parameter<double>("node_params.range_max", 0.0);
+  node_param_.is_azimuth_filter = declare_parameter<bool>("node_params.is_azimuth_filter", false);
+  node_param_.azimuth_min = declare_parameter<double>("node_params.azimuth_min", 0.0);
+  node_param_.azimuth_max = declare_parameter<double>("node_params.azimuth_max", 0.0);
+
+  // Subscriber
+  sub_radar_ = create_subscription<RadarScan>(
+    "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarThresholdFilterNode::onData, this, _1));
+
+  // Publisher
+  pub_radar_ = create_publisher<RadarScan>("~/output/radar", 1);
+}
+
+rcl_interfaces::msg::SetParametersResult RadarThresholdFilterNode::onSetParam(
+  const std::vector<rclcpp::Parameter> & 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)