From c49774912432aeef2ca840dbe251ba30b2aae8b5 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 14:24:04 +0900 Subject: [PATCH 01/14] feat(radar_static_pointcloud_filter): add radar_static_pointcloud_filter package Signed-off-by: scepter914 --- .../CMakeLists.txt | 41 ++++ .../radar_static_pointcloud_filter/README.md | 35 ++++ .../radar_static_pointcloud_filter.drawio.svg | 182 ++++++++++++++++++ .../radar_static_pointcloud_filter_node.hpp | 86 +++++++++ .../radar_static_pointcloud_filter.launch.xml | 15 ++ .../package.xml | 29 +++ .../radar_static_pointcloud_filter_node.cpp | 165 ++++++++++++++++ 7 files changed, 553 insertions(+) create mode 100644 sensing/radar_static_pointcloud_filter/CMakeLists.txt create mode 100644 sensing/radar_static_pointcloud_filter/README.md create mode 100644 sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg create mode 100644 sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp create mode 100644 sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml create mode 100644 sensing/radar_static_pointcloud_filter/package.xml create mode 100644 sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt new file mode 100644 index 0000000000000..c6c3f8bb6d7f5 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_static_pointcloud_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_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..79e32ea97a6d1 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/README.md @@ -0,0 +1,35 @@ +# 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..701e66d7c3385 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/docs/radar_static_pointcloud_filter.drawio.svg @@ -0,0 +1,182 @@ + + + + + + + +
+
+
+ + 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..1bad4d294aca9 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/include/radar_static_pointcloud_filter/radar_static_pointcloud_filter_node.hpp @@ -0,0 +1,86 @@ +// 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_; + + // Data buffer + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_; + + // 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..f3a65debb8089 --- /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..504d8828b58ca --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -0,0 +1,29 @@ + + + radar_static_pointcloud_filter + 0.1.0 + radar_static_pointcloud_filter + Satoshi Tanaka + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + + nav_msgs + message_filters + radar_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + tf2 + tf2_ros + tf2_geometry_msgs + + 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..3558a18997944 --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -0,0 +1,165 @@ +// 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) + .y(0.0) + .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) +{ + transform_ = transform_listener_->getTransform( + odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, + rclcpp::Duration::from_seconds(0.02)); + + 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) From 6405a02a98d2e92a992b521c6144d85daf21cef5 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 14:25:55 +0900 Subject: [PATCH 02/14] apply pre-commit Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter/README.md | 1 + .../radar_static_pointcloud_filter.drawio.svg | 367 +++++++++--------- .../radar_static_pointcloud_filter_node.hpp | 6 +- .../radar_static_pointcloud_filter.launch.xml | 10 +- .../package.xml | 6 +- 5 files changed, 205 insertions(+), 185 deletions(-) diff --git a/sensing/radar_static_pointcloud_filter/README.md b/sensing/radar_static_pointcloud_filter/README.md index 79e32ea97a6d1..1e480ef4354dc 100644 --- a/sensing/radar_static_pointcloud_filter/README.md +++ b/sensing/radar_static_pointcloud_filter/README.md @@ -1,4 +1,5 @@ # radar_static_pointcloud_filter + ## radar_static_pointcloud_filter_node Extract static/dynamic radar pointcloud by using doppler velocity and ego motion. 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 index 701e66d7c3385..c9e51ca2b38a4 100644 --- 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 @@ -1,182 +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 - - - - - - - -
-
-
+ + + + + + + +
+
+
+ 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... - - - - - - - - + + + + + + +
+
+
+ param.doppler_velocity_sd +
+
+
+ + param.doppler_velocity_sd + + + + + + + +
+
+
+ Sensor coordinate +
+
+
+ + Sensor coordi... + + + + + + +
+
+
+ World coordinate +
+
+
+ + World coordin... + - - - - - Viewer does not support full SVG 1.1 - - - + + + + + + + + + + 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 index 1bad4d294aca9..20b544adf351c 100644 --- 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 @@ -12,8 +12,8 @@ // 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__ +#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" @@ -83,4 +83,4 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node }; } // namespace radar_static_pointcloud_filter -#endif // RADAR_STATIC_POINTCLOUD_FILTER__RADAR_STATIC_POINTCLOUD_FILTER_NODE_HPP__ +#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 index f3a65debb8089..08fdeee74f15a 100644 --- 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 @@ -1,9 +1,9 @@ - - - - - + + + + + diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index 504d8828b58ca..15ef03e033f99 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -9,15 +9,15 @@ ament_cmake_auto - nav_msgs message_filters + nav_msgs radar_msgs rclcpp rclcpp_components - tier4_autoware_utils tf2 - tf2_ros tf2_geometry_msgs + tf2_ros + tier4_autoware_utils ament_clang_format ament_lint_auto From 2e8f0ba1a4f6902c3a159d6ff4fa4bc3cac8770c Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 14:48:57 +0900 Subject: [PATCH 03/14] refactor Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.hpp | 1 - 1 file changed, 1 deletion(-) 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 index 20b544adf351c..71ab5c45ea6ee 100644 --- 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 @@ -76,7 +76,6 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node NodeParam node_param_{}; // Function - bool isStaticPointcloud( const RadarReturn & radar_return, const Odometry::ConstSharedPtr & odom_msg, geometry_msgs::msg::TransformStamped::ConstSharedPtr transform); From 2aa1387a22e2a14f9bd40152bd5f67cf9b374738 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:10:42 +0900 Subject: [PATCH 04/14] use autoware_cmame in CMakeLists.txt Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter/CMakeLists.txt | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt index c6c3f8bb6d7f5..1f08f7842cfdf 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -1,20 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(radar_static_pointcloud_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() +find_package(autoware_cmake REQUIRED) +autoware_package() # Targets ament_auto_add_library(radar_static_pointcloud_filter_node_component SHARED From e1f2437d7bd7339cfe1d780fd2ae6db7931bcbf6 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:24:03 +0900 Subject: [PATCH 05/14] delete transform member variable Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.hpp | 3 --- .../radar_static_pointcloud_filter_node.cpp | 7 ++++--- 2 files changed, 4 insertions(+), 6 deletions(-) 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 index 71ab5c45ea6ee..e97e8fa5ca500 100644 --- 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 @@ -57,9 +57,6 @@ class RadarStaticPointcloudFilterNode : public rclcpp::Node using Sync = message_filters::Synchronizer; typename std::shared_ptr sync_ptr_; - // Data buffer - geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_; - // Callback void onData(const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg); 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 index 3558a18997944..15b4175731bf9 100644 --- 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 @@ -128,9 +128,10 @@ rcl_interfaces::msg::SetParametersResult RadarStaticPointcloudFilterNode::onSetP void RadarStaticPointcloudFilterNode::onData( const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg) { - transform_ = transform_listener_->getTransform( - odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, - rclcpp::Duration::from_seconds(0.02)); + geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_ = + transform_listener_->getTransform( + odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, + rclcpp::Duration::from_seconds(0.01)); RadarScan static_radar_{}; RadarScan dynamic_radar_{}; From a0f29e1964dec06ab9bfdfc2d17a85dc108a1807 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:31:35 +0900 Subject: [PATCH 06/14] add dependancy Signed-off-by: scepter914 --- sensing/radar_static_pointcloud_filter/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index 15ef03e033f99..ec0764822d6df 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -19,6 +19,7 @@ tf2_ros tier4_autoware_utils + autoware_cmake ament_clang_format ament_lint_auto ament_lint_common From bb7104f83a401cbd34c253387c74a88bced7935a Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 16:03:56 +0900 Subject: [PATCH 07/14] fix transform Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) 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 index 15b4175731bf9..53bf9faecfa5c 100644 --- 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 @@ -128,10 +128,16 @@ rcl_interfaces::msg::SetParametersResult RadarStaticPointcloudFilterNode::onSetP void RadarStaticPointcloudFilterNode::onData( const RadarScan::ConstSharedPtr radar_msg, const Odometry::ConstSharedPtr odom_msg) { - geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_ = - transform_listener_->getTransform( + 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.01)); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO(this->get_logger(), "Could not transform"); + return; + } RadarScan static_radar_{}; RadarScan dynamic_radar_{}; @@ -139,7 +145,7 @@ void RadarStaticPointcloudFilterNode::onData( dynamic_radar_.header = radar_msg->header; for (const auto & radar_return : radar_msg->returns) { - if (isStaticPointcloud(radar_return, odom_msg, transform_)) { + if (isStaticPointcloud(radar_return, odom_msg, transform)) { static_radar_.returns.emplace_back(radar_return); } else { dynamic_radar_.returns.emplace_back(radar_return); From fa833d90357dca7709d1170d0977063fc589a083 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Fri, 25 Nov 2022 12:03:53 +0900 Subject: [PATCH 08/14] add code maintainer Signed-off-by: scepter914 --- sensing/radar_static_pointcloud_filter/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index ec0764822d6df..5c48298eaa48e 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -4,6 +4,7 @@ 0.1.0 radar_static_pointcloud_filter Satoshi Tanaka + Shunsuke Miura Satoshi Tanaka Apache License 2.0 From 4ac5be2f66c58132b4254d3ed2b84a3e5b673afe Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 25 Nov 2022 03:05:43 +0000 Subject: [PATCH 09/14] ci(pre-commit): autofix --- sensing/radar_static_pointcloud_filter/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index 5c48298eaa48e..30739d5578cb9 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -4,7 +4,7 @@ 0.1.0 radar_static_pointcloud_filter Satoshi Tanaka - Shunsuke Miura + Shunsuke Miura Satoshi Tanaka Apache License 2.0 From 53437ff01e778863388ffbcaa289507c9ee1a3eb Mon Sep 17 00:00:00 2001 From: scepter914 Date: Fri, 25 Nov 2022 12:48:11 +0900 Subject: [PATCH 10/14] fix duration time Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 53bf9faecfa5c..0e86cb1b7fa55 100644 --- 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 @@ -133,7 +133,7 @@ void RadarStaticPointcloudFilterNode::onData( try { transform = transform_listener_->getTransform( odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, - rclcpp::Duration::from_seconds(0.01)); + rclcpp::Duration::from_seconds(0.1)); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform"); return; From 6fd087527fae7bec88c7f4213cdcdad363735d91 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Fri, 25 Nov 2022 18:10:27 +0900 Subject: [PATCH 11/14] change duration time Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 0e86cb1b7fa55..bc4d26a3b52c2 100644 --- 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 @@ -133,7 +133,7 @@ void RadarStaticPointcloudFilterNode::onData( try { transform = transform_listener_->getTransform( odom_msg->header.frame_id, radar_msg->header.frame_id, odom_msg->header.stamp, - rclcpp::Duration::from_seconds(0.1)); + rclcpp::Duration::from_seconds(0.2)); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform"); return; From 77f602b5857d25cfc62e6cae0b8ed918a1b6beaa Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Mon, 28 Nov 2022 16:14:46 +0900 Subject: [PATCH 12/14] Update sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp Co-authored-by: Yukihiro Saito Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index bc4d26a3b52c2..5cc2a8807c070 100644 --- 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 @@ -139,8 +139,8 @@ void RadarStaticPointcloudFilterNode::onData( return; } - RadarScan static_radar_{}; - RadarScan dynamic_radar_{}; + RadarScan static_radar{}; + RadarScan dynamic_radar{}; static_radar_.header = radar_msg->header; dynamic_radar_.header = radar_msg->header; From 86fd1998bf44262c813c01c33f261e4bc2246549 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 28 Nov 2022 16:26:58 +0900 Subject: [PATCH 13/14] fix from variable renaming Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 index 5cc2a8807c070..3f4f49c8bc24a 100644 --- 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 @@ -141,18 +141,18 @@ void RadarStaticPointcloudFilterNode::onData( RadarScan static_radar{}; RadarScan dynamic_radar{}; - static_radar_.header = radar_msg->header; - dynamic_radar_.header = radar_msg->header; + 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); + static_radar.returns.emplace_back(radar_return); } else { - dynamic_radar_.returns.emplace_back(radar_return); + dynamic_radar.returns.emplace_back(radar_return); } } - pub_static_radar_->publish(static_radar_); - pub_dynamic_radar_->publish(dynamic_radar_); + pub_static_radar_->publish(static_radar); + pub_dynamic_radar_->publish(dynamic_radar); } bool RadarStaticPointcloudFilterNode::isStaticPointcloud( From 42b3c342bb293f9344d4574760f650e680705377 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 28 Nov 2022 19:19:27 +0900 Subject: [PATCH 14/14] fix function of convert velocity from doppler velocity Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index 3f4f49c8bc24a..812a96e7cdcd5 100644 --- 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 @@ -42,8 +42,8 @@ bool update_param( geometry_msgs::msg::Vector3 getVelocity(const radar_msgs::msg::RadarReturn & radar) { return geometry_msgs::build() - .x(radar.doppler_velocity) - .y(0.0) + .x(radar.doppler_velocity * std::cos(radar.azimuth)) + .y(radar.doppler_velocity * std::sin(radar.azimuth)) .z(0.0); }