Skip to content

Commit f6b14ec

Browse files
scepter914scepter914
authored
feat(simple_object_merger): add simple_object_merger (autowarefoundation#4219)
* feat(simple_object_merger): add simple_object_merger Signed-off-by: scepter914 <scepter914@gmail.com> * refactor Signed-off-by: scepter914 <scepter914@gmail.com> * add timeout parameter Signed-off-by: scepter914 <scepter914@gmail.com> --------- Signed-off-by: scepter914 <scepter914@gmail.com>
1 parent 4c47a5f commit f6b14ec

File tree

6 files changed

+413
-0
lines changed

6 files changed

+413
-0
lines changed

‎perception/simple_object_merger/CMakeLists.txt

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(simple_object_merger)
3+
4+
# Dependencies
5+
find_package(autoware_cmake REQUIRED)
6+
autoware_package()
7+
8+
# Targets
9+
ament_auto_add_library(simple_object_merger_node_component SHARED
10+
src/simple_object_merger_node/simple_object_merger_node.cpp
11+
)
12+
13+
rclcpp_components_register_node(simple_object_merger_node_component
14+
PLUGIN "simple_object_merger::SimpleObjectMergerNode"
15+
EXECUTABLE simple_object_merger_node
16+
)
17+
18+
# Tests
19+
if(BUILD_TESTING)
20+
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
21+
find_package(ament_lint_auto REQUIRED)
22+
ament_lint_auto_find_test_dependencies()
23+
endif()
24+
25+
# Package
26+
ament_auto_package(
27+
INSTALL_TO_SHARE
28+
launch
29+
)

‎perception/simple_object_merger/README.md

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
# simple_object_merger
2+
3+
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.
4+
5+
## Algorithm
6+
7+
### Background
8+
9+
This package can merge multiple DetectedObjects without matching processing.
10+
[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.
11+
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.
12+
To merge 6 DetectedObjects topics, 6 [object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) nodes need to stand.
13+
14+
So this package aim to merge DetectedObjects simply.
15+
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.
16+
17+
### Limitation
18+
19+
- Sensor data drops and delay
20+
21+
Merged objects will not be published until all topic data is received when initializing.
22+
In addition, to care sensor data drops and delayed, this package has a parameter to judge timeout.
23+
When the latest time of the data of a topic is older than the timeout parameter, it is not merged for output objects.
24+
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.
25+
The timeout parameter should be determined by sensor cycle time.
26+
27+
- Post-processing
28+
29+
Because this package does not have matching processing, so it can be used only when post-processing is used.
30+
For now, [clustering processing](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_object_clustering) can be used as post-processing.
31+
32+
### Use case
33+
34+
Use case is as below.
35+
36+
- Multiple radar detection
37+
38+
This package can be used for multiple radar detection.
39+
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.
40+
41+
## Input
42+
43+
| Name | Type | Description |
44+
| ---- | ------------------------------------------------------------------ | ------------------------------------------------------ |
45+
| | std::vector<autoware_auto_perception_msgs/msg/DetectedObjects.msg> | 3D detected objects. Topic names are set by parameters |
46+
47+
## Output
48+
49+
| Name | Type | Description |
50+
| ------------------ | ----------------------------------------------------- | -------------- |
51+
| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Merged objects |
52+
53+
## Parameters
54+
55+
| Name | Type | Description | Default value |
56+
| :------------------ | :----------- | :----------------------------------- | :------------ |
57+
| `update_rate_hz` | double | Update rate. [hz] | 20.0 |
58+
| `new_frame_id` | string | The header frame_id of output topic. | "base_link" |
59+
| `timeout_threshold` | double | Threshold for timeout judgement [s]. | 1.0 |
60+
| `input_topics` | List[string] | Input topics name. | "[]" |

‎perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
16+
#define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_
17+
18+
#include "rclcpp/rclcpp.hpp"
19+
#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
20+
21+
#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
22+
23+
#include <chrono>
24+
#include <memory>
25+
#include <string>
26+
#include <vector>
27+
28+
namespace simple_object_merger
29+
{
30+
using autoware_auto_perception_msgs::msg::DetectedObject;
31+
using autoware_auto_perception_msgs::msg::DetectedObjects;
32+
33+
class SimpleObjectMergerNode : public rclcpp::Node
34+
{
35+
public:
36+
explicit SimpleObjectMergerNode(const rclcpp::NodeOptions & node_options);
37+
38+
struct NodeParam
39+
{
40+
double update_rate_hz{};
41+
double timeout_threshold{};
42+
std::vector<std::string> topic_names{};
43+
std::string new_frame_id{};
44+
};
45+
46+
private:
47+
// Subscriber
48+
rclcpp::Subscription<DetectedObjects>::SharedPtr sub_objects_{};
49+
std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array{};
50+
std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
51+
52+
// Callback
53+
void onData(const DetectedObjects::ConstSharedPtr msg, size_t array_number);
54+
55+
// Data Buffer
56+
std::vector<DetectedObjects::ConstSharedPtr> objects_data_{};
57+
geometry_msgs::msg::TransformStamped::ConstSharedPtr transform_;
58+
59+
// Publisher
60+
rclcpp::Publisher<DetectedObjects>::SharedPtr pub_objects_{};
61+
62+
// Timer
63+
rclcpp::TimerBase::SharedPtr timer_{};
64+
void onTimer();
65+
bool isDataReady();
66+
67+
// Parameter Server
68+
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
69+
rcl_interfaces::msg::SetParametersResult onSetParam(
70+
const std::vector<rclcpp::Parameter> & params);
71+
72+
// Parameter
73+
NodeParam node_param_{};
74+
75+
// Core
76+
size_t input_topic_size;
77+
};
78+
79+
} // namespace simple_object_merger
80+
81+
#endif // SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_

‎perception/simple_object_merger/launch/simple_object_merger.launch.xml

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<launch>
2+
<!-- Output -->
3+
<arg name="output/objects" default="~/output/objects"/>
4+
<!-- Parameter -->
5+
<arg name="update_rate_hz" default="20.0"/>
6+
<arg name="new_frame_id" default="base_link"/>
7+
<arg name="timeout_threshold" default="1.0"/>
8+
<arg name="input_topics" default="[]"/>
9+
10+
<!-- Node -->
11+
<node pkg="simple_object_merger" exec="simple_object_merger_node" name="simple_object_merger" output="screen">
12+
<remap from="~/output/objects" to="$(var output/objects)"/>
13+
<param name="update_rate_hz" value="$(var update_rate_hz)"/>
14+
<param name="timeout_threshold" value="$(var timeout_threshold)"/>
15+
<param name="new_frame_id" value="$(var new_frame_id)"/>
16+
<param name="input_topics" value="$(var input_topics)"/>
17+
</node>
18+
</launch>

‎perception/simple_object_merger/package.xml

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<package format="3">
3+
<name>simple_object_merger</name>
4+
<version>0.1.0</version>
5+
<description>simple_object_merger</description>
6+
<maintainer email="satoshi.tanaka@tier4.jp">Sathshi Tanaka</maintainer>
7+
<maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
8+
<maintainer email="yoshi.ri@tier4.jp">Yoshi Ri</maintainer>
9+
10+
<license>Apache License 2.0</license>
11+
<author email="satoshi.tanaka@tier4.jp">Sathshi Tanaka</author>
12+
13+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
14+
15+
<depend>autoware_auto_perception_msgs</depend>
16+
<depend>geometry_msgs</depend>
17+
<depend>rclcpp</depend>
18+
<depend>rclcpp_components</depend>
19+
<depend>tier4_autoware_utils</depend>
20+
21+
<build_depend>autoware_cmake</build_depend>
22+
<test_depend>ament_lint_auto</test_depend>
23+
<test_depend>autoware_lint_common</test_depend>
24+
25+
<export>
26+
<build_type>ament_cmake</build_type>
27+
</export>
28+
</package>

0 commit comments

Comments
 (0)