From bef07effb81334b990cf8bff4c2c538b2b3c972c Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 21 Nov 2022 14:45:40 +0900 Subject: [PATCH 01/12] feat(radar_scan_to_pointcloud2): add radar_scan_to_pointcloud2 package Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2/CMakeLists.txt | 41 +++++ sensing/radar_scan_to_pointcloud2/README.md | 30 ++++ .../radar_scan_to_pointcloud2_node.hpp | 65 ++++++++ .../radar_scan_to_pointcloud2.launch.xml | 11 ++ sensing/radar_scan_to_pointcloud2/package.xml | 26 +++ .../radar_scan_to_pointcloud2_node.cpp | 149 ++++++++++++++++++ 6 files changed, 322 insertions(+) create mode 100644 sensing/radar_scan_to_pointcloud2/CMakeLists.txt create mode 100644 sensing/radar_scan_to_pointcloud2/README.md create mode 100644 sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp create mode 100644 sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml create mode 100644 sensing/radar_scan_to_pointcloud2/package.xml create mode 100644 sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt new file mode 100644 index 0000000000000..ab695bb0730f4 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_scan_to_pointcloud2) + +# 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_scan_to_pointcloud2_node_component SHARED + src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +) + +rclcpp_components_register_node(radar_scan_to_pointcloud2_node_component + PLUGIN "radar_scan_to_pointcloud2::RadarScanToPointcloud2Node" + EXECUTABLE radar_scan_to_pointcloud2_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_scan_to_pointcloud2/README.md b/sensing/radar_scan_to_pointcloud2/README.md new file mode 100644 index 0000000000000..ac032c4558aa1 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/README.md @@ -0,0 +1,30 @@ +# radar_scan_to_pointcloud2 +## radar_scan_to_pointcloud2_node + +- Convert from `radar_msgs::msg::RadarScan` to `sensor_msgs::msg::PointCloud2` +- Calculation cost O(n) + - n: The number of radar return + +### Input topics + +| Name | Type | Description | +| ----------- | -------------------------- | ----------- | +| input/radar | radar_msgs::msg::RadarScan | RadarScan | + +### Output topics + +| Name | Type | Description | +| ------------ | ----------------------------- | ---------------------------- | +| output/radar | sensor_msgs::msg::PointCloud2 | PointCloud2 radar pointcloud | + +### Parameters + +| Name | Type | Description | +| -------------------- | ----------- | ------------------------------------------------------------------------------------------------------------------ | +| intensity_value_mode | std::string | The output's intensity value of pointcloud. Select from "amplitude" or "doppler_velocity". Default is "amplitude". | + +### How to launch + +``` +ros2 launch radar_scan_to_pointcloud2 radar_scan_to_pointcloud2.launch.xml +``` diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp new file mode 100644 index 0000000000000..a226fe69ecd4a --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -0,0 +1,65 @@ +// 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_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP__ +#define RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP__ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace radar_scan_to_pointcloud2 +{ +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; +using sensor_msgs::msg::PointCloud2; + +class RadarScanToPointcloud2Node : public rclcpp::Node +{ +public: + explicit RadarScanToPointcloud2Node(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + std::string intensity_value_mode{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_radar_{}; + + // Callback + void onData(const RadarScan::ConstSharedPtr radar_msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_pointcloud_{}; + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Parameter + NodeParam node_param_{}; +}; + +} // namespace radar_scan_to_pointcloud2 + +#endif // RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP__ diff --git a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml new file mode 100644 index 0000000000000..226cae8346b3f --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml new file mode 100644 index 0000000000000..6ea05c63d4106 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -0,0 +1,26 @@ + + + radar_scan_to_pointcloud2 + 0.1.0 + radar_scan_to_pointcloud2 + Satoshi Tanaka + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + + radar_msgs + rclcpp + rclcpp_components + pcl_conversions + pcl_ros + sensor_msgs + + ament_clang_format + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp new file mode 100644 index 0000000000000..b677ebcbb7a60 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -0,0 +1,149 @@ +// 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_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp" + +#include +#include +#include +#include + +using namespace std::literals; +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; +using std::placeholders::_1; + +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; +} + +pcl::PointXYZI getPointXYZI(const radar_msgs::msg::RadarReturn & radar, float intensity) +{ + const float x = radar.range * std::cos(radar.azimuth) * std::cos(radar.elevation); + const float y = radar.range * std::sin(radar.azimuth) * std::cos(radar.elevation); + const float z = radar.range * std::sin(radar.elevation); + return pcl::PointXYZI{x, y, z, intensity}; +} + +pcl::PointCloud toAmplitudePCL(const radar_msgs::msg::RadarScan & radar_scan) +{ + pcl::PointCloud pcl; + for (const auto & radar : radar_scan.returns) { + pcl.push_back(getPointXYZI(radar, radar.amplitude)); + } + return pcl; +} + +sensor_msgs::msg::PointCloud2 toAmplitudePointcloud2(const radar_msgs::msg::RadarScan & radar_scan) +{ + sensor_msgs::msg::PointCloud2 pointcloud_msg; + auto pcl_pointcloud = toAmplitudePCL(radar_scan); + pcl::toROSMsg(pcl_pointcloud, pointcloud_msg); + pointcloud_msg.header = radar_scan.header; + return pointcloud_msg; +} + +pcl::PointCloud toDopplerPCL(const radar_msgs::msg::RadarScan & radar_scan) +{ + pcl::PointCloud pcl; + for (const auto & radar : radar_scan.returns) { + pcl.push_back(getPointXYZI(radar, radar.doppler_velocity)); + } + return pcl; +} + +sensor_msgs::msg::PointCloud2 toDopplerPointcloud2(const radar_msgs::msg::RadarScan & radar_scan) +{ + sensor_msgs::msg::PointCloud2 pointcloud_msg; + auto pcl_pointcloud = toDopplerPCL(radar_scan); + pcl::toROSMsg(pcl_pointcloud, pointcloud_msg); + pointcloud_msg.header = radar_scan.header; + return pointcloud_msg; +} +} // namespace + +namespace radar_scan_to_pointcloud2 +{ +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; +using sensor_msgs::msg::PointCloud2; + +RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions & node_options) +: Node("radar_scan_to_pointcloud2", node_options) +{ + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarScanToPointcloud2Node::onSetParam, this, _1)); + + // Node Parameter + node_param_.intensity_value_mode = + declare_parameter("intensity_value_mode", "amplitude"); + + // Subscriber + sub_radar_ = create_subscription( + "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1)); + + // Publisher + pub_pointcloud_ = create_publisher("~/output/pointcloud", 1); +} + +rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + + try { + auto & p = node_param_; + update_param(params, "intensity_value_mode", p.intensity_value_mode); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + result.successful = true; + result.reason = "success"; + return result; +} + +void RadarScanToPointcloud2Node::onData(const RadarScan::ConstSharedPtr radar_msg) +{ + sensor_msgs::msg::PointCloud2 output; + if (node_param_.intensity_value_mode == "amplitude") { + output = toAmplitudePointcloud2(*radar_msg); + } else if (node_param_.intensity_value_mode == "doppler_velocity") { + output = toDopplerPointcloud2(*radar_msg); + } else { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, "Error intensity value mode."); + } + pub_pointcloud_->publish(output); +} + +} // namespace radar_scan_to_pointcloud2 + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_scan_to_pointcloud2::RadarScanToPointcloud2Node) From 5a9a4d228ce9f1629d2f8f64257e767b50f7261b Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 21 Nov 2022 14:46:44 +0900 Subject: [PATCH 02/12] apply pre-commit Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/README.md | 1 + .../radar_scan_to_pointcloud2_node.hpp | 6 +++--- sensing/radar_scan_to_pointcloud2/package.xml | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/README.md b/sensing/radar_scan_to_pointcloud2/README.md index ac032c4558aa1..f82a7d29f4de8 100644 --- a/sensing/radar_scan_to_pointcloud2/README.md +++ b/sensing/radar_scan_to_pointcloud2/README.md @@ -1,4 +1,5 @@ # radar_scan_to_pointcloud2 + ## radar_scan_to_pointcloud2_node - Convert from `radar_msgs::msg::RadarScan` to `sensor_msgs::msg::PointCloud2` diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp index a226fe69ecd4a..3f8020a1bf096 100644 --- a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP__ -#define RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP__ +#ifndef RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#define RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -62,4 +62,4 @@ class RadarScanToPointcloud2Node : public rclcpp::Node } // namespace radar_scan_to_pointcloud2 -#endif // RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP__ +#endif // RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 6ea05c63d4106..9b872eb35a92f 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -9,11 +9,11 @@ ament_cmake_auto + pcl_conversions + pcl_ros radar_msgs rclcpp rclcpp_components - pcl_conversions - pcl_ros sensor_msgs ament_clang_format From 25ca2ef345edfb87318a8ce024ce40a239e4d0cf Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 21 Nov 2022 15:16:33 +0900 Subject: [PATCH 03/12] fix parameters Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/README.md | 14 +++++----- .../radar_scan_to_pointcloud2_node.hpp | 10 +++++-- .../radar_scan_to_pointcloud2.launch.xml | 12 ++++++--- .../radar_scan_to_pointcloud2_node.cpp | 27 ++++++++++--------- 4 files changed, 39 insertions(+), 24 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/README.md b/sensing/radar_scan_to_pointcloud2/README.md index f82a7d29f4de8..eecc4bd328296 100644 --- a/sensing/radar_scan_to_pointcloud2/README.md +++ b/sensing/radar_scan_to_pointcloud2/README.md @@ -14,15 +14,17 @@ ### Output topics -| Name | Type | Description | -| ------------ | ----------------------------- | ---------------------------- | -| output/radar | sensor_msgs::msg::PointCloud2 | PointCloud2 radar pointcloud | +| Name | Type | Description | +| --------------------------- | ----------------------------- | ----------------------------------------------------------------- | +| output/amplitude_pointcloud | sensor_msgs::msg::PointCloud2 | PointCloud2 radar pointcloud whose intensity is amplitude. | +| output/doppler_pointcloud | sensor_msgs::msg::PointCloud2 | PointCloud2 radar pointcloud whose intensity is doppler velocity. | ### Parameters -| Name | Type | Description | -| -------------------- | ----------- | ------------------------------------------------------------------------------------------------------------------ | -| intensity_value_mode | std::string | The output's intensity value of pointcloud. Select from "amplitude" or "doppler_velocity". Default is "amplitude". | +| Name | Type | Description | +| ---------------------------- | ---- | ----------------------------------------------------------------------------------------- | +| publish_amplitude_pointcloud | bool | Whether publish radar pointcloud whose intensity is amplitude. Default is "true". | +| publish_doppler_pointcloud | bool | Whether publish radar pointcloud whose intensity is doppler velocity. Default is "false". | ### How to launch diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp index 3f8020a1bf096..7497f34d92ddd 100644 --- a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -38,7 +38,8 @@ class RadarScanToPointcloud2Node : public rclcpp::Node struct NodeParam { - std::string intensity_value_mode{}; + bool publish_amplitude_pointcloud{}; + bool publish_doppler_pointcloud{}; }; private: @@ -49,13 +50,18 @@ class RadarScanToPointcloud2Node : public rclcpp::Node void onData(const RadarScan::ConstSharedPtr radar_msg); // Publisher - rclcpp::Publisher::SharedPtr pub_pointcloud_{}; + rclcpp::Publisher::SharedPtr pub_amplitude_pointcloud_{}; + rclcpp::Publisher::SharedPtr pub_doppler_pointcloud_{}; // Parameter Server OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onSetParam( const std::vector & params); + // Data buffer + sensor_msgs::msg::PointCloud2 amplitude_pointcloud; + sensor_msgs::msg::PointCloud2 doppler_pointcloud; + // Parameter NodeParam node_param_{}; }; diff --git a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml index 226cae8346b3f..f94b0bfd91ea8 100644 --- a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml +++ b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml @@ -1,11 +1,15 @@ - - + + + + - - + + + + diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index b677ebcbb7a60..aa3d9f69c2c9d 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -101,15 +101,18 @@ RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions std::bind(&RadarScanToPointcloud2Node::onSetParam, this, _1)); // Node Parameter - node_param_.intensity_value_mode = - declare_parameter("intensity_value_mode", "amplitude"); + node_param_.publish_amplitude_pointcloud = + declare_parameter("publish_amplitude_pointcloud", true); + node_param_.publish_doppler_pointcloud = + declare_parameter("publish_doppler_pointcloud", false); // Subscriber sub_radar_ = create_subscription( "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1)); // Publisher - pub_pointcloud_ = create_publisher("~/output/pointcloud", 1); + pub_amplitude_pointcloud_ = create_publisher("~/output/amplitude_pointcloud", 1); + pub_doppler_pointcloud_ = create_publisher("~/output/doppler_pointcloud", 1); } rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam( @@ -119,7 +122,8 @@ rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam( try { auto & p = node_param_; - update_param(params, "intensity_value_mode", p.intensity_value_mode); + update_param(params, "publish_amplitude_pointcloud", p.publish_amplitude_pointcloud); + update_param(params, "publish_doppler_pointcloud", p.publish_doppler_pointcloud); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); @@ -132,15 +136,14 @@ rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam( void RadarScanToPointcloud2Node::onData(const RadarScan::ConstSharedPtr radar_msg) { - sensor_msgs::msg::PointCloud2 output; - if (node_param_.intensity_value_mode == "amplitude") { - output = toAmplitudePointcloud2(*radar_msg); - } else if (node_param_.intensity_value_mode == "doppler_velocity") { - output = toDopplerPointcloud2(*radar_msg); - } else { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, "Error intensity value mode."); + if (node_param_.publish_amplitude_pointcloud) { + amplitude_pointcloud = toAmplitudePointcloud2(*radar_msg); + pub_amplitude_pointcloud_->publish(amplitude_pointcloud); + } + if (node_param_.publish_doppler_pointcloud) { + doppler_pointcloud = toDopplerPointcloud2(*radar_msg); + pub_doppler_pointcloud_->publish(doppler_pointcloud); } - pub_pointcloud_->publish(output); } } // namespace radar_scan_to_pointcloud2 From bd190538875ee338bc0b7fcc46bd7b69926f4adc Mon Sep 17 00:00:00 2001 From: scepter914 Date: Mon, 21 Nov 2022 15:19:04 +0900 Subject: [PATCH 04/12] apply pre-commit Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/README.md b/sensing/radar_scan_to_pointcloud2/README.md index eecc4bd328296..100fd509ef35b 100644 --- a/sensing/radar_scan_to_pointcloud2/README.md +++ b/sensing/radar_scan_to_pointcloud2/README.md @@ -23,11 +23,11 @@ | Name | Type | Description | | ---------------------------- | ---- | ----------------------------------------------------------------------------------------- | -| publish_amplitude_pointcloud | bool | Whether publish radar pointcloud whose intensity is amplitude. Default is "true". | -| publish_doppler_pointcloud | bool | Whether publish radar pointcloud whose intensity is doppler velocity. Default is "false". | +| publish_amplitude_pointcloud | bool | Whether publish radar pointcloud whose intensity is amplitude. Default is `true`. | +| publish_doppler_pointcloud | bool | Whether publish radar pointcloud whose intensity is doppler velocity. Default is `false`. | ### How to launch -``` +```sh ros2 launch radar_scan_to_pointcloud2 radar_scan_to_pointcloud2.launch.xml ``` From 45d0f2db546558ac2b063cd397b4a91d447d9010 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 22 Nov 2022 18:32:28 +0900 Subject: [PATCH 05/12] make temporary variable Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index aa3d9f69c2c9d..ec7413d95a43c 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -44,8 +44,9 @@ bool update_param( pcl::PointXYZI getPointXYZI(const radar_msgs::msg::RadarReturn & radar, float intensity) { - const float x = radar.range * std::cos(radar.azimuth) * std::cos(radar.elevation); - const float y = radar.range * std::sin(radar.azimuth) * std::cos(radar.elevation); + const float r_xy = radar.range * std::cos(radar.elevation); + const float x = r_xy * std::cos(radar.azimuth); + const float y = r_xy * std::sin(radar.azimuth); const float z = radar.range * std::sin(radar.elevation); return pcl::PointXYZI{x, y, z, intensity}; } From 9d521f3452623f87f6bf724a280f32d1c53187c6 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:13:10 +0900 Subject: [PATCH 06/12] use autoware_cmame in CMakeLists.txt Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/CMakeLists.txt | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt index ab695bb0730f4..1fb63df6594bf 100644 --- a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -1,20 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(radar_scan_to_pointcloud2) -# 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_scan_to_pointcloud2_node_component SHARED From 5e61464f1700f06efe2050269373d6e399a42f43 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:17:17 +0900 Subject: [PATCH 07/12] refactor Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2_node.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index ec7413d95a43c..039f31fe10ba6 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -19,12 +19,6 @@ #include #include -using namespace std::literals; -using std::chrono::duration; -using std::chrono::duration_cast; -using std::chrono::nanoseconds; -using std::placeholders::_1; - namespace { template @@ -97,6 +91,8 @@ using sensor_msgs::msg::PointCloud2; RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions & node_options) : Node("radar_scan_to_pointcloud2", node_options) { + using std::placeholders::_1; + // Parameter Server set_param_res_ = this->add_on_set_parameters_callback( std::bind(&RadarScanToPointcloud2Node::onSetParam, this, _1)); From eb8ce300dc1bf61727b3391393a0e07326539f27 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:34:11 +0900 Subject: [PATCH 08/12] add dependancy Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 9b872eb35a92f..572db6cf3c225 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -16,6 +16,7 @@ rclcpp_components sensor_msgs + autoware_cmake ament_clang_format ament_lint_auto ament_lint_common From b53d5915e013aacdc1e9360d8ce2653d8df612e8 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 24 Nov 2022 15:57:52 +0900 Subject: [PATCH 09/12] fix build error at galactic enviroment Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2_node.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index 039f31fe10ba6..497f5b846e151 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -38,11 +38,13 @@ bool update_param( pcl::PointXYZI getPointXYZI(const radar_msgs::msg::RadarReturn & radar, float intensity) { + pcl::PointXYZI point; const float r_xy = radar.range * std::cos(radar.elevation); - const float x = r_xy * std::cos(radar.azimuth); - const float y = r_xy * std::sin(radar.azimuth); - const float z = radar.range * std::sin(radar.elevation); - return pcl::PointXYZI{x, y, z, intensity}; + point.x = r_xy * std::cos(radar.azimuth); + point.y = r_xy * std::sin(radar.azimuth); + point.z = radar.range * std::sin(radar.elevation); + point.intensity = intensity; + return point; } pcl::PointCloud toAmplitudePCL(const radar_msgs::msg::RadarScan & radar_scan) From 9f8429eabdd4d2b67a6c0d8587573952b4c5ef7f Mon Sep 17 00:00:00 2001 From: scepter914 Date: Fri, 25 Nov 2022 12:01:26 +0900 Subject: [PATCH 10/12] apply cpplint Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2_node.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp index 7497f34d92ddd..2b606bb472499 100644 --- a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -22,8 +22,6 @@ #include #include -#include -#include namespace radar_scan_to_pointcloud2 { From 19258726ed4e7658fb9f6819f84ce54dbff5ab0f Mon Sep 17 00:00:00 2001 From: scepter914 Date: Fri, 25 Nov 2022 12:02:10 +0900 Subject: [PATCH 11/12] add code maintainer Signed-off-by: scepter914 --- sensing/radar_scan_to_pointcloud2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 572db6cf3c225..dc6ba43812ce6 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -4,6 +4,7 @@ 0.1.0 radar_scan_to_pointcloud2 Satoshi Tanaka + Shunsuke Miura Satoshi Tanaka Apache License 2.0 From 62b51cbcd6503c4b1320266f6fbafcc7f6b32d79 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Fri, 25 Nov 2022 12:13:13 +0900 Subject: [PATCH 12/12] apply cpplint Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2_node.hpp | 1 + .../radar_scan_to_pointcloud2_node.cpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp index 2b606bb472499..bb331609f606d 100644 --- a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -22,6 +22,7 @@ #include #include +#include namespace radar_scan_to_pointcloud2 { diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp index 497f5b846e151..de4ac74db6429 100644 --- a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -19,6 +19,9 @@ #include #include +#include +#include + namespace { template