diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt new file mode 100644 index 0000000000000..1fb63df6594bf --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_scan_to_pointcloud2) + +# Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# 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..100fd509ef35b --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/README.md @@ -0,0 +1,33 @@ +# 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/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 | +| ---------------------------- | ---- | ----------------------------------------------------------------------------------------- | +| 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 +``` 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..bb331609f606d --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -0,0 +1,70 @@ +// 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 + +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 + { + bool publish_amplitude_pointcloud{}; + bool publish_doppler_pointcloud{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_radar_{}; + + // Callback + void onData(const RadarScan::ConstSharedPtr radar_msg); + + // Publisher + 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_{}; +}; + +} // 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..f94b0bfd91ea8 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml new file mode 100644 index 0000000000000..dc6ba43812ce6 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -0,0 +1,28 @@ + + + radar_scan_to_pointcloud2 + 0.1.0 + radar_scan_to_pointcloud2 + Satoshi Tanaka + Shunsuke Miura + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + + pcl_conversions + pcl_ros + radar_msgs + rclcpp + rclcpp_components + sensor_msgs + + autoware_cmake + 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..de4ac74db6429 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -0,0 +1,154 @@ +// 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 + +#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; +} + +pcl::PointXYZI getPointXYZI(const radar_msgs::msg::RadarReturn & radar, float intensity) +{ + pcl::PointXYZI point; + const float r_xy = radar.range * std::cos(radar.elevation); + 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) +{ + 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) +{ + using std::placeholders::_1; + + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarScanToPointcloud2Node::onSetParam, this, _1)); + + // Node Parameter + 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_amplitude_pointcloud_ = create_publisher("~/output/amplitude_pointcloud", 1); + pub_doppler_pointcloud_ = create_publisher("~/output/doppler_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, "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(); + return result; + } + result.successful = true; + result.reason = "success"; + return result; +} + +void RadarScanToPointcloud2Node::onData(const RadarScan::ConstSharedPtr radar_msg) +{ + 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); + } +} + +} // namespace radar_scan_to_pointcloud2 + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_scan_to_pointcloud2::RadarScanToPointcloud2Node)