feat(simple_object_merger): add simple_object_merger (#4219)

* feat(simple_object_merger): add simple_object_merger

Signed-off-by: scepter914 <[email protected]>

* refactor

Signed-off-by: scepter914 <[email protected]>

* add timeout parameter

Signed-off-by: scepter914 <[email protected]>

---------

Signed-off-by: scepter914 <[email protected]>

perception/simple_object_merger/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5)
+project(simple_object_merger)
+
+# Dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(simple_object_merger_node_component SHARED
+  src/simple_object_merger_node/simple_object_merger_node.cpp
+)
+
+rclcpp_components_register_node(simple_object_merger_node_component
+  PLUGIN "simple_object_merger::SimpleObjectMergerNode"
+  EXECUTABLE simple_object_merger_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/simple_object_merger/README.md

@@ -0,0 +1,60 @@
+# simple_object_merger
+
+This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without data association algorithm.
+
+## Algorithm
+
+### Background
+
+This package can merge multiple DetectedObjects without matching processing.
+[Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost.
+In addition, for now, [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node.
+To merge 6 DetectedObjects topics, 6 [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) nodes need to stand.
+
+So this package aim to merge DetectedObjects simply.
+This package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes.
+
+### Limitation
+
+- Sensor data drops and delay
+
+Merged objects will not be published until all topic data is received when initializing.
+In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout.
+When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects.
+For now specification of this package, if all topic data is received at first and after that the data drops, and the merged objects are published without objects which is judged as timeout.
+The timeout parameter should be determined by sensor cycle time.
+
+- Post-processing
+
+Because this package does not have matching processing, so it can be used only when post-processing is used.
+For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing.
+
+### Use case
+
+Use case is as below.
+
+- Multiple radar detection
+
+This package can be used for multiple radar detection.
+Since [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) will be included later process in radar faraway detection, this package can be used.
+
+## Input
+
+| Name | Type                                                               | Description                                            |
+| ---- | ------------------------------------------------------------------ | ------------------------------------------------------ |
+|      | std::vector<autoware_auto_perception_msgs/msg/DetectedObjects.msg> | 3D detected objects. Topic names are set by parameters |
+
+## Output
+
+| Name               | Type                                                  | Description    |
+| ------------------ | ----------------------------------------------------- | -------------- |
+| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Merged objects |
+
+## Parameters
+
+| Name                | Type         | Description                          | Default value |
+| :------------------ | :----------- | :----------------------------------- | :------------ |
+| `update_rate_hz`    | double       | Update rate. [hz]                    | 20.0          |
+| `new_frame_id`      | string       | The header frame_id of output topic. | "base_link"   |
+| `timeout_threshold` | double       | Threshold for timeout judgement [s]. | 1.0           |
+| `input_topics`      | List[string] | Input topics name.                   | "[]"          |

perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp

@@ -0,0 +1,81 @@
+// Copyright 2023 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 SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
+#define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace simple_object_merger
+{
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
+
+class SimpleObjectMergerNode : public rclcpp::Node
+{
+public:
+  explicit SimpleObjectMergerNode(const rclcpp::NodeOptions & node_options);
+
+  struct NodeParam
+  {
+    double update_rate_hz{};
+    double timeout_threshold{};
+    std::vector<std::string> topic_names{};
+    std::string new_frame_id{};
+  };
+
+private:
+  // Subscriber
+  rclcpp::Subscription<DetectedObjects>::SharedPtr sub_objects_{};
+  std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array{};
+  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
+
+  // Callback
+  void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number);
+
+  // Data Buffer
+  std::vector<DetectedObjects::ConstSharedPtr> objects_data_{};
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
+
+  // Publisher
+  rclcpp::Publisher<DetectedObjects>::SharedPtr pub_objects_{};
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_{};
+  void onTimer();
+  bool isDataReady();
+
+  // Parameter Server
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onSetParam(
+    const std::vector<rclcpp::Parameter> & params);
+
+  // Parameter
+  NodeParam node_param_{};
+
+  // Core
+  size_t input_topic_size;
+};
+
+}  // namespace simple_object_merger
+
+#endif  // SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_

perception/simple_object_merger/launch/simple_object_merger.launch.xml

@@ -0,0 +1,18 @@
+<launch>
+  <!-- Output -->
+  <arg name="output/objects" default="~/output/objects"/>
+  <!-- Parameter -->
+  <arg name="update_rate_hz" default="20.0"/>
+  <arg name="new_frame_id" default="base_link"/>
+  <arg name="timeout_threshold" default="1.0"/>
+  <arg name="input_topics" default="[]"/>
+
+  <!-- Node -->
+  <node pkg="simple_object_merger" exec="simple_object_merger_node" name="simple_object_merger" output="screen">
+    <remap from="~/output/objects" to="$(var output/objects)"/>
+    <param name="update_rate_hz" value="$(var update_rate_hz)"/>
+    <param name="timeout_threshold" value="$(var timeout_threshold)"/>
+    <param name="new_frame_id" value="$(var new_frame_id)"/>
+    <param name="input_topics" value="$(var input_topics)"/>
+  </node>
+</launch>

perception/simple_object_merger/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>simple_object_merger</name>
+  <version>0.1.0</version>
+  <description>simple_object_merger</description>
+  <maintainer email="[email protected]">Sathshi Tanaka</maintainer>
+  <maintainer email="[email protected]">Shunsuke Miura</maintainer>
+  <maintainer email="[email protected]">Yoshi Ri</maintainer>
+
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Sathshi Tanaka</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <build_depend>autoware_cmake</build_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>