Skip to content

Commit 1d53217

Browse files
Merge branch 'main' into test/image_projection_based_fusion/add_calc_iou_tests
2 parents 47fc998 + a792d4b commit 1d53217

File tree

12 files changed

+113
-35
lines changed

12 files changed

+113
-35
lines changed

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,10 @@ class EuclideanCluster : public EuclideanClusterInterface
3232
bool cluster(
3333
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
3434
std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
35+
36+
bool cluster(
37+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud,
38+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override;
3539
void setTolerance(float tolerance) { tolerance_ = tolerance; }
3640

3741
private:

perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp

+7
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,9 @@
1616

1717
#include <rclcpp/rclcpp.hpp>
1818

19+
#include <sensor_msgs/msg/point_cloud2.hpp>
20+
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
21+
1922
#include <pcl/point_cloud.h>
2023
#include <pcl/point_types.h>
2124

@@ -41,6 +44,10 @@ class EuclideanClusterInterface
4144
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
4245
std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) = 0;
4346

47+
virtual bool cluster(
48+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_msg,
49+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & output_clusters) = 0;
50+
4451
protected:
4552
bool use_height_ = true;
4653
int min_cluster_size_;

perception/euclidean_cluster/include/euclidean_cluster/utils.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,9 @@ void convertPointCloudClusters2Msg(
3131
const std_msgs::msg::Header & header,
3232
const std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters,
3333
tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
34+
void convertPointCloudClusters2Msg(
35+
const std_msgs::msg::Header & header, const std::vector<sensor_msgs::msg::PointCloud2> & clusters,
36+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
3437
void convertObjectMsg2SensorMsg(
3538
const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input,
3639
sensor_msgs::msg::PointCloud2 & output);

perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,9 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
3535
bool cluster(
3636
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
3737
std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters) override;
38+
bool cluster(
39+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud,
40+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override;
3841
void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; }
3942
void setTolerance(float tolerance) { tolerance_ = tolerance; }
4043
void setMinPointsNumberPerVoxel(int min_points_number_per_voxel)

perception/euclidean_cluster/lib/euclidean_cluster.cpp

+9
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,15 @@ EuclideanCluster::EuclideanCluster(
3333
: EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance)
3434
{
3535
}
36+
// TODO(badai-nguyen): implement input field copy for euclidean_cluster.cpp
37+
bool EuclideanCluster::cluster(
38+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
39+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters)
40+
{
41+
(void)pointcloud_msg;
42+
(void)clusters;
43+
return false;
44+
}
3645

3746
bool EuclideanCluster::cluster(
3847
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,

perception/euclidean_cluster/lib/utils.cpp

+19
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,25 @@ void convertPointCloudClusters2Msg(
6262
msg.feature_objects.push_back(feature_object);
6363
}
6464
}
65+
66+
void convertPointCloudClusters2Msg(
67+
const std_msgs::msg::Header & header, const std::vector<sensor_msgs::msg::PointCloud2> & clusters,
68+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg)
69+
{
70+
msg.header = header;
71+
for (const auto & ros_pointcloud : clusters) {
72+
tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
73+
feature_object.feature.cluster = ros_pointcloud;
74+
feature_object.object.kinematics.pose_with_covariance.pose.position =
75+
getCentroid(ros_pointcloud);
76+
autoware_auto_perception_msgs::msg::ObjectClassification classification;
77+
classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
78+
classification.probability = 1.0f;
79+
feature_object.object.classification.emplace_back(classification);
80+
msg.feature_objects.push_back(feature_object);
81+
}
82+
}
83+
6584
void convertObjectMsg2SensorMsg(
6685
const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input,
6786
sensor_msgs::msg::PointCloud2 & output)

perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp

+58-12
Original file line numberDiff line numberDiff line change
@@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
4040
min_points_number_per_voxel_(min_points_number_per_voxel)
4141
{
4242
}
43-
43+
// TODO(badai-nguyen): remove this function when field copying also implemented for
44+
// euclidean_cluster.cpp
4445
bool VoxelGridBasedEuclideanCluster::cluster(
4546
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & pointcloud,
4647
std::vector<pcl::PointCloud<pcl::PointXYZ>> & clusters)
48+
{
49+
(void)pointcloud;
50+
(void)clusters;
51+
return false;
52+
}
53+
54+
bool VoxelGridBasedEuclideanCluster::cluster(
55+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg,
56+
tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects)
4757
{
4858
// TODO(Saito) implement use_height is false version
4959

5060
// create voxel
61+
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
62+
int point_step = pointcloud_msg->point_step;
63+
pcl::fromROSMsg(*pointcloud_msg, *pointcloud);
5164
pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
5265
voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0);
5366
voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_);
@@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster(
8194

8295
// create map to search cluster index from voxel grid index
8396
std::unordered_map</* voxel grid index */ int, /* cluster index */ int> map;
97+
std::vector<sensor_msgs::msg::PointCloud2> temporary_clusters; // no check about cluster size
98+
std::vector<size_t> clusters_data_size;
99+
temporary_clusters.resize(cluster_indices.size());
84100
for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) {
85101
const auto & cluster = cluster_indices.at(cluster_idx);
102+
auto & temporary_cluster = temporary_clusters.at(cluster_idx);
86103
for (const auto & point_idx : cluster.indices) {
87104
map[point_idx] = cluster_idx;
88105
}
106+
temporary_cluster.height = pointcloud_msg->height;
107+
temporary_cluster.fields = pointcloud_msg->fields;
108+
temporary_cluster.point_step = point_step;
109+
temporary_cluster.data.resize(max_cluster_size_ * point_step);
110+
clusters_data_size.push_back(0);
89111
}
90112

91113
// create vector of point cloud cluster. vector index is voxel grid index.
92-
std::vector<pcl::PointCloud<pcl::PointXYZ>> temporary_clusters; // no check about cluster size
93-
temporary_clusters.resize(cluster_indices.size());
94-
for (const auto & point : pointcloud->points) {
114+
for (size_t i = 0; i < pointcloud->points.size(); ++i) {
115+
const auto & point = pointcloud->points.at(i);
95116
const int index =
96117
voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
97118
if (map.find(index) != map.end()) {
98-
temporary_clusters.at(map[index]).points.push_back(point);
119+
auto & cluster_data_size = clusters_data_size.at(map[index]);
120+
if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) {
121+
continue;
122+
}
123+
std::memcpy(
124+
&temporary_clusters.at(map[index]).data[cluster_data_size],
125+
&pointcloud_msg->data[i * point_step], point_step);
126+
cluster_data_size += point_step;
99127
}
100128
}
101129

102130
// build output and check cluster size
103131
{
104-
for (const auto & cluster : temporary_clusters) {
105-
if (!(min_cluster_size_ <= static_cast<int>(cluster.points.size()) &&
106-
static_cast<int>(cluster.points.size()) <= max_cluster_size_)) {
132+
for (size_t i = 0; i < temporary_clusters.size(); ++i) {
133+
auto & i_cluster_data_size = clusters_data_size.at(i);
134+
if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
135+
static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
107136
continue;
108137
}
109-
clusters.push_back(cluster);
110-
clusters.back().width = cluster.points.size();
111-
clusters.back().height = 1;
112-
clusters.back().is_dense = false;
138+
const auto & cluster = temporary_clusters.at(i);
139+
tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
140+
feature_object.feature.cluster = cluster;
141+
feature_object.feature.cluster.data.resize(i_cluster_data_size);
142+
feature_object.feature.cluster.header = pointcloud_msg->header;
143+
feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian;
144+
feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense;
145+
feature_object.feature.cluster.point_step = point_step;
146+
feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height;
147+
feature_object.feature.cluster.width =
148+
i_cluster_data_size / point_step / pointcloud_msg->height;
149+
150+
feature_object.object.kinematics.pose_with_covariance.pose.position =
151+
getCentroid(feature_object.feature.cluster);
152+
autoware_auto_perception_msgs::msg::ObjectClassification classification;
153+
classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
154+
classification.probability = 1.0f;
155+
feature_object.object.classification.emplace_back(classification);
156+
157+
objects.feature_objects.push_back(feature_object);
113158
}
159+
objects.header = pointcloud_msg->header;
114160
}
115161

116162
return true;

perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

+3-12
Original file line numberDiff line numberDiff line change
@@ -55,24 +55,15 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud(
5555
stop_watch_ptr_->toc("processing_time", true);
5656

5757
// convert ros to pcl
58-
pcl::PointCloud<pcl::PointXYZ>::Ptr raw_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
5958
if (input_msg->data.empty()) {
6059
// NOTE: prevent pcl log spam
6160
RCLCPP_WARN_STREAM_THROTTLE(
6261
this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
63-
} else {
64-
pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr);
6562
}
66-
67-
// clustering
68-
std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;
69-
if (!raw_pointcloud_ptr->empty()) {
70-
cluster_->cluster(raw_pointcloud_ptr, clusters);
71-
}
72-
73-
// build output msg
63+
// cluster and build output msg
7464
tier4_perception_msgs::msg::DetectedObjectsWithFeature output;
75-
convertPointCloudClusters2Msg(input_msg->header, clusters, output);
65+
66+
cluster_->cluster(input_msg, output);
7667
cluster_pub_->publish(output);
7768

7869
// build debug msg

planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

+2-7
Original file line numberDiff line numberDiff line change
@@ -87,12 +87,8 @@ boost::optional<LineString2d> NoStoppingAreaModule::getStopLineGeometry2d(
8787
const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}};
8888
std::vector<Point2d> collision_points;
8989
bg::intersection(area_poly, line, collision_points);
90-
if (collision_points.empty()) {
91-
continue;
92-
}
93-
const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1);
9490
if (!collision_points.empty()) {
95-
geometry_msgs::msg::Point left_point;
91+
const double yaw = tier4_autoware_utils::calcAzimuthAngle(p0, p1);
9692
const double w = planner_data_->vehicle_info_.vehicle_width_m;
9793
const double l = stop_line_margin;
9894
stop_line.emplace_back(
@@ -306,7 +302,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
306302

307303
const int num_ignore_nearest = 1; // Do not consider nearest lane polygon
308304
size_t ego_area_start_idx = closest_idx + num_ignore_nearest;
309-
size_t ego_area_end_idx = ego_area_start_idx;
310305
// return if area size is not intentional
311306
if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) {
312307
return ego_area;
@@ -330,7 +325,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
330325
}
331326
double dist_from_area_sum = 0.0;
332327
// decide end idx with extract distance
333-
ego_area_end_idx = ego_area_start_idx;
328+
size_t ego_area_end_idx = ego_area_start_idx;
334329
for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) {
335330
dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1));
336331
const auto & p = pp.at(i).point.pose.position;

planning/behavior_velocity_planner/src/node.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -311,6 +311,7 @@ void BehaviorVelocityPlannerNode::onParam()
311311
// lock(mutex_);
312312
planner_data_.velocity_smoother_ =
313313
std::make_unique<motion_velocity_smoother::AnalyticalJerkConstrainedSmoother>(*this);
314+
planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m);
314315
}
315316

316317
void BehaviorVelocityPlannerNode::onLaneletMap(

planning/rtc_interface/include/rtc_interface/rtc_interface.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@ class RTCInterface
8484
rclcpp::Service<AutoMode>::SharedPtr srv_auto_mode_;
8585
rclcpp::CallbackGroup::SharedPtr callback_group_;
8686
rclcpp::TimerBase::SharedPtr timer_;
87+
rclcpp::Clock::SharedPtr clock_;
8788
rclcpp::Logger logger_;
8889

8990
Module module_;

planning/rtc_interface/src/rtc_interface.cpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,8 @@ Module getModuleType(const std::string & module_name)
8181
namespace rtc_interface
8282
{
8383
RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc)
84-
: logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
84+
: clock_{node->get_clock()},
85+
logger_{node->get_logger().get_child("RTCInterface[" + name + "]")},
8586
is_auto_mode_enabled_{!enable_rtc},
8687
is_locked_{false}
8788
{
@@ -270,9 +271,7 @@ void RTCInterface::removeExpiredCooperateStatus()
270271
std::lock_guard<std::mutex> lock(mutex_);
271272
const auto itr = std::remove_if(
272273
registered_status_.statuses.begin(), registered_status_.statuses.end(),
273-
[](const auto & status) {
274-
return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0;
275-
});
274+
[this](const auto & status) { return (clock_->now() - status.stamp).seconds() > 10.0; });
276275

277276
registered_status_.statuses.erase(itr, registered_status_.statuses.end());
278277
}

0 commit comments

Comments
 (0)