fix(probabilistic_occupancy_grid_map): build and runtime on non-cuda environments (#10061)

* feat: improved data handling in the ogm. When using the full concatenated pointcloud the processing time decreases from 8ms to 4ms

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: added header blocks for non-cuda environments

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: removed the cuda includes from the global includes

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* fix: missed one header include

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: added more cuda related macros

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: lint errors

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: fixed errors during merge

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

---------

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

Author: knzo25

perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt

@@ -10,62 +10,98 @@ find_package(Eigen3 REQUIRED)
 find_package(PCL REQUIRED)
 
 if(NOT ${CUDA_FOUND})
-  message(WARNING "cuda was not found, so the autoware_probabilistic_occupancy_grid_map package will not be built.")
-  return()
+  message(WARNING "cuda was not found, so only autoware::occupancy_grid_map::LaserscanBasedOccupancyGridMapNode will be built.")
+else()
+  add_definitions(-DUSE_CUDA)
 endif()
 
 include_directories(
   include
   SYSTEM
-    ${CUDA_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIR}
     ${grid_map_ros_INCLUDE_DIRS}
 )
 
-# cSpell: ignore expt
-list(APPEND CUDA_NVCC_FLAGS --expt-relaxed-constexpr -diag-suppress 20012 --compiler-options -fPIC)
-set(CUDA_SEPARABLE_COMPILATION ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-
-cuda_add_library(${PROJECT_NAME}_cuda SHARED
-  lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu
-  lib/costmap_2d/occupancy_grid_map_projective_kernel.cu
-  lib/updater/binary_bayes_filter_updater_kernel.cu
-  lib/updater/log_odds_bayes_filter_updater_kernel.cu
-  lib/utils/utils_kernel.cu
-)
-
-target_link_libraries(${PROJECT_NAME}_cuda
-  ${CUDA_LIBRARIES}
-)
-
-ament_auto_add_library(${PROJECT_NAME}_common SHARED
-  lib/costmap_2d/occupancy_grid_map_base.cpp
-  lib/updater/binary_bayes_filter_updater.cpp
-  lib/utils/utils.cpp
-)
-target_link_libraries(${PROJECT_NAME}_common
-  ${PCL_LIBRARIES}
-  ${PROJECT_NAME}_cuda
-)
-
-# PointcloudBasedOccupancyGridMap
-ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED
-  src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
-  lib/costmap_2d/occupancy_grid_map_fixed.cpp
-  lib/costmap_2d/occupancy_grid_map_projective.cpp
-)
-
-target_link_libraries(pointcloud_based_occupancy_grid_map
-  ${PCL_LIBRARIES}
-  ${PROJECT_NAME}_common
-  ${PROJECT_NAME}_cuda
-)
+if(${CUDA_FOUND})
+  include_directories(
+    SYSTEM
+      ${CUDA_INCLUDE_DIRS}
+  )
+
+  # cSpell: ignore expt
+  list(APPEND CUDA_NVCC_FLAGS --expt-relaxed-constexpr -diag-suppress 20012 --compiler-options -fPIC)
+  set(CUDA_SEPARABLE_COMPILATION ON)
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  cuda_add_library(${PROJECT_NAME}_cuda SHARED
+    lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu
+    lib/costmap_2d/occupancy_grid_map_projective_kernel.cu
+    lib/updater/binary_bayes_filter_updater_kernel.cu
+    lib/updater/log_odds_bayes_filter_updater_kernel.cu
+    lib/utils/utils_kernel.cu
+  )
+
+  target_link_libraries(${PROJECT_NAME}_cuda
+    ${CUDA_LIBRARIES}
+  )
+
+  ament_auto_add_library(${PROJECT_NAME}_common SHARED
+    lib/costmap_2d/occupancy_grid_map_base.cpp
+    lib/updater/binary_bayes_filter_updater.cpp
+    lib/utils/utils.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}_common
+    ${PCL_LIBRARIES}
+    ${PROJECT_NAME}_cuda
+  )
+
+  # PointcloudBasedOccupancyGridMap
+  ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED
+    src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
+    lib/costmap_2d/occupancy_grid_map_fixed.cpp
+    lib/costmap_2d/occupancy_grid_map_projective.cpp
+  )
+
+  target_link_libraries(pointcloud_based_occupancy_grid_map
+    ${PCL_LIBRARIES}
+    ${PROJECT_NAME}_common
+    ${PROJECT_NAME}_cuda
+  )
+
+  rclcpp_components_register_node(pointcloud_based_occupancy_grid_map
+    PLUGIN "autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode"
+    EXECUTABLE pointcloud_based_occupancy_grid_map_node
+  )
+
+  # GridMapFusionNode
+  ament_auto_add_library(synchronized_grid_map_fusion SHARED
+    src/fusion/synchronized_grid_map_fusion_node.cpp
+    lib/fusion_policy/fusion_policy.cpp
+    lib/costmap_2d/occupancy_grid_map_fixed.cpp
+    lib/updater/log_odds_bayes_filter_updater.cpp
+    lib/utils/utils.cpp
+  )
+
+  target_link_libraries(synchronized_grid_map_fusion
+    ${PCL_LIBRARIES}
+  )
+
+  rclcpp_components_register_node(synchronized_grid_map_fusion
+    PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode"
+    EXECUTABLE synchronized_grid_map_fusion_node
+  )
+else()
+  ament_auto_add_library(${PROJECT_NAME}_common SHARED
+    lib/costmap_2d/occupancy_grid_map_base.cpp
+    lib/updater/binary_bayes_filter_updater.cpp
+    lib/utils/utils.cpp
+  )
+
+  target_link_libraries(${PROJECT_NAME}_common
+    ${PCL_LIBRARIES}
+  )
+endif()
 
-rclcpp_components_register_node(pointcloud_based_occupancy_grid_map
-  PLUGIN "autoware::occupancy_grid_map::PointcloudBasedOccupancyGridMapNode"
-  EXECUTABLE pointcloud_based_occupancy_grid_map_node
-)
 
 # LaserscanBasedOccupancyGridMap
 ament_auto_add_library(laserscan_based_occupancy_grid_map SHARED
@@ -83,28 +119,13 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map
   EXECUTABLE laserscan_based_occupancy_grid_map_node
 )
 
-# GridMapFusionNode
-ament_auto_add_library(synchronized_grid_map_fusion SHARED
-  src/fusion/synchronized_grid_map_fusion_node.cpp
-  lib/fusion_policy/fusion_policy.cpp
-  lib/costmap_2d/occupancy_grid_map_fixed.cpp
-  lib/updater/log_odds_bayes_filter_updater.cpp
-  lib/utils/utils.cpp
-)
 
-target_link_libraries(synchronized_grid_map_fusion
-  ${PCL_LIBRARIES}
-)
-
-rclcpp_components_register_node(synchronized_grid_map_fusion
-  PLUGIN "autoware::occupancy_grid_map::GridMapFusionNode"
-  EXECUTABLE synchronized_grid_map_fusion_node
-)
-
-install(
-  TARGETS ${PROJECT_NAME}_cuda
-  DESTINATION lib
-)
+if(${CUDA_FOUND})
+  install(
+    TARGETS ${PROJECT_NAME}_cuda
+    DESTINATION lib
+  )
+endif()
 
 ament_auto_package(
   INSTALL_TO_SHARE

perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp

@@ -52,9 +52,12 @@
 #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_
 #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_
 
+#ifdef USE_CUDA
 #include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp"
 
 #include <autoware/cuda_utils/cuda_unique_ptr.hpp>
+#endif
+
 #include <autoware/universe_utils/math/unit_conversion.hpp>
 #include <nav2_costmap_2d/costmap_2d.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -78,10 +81,12 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D
     const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y,
     const float resolution);
 
+#ifdef USE_CUDA
   virtual void updateWithPointCloud(
     [[maybe_unused]] const CudaPointCloud2 & raw_pointcloud,
     [[maybe_unused]] const CudaPointCloud2 & obstacle_pointcloud,
     [[maybe_unused]] const Pose & robot_pose, [[maybe_unused]] const Pose & scan_origin) {};
+#endif
 
   void updateOrigin(double new_origin_x, double new_origin_y) override;
 
@@ -102,28 +107,32 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D
 
   bool isCudaEnabled() const;
 
+#ifdef USE_CUDA
   void setCudaStream(const cudaStream_t & stream);
 
   const autoware::cuda_utils::CudaUniquePtr<std::uint8_t[]> & getDeviceCostmap() const;
 
   void copyDeviceCostmapToHost() const;
+#endif
 
 protected:
   rclcpp::Logger logger_{rclcpp::get_logger("pointcloud_based_occupancy_grid_map")};
   rclcpp::Clock clock_{RCL_ROS_TIME};
 
   double resolution_inv_;
+  bool use_cuda_;
 
+#ifdef USE_CUDA
   cudaStream_t stream_;
 
-  bool use_cuda_;
   autoware::cuda_utils::CudaUniquePtr<std::uint8_t[]> device_costmap_;
   autoware::cuda_utils::CudaUniquePtr<std::uint8_t[]> device_costmap_aux_;
 
   autoware::cuda_utils::CudaUniquePtr<Eigen::Matrix3f> device_rotation_map_;
   autoware::cuda_utils::CudaUniquePtr<Eigen::Vector3f> device_translation_map_;
   autoware::cuda_utils::CudaUniquePtr<Eigen::Matrix3f> device_rotation_scan_;
   autoware::cuda_utils::CudaUniquePtr<Eigen::Vector3f> device_translation_scan_;
+#endif
 };
 
 }  // namespace costmap_2d

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

@@ -15,7 +15,10 @@
 #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_
 #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_
 
+#ifdef USE_CUDA
 #include "autoware/cuda_utils/cuda_unique_ptr.hpp"
+#endif
+
 #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp"
 
 #include <Eigen/Core>
@@ -40,7 +43,9 @@ class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface
   Eigen::Matrix2f probability_matrix_;
   double v_ratio_;
 
+#ifdef USE_CUDA
   autoware::cuda_utils::CudaUniquePtr<float[]> device_probability_matrix_;
+#endif
 };
 
 }  // namespace costmap_2d

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

@@ -17,7 +17,10 @@
 
 #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp"
 #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp"
+
+#ifdef USE_CUDA
 #include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp"
+#endif
 
 #include <rclcpp/node.hpp>
 

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

@@ -16,8 +16,11 @@
 #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_
 
 #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp"
+
+#ifdef USE_CUDA
 #include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp"
 #include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp"
+#endif
 
 #include <builtin_interfaces/msg/time.hpp>
 #include <pcl_ros/transforms.hpp>
@@ -56,8 +59,10 @@ bool transformPointcloud(
   const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2,
   const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output);
 
+#ifdef USE_CUDA
 bool transformPointcloudAsync(
   CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame);
+#endif
 
 Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose);
 

perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp

@@ -82,6 +82,7 @@ OccupancyGridMapInterface::OccupancyGridMapInterface(
 : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION),
   use_cuda_(use_cuda)
 {
+#ifdef USE_CUDA
   if (use_cuda_) {
     min_height_ = -std::numeric_limits<double>::infinity();
     max_height_ = std::numeric_limits<double>::infinity();
@@ -99,12 +100,11 @@ OccupancyGridMapInterface::OccupancyGridMapInterface(
     device_rotation_scan_ = autoware::cuda_utils::make_unique<Eigen::Matrix3f>();
     device_translation_scan_ = autoware::cuda_utils::make_unique<Eigen::Vector3f>();
   }
+#endif
 }
 
 void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_origin_y)
 {
-  using autoware::occupancy_grid_map::utils::copyMapRegionLaunch;
-
   // project the new origin into the grid
   int cell_ox{static_cast<int>(std::floor((new_origin_x - origin_x_) / resolution_))};
   int cell_oy{static_cast<int>(std::floor((new_origin_y - origin_y_) / resolution_))};
@@ -130,6 +130,8 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori
   // we need a map to store the obstacles in the window temporarily
   unsigned char * local_map{nullptr};
 
+#ifdef USE_CUDA
+  using autoware::occupancy_grid_map::utils::copyMapRegionLaunch;
   if (use_cuda_) {
     copyMapRegionLaunch(
       device_costmap_.get(), lower_left_x, lower_left_y, size_x_, size_y_,
@@ -138,6 +140,13 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori
     cudaMemsetAsync(
       device_costmap_.get(), cost_value::NO_INFORMATION, size_x_ * size_y_ * sizeof(std::uint8_t),
       stream_);
+#else
+  if (use_cuda_) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("pointcloud_based_occupancy_grid_map"),
+      "The code was compiled without cuda.");
+    return;
+#endif
   } else {
     local_map = new unsigned char[cell_size_x * cell_size_y];
 
@@ -159,6 +168,7 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori
   int start_y{lower_left_y - cell_oy};
 
   // now we want to copy the overlapping information back into the map, but in its new location
+#ifdef USE_CUDA
   if (use_cuda_) {
     if (
       start_x < 0 || start_y < 0 || start_x + cell_size_x > size_x_ ||
@@ -174,6 +184,13 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori
     copyMapRegionLaunch(
       device_costmap_aux_.get(), 0, 0, cell_size_x, cell_size_y, device_costmap_.get(), start_x,
       start_y, size_x_, size_y_, cell_size_x, cell_size_y, stream_);
+#else
+  if (use_cuda_) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("pointcloud_based_occupancy_grid_map"),
+      "The code was compiled without cuda.");
+    return;
+#endif
   } else {
     copyMapRegion(
       local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
@@ -187,10 +204,18 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori
 
 void OccupancyGridMapInterface::resetMaps()
 {
+#ifdef USE_CUDA
   if (use_cuda_) {
     cudaMemsetAsync(
       device_costmap_.get(), cost_value::NO_INFORMATION, getSizeInCellsX() * getSizeInCellsY(),
       stream_);
+#else
+  if (use_cuda_) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("pointcloud_based_occupancy_grid_map"),
+      "The code was compiled without cuda.");
+    return;
+#endif
   } else {
     nav2_costmap_2d::Costmap2D::resetMaps();
   }
@@ -207,6 +232,7 @@ bool OccupancyGridMapInterface::isCudaEnabled() const
   return use_cuda_;
 }
 
+#ifdef USE_CUDA
 void OccupancyGridMapInterface::setCudaStream(const cudaStream_t & stream)
 {
   if (isCudaEnabled()) {
@@ -227,6 +253,7 @@ void OccupancyGridMapInterface::copyDeviceCostmapToHost() const
     cudaMemcpyDeviceToHost, stream_);
   cudaStreamSynchronize(stream_);
 }
+#endif
 
 }  // namespace costmap_2d
 }  // namespace autoware::occupancy_grid_map