Cuda acceleration

extras/ros-jazzy-gz-harmonic-install.sh

@@ -88,6 +88,7 @@ sudo apt update && apt install -y \
     ros-$DIST-ros2-controllers \
     ros-$DIST-teleop-tools \
     ros-$DIST-urdfdom-py \
+    ros-$DIST-marine-acoustic-msgs \
     ros-dev-tools
 
 echo

gazebo/dave_gz_multibeam_sonar/multibeam_sonar/CMakeLists.txt

@@ -1,24 +1,28 @@
 cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR)
 
 # Suppress developer warnings for the entire workspace.
+
 set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS TRUE CACHE BOOL
     "Suppress developer warnings" FORCE)
 # Set CMP0144 to NEW to ensure find_package
 # uses upper-case <PACKAGENAME>_ROOT variables.
+
 cmake_policy(SET CMP0144 NEW)
 
 project(multibeam_sonar)
 
+set(CUDA_ARCHITECTURE "60" CACHE STRING "Target CUDA SM version")
+
 find_package(ament_cmake REQUIRED)
 find_package(CUDAToolkit QUIET)
 
 if(CUDAToolkit_FOUND)
+
   enable_language(CUDA)
   find_package(CUDA REQUIRED)
   message(STATUS "CUDA found, enabling CUDA support.")
   include_directories(${CUDA_INCLUDE_DIRS})
-  set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -arch=sm_60")
-
+  set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -arch=sm_${CUDA_ARCHITECTURE}")
   find_package(gz-cmake3 REQUIRED)
   find_package(gz-sim8 REQUIRED)
   find_package(gz-sensors8 REQUIRED)
@@ -28,17 +32,10 @@ if(CUDAToolkit_FOUND)
   find_package(sensor_msgs REQUIRED)
   find_package(geometry_msgs REQUIRED)
   find_package(rosidl_default_generators REQUIRED)
-  find_package(PCL REQUIRED)
-  find_package(pcl_conversions REQUIRED)
   find_package(OpenCV REQUIRED)
   find_package(marine_acoustic_msgs REQUIRED)
   find_package(cv_bridge REQUIRED)
 
-  include_directories(${PCL_INCLUDE_DIRS})
-  link_directories(${PCL_LIBRARY_DIRS})
-  add_definitions(${PCL_DEFINITIONS})
-
-  # Set version variables
   set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
   set(GZ_RENDERING_VER ${gz-rendering8_VERSION_MAJOR})
   set(GZ_SENSORS_VER ${gz-sensors8_VERSION_MAJOR})
@@ -64,7 +61,9 @@ if(CUDAToolkit_FOUND)
   )
 
   set_target_properties(${PROJECT_NAME}
-                          PROPERTIES CUDA_SEPARABLE_COMPILATION ON)
+    PROPERTIES
+    CUDA_SEPARABLE_COMPILATION ON
+  )
 
   target_include_directories(${PROJECT_NAME}
     PUBLIC
@@ -76,11 +75,17 @@ if(CUDAToolkit_FOUND)
     ${gz-sim${GZ_SIM_VER}_INCLUDE_DIRS}
   )
 
+  find_library(CUBLAS_LIB cublas
+    HINTS ${CUDAToolkit_LIBRARY_DIR} /usr/local/cuda/lib64 /usr/lib/x86_64-linux-gnu /usr/lib)
+  if(NOT CUBLAS_LIB)
+    message(FATAL_ERROR "cuBLAS not found")
+  endif()
+
   target_link_libraries(${PROJECT_NAME}
-  ${PCL_LIBRARIES}
-  ${CUDA_LIBRARIES}
-  ${CUDA_CUFFT_LIBRARIES}
-  ${CUBLAS_LIB})
+    ${CUDA_LIBRARIES}
+    ${CUDA_CUFFT_LIBRARIES}
+    ${CUBLAS_LIB}
+  )
 
   install(TARGETS ${PROJECT_NAME}
     DESTINATION lib/${PROJECT_NAME}
@@ -95,22 +100,18 @@ if(CUDAToolkit_FOUND)
     rclcpp
     sensor_msgs
     rosidl_default_generators
-    PCL
-    pcl_conversions
     OpenCV
     marine_acoustic_msgs
     geometry_msgs
     cv_bridge
   )
 
-
-  # Environment hooks
   ament_environment_hooks(
     "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in"
   )
 
 else()
-  message(STATUS "CUDA Toolkit not found: Skipping CUDA-specific targets")
+  message(STATUS "CUDA Toolkit not found or disabled: Skipping CUDA-specific targets")
 endif()
 
 ament_package()

gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc

@@ -48,11 +48,7 @@
 #include "MultibeamSonarSensor.hh"
 #include "sonar_calculation_cuda.cuh"
 
-#include <pcl/features/normal_3d.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <pcl_conversions/pcl_conversions.h>
+#include <sys/stat.h>
 #include <cv_bridge/cv_bridge.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <marine_acoustic_msgs/msg/ping_info.hpp>
@@ -372,6 +368,9 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo
   this->debugFlag = sensorElement->Get<bool>("debugFlag", false).first;
   gzmsg << "Debug: " << this->debugFlag << std::endl;
 
+  this->blazingFlag = sensorElement->Get<bool>("blazingSonarImage", false).first;
+  gzmsg << "BlazingSonarImage: " << this->blazingFlag << std::endl;
+
   this->pointCloudTopicName =
     sensorElement->Get<std::string>("pointCloudTopicName", "point_cloud").first;
   gzmsg << "pointCloudTopicName: " << this->pointCloudTopicName << std::endl;
@@ -648,16 +647,6 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo
 
   // -- Pre calculations for sonar -- //
 
-  // Random number generator
-  gzmsg << "Initializing random number generator..." << std::endl;
-  this->randImage = cv::Mat(this->pointMsg.height(), this->pointMsg.width(), CV_32FC2);
-  uint64 randN = static_cast<uint64>(std::rand());
-  gzmsg << "Random seed: " << randN << std::endl;
-  cv::theRNG().state = randN;
-  cv::RNG rng = cv::theRNG();
-  rng.fill(this->randImage, cv::RNG::NORMAL, 0.0f, 1.0f);
-  gzmsg << "Random image generated with normal distribution." << std::endl;
-
   // Hamming window
   gzmsg << "Computing Hamming window for " << this->nFreq << " frequencies." << std::endl;
   this->window = new float[this->nFreq];
@@ -1069,7 +1058,6 @@ void MultibeamSonarSensor::Implementation::ComputeSonarImage()
   CArray2D P_Beams = NpsGazeboSonar::sonar_calculation_wrapper(
     this->pointCloudImage,        // cv::Mat& depth_image (the point cloud image)
     normal_image,                 // cv::Mat& normal_image
-    this->randImage,              // cv::Mat& rand_image
     hPixelSize,                   // hPixelSize
     vPixelSize,                   // vPixelSize
     hFOV,                         // hFOV
@@ -1093,7 +1081,9 @@ void MultibeamSonarSensor::Implementation::ComputeSonarImage()
     this->window,                 // _window
     this->beamCorrector,          // _beamCorrector
     this->beamCorrectorSum,       // _beamCorrectorSum
-    this->debugFlag);
+    this->debugFlag,              // debugFlag
+    this->blazingFlag             // _blazingFlag
+  );
 
   // For calc time measure
   auto stop = std::chrono::high_resolution_clock::now();

gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh

@@ -160,6 +160,9 @@ private:
     int ray_nElevationRays;
     float * rangeVector;
 
+    // Sonar image parameters
+    bool blazingFlag;
+
     // Debug flags and reflectivity
     bool debugFlag;
     bool constMu;