feat(radar_scan_to_pointcloud2): add radar_scan_to_pointcloud2 package

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
+)

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
+```

sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp

@@ -0,0 +1,71 @@
+// 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 <radar_msgs/msg/radar_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+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<RadarScan>::SharedPtr sub_radar_{};
+
+  // Callback
+  void onData(const RadarScan::ConstSharedPtr radar_msg);
+
+  // Publisher
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_amplitude_pointcloud_{};
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_doppler_pointcloud_{};
+
+  // Parameter Server
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & 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_

sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="input/radar" default="input/radar"/>
+  <arg name="output/amplitude_pointcloud" default="output/amplitude_pointcloud"/>
+  <arg name="output/doppler_pointcloud" default="output/doppler_pointcloud"/>
+  <arg name="publish_amplitude_pointcloud" default="true"/>
+  <arg name="publish_doppler_pointcloud" default="false"/>
+
+  <node pkg="radar_scan_to_pointcloud2" exec="radar_scan_to_pointcloud2_node" name="radar_scan_to_pointcloud2" output="screen">
+    <remap from="~/input/radar" to="$(var input/radar)"/>
+    <remap from="~/output/amplitude_pointcloud" to="$(var output/amplitude_pointcloud)"/>
+    <remap from="~/output/doppler_pointcloud" to="$(var output/doppler_pointcloud)"/>
+    <param name="publish_amplitude_pointcloud" value="$(var publish_amplitude_pointcloud)"/>
+    <param name="publish_doppler_pointcloud" value="$(var publish_doppler_pointcloud)"/>
+  </node>
+</launch>

sensing/radar_scan_to_pointcloud2/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>radar_scan_to_pointcloud2</name>
+  <version>0.1.0</version>
+  <description>radar_scan_to_pointcloud2</description>
+  <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
+  <author email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</author>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>radar_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp

@@ -0,0 +1,153 @@
+// 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 <pcl/pcl_base.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+using namespace std::literals;
+using std::chrono::duration;
+using std::chrono::duration_cast;
+using std::chrono::nanoseconds;
+using std::placeholders::_1;
+
+namespace
+{
+template <class T>
+bool update_param(
+  const std::vector<rclcpp::Parameter> & 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<T>();
+  return true;
+}
+
+pcl::PointXYZI getPointXYZI(const radar_msgs::msg::RadarReturn & radar, float intensity)
+{
+  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};
+}
+
+pcl::PointCloud<pcl::PointXYZI> toAmplitudePCL(const radar_msgs::msg::RadarScan & radar_scan)
+{
+  pcl::PointCloud<pcl::PointXYZI> 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<pcl::PointXYZI> toDopplerPCL(const radar_msgs::msg::RadarScan & radar_scan)
+{
+  pcl::PointCloud<pcl::PointXYZI> 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_.publish_amplitude_pointcloud =
+    declare_parameter<bool>("publish_amplitude_pointcloud", true);
+  node_param_.publish_doppler_pointcloud =
+    declare_parameter<bool>("publish_doppler_pointcloud", false);
+
+  // Subscriber
+  sub_radar_ = create_subscription<RadarScan>(
+    "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1));
+
+  // Publisher
+  pub_amplitude_pointcloud_ = create_publisher<PointCloud2>("~/output/amplitude_pointcloud", 1);
+  pub_doppler_pointcloud_ = create_publisher<PointCloud2>("~/output/doppler_pointcloud", 1);
+}
+
+rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam(
+  const std::vector<rclcpp::Parameter> & 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)