feat(radar_static_pointcloud_filter): add radar_static_pointcloud_fil…

…ter package (#2362)

* feat(radar_static_pointcloud_filter): add radar_static_pointcloud_filter package

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

* apply pre-commit

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

* refactor

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

* use autoware_cmame in CMakeLists.txt

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

* delete transform member variable

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

* add dependancy

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

* fix transform

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

* add code maintainer

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

* ci(pre-commit): autofix

* fix duration time

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

* change duration time

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

* Update sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Signed-off-by: scepter914 <scepter914@gmail.com>

* fix from variable renaming

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

* fix function of convert velocity from doppler velocity

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

Signed-off-by: scepter914 <scepter914@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

sensing/radar_static_pointcloud_filter/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.5)
+project(radar_static_pointcloud_filter)
+
+# Dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(radar_static_pointcloud_filter_node_component SHARED
+  src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp
+)
+
+rclcpp_components_register_node(radar_static_pointcloud_filter_node_component
+  PLUGIN "radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode"
+  EXECUTABLE radar_static_pointcloud_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
+  launch
+)

sensing/radar_static_pointcloud_filter/README.md

@@ -0,0 +1,36 @@
+# radar_static_pointcloud_filter
+
+## radar_static_pointcloud_filter_node
+
+Extract static/dynamic radar pointcloud by using doppler velocity and ego motion.
+Calculation cost is O(n). `n` is the number of radar pointcloud.
+
+### Input topics
+
+| Name           | Type                       | Description                |
+| -------------- | -------------------------- | -------------------------- |
+| input/radar    | radar_msgs::msg::RadarScan | RadarScan                  |
+| input/odometry | nav_msgs::msg::Odometry    | Ego vehicle odometry topic |
+
+### Output topics
+
+| Name                      | Type                       | Description              |
+| ------------------------- | -------------------------- | ------------------------ |
+| output/static_radar_scan  | radar_msgs::msg::RadarScan | static radar pointcloud  |
+| output/dynamic_radar_scan | radar_msgs::msg::RadarScan | dynamic radar pointcloud |
+
+### Parameters
+
+| Name                | Type   | Description                                          |
+| ------------------- | ------ | ---------------------------------------------------- |
+| doppler_velocity_sd | double | Standard deviation for radar doppler velocity. [m/s] |
+
+### How to launch
+
+```sh
+ros2 launch radar_static_pointcloud_filter radar_static_pointcloud_filter.launch
+```
+
+### Algorithm
+
+![algorithm](docs/radar_static_pointcloud_filter.drawio.svg)

sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp

@@ -0,0 +1,82 @@
+// 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_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_
+#define RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_
+
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <radar_msgs/msg/radar_scan.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace radar_static_pointcloud_filter
+{
+using nav_msgs::msg::Odometry;
+using radar_msgs::msg::RadarReturn;
+using radar_msgs::msg::RadarScan;
+
+class RadarStaticPointcloudFilterNode : public rclcpp::Node
+{
+public:
+  explicit RadarStaticPointcloudFilterNode(const rclcpp::NodeOptions & node_options);
+
+  struct NodeParam
+  {
+    double doppler_velocity_sd{};
+  };
+
+private:
+  // Subscriber
+  message_filters::Subscriber<RadarScan> sub_radar_{};
+  message_filters::Subscriber<Odometry> sub_odometry_{};
+  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
+
+  using SyncPolicy = message_filters::sync_policies::ApproximateTime<RadarScan, Odometry>;
+  using Sync = message_filters::Synchronizer<SyncPolicy>;
+  typename std::shared_ptr<Sync> sync_ptr_;
+
+  // Callback
+  void onData(const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg);
+
+  // Publisher
+  rclcpp::Publisher<RadarScan>::SharedPtr pub_static_radar_{};
+  rclcpp::Publisher<RadarScan>::SharedPtr pub_dynamic_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 isStaticPointcloud(
+    const RadarReturn & radar_return, const Odometry::ConstSharedPtr & odom_msg,
+    geometry_msgs::msg::TransformStamped::ConstSharedPtr transform);
+};
+}  // namespace radar_static_pointcloud_filter
+
+#endif  // RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP_