Skip to content

Commit 339b4bd

Browse files
tier4-autoware-public-bot[bot]tier4-autoware-public-bot[bot]
authored
Merge pull request #842 from tier4/beta-to-tier4-main-sync
chore: sync beta branch beta/v0.12.0 with tier4/main
2 parents 5b22bd3 + 18427ca commit 339b4bd

File tree

40 files changed

+1656
-293
lines changed

40 files changed

+1656
-293
lines changed

‎common/motion_utils/src/resample/resample.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -109,13 +109,16 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
109109
}
110110

111111
std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
112-
const std::vector<geometry_msgs::msg::Pose> & points,
112+
const std::vector<geometry_msgs::msg::Pose> & points_raw,
113113
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
114114
const bool use_lerp_for_z)
115115
{
116+
// Remove overlap points for resampling
117+
const auto points = motion_utils::removeOverlapPoints(points_raw);
118+
116119
// validate arguments
117120
if (!resample_utils::validate_arguments(points, resampled_arclength)) {
118-
return points;
121+
return points_raw;
119122
}
120123

121124
std::vector<geometry_msgs::msg::Point> position(points.size());

‎common/object_recognition_utils/include/object_recognition_utils/transform.hpp

+64
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,22 @@
1515
#ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
1616
#define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
1717

18+
#include <pcl_ros/transforms.hpp>
19+
1820
#include <geometry_msgs/msg/transform.hpp>
21+
#include <sensor_msgs/msg/point_cloud2.hpp>
22+
#include <sensor_msgs/msg/point_field.hpp>
1923

2024
#include <boost/optional.hpp>
2125

2226
#include <tf2_ros/buffer.h>
2327
#include <tf2_ros/transform_listener.h>
2428
#ifdef ROS_DISTRO_GALACTIC
29+
#include <tf2_eigen/tf2_eigen.h>
2530
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
2631
#else
32+
#include <tf2_eigen/tf2_eigen.hpp>
33+
2734
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
2835
#endif
2936

@@ -45,6 +52,23 @@ namespace detail
4552
return boost::none;
4653
}
4754
}
55+
56+
[[maybe_unused]] inline boost::optional<Eigen::Matrix4f> getTransformMatrix(
57+
const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header,
58+
const tf2_ros::Buffer & tf_buffer)
59+
{
60+
try {
61+
geometry_msgs::msg::TransformStamped transform_stamped;
62+
transform_stamped = tf_buffer.lookupTransform(
63+
in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp,
64+
rclcpp::Duration::from_seconds(1.0));
65+
Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
66+
return mat;
67+
} catch (tf2::TransformException & e) {
68+
RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what());
69+
return boost::none;
70+
}
71+
}
4872
} // namespace detail
4973

5074
namespace object_recognition_utils
@@ -79,6 +103,46 @@ bool transformObjects(
79103
}
80104
return true;
81105
}
106+
template <class T>
107+
bool transformObjectsWithFeature(
108+
const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
109+
T & output_msg)
110+
{
111+
output_msg = input_msg;
112+
if (input_msg.header.frame_id != target_frame_id) {
113+
output_msg.header.frame_id = target_frame_id;
114+
tf2::Transform tf_target2objects_world;
115+
tf2::Transform tf_target2objects;
116+
tf2::Transform tf_objects_world2objects;
117+
const auto ros_target2objects_world = detail::getTransform(
118+
tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
119+
if (!ros_target2objects_world) {
120+
return false;
121+
}
122+
const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer);
123+
if (!tf_matrix) {
124+
RCLCPP_WARN(
125+
rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix");
126+
return false;
127+
}
128+
for (auto & feature_object : output_msg.feature_objects) {
129+
// transform object
130+
tf2::fromMsg(
131+
feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
132+
tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
133+
tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose);
134+
135+
// transform cluster
136+
sensor_msgs::msg::PointCloud2 transformed_cluster;
137+
pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster);
138+
transformed_cluster.header.frame_id = target_frame_id;
139+
feature_object.feature.cluster = transformed_cluster;
140+
}
141+
output_msg.header.frame_id = target_frame_id;
142+
return true;
143+
}
144+
return true;
145+
}
82146
} // namespace object_recognition_utils
83147

84148
#endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_

‎common/object_recognition_utils/package.xml

+6
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,13 @@
1717
<depend>geometry_msgs</depend>
1818
<depend>interpolation</depend>
1919
<depend>libboost-dev</depend>
20+
<depend>pcl_conversions</depend>
21+
<depend>pcl_ros</depend>
2022
<depend>rclcpp</depend>
23+
<depend>sensor_msgs</depend>
24+
<depend>std_msgs</depend>
25+
<depend>tf2</depend>
26+
<depend>tf2_eigen</depend>
2127
<depend>tier4_autoware_utils</depend>
2228

2329
<test_depend>ament_cmake_ros</test_depend>

‎launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

+45-1
Original file line numberDiff line numberDiff line change
@@ -79,12 +79,54 @@
7979
<let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
8080
<include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
8181
<arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
82-
<arg name="output_clusters" value="clusters"/>
82+
<arg name="output_clusters" value="euclidean_cluster/clusters"/>
8383
<arg name="use_pointcloud_map" value="false"/>
8484
<arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
8585
</include>
8686
</group>
8787

88+
<!-- roi based clustering -->
89+
<group>
90+
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
91+
<arg name="input/camera_info0" value="$(var camera_info0)"/>
92+
<arg name="input/rois0" value="$(var detection_rois0)"/>
93+
<arg name="input/camera_info1" value="$(var camera_info1)"/>
94+
<arg name="input/rois1" value="$(var detection_rois1)"/>
95+
<arg name="input/camera_info2" value="$(var camera_info2)"/>
96+
<arg name="input/rois2" value="$(var detection_rois2)"/>
97+
<arg name="input/camera_info3" value="$(var camera_info3)"/>
98+
<arg name="input/rois3" value="$(var detection_rois3)"/>
99+
<arg name="input/camera_info4" value="$(var camera_info4)"/>
100+
<arg name="input/rois4" value="$(var detection_rois4)"/>
101+
<arg name="input/camera_info5" value="$(var camera_info5)"/>
102+
<arg name="input/rois5" value="$(var detection_rois5)"/>
103+
<arg name="input/camera_info6" value="$(var camera_info6)"/>
104+
<arg name="input/rois6" value="$(var detection_rois6)"/>
105+
<arg name="input/camera_info7" value="$(var camera_info7)"/>
106+
<arg name="input/rois7" value="$(var detection_rois7)"/>
107+
<arg name="input/rois_number" value="$(var image_number)"/>
108+
<arg name="input/image0" value="$(var image_raw0)"/>
109+
<arg name="input/image1" value="$(var image_raw1)"/>
110+
<arg name="input/image2" value="$(var image_raw2)"/>
111+
<arg name="input/image3" value="$(var image_raw3)"/>
112+
<arg name="input/image4" value="$(var image_raw4)"/>
113+
<arg name="input/image5" value="$(var image_raw5)"/>
114+
<arg name="input/image6" value="$(var image_raw6)"/>
115+
<arg name="input/image7" value="$(var image_raw7)"/>
116+
<arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
117+
<arg name="output_clusters" value="roi_cluster/clusters"/>
118+
</include>
119+
</group>
120+
121+
<!-- simple_cluster_merger -->
122+
<group>
123+
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
124+
<arg name="input/cluster0" value="euclidean_cluster/clusters"/>
125+
<arg name="input/cluster1" value="roi_cluster/clusters"/>
126+
<arg name="output/clusters" value="clusters"/>
127+
</include>
128+
</group>
129+
88130
<group>
89131
<include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
90132
<arg name="input/objects" value="clusters"/>
@@ -96,6 +138,8 @@
96138
<let name="input/clustering" value="/perception/object_recognition/detection/clustering/clusters"/>
97139
<push-ros-namespace namespace="camera_lidar_fusion"/>
98140
<!-- Fusion camera-lidar to classify -->
141+
142+
<!-- euclidean clustering -->
99143
<group>
100144
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_cluster_fusion.launch.xml">
101145
<arg name="input/camera_info0" value="$(var camera_info0)"/>

‎launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

+43-1
Original file line numberDiff line numberDiff line change
@@ -143,12 +143,54 @@
143143
<arg name="input/image5" value="$(var image_raw5)"/>
144144
<arg name="input/image6" value="$(var image_raw6)"/>
145145
<arg name="input/image7" value="$(var image_raw7)"/>
146-
<arg name="output/clusters" value="clusters"/>
146+
<arg name="output/clusters" value="euclidean_cluster/clusters"/>
147147
<arg name="remove_unknown" value="$(var remove_unknown)"/>
148148
<arg name="trust_distance" value="$(var trust_distance)"/>
149149
</include>
150150
</group>
151151

152+
<!-- roi based clustering -->
153+
<group>
154+
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
155+
<arg name="input/camera_info0" value="$(var camera_info0)"/>
156+
<arg name="input/rois0" value="$(var detection_rois0)"/>
157+
<arg name="input/camera_info1" value="$(var camera_info1)"/>
158+
<arg name="input/rois1" value="$(var detection_rois1)"/>
159+
<arg name="input/camera_info2" value="$(var camera_info2)"/>
160+
<arg name="input/rois2" value="$(var detection_rois2)"/>
161+
<arg name="input/camera_info3" value="$(var camera_info3)"/>
162+
<arg name="input/rois3" value="$(var detection_rois3)"/>
163+
<arg name="input/camera_info4" value="$(var camera_info4)"/>
164+
<arg name="input/rois4" value="$(var detection_rois4)"/>
165+
<arg name="input/camera_info5" value="$(var camera_info5)"/>
166+
<arg name="input/rois5" value="$(var detection_rois5)"/>
167+
<arg name="input/camera_info6" value="$(var camera_info6)"/>
168+
<arg name="input/rois6" value="$(var detection_rois6)"/>
169+
<arg name="input/camera_info7" value="$(var camera_info7)"/>
170+
<arg name="input/rois7" value="$(var detection_rois7)"/>
171+
<arg name="input/rois_number" value="$(var image_number)"/>
172+
<arg name="input/image0" value="$(var image_raw0)"/>
173+
<arg name="input/image1" value="$(var image_raw1)"/>
174+
<arg name="input/image2" value="$(var image_raw2)"/>
175+
<arg name="input/image3" value="$(var image_raw3)"/>
176+
<arg name="input/image4" value="$(var image_raw4)"/>
177+
<arg name="input/image5" value="$(var image_raw5)"/>
178+
<arg name="input/image6" value="$(var image_raw6)"/>
179+
<arg name="input/image7" value="$(var image_raw7)"/>
180+
<arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
181+
<arg name="output_clusters" value="roi_cluster/clusters"/>
182+
</include>
183+
</group>
184+
185+
<!-- simple_cluster_merger -->
186+
<group>
187+
<include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
188+
<arg name="input/cluster0" value="euclidean_cluster/clusters"/>
189+
<arg name="input/cluster1" value="roi_cluster/clusters"/>
190+
<arg name="output/clusters" value="clusters"/>
191+
</include>
192+
</group>
193+
152194
<group>
153195
<include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
154196
<arg name="input/objects" value="clusters"/>

‎launch/tier4_perception_launch/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
<buildtool_depend>ament_cmake_auto</buildtool_depend>
1313
<buildtool_depend>autoware_cmake</buildtool_depend>
1414

15+
<exec_depend>cluster_merger</exec_depend>
1516
<exec_depend>compare_map_segmentation</exec_depend>
1617
<exec_depend>crosswalk_traffic_light_estimator</exec_depend>
1718
<exec_depend>detected_object_feature_remover</exec_depend>

‎perception/cluster_merger/CMakeLists.txt

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(cluster_merger)
3+
4+
# find dependencies
5+
find_package(autoware_cmake REQUIRED)
6+
autoware_package()
7+
8+
# Targets
9+
ament_auto_add_library(cluster_merger_node_component SHARED
10+
src/cluster_merger/node.cpp
11+
)
12+
13+
rclcpp_components_register_node(cluster_merger_node_component
14+
PLUGIN "cluster_merger::ClusterMergerNode"
15+
EXECUTABLE cluster_merger_node)
16+
17+
18+
if(BUILD_TESTING)
19+
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
20+
find_package(ament_lint_auto REQUIRED)
21+
ament_lint_auto_find_test_dependencies()
22+
endif()
23+
24+
ament_auto_package(
25+
INSTALL_TO_SHARE
26+
launch
27+
)

‎perception/cluster_merger/README.md

+72
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
# cluster merger
2+
3+
## Purpose
4+
5+
cluster_merger is a package for merging pointcloud clusters as detected objects with feature type.
6+
7+
## Inner-working / Algorithms
8+
9+
The clusters of merged topics are simply concatenated from clusters of input topics.
10+
11+
## Input / Output
12+
13+
### Input
14+
15+
| Name | Type | Description |
16+
| ---------------- | -------------------------------------------------------- | ------------------- |
17+
| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters |
18+
| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters |
19+
20+
### Output
21+
22+
| Name | Type | Description |
23+
| ----------------- | ----------------------------------------------------- | --------------- |
24+
| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters |
25+
26+
## Parameters
27+
28+
| Name | Type | Description | Default value |
29+
| :---------------- | :----- | :----------------------------------- | :------------ |
30+
| `output_frame_id` | string | The header frame_id of output topic. | base_link |
31+
32+
## Assumptions / Known limits
33+
34+
<!-- Write assumptions and limitations of your implementation.
35+
36+
Example:
37+
This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
38+
Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
39+
-->
40+
41+
## (Optional) Error detection and handling
42+
43+
<!-- Write how to detect errors and how to recover from them.
44+
45+
Example:
46+
This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
47+
-->
48+
49+
## (Optional) Performance characterization
50+
51+
<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
52+
53+
Example:
54+
### Complexity
55+
56+
This algorithm is O(N).
57+
58+
### Processing time
59+
60+
...
61+
-->
62+
63+
## (Optional) References/External links
64+
65+
<!-- Write links you referred to when you implemented.
66+
67+
Example:
68+
[1] {link_to_a_thesis}
69+
[2] {link_to_an_issue}
70+
-->
71+
72+
## (Optional) Future extensions / Unimplemented parts

0 commit comments

Comments
 (0)