diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt new file mode 100644 index 0000000000000..1f08f7842cfdf --- /dev/null +++ b/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 +) diff --git a/sensing/radar_static_pointcloud_filter/README.md b/sensing/radar_static_pointcloud_filter/README.md new file mode 100644 index 0000000000000..1e480ef4354dc --- /dev/null +++ b/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) diff --git a/sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg b/sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg new file mode 100644 index 0000000000000..c9e51ca2b38a4 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg @@ -0,0 +1,201 @@ + + + + + + + +
+
+
+ Vehicle +
+
+
+ + Vehicle + + + + + + + +
+
+
+ Radar +
+
+
+ + Radar + + + + + + + + +
+
+
+ v_ego +
+
+
+ + v_ego + + + + + + + +
+
+
+ v_world = v_ego + transpose(v_doppler) +
+
+
+ + v_world = v_ego + transpose(v_doppler) + + + + + + + + +
+
+
+ v_doppler +
+
+
+ + v_doppler + + + + + + +
+
+
+ if - param.doppler_velocity_sd < v_world < param.doppler_velocity_sd +
+ then consider static point +
+
+
+ + + if - param.doppler_velocity_sd < v_worl... + + + + + + + +
+
+
+ param.doppler_velocity_sd +
+
+
+ + param.doppler_velocity_sd + + + + + + + +
+
+
+ Sensor coordinate +
+
+
+ + Sensor coordi... + + + + + + +
+
+
+ World coordinate +
+
+
+ + World coordin... + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp new file mode 100644 index 0000000000000..e97e8fa5ca500 --- /dev/null +++ b/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 + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +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 sub_radar_{}; + message_filters::Subscriber sub_odometry_{}; + std::shared_ptr transform_listener_; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + using Sync = message_filters::Synchronizer; + typename std::shared_ptr sync_ptr_; + + // Callback + void onData(const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_static_radar_{}; + rclcpp::Publisher::SharedPtr pub_dynamic_radar_{}; + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & 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_ diff --git a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml new file mode 100644 index 0000000000000..08fdeee74f15a --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml new file mode 100644 index 0000000000000..30739d5578cb9 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -0,0 +1,31 @@ + + + radar_static_pointcloud_filter + 0.1.0 + radar_static_pointcloud_filter + Satoshi Tanaka + Shunsuke Miura + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + + message_filters + nav_msgs + radar_msgs + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + autoware_cmake + ament_clang_format + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp new file mode 100644 index 0000000000000..812a96e7cdcd5 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -0,0 +1,172 @@ +// 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_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp" + +#include +#include +#include + +#include +#include +#include + +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; +} + +geometry_msgs::msg::Vector3 getVelocity(const radar_msgs::msg::RadarReturn & radar) +{ + return geometry_msgs::build() + .x(radar.doppler_velocity * std::cos(radar.azimuth)) + .y(radar.doppler_velocity * std::sin(radar.azimuth)) + .z(0.0); +} + +geometry_msgs::msg::Vector3 getTransformedVelocity( + const geometry_msgs::msg::Vector3 velocity, + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform) +{ + geometry_msgs::msg::Vector3Stamped velocity_stamped{}; + velocity_stamped.vector = velocity; + geometry_msgs::msg::Vector3Stamped transformed_velocity_stamped{}; + tf2::doTransform(velocity_stamped, transformed_velocity_stamped, *transform); + return velocity_stamped.vector; +} + +geometry_msgs::msg::Vector3 compensateEgoVehicleTwist( + const radar_msgs::msg::RadarReturn & radar, + const geometry_msgs::msg::TwistWithCovariance & ego_vehicle_twist_with_covariance, + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform) +{ + const geometry_msgs::msg::Vector3 radar_velocity = getVelocity(radar); + const geometry_msgs::msg::Vector3 v_r = getTransformedVelocity(radar_velocity, transform); + + const auto v_e = ego_vehicle_twist_with_covariance.twist.linear; + return geometry_msgs::build() + .x(v_r.x + v_e.x) + .y(v_r.y + v_e.y) + .z(v_r.z + v_e.z); +} +} // namespace + +namespace radar_static_pointcloud_filter +{ +using nav_msgs::msg::Odometry; +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; + +RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( + const rclcpp::NodeOptions & node_options) +: Node("radar_static_pointcloud_filter", node_options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarStaticPointcloudFilterNode::onSetParam, this, _1)); + + // Node Parameter + node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd", 2.0); + + // Subscriber + transform_listener_ = std::make_shared(this); + + sub_radar_.subscribe(this, "~/input/radar", rclcpp::QoS{1}.get_rmw_qos_profile()); + sub_odometry_.subscribe(this, "~/input/odometry", rclcpp::QoS{1}.get_rmw_qos_profile()); + + sync_ptr_ = std::make_shared(SyncPolicy(10), sub_radar_, sub_odometry_); + sync_ptr_->registerCallback(std::bind(&RadarStaticPointcloudFilterNode::onData, this, _1, _2)); + + // Publisher + pub_static_radar_ = create_publisher("~/output/static_radar_scan", 1); + pub_dynamic_radar_ = create_publisher("~/output/dynamic_radar_scan", 1); +} + +rcl_interfaces::msg::SetParametersResult RadarStaticPointcloudFilterNode::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + try { + auto & p = node_param_; + update_param(params, "doppler_velocity_sd", p.doppler_velocity_sd); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + result.successful = true; + result.reason = "success"; + return result; +} + +void RadarStaticPointcloudFilterNode::onData( + const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg) +{ + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform; + + try { + transform = transform_listener_->getTransform( + odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, + rclcpp::Duration::from_seconds(0.2)); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO(this->get_logger(), "Could not transform"); + return; + } + + RadarScan static_radar{}; + RadarScan dynamic_radar{}; + static_radar.header = radar_msg->header; + dynamic_radar.header = radar_msg->header; + + for (const auto & radar_return : radar_msg->returns) { + if (isStaticPointcloud(radar_return, odom_msg, transform)) { + static_radar.returns.emplace_back(radar_return); + } else { + dynamic_radar.returns.emplace_back(radar_return); + } + } + pub_static_radar_->publish(static_radar); + pub_dynamic_radar_->publish(dynamic_radar); +} + +bool RadarStaticPointcloudFilterNode::isStaticPointcloud( + const RadarReturn & radar_return, const Odometry::ConstSharedPtr & odom_msg, + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform) +{ + geometry_msgs::msg::Vector3 compensated_velocity = + compensateEgoVehicleTwist(radar_return, odom_msg->twist, transform); + + return (-node_param_.doppler_velocity_sd < compensated_velocity.x) && + (compensated_velocity.x < node_param_.doppler_velocity_sd); +} + +} // namespace radar_static_pointcloud_filter + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_static_pointcloud_filter::RadarStaticPointcloudFilterNode)