feat: add radar tracks msgs converter (tier4#1012)

* add radar_tracks_msgs_converter

Signed-off-by: scepter914 <scepter914@gmail.com>

* apply format

Signed-off-by: scepter914 <scepter914@gmail.com>

* apply format

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix compile error on autoware cmake

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix namespace for cpplint

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix for humble

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix paramter copy

Signed-off-by: scepter914 <scepter914@gmail.com>

* update README

Signed-off-by: scepter914 <scepter914@gmail.com>

* add RadarTrackObjectID enum class

Signed-off-by: scepter914 <scepter914@gmail.com>

perception/radar_tracks_msgs_converter/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.5)
+project(radar_tracks_msgs_converter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+## 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 -Werror)
+endif()
+
+include_directories(
+  include
+)
+
+## Targets
+ament_auto_add_library(radar_tracks_msgs_converter_node_component SHARED
+  src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
+)
+
+rclcpp_components_register_node(radar_tracks_msgs_converter_node_component
+  PLUGIN "radar_tracks_msgs_converter::RadarTracksMsgsConverterNode"
+  EXECUTABLE radar_tracks_msgs_converter_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
+)

perception/radar_tracks_msgs_converter/README.md

@@ -0,0 +1,44 @@
+# radar_tracks_msgs_converter
+
+This package convert from [radar_msgs/msg/RadarTracks](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) into [autoware_auto_perception_msgs/msg/TrackedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/TrackedObject.idl).
+
+- Calculation cost is O(n).
+  - n: The number of radar objects
+
+## Design
+
+### Input / Output
+
+- Input
+  - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Converted topic
+  - `~/input/twist` (geometry_msgs/msg/TwistStamped.msg): Ego vehicle twist
+- Output
+  - `~/output/radar_objects` (autoware_auto_perception_msgs/msg/TrackedObject.msg): Converted topic
+
+### Parameters
+
+- `update_rate_hz` (double): The update rate [hz].
+  - Default parameter is 20.0
+- `new_frame_id` (string): The header frame of output topic.
+  - Default parameter is "base_link"
+- `use_twist_compensation` (bool): If the parameter is true, then the twist of output objects' topic is compensated by ego vehicle motion.
+  - Default parameter is "false"
+
+## Note
+
+This package convert the label from `radar_msgs/msg/RadarTrack.msg` to Autoware label.
+Label id is defined as below.
+
+|            | RadarTrack | Autoware |
+| ---------- | ---------- | -------- |
+| UNKNOWN    | 32000      | 0        |
+| CAR        | 32001      | 1        |
+| TRUCK      | 32002      | 2        |
+| BUS        | 32003      | 3        |
+| TRAILER    | 32004      | 4        |
+| MOTORCYCLE | 32005      | 5        |
+| BICYCLE    | 32006      | 6        |
+| PEDESTRIAN | 32007      | 7        |
+
+- [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg): additional vendor-specific classifications are permitted starting from 32000.
+- [Autoware objects label](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl)

perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp

@@ -0,0 +1,95 @@
+// 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_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
+#define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+#include "autoware_auto_perception_msgs/msg/shape.hpp"
+#include "autoware_auto_perception_msgs/msg/tracked_object.hpp"
+#include "autoware_auto_perception_msgs/msg/tracked_object_kinematics.hpp"
+#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "radar_msgs/msg/radar_tracks.hpp"
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace radar_tracks_msgs_converter
+{
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+using autoware_auto_perception_msgs::msg::Shape;
+using autoware_auto_perception_msgs::msg::TrackedObject;
+using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
+using autoware_auto_perception_msgs::msg::TrackedObjects;
+using geometry_msgs::msg::TwistStamped;
+using radar_msgs::msg::RadarTracks;
+
+class RadarTracksMsgsConverterNode : public rclcpp::Node
+{
+public:
+  explicit RadarTracksMsgsConverterNode(const rclcpp::NodeOptions & node_options);
+
+  struct NodeParam
+  {
+    double update_rate_hz{};
+    std::string new_frame_id{};
+    bool use_twist_compensation{};
+  };
+
+private:
+  // Subscriber
+  rclcpp::Subscription<RadarTracks>::SharedPtr sub_radar_{};
+  rclcpp::Subscription<TwistStamped>::SharedPtr sub_twist_{};
+  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
+
+  // Callback
+  void onRadarTracks(const RadarTracks::ConstSharedPtr msg);
+  void onTwist(const TwistStamped::ConstSharedPtr msg);
+
+  // Data Buffer
+  RadarTracks::ConstSharedPtr radar_data_{};
+  TwistStamped::ConstSharedPtr twist_data_{};
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
+
+  // Publisher
+  rclcpp::Publisher<TrackedObjects>::SharedPtr pub_radar_{};
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_{};
+
+  bool isDataReady();
+  void onTimer();
+
+  // Parameter Server
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & params);
+
+  // Parameter
+  NodeParam node_param_{};
+
+  // Core
+  TrackedObjects convertRadarTrackToTrackedObjects();
+  uint8_t convertClassification(const uint16_t classification);
+};
+
+}  // namespace radar_tracks_msgs_converter
+
+#endif  // RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_

perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="input/radar_objects" default="input/radar_objects"/>
+  <arg name="input/twist" default="input/twist"/>
+  <arg name="output/radar_objects" default="output/radar_objects"/>
+  <arg name="update_rate_hz" default="20.0"/>
+  <arg name="use_twist_compensation" default="false"/>
+
+  <node pkg="radar_tracks_msgs_converter" exec="radar_tracks_msgs_converter_node" name="radar_tracks_msgs_converter" output="screen">
+    <remap from="~/input/radar_objects" to="$(var input/radar_objects)"/>
+    <remap from="~/input/twist" to="$(var input/twist)"/>
+    <remap from="~/output/radar_objects" to="$(var output/radar_objects)"/>
+    <param name="update_rate_hz" value="$(var update_rate_hz)"/>
+    <param name="use_twist_compensation" value="$(var use_twist_compensation)"/>
+  </node>
+</launch>

perception/radar_tracks_msgs_converter/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>radar_tracks_msgs_converter</name>
+  <version>0.0.1</version>
+  <description>radar_tracks_msgs_converter</description>
+  <maintainer email="satoshi.tanaka@tier4.jp">scepter914</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="satoshi.tanaka@tier4.jp">scepter914</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>radar_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>