feat(radar_threshold_filter): add radar_threshold_filter package

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,81 @@
+// 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);
+
+  // Data Buffer
+  RadarScan::ConstSharedPtr radar_data_{};
+
+  // 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);
+  bool isDataReady();
+};
+
+}  // 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,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<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>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</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>