fix(euclidean_cluster, ground_segmentation): cherry-pick bug fix PRs (#…

…1378)

* fix(euclidean_cluster): fix euclidean cluster params (autowarefoundation#7662)

* fix(euclidean_cluster): fix max and min cluster size

Signed-off-by: beginningfan <beginning.fan@autocore.ai>

* fix(gnss_poser): fix a typo

Signed-off-by: beginningfan <beginning.fan@autocore.ai>

* fix(euclidean_cluster): fix min cluster size

Signed-off-by: beginningfan <beginning.fan@autocore.ai>

* style(pre-commit): autofix

---------

Signed-off-by: beginningfan <beginning.fan@autocore.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* fix(euclidean_cluster): fix max_cluster_size bug (autowarefoundation#7734)

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix(ground_segmentation): fix bug  (autowarefoundation#7771)

---------

Signed-off-by: beginningfan <beginning.fan@autocore.ai>
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Co-authored-by: beginningfan <103237402+beginningfan@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>

launch/tier4_perception_launch/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>detected_object_feature_remover</exec_depend>
   <exec_depend>detected_object_validation</exec_depend>
   <exec_depend>detection_by_tracker</exec_depend>
+  <exec_depend>elevation_map_loader</exec_depend>
   <exec_depend>euclidean_cluster</exec_depend>
   <exec_depend>ground_segmentation</exec_depend>
   <exec_depend>image_projection_based_fusion</exec_depend>

perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp

@@ -106,7 +106,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
     temporary_cluster.height = pointcloud_msg->height;
     temporary_cluster.fields = pointcloud_msg->fields;
     temporary_cluster.point_step = point_step;
-    temporary_cluster.data.resize(max_cluster_size_ * point_step);
+    temporary_cluster.data.resize(cluster.indices.size() * point_step);
     clusters_data_size.push_back(0);
   }
 
@@ -117,13 +117,17 @@ bool VoxelGridBasedEuclideanCluster::cluster(
       voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
     if (map.find(index) != map.end()) {
       auto & cluster_data_size = clusters_data_size.at(map[index]);
-      if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) {
+      if (cluster_data_size > std::size_t(max_cluster_size_ * point_step)) {
         continue;
       }
       std::memcpy(
         &temporary_clusters.at(map[index]).data[cluster_data_size],
         &pointcloud_msg->data[i * point_step], point_step);
       cluster_data_size += point_step;
+      if (cluster_data_size == temporary_clusters.at(map[index]).data.size()) {
+        temporary_clusters.at(map[index])
+          .data.resize(temporary_clusters.at(map[index]).data.size() * 2);
+      }
     }
   }
 

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp

@@ -292,12 +292,13 @@ void RANSACGroundFilterComponent::filter(
   no_ground_cloud_msg_ptr->header = input->header;
   no_ground_cloud_msg_ptr->fields = input->fields;
   no_ground_cloud_msg_ptr->point_step = point_step;
+  no_ground_cloud_msg_ptr->data.resize(input->data.size());
   size_t output_size = 0;
   // use not downsampled pointcloud for extract pointcloud that higher than height threshold
   for (size_t global_size = 0; global_size < input->data.size(); global_size += point_step) {
-    float x = *reinterpret_cast<float *>(input->data[global_size + x_offset]);
-    float y = *reinterpret_cast<float *>(input->data[global_size + y_offset]);
-    float z = *reinterpret_cast<float *>(input->data[global_size + z_offset]);
+    float x = *reinterpret_cast<const float *>(&input->data[global_size + x_offset]);
+    float y = *reinterpret_cast<const float *>(&input->data[global_size + y_offset]);
+    float z = *reinterpret_cast<const float *>(&input->data[global_size + z_offset]);
     const Eigen::Vector3d transformed_point = plane_affine.inverse() * Eigen::Vector3d(x, y, z);
     if (std::abs(transformed_point.z()) > height_threshold_) {
       std::memcpy(