Merge pull request #2046 from tier4/chore/cherry-pick-10344

fix(autoware_probabilistic_occupancy_grid_map): fixed transform issues and border conditions (autowarefoundation#10344)

Author: satoshi-ota

perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp

@@ -16,7 +16,7 @@
 #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_
 
 #ifdef USE_CUDA
-#include "autoware/cuda_utils/cuda_unique_ptr.hpp"
+#include <autoware/cuda_utils/cuda_unique_ptr.hpp>
 #endif
 
 #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp"

perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp

@@ -20,6 +20,8 @@
 #ifdef USE_CUDA
 #include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp"
 #include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp"
+
+#include <autoware/cuda_utils/cuda_unique_ptr.hpp>
 #endif
 
 #include <builtin_interfaces/msg/time.hpp>
@@ -61,7 +63,9 @@ bool transformPointcloud(
 
 #ifdef USE_CUDA
 bool transformPointcloudAsync(
-  CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame);
+  CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame,
+  autoware::cuda_utils::CudaUniquePtr<Eigen::Matrix3f> & device_rotation,
+  autoware::cuda_utils::CudaUniquePtr<Eigen::Vector3f> & device_translation);
 #endif
 
 Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose);

perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp

@@ -44,7 +44,7 @@ void copyMapRegionLaunch(
 
 void transformPointCloudLaunch(
   std::uint8_t * points, std::size_t num_points, std::size_t points_step,
-  const Eigen::Matrix3f & rotation, const Eigen::Vector3f & translation, cudaStream_t stream);
+  const Eigen::Matrix3f * rotation, const Eigen::Vector3f * translation, cudaStream_t stream);
 
 }  // namespace utils
 }  // namespace autoware::occupancy_grid_map

perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu

@@ -133,6 +133,10 @@ __global__ void fillUnknownSpaceKernel(
   int angle_bin_index = idx / range_bins;
   int range_bin_index = idx % range_bins;
 
+  if (range_bin_index == range_bins - 1) {
+    return;
+  }
+
   std::uint64_t obs_range_and_x =
     obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 0];
   std::uint64_t obs_range_and_y =
@@ -149,7 +153,7 @@ __global__ void fillUnknownSpaceKernel(
   int next_raw_range_bin_index = range_bin_index + 1;
   int next_obs_range_bin_index = range_bin_index + 1;
 
-  for (; next_raw_range_bin_index < range_bins; next_raw_range_bin_index++) {
+  for (; next_raw_range_bin_index < range_bins - 1; next_raw_range_bin_index++) {
     std::uint64_t next_raw_range_and_x =
       raw_points_tensor[2 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 0];
     std::uint32_t next_raw_range_int = next_raw_range_and_x >> 32;
@@ -160,7 +164,7 @@ __global__ void fillUnknownSpaceKernel(
     }
   }
 
-  for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) {
+  for (; next_obs_range_bin_index < range_bins - 1; next_obs_range_bin_index++) {
     std::uint64_t next_obs_range_and_x =
       obstacle_points_tensor[2 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0];
     std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32;
@@ -289,7 +293,7 @@ __global__ void fillObstaclesKernel(
 
   // Look for the next obstacle point
   int next_obs_range_bin_index = range_bin_index + 1;
-  for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) {
+  for (; next_obs_range_bin_index < range_bins - 1; next_obs_range_bin_index++) {
     std::uint64_t next_obs_range_and_x =
       obstacle_points_tensor[2 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0];
     std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32;
@@ -299,6 +303,8 @@ __global__ void fillObstaclesKernel(
     }
   }
 
+  next_obs_range_bin_index = std::min<int>(next_obs_range_bin_index, range_bins - 1);
+
   std::uint64_t next_obs_range_and_x =
     obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 0];
   std::uint64_t next_obs_range_and_y =

perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective_kernel.cu

@@ -255,6 +255,10 @@ __global__ void fillUnknownSpaceKernel(
   int angle_bin_index = idx / range_bins;
   int range_bin_index = idx % range_bins;
 
+  if (range_bin_index == range_bins - 1) {
+    return;
+  }
+
   std::uint64_t obs_range_and_x =
     obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 0];
   std::uint64_t obs_range_and_y =
@@ -277,7 +281,7 @@ __global__ void fillUnknownSpaceKernel(
   int next_raw_range_bin_index = range_bin_index + 1;
   int next_obs_range_bin_index = range_bin_index + 1;
 
-  for (; next_raw_range_bin_index < range_bins; next_raw_range_bin_index++) {
+  for (; next_raw_range_bin_index < range_bins - 1; next_raw_range_bin_index++) {
     std::uint64_t next_raw_range_and_x =
       raw_points_tensor[6 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 0];
     std::uint32_t next_raw_range_int = next_raw_range_and_x >> 32;
@@ -292,7 +296,7 @@ __global__ void fillUnknownSpaceKernel(
     }
   }
 
-  for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) {
+  for (; next_obs_range_bin_index < range_bins - 1; next_obs_range_bin_index++) {
     std::uint64_t next_obs_range_and_x =
       obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0];
     std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32;
@@ -414,7 +418,7 @@ __global__ void fillObstaclesKernel(
 
   // Look for the next obstacle point
   int next_obs_range_bin_index = range_bin_index + 1;
-  for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) {
+  for (; next_obs_range_bin_index < range_bins - 1; next_obs_range_bin_index++) {
     std::uint64_t next_obs_range_and_x =
       obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0];
     std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32;
@@ -424,6 +428,8 @@ __global__ void fillObstaclesKernel(
     }
   }
 
+  next_obs_range_bin_index = std::min<int>(next_obs_range_bin_index, range_bins - 1);
+
   std::uint64_t next_obs_range_and_x =
     obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 0];
   std::uint64_t next_obs_range_and_y =

perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp

@@ -14,6 +14,10 @@
 
 #include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp"
 
+#ifdef USE_CUDA
+#include <autoware/cuda_utils/cuda_check_error.hpp>
+#endif
+
 #include <autoware_utils/geometry/geometry.hpp>
 
 #include <string>
@@ -49,7 +53,9 @@ bool transformPointcloud(
 
 #ifdef USE_CUDA
 bool transformPointcloudAsync(
-  CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame)
+  CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame,
+  autoware::cuda_utils::CudaUniquePtr<Eigen::Matrix3f> & device_rotation,
+  autoware::cuda_utils::CudaUniquePtr<Eigen::Vector3f> & device_translation)
 {
   geometry_msgs::msg::TransformStamped tf_stamped;
   // lookup transform
@@ -66,9 +72,18 @@ bool transformPointcloudAsync(
   Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast<float>();
   Eigen::Matrix3f rotation = tf_matrix.block<3, 3>(0, 0);
   Eigen::Vector3f translation = tf_matrix.block<3, 1>(0, 3);
+
+  CHECK_CUDA_ERROR(cudaMemcpyAsync(
+    device_rotation.get(), rotation.data(), sizeof(Eigen::Matrix3f), cudaMemcpyHostToDevice,
+    input.stream));
+  CHECK_CUDA_ERROR(cudaMemcpyAsync(
+    device_translation.get(), translation.data(), sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice,
+    input.stream));
+
   transformPointCloudLaunch(
-    input.data.get(), input.width * input.height, input.point_step, rotation, translation,
-    input.stream);
+    input.data.get(), input.width * input.height, input.point_step, device_rotation.get(),
+    device_translation.get(), input.stream);
+
   input.header.frame_id = target_frame;
   return true;
 }

perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils_kernel.cu

@@ -236,15 +236,15 @@ void __device__ raytrace(
 
 __global__ void transformPointCloudKernel(
   std::uint8_t * points, std::size_t num_points, std::size_t points_step,
-  const Eigen::Matrix3f & rotation, const Eigen::Vector3f & translation)
+  const Eigen::Matrix3f * rotation, const Eigen::Vector3f * translation)
 {
   int idx = blockIdx.x * blockDim.x + threadIdx.x;
   if (idx >= num_points) {
     return;
   }
 
   Eigen::Map<Eigen::Vector3f> point_map(reinterpret_cast<float *>(points + idx * points_step));
-  point_map = rotation * point_map + translation;
+  point_map = rotation[0] * point_map + translation[0];
 }
 
 __global__ void copyMapRegionKernel(
@@ -271,10 +271,10 @@ __global__ void copyMapRegionKernel(
 
 void transformPointCloudLaunch(
   std::uint8_t * points, std::size_t num_points, std::size_t points_step,
-  const Eigen::Matrix3f & rotation, const Eigen::Vector3f & translation, cudaStream_t stream)
+  const Eigen::Matrix3f * rotation, const Eigen::Vector3f * translation, cudaStream_t stream)
 {
   // Launch kernel
-  int threads_per_block = 256;
+  const int threads_per_block = 256;
   int num_blocks = (num_points + threads_per_block - 1) / threads_per_block;
   transformPointCloudKernel<<<num_blocks, threads_per_block, 0, stream>>>(
     points, num_points, points_step, rotation, translation);
@@ -291,6 +291,10 @@ void copyMapRegionLaunch(
   const int num_blocks =
     (region_size_x * region_size_y + threads_per_block - 1) / threads_per_block;
 
+  if (num_blocks == 0) {
+    return;
+  }
+
   copyMapRegionKernel<<<num_blocks, threads_per_block, 0, stream>>>(
     source_map, sm_lower_left_x, sm_lower_left_y, sm_size_x, sm_size_y, dest_map, dm_lower_left_x,
     dm_lower_left_y, dm_size_x, dm_size_y, region_size_x, region_size_y);

perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

@@ -122,6 +122,9 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode(
   occupancy_grid_map_ptr_->setCudaStream(stream_);
   occupancy_grid_map_updater_ptr_->setCudaStream(stream_);
 
+  device_rotation_ = autoware::cuda_utils::make_unique<Eigen::Matrix3f>();
+  device_translation_ = autoware::cuda_utils::make_unique<Eigen::Vector3f>();
+
   occupancy_grid_map_ptr_->initRosParam(*this);
   occupancy_grid_map_updater_ptr_->initRosParam(*this);
 
@@ -185,12 +188,14 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw()
   if (use_height_filter_) {
     // Make sure that the frame is base_link
     if (raw_pointcloud_.header.frame_id != base_link_frame_) {
-      if (!utils::transformPointcloudAsync(raw_pointcloud_, *tf2_, base_link_frame_)) {
+      if (!utils::transformPointcloudAsync(
+            raw_pointcloud_, *tf2_, base_link_frame_, device_rotation_, device_translation_)) {
         return;
       }
     }
     if (obstacle_pointcloud_.header.frame_id != base_link_frame_) {
-      if (!utils::transformPointcloudAsync(obstacle_pointcloud_, *tf2_, base_link_frame_)) {
+      if (!utils::transformPointcloudAsync(
+            obstacle_pointcloud_, *tf2_, base_link_frame_, device_rotation_, device_translation_)) {
         return;
       }
     }

perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp

@@ -81,6 +81,9 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node
   CudaPointCloud2 raw_pointcloud_;
   CudaPointCloud2 obstacle_pointcloud_;
 
+  autoware::cuda_utils::CudaUniquePtr<Eigen::Matrix3f> device_rotation_;
+  autoware::cuda_utils::CudaUniquePtr<Eigen::Vector3f> device_translation_;
+
   // ROS Parameters
   std::string map_frame_;
   std::string base_link_frame_;

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp

@@ -170,7 +170,7 @@ class ObstacleStopModule : public PluginModuleInterface
     const std::shared_ptr<PlannerData::Object> object, const double dist_to_bumper,
     const std::vector<Polygon2d> & decimated_traj_polys_with_lat_margin,
     const std::optional<std::pair<geometry_msgs::msg::Point, double>> & collision_point) const;
-  
+
   std::optional<std::pair<geometry_msgs::msg::Point, double>> get_nearest_collision_point(
     const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
     const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info,

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/types.hpp

@@ -69,11 +69,10 @@ struct StopObstacle
   {
   }
   StopObstacle(
-    const rclcpp::Time & arg_stamp, const bool arg_is_point_cloud,
-    const double arg_lon_velocity, const geometry_msgs::msg::Point & arg_collision_point,
+    const rclcpp::Time & arg_stamp, const bool arg_is_point_cloud, const double arg_lon_velocity,
+    const geometry_msgs::msg::Point & arg_collision_point,
     const double arg_dist_to_collide_on_decimated_traj, const double arg_braking_dist = 0.0)
-  :
-    stamp(arg_stamp),
+  : stamp(arg_stamp),
     velocity(arg_lon_velocity),
     collision_point(arg_collision_point),
     dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj),