diff --git a/.dockerignore b/.dockerignore index 41c7f2d2e..9ce9de889 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,9 +1,19 @@ -.vs -.vscode -Builds/* -Testing/* -.idea -cmake-build-debug -cmake-build-release -build -external/vcpkg +# Ignore everything by default +* + +# First-order allow exception for select directories +!/.clang-format +!/.githooks +!/.git +!/CMakeLists.txt +!/docs +!/extensions +!/extensions.repos +!external/CMakeLists.txt +!/include +!/ros2_standalone +!/setup.py +!/install_deps.py +!/src +!/test +!/tools diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 000000000..8724e8d8a --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,14 @@ +version: 2 +updates: + - package-ecosystem: "docker" + directory: "/" + schedule: + interval: "daily" + commit-message: + prefix: "🐳 " + - package-ecosystem: "github-actions" + directory: "/" + schedule: + interval: "daily" + commit-message: + prefix: "🛠️ " diff --git a/.github/workflows/build-and-test.yml b/.github/workflows/build-and-test.yml new file mode 100644 index 000000000..1ebec7150 --- /dev/null +++ b/.github/workflows/build-and-test.yml @@ -0,0 +1,226 @@ +name: build-and-test + +run-name: Build & test for ${{ github.ref_name }} on ${{ github.sha }} + +on: + push: + branches: + - 'main' + - 'develop' + pull_request: + types: [opened, synchronize, reopened] + +# Cancel previous runs if new changes on PR/branch +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: +###### PREPARE WORKFLOW ###### + checkout-repository: + runs-on: self-hosted + steps: + - name: Checkout repository code + uses: actions/checkout@v4 + with: + clean: false + - name: Clean repository excluding RGL blobs repo + run: git clean -ffdx -e external/rgl_blobs + - name: Import private extensions + run: vcs import < extensions.repos + - name: Install taped test runtime dependencies + run: ./setup.py --fetch-rgl-blobs + + load-env: + uses: ./.github/workflows/load-env-subworkflow.yml + +###### BUILD USING RGL DOCKER ###### + build-core: + needs: [checkout-repository, load-env] + runs-on: self-hosted + steps: + - run: ' + docker build --build-context optix=${{ needs.load-env.outputs.optix-install-dir }} + --build-arg WITH_PCL=1 --build-arg WITH_ROS2=1 + --build-arg BUILD_CMD="./setup.py" + --target=exporter --output=bin-core .' + + build-pcl: + needs: [checkout-repository, load-env] + runs-on: self-hosted + steps: + - run: ' + docker build --build-context optix=${{ needs.load-env.outputs.optix-install-dir }} + --build-arg WITH_PCL=1 --build-arg WITH_ROS2=1 + --build-arg BUILD_CMD="./setup.py --with-pcl --build-taped-test" + --target=exporter --output=bin-pcl .' + + build-ros2: + needs: [ checkout-repository, load-env ] + runs-on: self-hosted + steps: + - run: ' + docker build --build-context optix=${{ needs.load-env.outputs.optix-install-dir }} + --build-arg WITH_PCL=1 --build-arg WITH_ROS2=1 + --build-arg BUILD_CMD=". /opt/ros/\$ROS_DISTRO/setup.sh && ./setup.py --with-ros2-standalone" + --target=exporter --output=bin-ros2 .' + + build-udp: + needs: [ checkout-repository, load-env ] + runs-on: self-hosted + steps: + - run: ' + docker build --build-context optix=${{ needs.load-env.outputs.optix-install-dir }} + --build-arg WITH_PCL=1 --build-arg WITH_ROS2=1 + --build-arg BUILD_CMD="./setup.py --with-udp" + --target=exporter --output=bin-udp .' + + build-snow: + needs: [ checkout-repository, load-env ] + runs-on: self-hosted + steps: + - run: ' + docker build --build-context optix=${{ needs.load-env.outputs.optix-install-dir }} + --build-arg WITH_PCL=1 --build-arg WITH_ROS2=1 + --build-arg BUILD_CMD="./setup.py --with-snow" + --target=exporter --output=bin-snow .' + + build-all: + needs: [ checkout-repository, load-env ] + runs-on: self-hosted + steps: + - run: ' + docker build --build-context optix=${{ needs.load-env.outputs.optix-install-dir }} + --build-arg WITH_PCL=1 --build-arg WITH_ROS2=1 + --build-arg BUILD_CMD=". /opt/ros/\$ROS_DISTRO/setup.sh && ./setup.py --with-pcl --with-ros2-standalone --with-udp --with-snow --build-taped-test" + --target=exporter --output=bin-all .' + +####### TEST WITH RGL DOCKER IMAGE ###### + test-core-dev: + needs: [build-core] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + cd bin-core/bin/test && ./RobotecGPULidar_test' + docker-image: localhost:5000/robotecgpulidar-all:latest + + test-pcl-dev: + needs: [ build-pcl ] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + export RGL_TAPED_TEST_DATA_DIR=$(pwd)/external/rgl_blobs && + cd bin-pcl/bin/test && ./RobotecGPULidar_test && ./RobotecGPULidar_taped_test' + docker-image: localhost:5000/robotecgpulidar-all:latest + + test-ros2-dev: + needs: [ build-ros2 ] + uses: ./.github/workflows/test-subworkflow.yml + with: + # Source ROS2 and radar_msgs, standalone build is tested in prod environment + # Run tests twice, each for different RMW implementation + test-command: ' + . /opt/ros/$ROS_DISTRO/setup.sh && + . /opt/rgl/external/radar_msgs/install/setup.sh && + cd bin-ros2/bin/test && + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp && ./RobotecGPULidar_test && + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp && ./RobotecGPULidar_test' + docker-image: localhost:5000/robotecgpulidar-all:latest + + test-udp-dev: + needs: [ build-udp ] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + cd bin-udp/bin/test && ./RobotecGPULidar_test' + docker-image: localhost:5000/robotecgpulidar-all:latest + + test-snow-dev: + needs: [ build-snow ] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + cd bin-snow/bin/test && ./RobotecGPULidar_test' + docker-image: localhost:5000/robotecgpulidar-all:latest + + test-all-dev: + needs: [ build-all ] + uses: ./.github/workflows/test-subworkflow.yml + with: + # Source ROS2 and radar_msgs, standalone build is tested in prod environment + # Set `RGL_TEST_VLP16_CALIB_FILE` for UDP-ROS2 integration test + # Run tests twice, each for different RMW implementation + test-command: ' + . /opt/ros/$ROS_DISTRO/setup.sh && + . /opt/rgl/external/radar_msgs/install/setup.sh && + export RGL_TEST_VLP16_CALIB_FILE=$(pwd)/extensions/udp/test/resources/Ros2Vlp16Calib.yaml && + export RGL_TAPED_TEST_DATA_DIR=$(pwd)/external/rgl_blobs && + cd bin-all/bin/test && + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp && ./RobotecGPULidar_test && ./RobotecGPULidar_taped_test && + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp && ./RobotecGPULidar_test && ./RobotecGPULidar_taped_test' + docker-image: localhost:5000/robotecgpulidar-all:latest + +####### TEST WITH CLEAN UBUNTU DOCKER IMAGE ###### + test-core-prod: + needs: [build-core] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + cd bin-core/bin/test && ./RobotecGPULidar_test' + docker-image: nvidia/cuda:11.7.1-base-ubuntu22.04 + + test-pcl-prod: + needs: [ build-pcl ] + uses: ./.github/workflows/test-subworkflow.yml + with: + # Additionally, install PCL extension dependent libraries for runtime + test-command: ' + apt update && apt install -y libxcursor1 libgl1 && + export RGL_TAPED_TEST_DATA_DIR=$(pwd)/external/rgl_blobs && + cd bin-pcl/bin/test && ./RobotecGPULidar_test && ./RobotecGPULidar_taped_test' + docker-image: nvidia/cuda:11.7.1-base-ubuntu22.04 + + test-ros2-prod: + needs: [ build-ros2 ] + uses: ./.github/workflows/test-subworkflow.yml + with: + # Copy ROS2 libraries to be visible for libRobotecGPULidar.so + # Run tests twice, each for different RMW implementation + test-command: ' + cp -p bin-ros2/lib/ros2_standalone/* bin-ros2/lib && + cd bin-ros2/bin/test && + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp && ./RobotecGPULidar_test && + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp && ./RobotecGPULidar_test' + docker-image: nvidia/cuda:11.7.1-base-ubuntu22.04 + + test-udp-prod: + needs: [ build-udp ] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + cd bin-udp/bin/test && ./RobotecGPULidar_test' + docker-image: nvidia/cuda:11.7.1-base-ubuntu22.04 + + test-snow-prod: + needs: [ build-snow ] + uses: ./.github/workflows/test-subworkflow.yml + with: + test-command: ' + cd bin-snow/bin/test && ./RobotecGPULidar_test' + docker-image: nvidia/cuda:11.7.1-base-ubuntu22.04 + + test-all-prod: + needs: [ build-all ] + uses: ./.github/workflows/test-subworkflow.yml + with: + # Install PCL extension dependent libraries for runtime + # Copy ROS2 libraries to be visible for libRobotecGPULidar.so + # Run tests twice, each for different RMW implementation + test-command: ' + apt update && apt install -y libxcursor1 libgl1 && + export RGL_TAPED_TEST_DATA_DIR=$(pwd)/external/rgl_blobs && + cp -p bin-all/lib/ros2_standalone/* bin-all/lib && + cd bin-all/bin/test && + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp && ./RobotecGPULidar_test && ./RobotecGPULidar_taped_test && + export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp && ./RobotecGPULidar_test && ./RobotecGPULidar_taped_test' + docker-image: nvidia/cuda:11.7.1-base-ubuntu22.04 diff --git a/.github/workflows/load-env-subworkflow.yml b/.github/workflows/load-env-subworkflow.yml new file mode 100644 index 000000000..e48d459fe --- /dev/null +++ b/.github/workflows/load-env-subworkflow.yml @@ -0,0 +1,23 @@ +name: load-env-subworkflow + +on: + workflow_call: + outputs: + optix-install-dir: + value: ${{ jobs.load-env.outputs.optix-install-dir }} + +jobs: + load-env: + runs-on: self-hosted + outputs: + optix-install-dir: ${{ steps.set-envs.outputs.optix-install-dir }} + steps: + - id: check-envs + run: | + if [[ -z "${OptiX_INSTALL_DIR}" ]]; then + echo "OptiX_INSTALL_DIR env is empty" + exit 1 + fi + - id: set-envs + run: | + echo "optix-install-dir=$OptiX_INSTALL_DIR" | tee -a $GITHUB_OUTPUT diff --git a/.github/workflows/test-subworkflow.yml b/.github/workflows/test-subworkflow.yml new file mode 100644 index 000000000..318dae373 --- /dev/null +++ b/.github/workflows/test-subworkflow.yml @@ -0,0 +1,31 @@ +name: test-subworkflow + +on: + workflow_call: + inputs: + test-command: + required: true + type: string + docker-image: + required: true + type: string + +permissions: + contents: read + +jobs: + test: + defaults: + run: + shell: bash + runs-on: self-hosted + container: + image: ${{ inputs.docker-image }} + env: + NVIDIA_DRIVER_CAPABILITIES: all + options: + --rm + --gpus all + steps: + - name: test + run: ${{ inputs.test-command }} diff --git a/.gitignore b/.gitignore index 0e6a9610a..b20036ea6 100644 --- a/.gitignore +++ b/.gitignore @@ -39,4 +39,8 @@ Testing/* cmake-build-debug cmake-build-release build -external/vcpkg +external/* +!external/CMakeLists.txt +extensions/udp +extensions/snow +CMakeSettings.json diff --git a/CHANGELOG.md b/CHANGELOG.md index 9b6caeb7c..6c278cf40 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,65 @@ # Change Log +## [0.17.0] 11 June 2024 + +### Added +- Added radar sensor simulation + - Added API call to process point cloud to create radar-like output + - `rgl_node_points_radar_postprocess` + - Added API call to remove ground using RANSAC method to fit the plane model to the point cloud + - `rgl_node_points_remove_ground` + - Added API call to filter ground based on the incident angle of the ray hitting the mesh triangle + - `rgl_node_points_filter_ground` + - Added API call to publish [RadarScan](http://docs.ros.org/en/noetic/api/radar_msgs/html/msg/RadarScan.html) message into ROS2 topic + - `rgl_node_publish_ros2_radarscan` + - Added new fields (point attributes) calculation: + - Velocity of the hit point on the entity. Depends on entity's linear and angular velocity, and mesh deformations (inferred from calls `rgl_entity_set_pose`, `rgl_scene_set_time` and `rgl_mesh_update_vertices`). + - `RGL_FIELD_ABSOLUTE_VELOCITY_VEC3_F32` + - `RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32` + - `RGL_FIELD_RADIAL_SPEED_F32` + - Normal vector of the mesh triangle where the hit-point is located + - `RGL_FIELD_NORMAL_VEC3_F32` + - Incident angle of the ray hitting the mesh triangle + - `RGL_FIELD_INCIDENT_ANGLE_F32` + - Azimuth and elevation angle of the hit point + - `RGL_FIELD_AZIMUTH_F32` + - `RGL_FIELD_ELEVATION_F32` +- Added API call to compact point cloud by given field (`RGL_FIELD_IS_HIT_I32` or `RGL_FIELD_IS_GROUND_I32`) + - `rgl_node_points_compact_by_field` + - At the same time, `rgl_node_points_compact` became deprecated +- Added set of calls to check if API objects are still valid + - `rgl_mesh_is_alive` + - `rgl_entity_is_alive` + - `rgl_texture_is_alive` + - `rgl_node_is_alive` +- Added raytrace node configurations + - Added API call to configure values for non-hit points + - `rgl_node_raytrace_configure_non_hits` + - Added API call to configure sensor velocity (necessary for velocity distortion or calculating fields `RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32` and `RGL_FIELD_RADIAL_SPEED_F32`) + - `rgl_node_raytrace_configure_velocity` + - Added API call to configure (turn on/off) velocity distortion + - `rgl_node_raytrace_configure_distortion` +- Added support for LiDAR reflective value (similar to intensity but set as a single floating-point value for the entire entity) + - Added API call to set laser retro value to the entity + - `rgl_entity_set_laser_retro` + - Note: could be replaced with 1x1 float-type texture in the future +- Added CI to the project using GitHub actions +- Added static build option + - Added variable to the CMakeLists (default OFF): + - `RGL_BUILD_STATIC` + +### Changed +- Optimized tape files format to be lighter and be loaded faster + - Note: old tape files are not compatible +- Improved docker-based build pipeline + - Introduced multistage dockerfile for the build process + - Decoupled dependency installation from compilation scripts (better caching) + +### Removed +- Removed API call for modifying RaytraceNode to apply velocity distortion + - `rgl_node_raytrace_with_distortion` + - Replaced by `rgl_node_raytrace_configure_distortion` + ## [0.16.2] 19 November 2023 ### Fixed diff --git a/CMakeLists.txt b/CMakeLists.txt index 3482d3d5d..6c2ca5194 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,19 @@ cmake_minimum_required(VERSION 3.18) # 3.18 To automatically detect CUDA_ARCHITECTURES set(CMAKE_CXX_STANDARD 20) set(CMAKE_CUDA_STANDARD 17) + # Build Release by default; CMAKE_BUILD_TYPE needs to be set before project(...) if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel" FORCE) endif(NOT CMAKE_BUILD_TYPE) + +# Use debug postfix 'd' by default. +if(NOT CMAKE_DEBUG_POSTFIX) + set(CMAKE_DEBUG_POSTFIX "d" CACHE STRING + "Choose the debug postfix used when building Debug configuration") +endif() + project(RobotecGPULidar C CXX CUDA) # Logging default settings (can be changed via API call) @@ -18,9 +26,15 @@ set(RGL_LOG_FILE "" CACHE STRING # STRING prevents from expanding relative path set(RGL_AUTO_TAPE_PATH "" CACHE STRING # STRING prevents from expanding relative paths "If non-empty, defines a path for the automatic tape (started on the first API call)") +# Library configuration +set(RGL_BUILD_STATIC OFF CACHE BOOL + "Builds RobotecGPULidar as a statically linkable library instead of a shared one") + # Test configuration set(RGL_BUILD_TESTS ON CACHE BOOL "Enables building test. GTest will be automatically downloaded") +set(RGL_BUILD_TAPED_TESTS OFF CACHE BOOL + "Enables building taped test.") # Tools configuration set(RGL_BUILD_TOOLS ON CACHE BOOL "Enables building RGL executable tools") @@ -32,6 +46,9 @@ set(RGL_BUILD_ROS2_EXTENSION OFF CACHE BOOL "Enables building ROS2 extension. It requires installed and sourced ROS2.") set(RGL_BUILD_UDP_EXTENSION OFF CACHE BOOL "Enables building UDP extension.") +set(RGL_BUILD_SNOW_EXTENSION OFF CACHE BOOL + "Enables building snow simulation extension.") + # Hide automatically generated CTest targets set_property(GLOBAL PROPERTY CTEST_TARGETS_ADDED 1) @@ -76,10 +93,12 @@ add_custom_command( VERBATIM) add_library(optixPrograms optixProgramsPtx.c) -add_library(RobotecGPULidar SHARED +set(RGL_SOURCE_FILES src/api/apiCommon.cpp src/api/apiCore.cpp - src/Tape.cpp + src/tape/TapePlayer.cpp + src/tape/TapeRecorder.cpp + src/tape/PlaybackState.cpp src/Logger.cpp src/Optix.cpp src/gpu/helpersKernels.cu @@ -95,13 +114,15 @@ add_library(RobotecGPULidar SHARED src/graph/GaussianNoiseAngularHitpointNode.cpp src/graph/GaussianNoiseAngularRayNode.cpp src/graph/GaussianNoiseDistanceNode.cpp - src/graph/CompactPointsNode.cpp + src/graph/CompactByFieldPointsNode.cpp src/graph/FormatPointsNode.cpp src/graph/RaytraceNode.cpp src/graph/TransformPointsNode.cpp src/graph/TransformRaysNode.cpp src/graph/FromArrayPointsNode.cpp src/graph/FromMat3x4fRaysNode.cpp + src/graph/FilterGroundPointsNode.cpp + src/graph/RadarPostprocessPointsNode.cpp src/graph/SetRangeRaysNode.cpp src/graph/SetRaysRingIdsRaysNode.cpp src/graph/SetTimeOffsetsRaysNode.cpp @@ -110,11 +131,22 @@ add_library(RobotecGPULidar SHARED src/graph/YieldPointsNode.cpp ) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +if (RGL_BUILD_STATIC) + add_library(RobotecGPULidar STATIC ${RGL_SOURCE_FILES}) + target_compile_definitions(RobotecGPULidar PUBLIC RGL_STATIC) + set(RGL_SPDLOG_VARIANT spdlog_header_only) +else () + add_library(RobotecGPULidar SHARED ${RGL_SOURCE_FILES}) + set(RGL_SPDLOG_VARIANT spdlog) +endif () + set_property(TARGET RobotecGPULidar PROPERTY POSITION_INDEPENDENT_CODE ON) target_compile_definitions(RobotecGPULidar PUBLIC RGL_BUILD_PCL_EXTENSION=$) target_compile_definitions(RobotecGPULidar PUBLIC RGL_BUILD_ROS2_EXTENSION=$) target_compile_definitions(RobotecGPULidar PUBLIC RGL_BUILD_UDP_EXTENSION=$) +target_compile_definitions(RobotecGPULidar PUBLIC RGL_BUILD_SNOW_EXTENSION=$) if (RGL_BUILD_PCL_EXTENSION) add_subdirectory(extensions/pcl) @@ -128,13 +160,17 @@ if (RGL_BUILD_UDP_EXTENSION) add_subdirectory(extensions/udp) endif() +if (RGL_BUILD_SNOW_EXTENSION) + add_subdirectory(extensions/snow) +endif() + target_include_directories(RobotecGPULidar PUBLIC include PRIVATE src ) target_link_libraries(RobotecGPULidar PRIVATE - spdlog + ${RGL_SPDLOG_VARIANT} yaml-cpp optixPrograms cmake_git_version_tracking @@ -164,7 +200,7 @@ if (NOT ("RGL_LOG_LEVEL_${RGL_LOG_LEVEL}" IN_LIST RGL_AVAILABLE_LOG_LEVELS)) message(FATAL_ERROR "Incorrect RGL_LOG_LEVEL value: ${RGL_LOG_LEVEL}") endif() -if (WIN32 AND RGL_AUTO_TAPE_PATH) +if (WIN32 AND (RGL_AUTO_TAPE_PATH OR RGL_BUILD_TAPED_TESTS)) message(FATAL_ERROR "(Auto)Tape not supported on Windows") endif() @@ -177,12 +213,23 @@ target_compile_definitions(RobotecGPULidar PRIVATE RGL_BUILD # Used in headers to differentiate whether it is parsed as library or client's code, affects __declspec on Windows. ) +# Enable relative paths in the build RPATH for executables +set(CMAKE_BUILD_RPATH_USE_ORIGIN ON) + # Include tests -if (RGL_BUILD_TESTS) +if (RGL_BUILD_TESTS OR RGL_BUILD_TAPED_TESTS) enable_testing() - add_subdirectory(test) + + if (RGL_BUILD_TESTS) + add_subdirectory(test) + endif() + + if (RGL_BUILD_TAPED_TESTS) + add_subdirectory(test/taped_test) + endif() endif() + # Include tools if (RGL_BUILD_TOOLS) add_subdirectory(tools) diff --git a/Dockerfile b/Dockerfile index bb011733e..fa561a871 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,29 +1,122 @@ -FROM nvidia/cuda:11.7.1-devel-ubuntu22.04 +ARG BASE_IMAGE=base +ARG WITH_PCL=0 +ARG WITH_ROS2=0 +# Stage from full image tag name for dependabot detection +FROM nvidia/cuda:11.7.1-devel-ubuntu22.04 as base +################################################################################ +# MARK: prepper - prep rgl dependencies +################################################################################ +### Core dependencies stage +FROM $BASE_IMAGE as prepper-core ARG DEBIAN_FRONTEND=noninteractive -RUN apt update +# Edit apt config for caching and update once +RUN mv /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \ + echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' \ + > /etc/apt/apt.conf.d/keep-cache && \ + apt-get update -RUN apt install -y cmake +# Install bootstrap tools for install scripts +RUN --mount=type=cache,sharing=locked,target=/var/cache/apt \ + apt-get install -y --no-install-recommends \ + cmake \ + git \ + python3 \ + sudo -# Install vcpkg dependencies -RUN apt install -y \ - git \ - curl \ - zip \ - unzip \ - tar \ - pkg-config \ - freeglut3-dev \ - libglew-dev \ - libglfw3-dev \ - python3 +# Set working directory using standard opt path +WORKDIR /opt/rgl -# Install RGL dependencies via vcpkg -COPY setup.py / -RUN /setup.py --install-pcl-deps -RUN rm /setup.py +# Copy only dependencies definition files +COPY ./install_deps.py . -WORKDIR /code +# Install dependencies while caching apt downloads +RUN --mount=type=cache,sharing=locked,target=/var/cache/apt \ + ./install_deps.py -RUN git config --system --add safe.directory /code +### PCL extension dependencies stage (added on top of core depenencies based on `WITH_PCL` argument) +FROM prepper-core AS prepper-pcl-0 +# Do nothing, PCL extension is not enabled +FROM prepper-core AS prepper-pcl-1 +# Copy only dependencies definition files for PCL extension +COPY ./extensions/pcl/install_deps.py . + +# Install PCL extension dependencies while caching apt downloads +RUN --mount=type=cache,sharing=locked,target=/var/cache/apt \ + ./install_deps.py + +### ROS2 extension dependencies stage (added on top of PCL depenencies based on `WITH_ROS2` argument) +FROM prepper-pcl-${WITH_PCL} AS prepper-ros2-0 +# Do nothing, ROS2 extension is not enabled +FROM prepper-pcl-${WITH_PCL} AS prepper-ros2-1 + +# Install ROS2: Setup sources.list and keys +RUN . /etc/os-release && \ + echo "deb http://packages.ros.org/ros2/ubuntu $UBUNTU_CODENAME main" > /etc/apt/sources.list.d/ros2-latest.list && \ + apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && \ + apt-get update + +ARG ROS_DISTRO=humble +ENV ROS_DISTRO=$ROS_DISTRO +# Install ROS2: Install packages +RUN --mount=type=cache,sharing=locked,target=/var/cache/apt \ + apt-get install -y --no-install-recommends \ + # Packages for RGL ROS2 standalone build + patchelf \ + pkg-config \ + ros-$ROS_DISTRO-cyclonedds \ + ros-$ROS_DISTRO-fastrtps \ + ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ + ros-$ROS_DISTRO-rmw-fastrtps-cpp \ + ros-$ROS_DISTRO-ros-core \ + # Packages for UDP-ROS2 integration test + # psmisc for`killall` command + psmisc \ + ros-$ROS_DISTRO-velodyne-driver \ + ros-$ROS_DISTRO-velodyne-pointcloud + +# Copy only dependencies definition files for ROS2 extension +COPY ./extensions/ros2/install_deps.py . + +# Install ROS2 extension dependencies while caching apt downloads +RUN --mount=type=cache,sharing=locked,target=/var/cache/apt \ + . /opt/ros/$ROS_DISTRO/setup.sh && \ + ./install_deps.py + +### Final prepper stage with selected extensions +FROM prepper-ros2-${WITH_ROS2} AS prepper + +################################################################################ +# MARK: builder - build rgl binaries +################################################################################ +FROM prepper AS builder +ARG OptiX_INSTALL_DIR=/optix + +# Copy rest of source tree +COPY . . + +ARG BUILD_CMD="./setup.py" +RUN --mount=type=bind,from=optix,target=${OptiX_INSTALL_DIR} \ + sh -c "$BUILD_CMD" + +################################################################################ +# MARK: dancer - multi-stage for cache dancing +################################################################################ +FROM builder AS dancer + +# Copy entire build directory +# RUN mkdir /dancer && \ +# cp -rT build /dancer + +# Copy only the lib and bin directories +RUN mkdir /dancer && \ + cp -r build/bin /dancer/ && \ + cp -r build/lib /dancer/ + +################################################################################ +# MARK: exporter - export rgl binaries and executables +################################################################################ +FROM scratch AS exporter + +COPY --from=dancer /dancer / diff --git a/README.md b/README.md index fababfb48..0b41eb55a 100644 --- a/README.md +++ b/README.md @@ -67,14 +67,32 @@ An introduction to the RGL API along with an example can be found [here](docs/Us ## Building in Docker (Linux) -1. Set up [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html#docker) -2. Download [NVidia OptiX](https://developer.nvidia.com/designworks/optix/downloads/legacy) **7.2** -3. `export OptiX_INSTALL_DIR=` -4. `docker build . --tag rgl:latest` -5. `docker run --net=host --gpus all -v $(pwd):/code -v ${OptiX_INSTALL_DIR}:/optix -e OptiX_INSTALL_DIR=/optix -e NVIDIA_DRIVER_CAPABILITIES=all -it rgl:latest /bin/bash` -6. `./setup.py --clean-build --with-pcl` - -*Note: Currently dockerfile doesn't contain ROS2 installation to support ROS2 extension.* +1. Download [NVidia OptiX](https://developer.nvidia.com/designworks/optix/downloads/legacy) **7.2** +2. `export OptiX_INSTALL_DIR=` +3. `docker build --build-context optix=${OptiX_INSTALL_DIR} --target=exporter --output=build .` + - The binaries will be exported to the `build` directory +4. To build RGL with extensions, docker must install additional dependencies. + - It could be enabled by setting the following arguments: + - `--build-arg WITH_PCL=1` - adds stage to install dependencies for PCL extension + - `--build-arg WITH_ROS2=1` - adds stage to install dependencies for ROS2 extension + - By default, the build command compiles the core part of the library only. To include extensions it must be overwritten: + - `--build-arg BUILD_CMD="./setup.py --with-pcl"` - includes PCL extension + - `--build-arg BUILD_CMD='. /opt/ros/\$ROS_DISTRO/setup.sh && ./setup.py --with-ros2'` - includes ROS2 extension (ROS2 must be sourced first) + - The command for building RGL with PCL and ROS2 extensions would be: + +```shell +docker build \ + --build-arg WITH_ROS2=1 \ + --build-arg WITH_PCL=1 \ + --build-arg BUILD_CMD='\ + . /opt/ros/\$ROS_DISTRO/setup.sh && \ + ./setup.py \ + --with-ros2 \ + --with-pcl' \ + --build-context optix=$OptiX_INSTALL_DIR \ + --target=exporter \ + --output=build . +``` ## Building on Ubuntu 22 @@ -83,7 +101,8 @@ An introduction to the RGL API along with an example can be found [here](docs/Us 1. You may be asked to create a Nvidia account to download 3. Export environment variable: 1. `export OptiX_INSTALL_DIR=`. -4. Use `setup.py` script to build. +4. Install dependencies with command: `./setup.py --install-deps` +5. Use `setup.py` script to build. - It will use CMake to generate files for the build system (make) and the build. - You can pass optional CMake and make parameters, e.g. - `./setup.py --cmake="-DCMAKE_BUILD_TYPE=Debug" --make="-j 16"` @@ -99,7 +118,8 @@ An introduction to the RGL API along with an example can be found [here](docs/Us - install the framework and set the environment variable `OptiX_INSTALL_DIR` 4. Install [Python3](https://www.python.org/downloads/). 5. Run `x64 Native Tools Command Prompt for VS 20xx` and navigate to the RGL repository. -6. Run `python setup.py` command to build the project. +6. Run `python setup.py --install-deps` command to install dependencies. +7. Run `python setup.py` command to build the project. - It will use CMake to generate files for the build system (ninja) and build. - You can pass optional CMake and ninja parameters, e.g. - `python setup.py --cmake="-DCMAKE_BUILD_TYPE=Debug" --ninja="-j 16"` @@ -112,3 +132,6 @@ An introduction to the RGL API along with an example can be found [here](docs/Us The development of this project was made possible thanks to cooperation with Tier IV - challenging needs in terms of features and performance of Tier IV's project allowed to significantly enrich Robotec GPU Lidar with features such as Gaussian noise and animated meshes as well as optimize it to provide real-time performance with many lidars. + +Additionally, we would like to express our gratitude to Dexory for their contribution to enhancing docker-based build pipeline, +ensuring a more robust and efficient workflow, and the overall development of the project. diff --git a/docs/PclExtension.md b/docs/PclExtension.md index 256d6cff2..814865036 100644 --- a/docs/PclExtension.md +++ b/docs/PclExtension.md @@ -10,7 +10,8 @@ For operations listed above, the extension uses [Point Cloud Library](https://po ## Building -Before building RGL PCL extension, it is necessary to install the required dependencies. Run the setup script with the `--install-pcl-deps` flag to download and install them. It could take some time (Point Cloud Library is built from source): +Before building RGL PCL extension, it is necessary to install the required dependencies. +Run `setup.py --install-pcl-deps` to get them. It could take some time (Point Cloud Library is built from source): ```bash # Linux: diff --git a/docs/Ros2Extension.md b/docs/Ros2Extension.md index d535d479d..20c43a9dd 100644 --- a/docs/Ros2Extension.md +++ b/docs/Ros2Extension.md @@ -16,6 +16,8 @@ RGL ROS2 extension can be built in two flavors: - **standalone** - ROS2 installation is not required on the target machine. RGL build will include all required ROS2 dependencies. - **overlay** - Assumes the existence of supported ROS2 installation on the target machine. RGL will try to use the existing installation of ROS2 dependencies. +Before building RGL PCL extension, it is necessary to get the required dependencies. +For some, the process has been automated - run `setup.py --install-ros2-deps` to get them. ### Ubuntu 22 diff --git a/extensions.repos b/extensions.repos new file mode 100644 index 000000000..fea894598 --- /dev/null +++ b/extensions.repos @@ -0,0 +1,11 @@ +repositories: + extensions/udp: + type: git + url: git@github.com:RobotecAI/RGL-extension-udp.git + version: v0.17.0 + + extensions/snow: + type: git + url: git@github.com:RobotecAI/RGL-extension-snow.git + version: v0.17.0 + diff --git a/extensions/pcl/CMakeLists.txt b/extensions/pcl/CMakeLists.txt index cc5e86e91..c759a2d59 100644 --- a/extensions/pcl/CMakeLists.txt +++ b/extensions/pcl/CMakeLists.txt @@ -1,10 +1,11 @@ -find_package(PCL 1.12 CONFIG REQUIRED COMPONENTS common io filters visualization) +find_package(PCL 1.12 CONFIG REQUIRED COMPONENTS common io filters segmentation visualization) target_sources(RobotecGPULidar PRIVATE src/api/apiPcl.cpp src/graph/DownSamplePointsNode.cpp src/graph/VisualizePointsNode.cpp + src/graph/RemoveGroundPointsNode.cpp ) target_include_directories(RobotecGPULidar PUBLIC ${PCL_INCLUDE_DIRS}) @@ -12,5 +13,6 @@ target_link_directories(RobotecGPULidar PRIVATE ${PCL_LIBRARY_DIRS}) target_link_libraries(RobotecGPULidar PRIVATE ${PCL_LIBRARIES}) target_include_directories(RobotecGPULidar + PUBLIC include PRIVATE src ) diff --git a/include/rgl/api/extensions/pcl.h b/extensions/pcl/include/rgl/api/extensions/pcl.h similarity index 61% rename from include/rgl/api/extensions/pcl.h rename to extensions/pcl/include/rgl/api/extensions/pcl.h index e0709a440..f7cdcfd93 100644 --- a/include/rgl/api/extensions/pcl.h +++ b/extensions/pcl/include/rgl/api/extensions/pcl.h @@ -51,3 +51,20 @@ RGL_API rgl_status_t rgl_node_points_downsample(rgl_node_t* node, float leaf_siz */ RGL_API rgl_status_t rgl_node_points_visualize(rgl_node_t* node, const char* window_name, int window_width = 1280, int window_height = 1024, bool fullscreen = false); + +/** + * Creates or modifies RemoveGroundNode. + * The node removes points belonging to the ground by approximating the ground to a plane. + * It uses RANSAC method to fit the plane model to the point cloud (with additional angular constraints to improve robustness). + * The plane must be perpendicular to a user-specified axis (sensor_up_axis), up to a user-specified angle threshold (ground_angle_threshold) + * Note: It is assumed that the point cloud on the input is in the sensor frame and the sensor is perpendicular to the ground. + * Graph input: point cloud + * Graph output: point cloud + * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. + * @param sensor_up_axis Axis of the sensor that directs up. It is assumed that the ground is perpendicular to that axis. + * @param ground_angle_threshold The maximum allowed difference between the plane normal and the given axis (in radians). Used when fitting plane model. + * @param ground_distance_threshold The maximum point's distance to the plane to consider that point as belonging to the ground (in distance units). Used when fitting plane model. + * @param ground_filter_distance The maximum point's distance to the ground to filter out that point (in distance units). Used when plane model is already approximated. + */ +RGL_API rgl_status_t rgl_node_points_remove_ground(rgl_node_t* node, rgl_axis_t sensor_up_axis, float ground_angle_threshold, + float ground_distance_threshold, float ground_filter_distance); diff --git a/extensions/pcl/install_deps.py b/extensions/pcl/install_deps.py new file mode 100755 index 000000000..820cef85d --- /dev/null +++ b/extensions/pcl/install_deps.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +import os +import platform +import sys +import subprocess + + +class Config: + # Default values for Linux + VCPKG_TAG = "2023.08.09" + # Paths relative to project root + VCPKG_DIR = os.path.join("external", "vcpkg") + VCPKG_EXEC = "vcpkg" + VCPKG_BOOTSTRAP = "bootstrap-vcpkg.sh" + VCPKG_TRIPLET = "x64-linux" + + def __init__(self): + if on_windows(): + self.VCPKG_EXEC = "vcpkg.exe" + self.VCPKG_BOOTSTRAP = "bootstrap-vcpkg.bat" + self.VCPKG_TRIPLET = "x64-windows" + + +def install_deps(): + cfg = Config() + + # Clone vcpkg + if not os.path.isdir(cfg.VCPKG_DIR): + if on_linux(): + print("Installing dependencies for vcpkg...") + run_subprocess_command(""" + sudo apt-get install -y \ + curl \ + freeglut3-dev \ + git \ + libglew-dev \ + libglfw3-dev \ + pkg-config \ + tar \ + unzip \ + zip + """) + run_subprocess_command( + f"git clone -b {cfg.VCPKG_TAG} --single-branch --depth 1 https://github.com/microsoft/vcpkg {cfg.VCPKG_DIR}") + # Bootstrap vcpkg + if not os.path.isfile(os.path.join(cfg.VCPKG_DIR, cfg.VCPKG_EXEC)): + run_subprocess_command(f"{os.path.join(cfg.VCPKG_DIR, cfg.VCPKG_BOOTSTRAP)}") + + # Install dependencies via vcpkg + run_subprocess_command( + f"{os.path.join(cfg.VCPKG_DIR, cfg.VCPKG_EXEC)} install --clean-after-build pcl[core,visualization]:{cfg.VCPKG_TRIPLET}") + + print("PCL deps installed successfully") + + +def are_deps_installed() -> bool: + cfg = Config() + return os.path.isdir(cfg.VCPKG_DIR) + + +def on_windows(): + return platform.system() == "Windows" + + +def on_linux(): + return platform.system() == "Linux" + + +def run_subprocess_command(command: str, shell=True, stderr=sys.stderr, stdout=sys.stdout): + print(f"Executing command: '{command}'") + process = subprocess.Popen(command, shell=shell, stderr=stderr, stdout=stdout) + process.wait() + if process.returncode != 0: + raise RuntimeError(f"Failed to execute command: '{command}'") + + +if __name__ == "__main__": + print("Important: this script should be executed from the root of the project (e.g. `./extensions/pcl/install_deps.py`)") + + sys.exit(install_deps()) diff --git a/extensions/pcl/src/api/apiPcl.cpp b/extensions/pcl/src/api/apiPcl.cpp index 06c26744d..0e35d567e 100644 --- a/extensions/pcl/src/api/apiPcl.cpp +++ b/extensions/pcl/src/api/apiPcl.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -72,9 +73,9 @@ RGL_API rgl_status_t rgl_graph_write_pcd_file(rgl_node_t node, const char* file_ return status; } -void TapePlayer::tape_graph_write_pcd_file(const YAML::Node& yamlNode) +void TapePcl::tape_graph_write_pcd_file(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_graph_write_pcd_file(tapeNodes.at(yamlNode[0].as()), yamlNode[1].as().c_str()); + rgl_graph_write_pcd_file(state.nodes.at(yamlNode[0].as()), yamlNode[1].as().c_str()); } RGL_API rgl_status_t rgl_node_points_downsample(rgl_node_t* node, float leaf_size_x, float leaf_size_y, float leaf_size_z) @@ -89,12 +90,12 @@ RGL_API rgl_status_t rgl_node_points_downsample(rgl_node_t* node, float leaf_siz return status; } -void TapePlayer::tape_node_points_downsample(const YAML::Node& yamlNode) +void TapePcl::tape_node_points_downsample(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_points_downsample(&node, yamlNode[1].as(), yamlNode[2].as(), yamlNode[3].as()); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_visualize(rgl_node_t* node, const char* window_name, int32_t window_width, @@ -114,12 +115,41 @@ RGL_API rgl_status_t rgl_node_points_visualize(rgl_node_t* node, const char* win return status; } -void TapePlayer::tape_node_points_visualize(const YAML::Node& yamlNode) +void TapePcl::tape_node_points_visualize(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_points_visualize(&node, yamlNode[1].as().c_str(), yamlNode[2].as(), yamlNode[3].as(), yamlNode[4].as()); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); +} + +RGL_API rgl_status_t rgl_node_points_remove_ground(rgl_node_t* node, rgl_axis_t sensor_up_axis, float ground_angle_threshold, + float ground_distance_threshold, float ground_filter_distance) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_node_points_remove_ground(node={}, sensor_up_axis={}, ground_angle_threshold={}, " + "ground_distance_threshold={}, ground_filter_distance={})", + repr(node), sensor_up_axis, ground_angle_threshold, ground_distance_threshold, ground_filter_distance); + + CHECK_ARG((sensor_up_axis == RGL_AXIS_X) || (sensor_up_axis == RGL_AXIS_Y) || (sensor_up_axis == RGL_AXIS_Z)); + CHECK_ARG(ground_angle_threshold > 0); + CHECK_ARG(ground_distance_threshold > 0); + CHECK_ARG(ground_filter_distance > 0); + + createOrUpdateNode(node, sensor_up_axis, ground_angle_threshold, ground_distance_threshold, + ground_filter_distance); + }); + TAPE_HOOK(node, sensor_up_axis, ground_angle_threshold, ground_distance_threshold, ground_filter_distance); + return status; +} + +void TapePcl::tape_node_points_remove_ground(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_remove_ground(&node, (rgl_axis_t) yamlNode[1].as>(), + yamlNode[2].as(), yamlNode[3].as(), yamlNode[4].as()); + state.nodes.insert({nodeId, node}); } } diff --git a/extensions/pcl/src/graph/NodesPcl.hpp b/extensions/pcl/src/graph/NodesPcl.hpp index f9db4cf07..7567c0e44 100644 --- a/extensions/pcl/src/graph/NodesPcl.hpp +++ b/extensions/pcl/src/graph/NodesPcl.hpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include @@ -56,6 +58,47 @@ struct DownSamplePointsNode : IPointsNodeSingleInput GPUFieldDescBuilder gpuFieldDescBuilder; }; +struct RemoveGroundPointsNode : IPointsNodeSingleInput +{ + using Ptr = std::shared_ptr; + void setParameters(rgl_axis_t sensorUpAxis, float groundAngleThreshold, float groundDistanceThreshold, + float groundFilterDistance); + + // Node + void validateImpl() override; + void enqueueExecImpl() override; + + // Node requirements + std::vector getRequiredFieldList() const override; + + // Point cloud description + size_t getWidth() const override; + size_t getHeight() const override { return 1; } + + // Data getters + IAnyArray::ConstPtr getFieldData(rgl_field_t field) override; + +private: + // Data containers + std::vector::type> filteredIndicesHost; + DeviceAsyncArray::type>::Ptr filteredIndices = DeviceAsyncArray::type>::create( + arrayMgr); + DeviceAsyncArray::Ptr formattedInput = DeviceAsyncArray::create(arrayMgr); + HostPinnedArray::Ptr formattedInputHost = HostPinnedArray::create(); + + // PCL members + pcl::PointCloud::Ptr toFilterPointCloud = std::make_shared>(); + pcl::PointIndices::Ptr groundIndices = std::make_shared(); + pcl::ModelCoefficients planeCoefficients; + pcl::SACSegmentation segmentation; + float groundFilterDistance; + + // RGL related members + GPUFieldDescBuilder gpuFieldDescBuilder; + std::mutex getFieldDataMutex; + mutable CacheManager cacheManager; +}; + struct VisualizePointsNode : IPointsNodeSingleInput { static const int FRAME_RATE = 60; diff --git a/extensions/pcl/src/graph/RemoveGroundPointsNode.cpp b/extensions/pcl/src/graph/RemoveGroundPointsNode.cpp new file mode 100644 index 000000000..d693f66fe --- /dev/null +++ b/extensions/pcl/src/graph/RemoveGroundPointsNode.cpp @@ -0,0 +1,153 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include + +void RemoveGroundPointsNode::setParameters(rgl_axis_t sensorUpAxis, float groundAngleThreshold, float groundDistanceThreshold, + float groundFilterDistance) +{ + this->groundFilterDistance = groundFilterDistance; + + planeCoefficients = pcl::ModelCoefficients(); // Reset coefficients (they are optimized every run) + + // Setup segmentation options + segmentation.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + segmentation.setAxis(Eigen::Vector3f(sensorUpAxis == RGL_AXIS_X ? 1.0 : 0.0, sensorUpAxis == RGL_AXIS_Y ? 1.0 : 0.0, + sensorUpAxis == RGL_AXIS_Z ? 1.0 : 0.0)); + segmentation.setEpsAngle(groundAngleThreshold); + segmentation.setOptimizeCoefficients(true); + segmentation.setMethodType(pcl::SAC_RANSAC); + segmentation.setDistanceThreshold(groundDistanceThreshold); + segmentation.setInputCloud(toFilterPointCloud); + constexpr int maxIterations = 500; + segmentation.setMaxIterations(maxIterations); +} + +void RemoveGroundPointsNode::validateImpl() +{ + IPointsNodeSingleInput::validateImpl(); + + // Needed to clear cache because fields in the pipeline may have changed + // In fact, the cache manager is no longer useful here + // To be kept/removed in some future refactor (when resolving comment in the `enqueueExecImpl`) + cacheManager.clear(); +} + +void RemoveGroundPointsNode::enqueueExecImpl() +{ + cacheManager.trigger(); + + if (input->getPointCount() == 0) { + filteredIndices->resize(0, false, false); + return; + } + + // Get formatted input data (SSE2-aligned XYZ) + FormatPointsNode::formatAsync(formattedInput, input, getRequiredFieldList(), gpuFieldDescBuilder); + formattedInputHost->copyFrom(formattedInput); + + // Copy data into pcl::PointCloud + auto pointCount = input->getPointCount(); + toFilterPointCloud->resize(pointCount); + const pcl::PointXYZ* begin = reinterpret_cast(formattedInputHost->getReadPtr()); + const pcl::PointXYZ* end = begin + pointCount; + toFilterPointCloud->assign(begin, end, pointCount); + + // Segment ground and approximate plane coefficients + segmentation.segment(*groundIndices, planeCoefficients); + + // Select ground indices (points within given distance to approximate plane model). Can be optimized (GPU?) + auto planeCoefficientsEigen = Eigen::Vector4f(planeCoefficients.values[0], planeCoefficients.values[1], + planeCoefficients.values[2], planeCoefficients.values[3]); + auto planeModel = pcl::SampleConsensusModelPerpendicularPlane(toFilterPointCloud); + planeModel.selectWithinDistance(planeCoefficientsEigen, groundFilterDistance, groundIndices->indices); + + // Compute non-ground indices. Can be optimized (GPU?) + if (groundIndices->indices.empty()) { + filteredIndicesHost.resize(pointCount); + std::iota(filteredIndicesHost.begin(), filteredIndicesHost.end(), 0); + filteredIndices->resize(0, false, false); + } else { + filteredIndicesHost.resize(pointCount - groundIndices->indices.size()); + int currentGroundIdx = 0; + int currentNonGroundIdx = 0; + for (int i = 0; i < pointCount; ++i) { + if (i == groundIndices->indices[currentGroundIdx]) { + ++currentGroundIdx; + continue; + } + filteredIndicesHost[currentNonGroundIdx] = i; + ++currentNonGroundIdx; + } + } + + filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); + + // getFieldData may be called in client's thread from rgl_graph_get_result_data + // Doing job there would be: + // - unexpected (job was supposed to be done asynchronously) + // - hard to implement: + // - to avoid blocking on yet-running graph stream, we would need do it in copy stream, which would require + // temporary rebinding DAAs to copy stream, which seems like nightmarish idea + // Therefore, once we know what fields are requested, we compute them eagerly + // This is supposed to be removed in some future refactor (e.g. when introducing LayeredSoA) + for (auto&& field : cacheManager.getKeys()) { + getFieldData(field); + } +} + +size_t RemoveGroundPointsNode::getWidth() const +{ + this->synchronize(); + return filteredIndices->getCount(); +} + +IAnyArray::ConstPtr RemoveGroundPointsNode::getFieldData(rgl_field_t field) +{ + std::lock_guard lock{getFieldDataMutex}; + + if (!cacheManager.contains(field)) { + auto fieldData = createArray(field, arrayMgr); + fieldData->resize(filteredIndices->getCount(), false, false); + cacheManager.insert(field, fieldData, true); + } + + if (!cacheManager.isLatest(field)) { + auto fieldData = cacheManager.getValue(field); + fieldData->resize(filteredIndices->getCount(), false, false); + char* outPtr = static_cast(fieldData->getRawWritePtr()); + auto fieldArray = input->getFieldData(field); + if (!isDeviceAccessible(fieldArray->getMemoryKind())) { + auto msg = fmt::format("RemoveGround requires its input to be device-accessible, {} is not", field); + throw InvalidPipeline(msg); + } + const char* inputPtr = static_cast(fieldArray->getRawReadPtr()); + gpuFilter(getStreamHandle(), filteredIndices->getCount(), filteredIndices->getReadPtr(), outPtr, inputPtr, + getFieldSize(field)); + CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); + cacheManager.setUpdated(field); + } + + return cacheManager.getValue(field); +} + +std::vector RemoveGroundPointsNode::getRequiredFieldList() const +{ + // SSE2-aligned XYZ + return {XYZ_VEC3_F32, PADDING_32}; +} diff --git a/extensions/pcl/src/tape/TapePcl.hpp b/extensions/pcl/src/tape/TapePcl.hpp new file mode 100644 index 000000000..3d1c02877 --- /dev/null +++ b/extensions/pcl/src/tape/TapePcl.hpp @@ -0,0 +1,38 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +class TapePcl +{ + static void tape_graph_write_pcd_file(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_node_points_downsample(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_node_points_visualize(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_node_points_remove_ground(const YAML::Node& yamlNode, PlaybackState& state); + + // Called once in the translation unit + static inline bool autoExtendTapeFunctions = std::invoke([]() { + std::map tapeFunctions = { + TAPE_CALL_MAPPING("rgl_graph_write_pcd_file", TapePcl::tape_graph_write_pcd_file), + TAPE_CALL_MAPPING("rgl_node_points_downsample", TapePcl::tape_node_points_downsample), + TAPE_CALL_MAPPING("rgl_node_points_visualize", TapePcl::tape_node_points_visualize), + TAPE_CALL_MAPPING("rgl_node_points_remove_ground", TapePcl::tape_node_points_remove_ground), + }; + TapePlayer::extendTapeFunctions(tapeFunctions); + return true; + }); +}; diff --git a/extensions/ros2/CMakeLists.txt b/extensions/ros2/CMakeLists.txt index b80cba496..dbd212286 100644 --- a/extensions/ros2/CMakeLists.txt +++ b/extensions/ros2/CMakeLists.txt @@ -5,15 +5,38 @@ endif() find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(radar_msgs REQUIRED) target_sources(RobotecGPULidar PRIVATE src/api/apiRos2.cpp src/graph/Ros2PublishPointsNode.cpp + src/graph/Ros2PublishPointVelocityMarkersNode.cpp + src/graph/Ros2PublishRadarScanNode.cpp ) -target_include_directories(RobotecGPULidar PUBLIC ${rclcpp_INCLUDE_DIRS} ${sensor_msgs_INCLUDE_DIRS}) -target_link_libraries(RobotecGPULidar PRIVATE ${rclcpp_LIBRARIES} ${sensor_msgs_LIBRARIES}) +target_include_directories(RobotecGPULidar PUBLIC + ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} + ${radar_msgs_INCLUDE_DIRS} +) + +# These libraries list, imported from ROS2 packages, +# contain a large number of duplicates, which results +# in linking error (LNK1170) due to exceeded line length in rsp file. +list(REMOVE_DUPLICATES radar_msgs_LIBRARIES) +list(REMOVE_DUPLICATES visualization_msgs_LIBRARIES) +list(REMOVE_DUPLICATES sensor_msgs_LIBRARIES) + +target_link_libraries(RobotecGPULidar PRIVATE + ${rclcpp_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${radar_msgs_LIBRARIES} +) target_include_directories(RobotecGPULidar + PUBLIC include PRIVATE src ) diff --git a/include/rgl/api/extensions/ros2.h b/extensions/ros2/include/rgl/api/extensions/ros2.h similarity index 75% rename from include/rgl/api/extensions/ros2.h rename to extensions/ros2/include/rgl/api/extensions/ros2.h index befbe9cef..d1cc316db 100644 --- a/include/rgl/api/extensions/ros2.h +++ b/extensions/ros2/include/rgl/api/extensions/ros2.h @@ -87,3 +87,22 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, con rgl_qos_policy_reliability_t qos_reliability, rgl_qos_policy_durability_t qos_durability, rgl_qos_policy_history_t qos_history, int32_t qos_history_depth); + +/** + * Creates or modifies Ros2PublishRadarScanNode. + * The node publishes a RadarScan message to the ROS2 topic using specified Quality of Service settings. + * The message header stamp gets time from the raytraced scene. If the scene has no time, header will get the actual time. + * Graph input: point cloud + * Graph output: point cloud + * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. + * @param topic_name Topic name to publish on. + * @param frame_id Frame this data is associated with. + * @param qos_reliability QoS reliability policy. + * @param qos_durability QoS durability policy. + * @param qos_history QoS history policy. + * @param qos_history_depth QoS history depth. If history policy is KEEP_ALL, depth is ignored but must always be non-negative. + */ +RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const char* topic_name, const char* frame_id, + rgl_qos_policy_reliability_t qos_reliability, + rgl_qos_policy_durability_t qos_durability, + rgl_qos_policy_history_t qos_history, int32_t qos_history_depth); diff --git a/extensions/ros2/install_deps.py b/extensions/ros2/install_deps.py new file mode 100755 index 000000000..a4773790b --- /dev/null +++ b/extensions/ros2/install_deps.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +import os +import platform +import sys +import subprocess +from pathlib import Path + + +class Config: + SUPPORTED_ROS_DISTROS = ["humble"] + + # Paths relative to project root + RADAR_MSGS_DIR = os.path.join("external", "radar_msgs") + RADAR_MSGS_INSTALL_DIR = os.path.join("external", "radar_msgs", "install") + RADAR_MSGS_COMMIT = "47d2f26906ef38fa15ada352aea6b5aad547781d" + + +def install_deps(): + cfg = Config() + + # Install dependencies for ROS2 extension + check_ros2_version() + install_ros2_deps(cfg) + + print("ROS2 deps installed successfully") + + +def are_deps_installed() -> bool: + cfg = Config() + return os.path.isdir(cfg.RADAR_MSGS_INSTALL_DIR) + + +def install_ros2_deps(cfg): + # Install colcon if needed + if not is_command_available("colcon --help"): + if on_windows(): + run_subprocess_command("pip install colcon-common-extensions") + else: + run_subprocess_command("sudo apt-get install -y python3-colcon-common-extensions") + # Clone radar msgs + if not os.path.isdir(cfg.RADAR_MSGS_DIR): + run_subprocess_command("ls") + run_subprocess_command( + f"git clone --single-branch --depth 1 https://github.com/ros-perception/radar_msgs.git {cfg.RADAR_MSGS_DIR}") + run_subprocess_command(f"cd {cfg.RADAR_MSGS_DIR} && git checkout {cfg.RADAR_MSGS_COMMIT} && cd ..") + # Build radar msgs + if not os.path.isdir(cfg.RADAR_MSGS_INSTALL_DIR): + original_path = Path.cwd() + os.chdir(cfg.RADAR_MSGS_DIR) + run_subprocess_command(f"colcon build") + os.chdir(original_path) + # TODO: cyclonedds rmw may be installed here (instead of manually in readme) + + +def check_ros2_version(): + if "ROS_DISTRO" not in os.environ and "AMENT_PREFIX_PATH" not in os.environ: + raise RuntimeError("ROS2 environment not found! Make sure you have sourced ROS2 setup file") + if os.environ["ROS_DISTRO"] not in Config().SUPPORTED_ROS_DISTROS: + raise RuntimeError(f"ROS distro '{os.environ['ROS_DISTRO']}' not supported. Choose one of {Config().SUPPORTED_ROS_DISTROS}") + + +def on_windows(): + return platform.system() == "Windows" + + +def run_subprocess_command(command: str, shell=True, stderr=sys.stderr, stdout=sys.stdout): + print(f"Executing command: '{command}'") + process = subprocess.Popen(command, shell=shell, stderr=stderr, stdout=stdout) + process.wait() + if process.returncode != 0: + raise RuntimeError(f"Failed to execute command: '{command}'") + + +def is_command_available(command): + process = subprocess.Popen(f"{command}", shell=True, stderr=subprocess.DEVNULL, stdout=subprocess.DEVNULL) + process.wait() + return process.returncode == 0 + + +if __name__ == "__main__": + print("Important: this script should be executed from the root of the project (e.g. `./extensions/ros2/install_deps.py`)") + + sys.exit(install_deps()) diff --git a/extensions/ros2/src/Ros2InitGuard.hpp b/extensions/ros2/src/Ros2InitGuard.hpp new file mode 100644 index 000000000..3903b5847 --- /dev/null +++ b/extensions/ros2/src/Ros2InitGuard.hpp @@ -0,0 +1,86 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +/** + * Wrapper around RGL-specific ROS2 resources shared between RGL Nodes. + * - Handles (de)initialization of rclcpp and creation of ROS2 node + * - Keeps track of ROS2 publishers to avoid creating duplicates + */ +struct Ros2InitGuard +{ + rclcpp::Node& getNode() const { return *node; } + + static inline std::shared_ptr acquire() + { + static std::weak_ptr weak{}; + std::shared_ptr shared{}; + if (auto locked = weak.lock()) { + shared = locked; + } else { + shared = std::shared_ptr(new Ros2InitGuard()); + weak = shared; + } + return shared; + } + + template + rclcpp::Publisher::SharedPtr createUniquePublisher(const std::string& topicName, const rclcpp::QoS& qos) + { + if (hasTopic(topicName)) { + auto msg = fmt::format("ROS2 publisher with the same topic name ({}) already exist!", topicName); + throw InvalidAPIArgument(msg); + } + auto publisher = node->create_publisher(topicName, qos); + publishers.insert({topicName, publisher}); + return publisher; + } + + ~Ros2InitGuard() + { + if (isRclcppInitializedByRGL) { + rclcpp::shutdown(); + isRclcppInitializedByRGL = false; + } + } + +private: + Ros2InitGuard() + { + // Check if rclcpp initialized + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + isRclcppInitializedByRGL = true; + } + node = std::make_shared(nodeName); + } + + bool hasTopic(const std::string& query) + { + for (auto it = publishers.begin(); it != publishers.end();) { + it = it->second.expired() ? publishers.erase(it) : ++it; + } + return publishers.contains(query); + } + +private: + rclcpp::Node::SharedPtr node; + bool isRclcppInitializedByRGL{false}; + std::map> publishers; + inline static std::string nodeName = "RobotecGPULidar"; +}; diff --git a/extensions/ros2/src/api/apiRos2.cpp b/extensions/ros2/src/api/apiRos2.cpp index be7a7bb9d..e4c1e6cc8 100644 --- a/extensions/ros2/src/api/apiRos2.cpp +++ b/extensions/ros2/src/api/apiRos2.cpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -50,12 +51,12 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish(rgl_node_t* node, const char* return status; } -void TapePlayer::tape_node_points_ros2_publish(const YAML::Node& yamlNode) +void TapeRos2::tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state) { - size_t nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes[nodeId] : nullptr; + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; rgl_node_points_ros2_publish(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str()); - tapeNodes.insert(std::make_pair(nodeId, node)); + state.nodes.insert(std::make_pair(nodeId, node)); } RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, const char* topic_name, const char* frame_id, @@ -80,14 +81,48 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, con return status; } -void TapePlayer::tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode) +void TapeRos2::tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state) { - size_t nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes[nodeId] : nullptr; + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; rgl_node_points_ros2_publish_with_qos(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str(), (rgl_qos_policy_reliability_t) yamlNode[3].as(), (rgl_qos_policy_durability_t) yamlNode[4].as(), (rgl_qos_policy_history_t) yamlNode[5].as(), yamlNode[6].as()); - tapeNodes.insert(std::make_pair(nodeId, node)); + state.nodes.insert(std::make_pair(nodeId, node)); +} + +RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const char* topic_name, const char* frame_id, + rgl_qos_policy_reliability_t qos_reliability, + rgl_qos_policy_durability_t qos_durability, + rgl_qos_policy_history_t qos_history, int32_t qos_history_depth) +{ + auto status = rglSafeCall([&]() { + RGL_DEBUG( + "tape_node_publish_ros2_radarscan(node={}, topic_name={}, frame_id={}, qos_reliability={}, qos_durability={}, " + "qos_history={}, qos_history_depth={})", + repr(node), topic_name, frame_id, qos_reliability, qos_durability, qos_history, qos_history_depth); + CHECK_ARG(topic_name != nullptr); + CHECK_ARG(topic_name[0] != '\0'); + CHECK_ARG(frame_id != nullptr); + CHECK_ARG(frame_id[0] != '\0'); + CHECK_ARG(qos_history_depth >= 0); + + createOrUpdateNode(node, topic_name, frame_id, qos_reliability, qos_durability, qos_history, + qos_history_depth); + }); + TAPE_HOOK(node, topic_name, frame_id, qos_reliability, qos_durability, qos_history, qos_history_depth); + return status; +} + +void TapeRos2::tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; + rgl_node_publish_ros2_radarscan(&node, yamlNode[1].as().c_str(), yamlNode[2].as().c_str(), + (rgl_qos_policy_reliability_t) yamlNode[3].as(), + (rgl_qos_policy_durability_t) yamlNode[4].as(), + (rgl_qos_policy_history_t) yamlNode[5].as(), yamlNode[6].as()); + state.nodes.insert(std::make_pair(nodeId, node)); } } diff --git a/extensions/ros2/src/graph/NodesRos2.hpp b/extensions/ros2/src/graph/NodesRos2.hpp index 58e06125e..80db5e769 100644 --- a/extensions/ros2/src/graph/NodesRos2.hpp +++ b/extensions/ros2/src/graph/NodesRos2.hpp @@ -23,8 +23,38 @@ #include #include #include +#include +#include +#include -struct Ros2PublishPointsNode : IPointsNodeSingleInput +struct Ros2Node : IPointsNodeSingleInput +{ + Ros2Node() { ros2InitGuard = Ros2InitGuard::acquire(); } + + void enqueueExecImpl() final + { + if (!rclcpp::ok()) { + throw InvalidPipeline("Unable to execute Ros2Node because ROS2 has been shut down."); + } + ros2EnqueueExecImpl(); + } + + void validateImpl() final + { + IPointsNodeSingleInput::validateImpl(); + ros2ValidateImpl(); + } + + virtual ~Ros2Node() = default; + +protected: + std::shared_ptr ros2InitGuard; + + virtual void ros2EnqueueExecImpl() = 0; + virtual void ros2ValidateImpl() = 0; +}; + +struct Ros2PublishPointsNode : Ros2Node { using Ptr = std::shared_ptr; @@ -33,25 +63,62 @@ struct Ros2PublishPointsNode : IPointsNodeSingleInput rgl_qos_policy_durability_t qosDurability = QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, rgl_qos_policy_history_t qosHistory = QOS_POLICY_HISTORY_SYSTEM_DEFAULT, int32_t qosHistoryDepth = 10); - // Node - void validateImpl() override; - void enqueueExecImpl() override; + // Ros2Node + void ros2ValidateImpl() override; + void ros2EnqueueExecImpl() override; - ~Ros2PublishPointsNode(); + ~Ros2PublishPointsNode() override = default; private: DeviceAsyncArray::Ptr inputFmtData = DeviceAsyncArray::create(arrayMgr); - std::string topicName{}; - std::string frameId{}; - rclcpp::Publisher::SharedPtr ros2Publisher; sensor_msgs::msg::PointCloud2 ros2Message; - static bool isRclcppInitializedByRGL; - static rclcpp::Node::SharedPtr ros2Node; - static std::string ros2NodeName; - static std::set ros2TopicNames; - void updateRos2Message(const std::vector& fields, bool isDense); }; + + +struct Ros2PublishPointVelocityMarkersNode : Ros2Node +{ + using Ptr = std::shared_ptr; + + void setParameters(const char* topicName, const char* frameId, rgl_field_t velocityField); + std::vector getRequiredFieldList() const override { return {XYZ_VEC3_F32, velocityField}; } + + // Ros2Node + void ros2ValidateImpl() override; + void ros2EnqueueExecImpl() override; + + ~Ros2PublishPointVelocityMarkersNode() override = default; + +private: + std::string frameId; + rclcpp::Publisher::SharedPtr linesPublisher; + HostPinnedArray::Ptr pos = HostPinnedArray::create(); + HostPinnedArray::Ptr vel = HostPinnedArray::create(); + visualization_msgs::msg::Marker marker; + rgl_field_t velocityField; + + const visualization_msgs::msg::Marker& makeLinesMarker(); +}; + +struct Ros2PublishRadarScanNode : Ros2Node +{ + void setParameters(const char* topicName, const char* frameId, rgl_qos_policy_reliability_t qosReliability, + rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory, int32_t qosHistoryDepth); + std::vector getRequiredFieldList() const override + { + return {DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32, /* placeholder for amplitude */ PADDING_32}; + } + + // Ros2Node + void ros2ValidateImpl() override; + void ros2EnqueueExecImpl() override; + +private: + radar_msgs::msg::RadarScan ros2Message; + rclcpp::Publisher::SharedPtr ros2Publisher; + DeviceAsyncArray::Ptr formattedData = DeviceAsyncArray::create(arrayMgr); + GPUFieldDescBuilder fieldDescBuilder; +}; diff --git a/extensions/ros2/src/graph/Ros2PublishPointVelocityMarkersNode.cpp b/extensions/ros2/src/graph/Ros2PublishPointVelocityMarkersNode.cpp new file mode 100644 index 000000000..94ad8df69 --- /dev/null +++ b/extensions/ros2/src/graph/Ros2PublishPointVelocityMarkersNode.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +void Ros2PublishPointVelocityMarkersNode::setParameters(const char* topicName, const char* frameId, rgl_field_t velocityField) +{ + if (velocityField != RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32 && velocityField != RGL_FIELD_ABSOLUTE_VELOCITY_VEC3_F32) { + auto msg = fmt::format("{} cannot publish fields other than `RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32` and " + "`RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32` (`{}` was requested)", + getName(), toString(velocityField)); + throw InvalidAPIArgument(msg); + } + this->frameId = frameId; + auto qos = rclcpp::QoS(10); // Use system default QoS + linesPublisher = ros2InitGuard->createUniquePublisher(topicName, qos); + this->velocityField = velocityField; +} + +void Ros2PublishPointVelocityMarkersNode::ros2ValidateImpl() +{ + if (!input->isDense()) { + throw InvalidPipeline(fmt::format("{} requires a compacted point cloud (dense)", getName())); + } +} +void Ros2PublishPointVelocityMarkersNode::ros2EnqueueExecImpl() +{ + pos->copyFrom(input->getFieldData(RGL_FIELD_XYZ_VEC3_F32)); + vel->copyFrom(input->getFieldData(velocityField)); + linesPublisher->publish(makeLinesMarker()); +} + +const visualization_msgs::msg::Marker& Ros2PublishPointVelocityMarkersNode::makeLinesMarker() +{ + marker.header.stamp = Scene::instance().getTime().has_value() ? + Scene::instance().getTime().value().asRos2Msg() : + static_cast(ros2InitGuard->getNode().get_clock()->now()); + marker.header.frame_id = this->frameId; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.32; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.02; // Line width diameter + marker.points.resize(pos->getCount() * 2); + geometry_msgs::msg::Point origin, end; + for (int i = 0; i < pos->getCount(); i++) { + origin.x = pos->at(i).x(); + origin.y = pos->at(i).y(); + origin.z = pos->at(i).z(); + end.x = origin.x + vel->at(i).x(); + end.y = origin.y + vel->at(i).y(); + end.z = origin.z + vel->at(i).z(); + marker.points[2 * i] = origin; + marker.points[2 * i + 1] = end; + } + return marker; +} \ No newline at end of file diff --git a/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp b/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp index 6a509e19b..573307157 100644 --- a/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp +++ b/extensions/ros2/src/graph/Ros2PublishPointsNode.cpp @@ -16,58 +16,34 @@ #include #include -bool Ros2PublishPointsNode::isRclcppInitializedByRGL = false; -rclcpp::Node::SharedPtr Ros2PublishPointsNode::ros2Node = nullptr; -std::string Ros2PublishPointsNode::ros2NodeName = "RobotecGPULidar"; -std::set Ros2PublishPointsNode::ros2TopicNames = {}; - void Ros2PublishPointsNode::setParameters(const char* topicName, const char* frameId, rgl_qos_policy_reliability_t qosReliability, rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory, int32_t qosHistoryDepth) { - // Check if rclcpp initialized - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - isRclcppInitializedByRGL = true; - } - - // Create ROS2 node - if (ros2Node.get() == nullptr) { - ros2Node = std::make_shared(ros2NodeName); - } - - if (ros2TopicNames.contains(topicName) && this->topicName != topicName) { - throw InvalidAPIArgument("ROS2 publisher with the same topic name already exist!"); - } - - if (ros2Publisher.get() != nullptr) { - ros2TopicNames.erase(this->topicName); - ros2Publisher.reset(); - } - - this->topicName = topicName; - this->frameId = frameId; - ros2TopicNames.insert(topicName); - - rclcpp::QoS qos = rclcpp::QoS(qosHistoryDepth); + ros2Message.header.frame_id = frameId; + auto qos = rclcpp::QoS(qosHistoryDepth); qos.reliability(static_cast(qosReliability)); qos.durability(static_cast(qosDurability)); qos.history(static_cast(qosHistory)); - - ros2Publisher = ros2Node->create_publisher(topicName, qos); + ros2Publisher = ros2InitGuard->createUniquePublisher(topicName, qos); } -void Ros2PublishPointsNode::validateImpl() +void Ros2PublishPointsNode::ros2ValidateImpl() { - IPointsNodeSingleInput::validateImpl(); if (input->getHeight() != 1) { - throw InvalidPipeline("ROS2 publish support unorganized pointclouds only"); + throw InvalidPipeline("ROS2 publish supports unorganized pointclouds only"); } + + if (!input->hasField(RGL_FIELD_DYNAMIC_FORMAT)) { + auto msg = fmt::format("{} requires a formatted point cloud", getName()); + throw InvalidPipeline(msg); + } + updateRos2Message(input->getRequiredFieldList(), input->isDense()); } -void Ros2PublishPointsNode::enqueueExecImpl() +void Ros2PublishPointsNode::ros2EnqueueExecImpl() { auto fieldData = input->getFieldData(RGL_FIELD_DYNAMIC_FORMAT)->asTyped()->asSubclass(); int count = input->getPointCount(); @@ -82,31 +58,14 @@ void Ros2PublishPointsNode::enqueueExecImpl() // For now, only default scene is supported. ros2Message.header.stamp = Scene::instance().getTime().has_value() ? Scene::instance().getTime()->asRos2Msg() : - static_cast(ros2Node->get_clock()->now()); - if (!rclcpp::ok()) { - throw std::runtime_error("Unable to publish a message because ROS2 has been shut down."); - } + static_cast(ros2InitGuard->getNode().get_clock()->now()); ros2Publisher->publish(ros2Message); } -Ros2PublishPointsNode::~Ros2PublishPointsNode() -{ - ros2TopicNames.erase(topicName); - ros2Publisher.reset(); - - if (ros2TopicNames.empty()) { - ros2Node.reset(); - - if (isRclcppInitializedByRGL) { - rclcpp::shutdown(); - isRclcppInitializedByRGL = false; - } - } -} void Ros2PublishPointsNode::updateRos2Message(const std::vector& fields, bool isDense) { - ros2Message = sensor_msgs::msg::PointCloud2(); + ros2Message.fields.clear(); int offset = 0; for (const auto& field : fields) { auto ros2fields = toRos2Fields(field); @@ -127,6 +86,8 @@ void Ros2PublishPointsNode::updateRos2Message(const std::vector& fi offset += ros2sizes[i]; } } - ros2Message.height = 1, ros2Message.point_step = offset, ros2Message.is_dense = isDense, ros2Message.is_bigendian = false, - ros2Message.header.frame_id = frameId; + ros2Message.height = 1; + ros2Message.point_step = offset; + ros2Message.is_dense = isDense; + ros2Message.is_bigendian = false; } diff --git a/extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp b/extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp new file mode 100644 index 000000000..35844dcd8 --- /dev/null +++ b/extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp @@ -0,0 +1,51 @@ +// Copyright 2024 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +void Ros2PublishRadarScanNode::setParameters(const char* topicName, const char* frameId, + rgl_qos_policy_reliability_t qosReliability, + rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory, + int32_t qosHistoryDepth) +{ + ros2Message.header.frame_id = frameId; + auto qos = rclcpp::QoS(qosHistoryDepth); + qos.reliability(static_cast(qosReliability)); + qos.durability(static_cast(qosDurability)); + qos.history(static_cast(qosHistory)); + ros2Publisher = ros2InitGuard->createUniquePublisher(topicName, qos); +} + +void Ros2PublishRadarScanNode::ros2ValidateImpl() +{ + if (input->getHeight() != 1) { + throw InvalidPipeline("ROS2 radar publish supports unorganized pointclouds only"); + } +} + +void Ros2PublishRadarScanNode::ros2EnqueueExecImpl() +{ + ros2Message.header.stamp = Scene::instance().getTime().has_value() ? + Scene::instance().getTime().value().asRos2Msg() : + static_cast(ros2InitGuard->getNode().get_clock()->now()); + std::vector fields = this->getRequiredFieldList(); + FormatPointsNode::formatAsync(formattedData, input, fields, fieldDescBuilder); + ros2Message.returns.resize(input->getPointCount()); + CHECK_CUDA(cudaMemcpyAsync(ros2Message.returns.data(), formattedData->getReadPtr(), + formattedData->getSizeOf() * formattedData->getCount(), cudaMemcpyDeviceToHost, + formattedData->getStream()->getHandle())); + CHECK_CUDA(cudaStreamSynchronize(formattedData->getStream()->getHandle())); + ros2Publisher->publish(ros2Message); +} diff --git a/extensions/ros2/src/tape/TapeRos2.hpp b/extensions/ros2/src/tape/TapeRos2.hpp new file mode 100644 index 000000000..1bc4f9325 --- /dev/null +++ b/extensions/ros2/src/tape/TapeRos2.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +class TapeRos2 +{ + static void tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state); + + // Called once in the translation unit + static inline bool autoExtendTapeFunctions = std::invoke([]() { + std::map tapeFunctions = { + TAPE_CALL_MAPPING("rgl_node_points_ros2_publish", TapeRos2::tape_node_points_ros2_publish), + TAPE_CALL_MAPPING("rgl_node_points_ros2_publish_with_qos", TapeRos2::tape_node_points_ros2_publish_with_qos), + TAPE_CALL_MAPPING("rgl_node_publish_ros2_radarscan", TapeRos2::tape_node_publish_ros2_radarscan), + }; + TapePlayer::extendTapeFunctions(tapeFunctions); + return true; + }); +}; diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt index 57182e149..c622a32eb 100644 --- a/external/CMakeLists.txt +++ b/external/CMakeLists.txt @@ -1,19 +1,7 @@ -include(FetchContent) -FetchContent_Declare( - spdlog - GIT_REPOSITORY https://github.com/gabime/spdlog.git - GIT_TAG v1.9.2 - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(spdlog) - -FetchContent_Declare( - cmake_git_version_tracking - GIT_REPOSITORY https://github.com/andrew-hardin/cmake-git-version-tracking.git - GIT_TAG 904dbda1336ba4b9a1415a68d5f203f576b696bb -) -FetchContent_MakeAvailable(cmake_git_version_tracking) +add_subdirectory("${PROJECT_SOURCE_DIR}/external/spdlog") +add_subdirectory("${PROJECT_SOURCE_DIR}/external/yaml-cpp") +add_subdirectory("${PROJECT_SOURCE_DIR}/external/cmake_git_version_tracking") # An attempt to disable targets from yaml-cpp; had difficulties with some of them # More details here: https://github.com/jbeder/yaml-cpp/issues/1158 @@ -22,13 +10,6 @@ set(YAML_CPP_BUILD_CONTRIB OFF CACHE INTERNAL "Disable yaml-cpp artifacts") set(YAML_CPP_BUILD_TOOLS OFF CACHE INTERNAL "Disable yaml-cpp artifacts") set(YAML_CPP_INSTALL OFF CACHE INTERNAL "Disable yaml-cpp artifacts") set(YAML_CPP_FORMAT_SOURCE OFF CACHE INTERNAL "Disable yaml-cpp artifacts") -FetchContent_Declare( - yaml-cpp - GIT_REPOSITORY https://github.com/jbeder/yaml-cpp.git - GIT_TAG yaml-cpp-0.7.0 - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(yaml-cpp) set_property(TARGET yaml-cpp PROPERTY POSITION_INDEPENDENT_CODE ON) # Disable compilation warnings for dependencies @@ -50,16 +31,11 @@ if (UNIX) set_target_properties(spdlog PROPERTIES POSITION_INDEPENDENT_CODE ON) endif() -if (${RGL_BUILD_TESTS}) +if (${RGL_BUILD_TESTS} OR ${RGL_BUILD_TAPED_TESTS}) + add_subdirectory("${PROJECT_SOURCE_DIR}/external/googletest") + set(INSTALL_GTEST OFF CACHE INTERNAL "Disable installation of googletest") - FetchContent_Declare( - googletest - GIT_REPOSITORY https://github.com/google/googletest - GIT_TAG release-1.11.0 - GIT_SHALLOW TRUE - ) # For Windows: Prevent overriding the parent project's compiler/linker settings set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) - FetchContent_MakeAvailable(googletest) endif() diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index 941efa139..59116966b 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -18,12 +18,19 @@ #include #include +#ifdef __cplusplus +#include +#endif + #ifdef __cplusplus #define NO_MANGLING extern "C" #else // NOT __cplusplus #define NO_MANGLING #endif +#if defined RGL_STATIC +#define RGL_VISIBLE +#else #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define RGL_VISIBLE __attribute__((dllexport)) @@ -38,12 +45,13 @@ #define RGL_VISIBLE #endif #endif // _WIN32 || __CYGWIN__ +#endif // RGL_STATIC #define RGL_API NO_MANGLING RGL_VISIBLE #define RGL_VERSION_MAJOR 0 -#define RGL_VERSION_MINOR 16 -#define RGL_VERSION_PATCH 2 +#define RGL_VERSION_MINOR 17 +#define RGL_VERSION_PATCH 0 // Invalid Entity ID is assign to rays that does not hit any Entity. // Cannot be assigned to Mesh manually. It is reserved for internal raytracing use. @@ -61,6 +69,12 @@ typedef struct float value[2]; } rgl_vec2f; +#ifdef __cplusplus +static_assert(sizeof(rgl_vec2f) == 2 * sizeof(float)); +static_assert(std::is_trivial::value); +static_assert(std::is_standard_layout::value); +#endif + /** * Three consecutive 32-bit floats. */ @@ -69,8 +83,10 @@ typedef struct float value[3]; } rgl_vec3f; -#ifndef __cplusplus +#ifdef __cplusplus static_assert(sizeof(rgl_vec3f) == 3 * sizeof(float)); +static_assert(std::is_trivial::value); +static_assert(std::is_standard_layout::value); #endif /** @@ -81,6 +97,12 @@ typedef struct int32_t value[3]; } rgl_vec3i; +#ifdef __cplusplus +static_assert(sizeof(rgl_vec3i) == 3 * sizeof(int32_t)); +static_assert(std::is_trivial::value); +static_assert(std::is_standard_layout::value); +#endif + /** * Row-major matrix with 3 rows and 4 columns of 32-bit floats. * Right-handed coordinate system. @@ -90,6 +112,45 @@ typedef struct float value[3][4]; } rgl_mat3x4f; +#ifdef __cplusplus +static_assert(sizeof(rgl_mat3x4f) == 3 * 4 * sizeof(float)); +static_assert(std::is_trivial::value); +static_assert(std::is_standard_layout::value); +#endif + +/** + * Radar parameters applied at a given distance range. + */ +typedef struct +{ + /** + * The beginning distance range for the parameters. + */ + float begin_distance; + /** + * The end range distance for the parameters. + */ + float end_distance; + /** + * The maximum distance difference to create a new radar detection (in simulation units). + */ + float distance_separation_threshold; + /** + * The maximum radial speed difference to create a new radar detection (in simulation units) + */ + float radial_speed_separation_threshold; + /** + * The maximum azimuth difference to create a new radar detection (in radians). + */ + float azimuth_separation_threshold; +} rgl_radar_scope_t; + +#ifdef __cplusplus +static_assert(sizeof(rgl_radar_scope_t) == 5 * sizeof(float)); +static_assert(std::is_trivial::value); +static_assert(std::is_standard_layout::value); +#endif + /** * Represents on-GPU Mesh that can be referenced by Entities on the Scene. * Each Mesh can be referenced by any number of Entities on different Scenes. @@ -109,7 +170,9 @@ typedef struct Entity* rgl_entity_t; typedef struct Texture* rgl_texture_t; /** - * TODO(prybicki) + * Opaque handle for a computational graph node in RGL. + * Represents a single computational step, e.g. introducing gaussian noise, raytracing or downsampling. + * Nodes form a directed acyclic graph, dictating execution order. */ typedef struct Node* rgl_node_t; @@ -129,6 +192,7 @@ typedef enum : int32_t RGL_EXTENSION_PCL = 0, RGL_EXTENSION_ROS2 = 1, RGL_EXTENSION_UDP = 2, + RGL_EXTENSION_SNOW = 3, RGL_EXTENSION_COUNT } rgl_extension_t; @@ -245,14 +309,78 @@ typedef enum : int32_t RGL_FIELD_XYZ_VEC3_F32 = 1, RGL_FIELD_INTENSITY_F32, RGL_FIELD_IS_HIT_I32, + RGL_FIELD_IS_GROUND_I32, RGL_FIELD_RAY_IDX_U32, - RGL_FIELD_POINT_IDX_U32, RGL_FIELD_ENTITY_ID_I32, RGL_FIELD_DISTANCE_F32, + /** + * Azimuth angle of the hit point in radians. + * Currently only compatible with engines that generate rays as follows: + * uses a left-handed coordinate system, rotation applies in ZXY order, up vector is Y, forward vector is Z + */ RGL_FIELD_AZIMUTH_F32, + /** + * Elevation angle of the hit point in radians. + * Currently only compatible with engines that generate rays as follows: + * uses a left-handed coordinate system, rotation applies in ZXY order, up vector is Y, forward vector is Z + */ + RGL_FIELD_ELEVATION_F32, RGL_FIELD_RING_ID_U16, RGL_FIELD_RETURN_TYPE_U8, RGL_FIELD_TIME_STAMP_F64, + + /** + * Velocity of the hit point on the entity. + * It depends on entity's + * - linear velocity + * - angular velocity + * - mesh deformations (e.g. skinning) + * The aforementioned are inferred from calls to `rgl_entity_set_pose`, `rgl_scene_set_time` and `rgl_mesh_update_vertices`. + */ + RGL_FIELD_ABSOLUTE_VELOCITY_VEC3_F32, + + /** + * Velocity of the hit point on the entity, in the coordinate frame of rays source. + */ + RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32, + + /** + * Scalar describing distance increase per second between hit point and ray source. + */ + RGL_FIELD_RADIAL_SPEED_F32, + + /** + * Radar-specific fields. At the moment, placeholders only to implement in the future. + */ + RGL_FIELD_POWER_F32, + RGL_FIELD_RCS_F32, // Radar cross-section + RGL_FIELD_NOISE_F32, + RGL_FIELD_SNR_F32, // Signal-to-noise ratio + + /** + * Normal vector of the mesh triangle where the hit-point is located. + * Assumes right-hand rule of vertices ordering. + */ + RGL_FIELD_NORMAL_VEC3_F32, + + /** + * Incident angle of the ray hitting the mesh triangle in radians. + * In range [0, PI/2) rad, where 0 means the ray hit the triangle perpendicularly. + */ + RGL_FIELD_INCIDENT_ANGLE_F32, + + /** + * 3x4 matrix describing pose of the ray in the world coordinate system. + */ + RGL_FIELD_RAY_POSE_MAT3x4_F32, + + /** + * Lidar reflective value. Similar to the `RGL_FIELD_INTENSITY_F32` but set as a single value for the entire entity. + * Could be replaced with `RGL_FIELD_INTENSITY_F32` and a 1x1 texture when float-type texture will be supported. + * For non-hit points zero is assigned. + */ + RGL_FIELD_LASER_RETRO_F32, + // Dummy fields RGL_FIELD_PADDING_8 = 1024, RGL_FIELD_PADDING_16, @@ -350,12 +478,21 @@ RGL_API rgl_status_t rgl_mesh_destroy(rgl_mesh_t mesh); /** * Updates Mesh vertex data. The number of vertices must not change. * This function is intended to update animated Meshes. + * Should be called after rgl_scene_set_time to ensure proper velocity computation. * @param mesh Mesh to modify * @param vertices An array of rgl_vec3f or binary-compatible data representing Mesh vertices * @param vertex_count Number of elements in the vertices array. It must be equal to the original vertex count! */ RGL_API rgl_status_t rgl_mesh_update_vertices(rgl_mesh_t mesh, const rgl_vec3f* vertices, int32_t vertex_count); +/** + * Assigns value true to out_alive if the given mesh is known and has not been destroyed, + * assigns value false otherwise. + * @param mesh Mesh to check if alive + * @param out_alive Boolean set to indicate if alive + */ +RGL_API rgl_status_t rgl_mesh_is_alive(rgl_mesh_t mesh, bool* out_alive); + /******************************** ENTITY ********************************/ /** @@ -376,6 +513,7 @@ RGL_API rgl_status_t rgl_entity_destroy(rgl_entity_t entity); /** * Changes transform (position, rotation, scaling) of the given Entity. + * Should be called after rgl_scene_set_time to ensure proper velocity computation. * @param entity Entity to modify * @param transform Pointer to rgl_mat3x4f (or binary-compatible data) representing desired (Entity -> world) coordinate system transform. */ @@ -391,10 +529,27 @@ RGL_API rgl_status_t rgl_entity_set_id(rgl_entity_t entity, int32_t id); /** * Assign intensity texture to the given Entity. The assumption is that the Entity can hold only one intensity texture. * @param entity Entity to modify. - * @apram texture Texture to assign. + * @param texture Texture to assign. */ RGL_API rgl_status_t rgl_entity_set_intensity_texture(rgl_entity_t entity, rgl_texture_t texture); +/** + * Set laser retro value for the given Entity. + * The value can be retrieved from `RGL_FIELD_LASER_RETRO_F32` point cloud field. + * Default retro for the Entity is zero. + * @param entity Entity to modify. + * @param retro Laser retro value to set. + */ +RGL_API rgl_status_t rgl_entity_set_laser_retro(rgl_entity_t entity, float retro); + +/** + * Assigns value true to out_alive if the given entity is known and has not been destroyed, + * assigns value false otherwise. + * @param entity Entity to check if alive + * @param out_alive Boolean set to indicate if alive + */ +RGL_API rgl_status_t rgl_entity_is_alive(rgl_entity_t entity, bool* out_alive); + /******************************* TEXTURE *******************************/ /** @@ -410,16 +565,25 @@ RGL_API rgl_status_t rgl_texture_create(rgl_texture_t* out_texture, const void* /** * Informs that the given texture will be no longer used. * The texture will be destroyed after all referring Entities are destroyed. - * @param mesh Texture to be marked as no longer needed + * @param texture Texture to be marked as no longer needed */ RGL_API rgl_status_t rgl_texture_destroy(rgl_texture_t texture); +/** + * Assigns value true to out_alive if the given texture is known and has not been destroyed, + * assigns value false otherwise. + * @param texture Texture to check if alive + * @param out_alive Boolean set to indicate if alive + */ +RGL_API rgl_status_t rgl_texture_is_alive(rgl_texture_t texture, bool* out_alive); + /******************************** SCENE ********************************/ /** * Sets time for the given Scene. * Time indicates a specific point when the ray trace is performed in the simulation timeline. - * Timestamp is used to fill field RGL_FIELD_TIME_STAMP_F64 or for ROS2 publishing. + * Calling this function before updating entities/meshes is required to compute velocity of the hit points. + * Timestamp may be also used to fill field RGL_FIELD_TIME_STAMP_F64 or for ROS2 publishing. * @param scene Scene where time will be set. Pass NULL to use the default Scene. * @param nanoseconds Timestamp in nanoseconds. */ @@ -498,6 +662,7 @@ RGL_API rgl_status_t rgl_node_rays_transform(rgl_node_t* node, const rgl_mat3x4f */ RGL_API rgl_status_t rgl_node_points_transform(rgl_node_t* node, const rgl_mat3x4f* transform); +// TODO: remove scene parameter here and in other API calls /** * Creates or modifies RaytraceNode. * The Node performs GPU-accelerated raytracing on the given Scene. @@ -510,23 +675,41 @@ RGL_API rgl_status_t rgl_node_points_transform(rgl_node_t* node, const rgl_mat3x RGL_API rgl_status_t rgl_node_raytrace(rgl_node_t* node, rgl_scene_t scene); /** - * Creates or modifies RaytraceNode. - * The same as rgl_node_raytrace, but it applies velocity distortion additionally. - * To perform raytrace with velocity distortion the time offsets must be set to the rays (using rgl_node_rays_set_time_offsets). - * The velocities passed to that node must be in the local coordinate frame in which rays are described. + * Modifies RaytraceNode to apply sensor velocity. + * Necessary for velocity distortion or calculating fields: RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32 and RGL_FIELD_RADIAL_SPEED_F32. + * Relative velocity calculation: + * To calculate relative velocity the pipeline must allow to compute absolute velocities. For more details refer to API calls documentation: + * `rgl_scene_set_time`, `rgl_entity_set_pose`, and `rgl_mesh_update_vertices` + * @param node RaytraceNode to modify + * @param linear_velocity 3D vector for linear velocity in units per second. + * @param angular_velocity 3D vector for angular velocity in radians per second (roll, pitch, yaw). + */ +RGL_API rgl_status_t rgl_node_raytrace_configure_velocity(rgl_node_t node, const rgl_vec3f* linear_velocity, + const rgl_vec3f* angular_velocity); + +/** + * Modifies RaytraceNode to apply sensor distortion. + * Requires time offsets set to rays using rgl_node_rays_set_time_offsets. * NOTE: * The distortion takes into account only sensor velocity. The velocity of the objects being scanned by the sensor is not considered. - * Graph input: rays - * Graph output: point cloud (sparse) - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param scene Handle to a scene to perform raytracing on. Pass null to use the default scene - * @param linear_velocity Pointer to a single 3D vector describing the linear velocity of the sensor. - * The velocity is in units per second. - * @param angular_velocity Pointer to a single 3D vector describing the delta angular velocity of the sensor in euler angles (roll, pitch, yaw). - * The velocity is in radians per second. + * Use rgl_node_raytrace_configure_velocity to set sensor velocity. + * @param node RaytraceNode to modify + * @param enable If true, velocity distortion feature will be enabled. + */ +RGL_API rgl_status_t rgl_node_raytrace_configure_distortion(rgl_node_t node, bool enable); + +/** + * Modifies RaytraceNode to set non-hit values for distance. + * Default non-hit value for the RGL_FIELD_DISTANCE_F32 field is set to infinity. + * This function allows to set custom values: + * - for non-hits closer than a minimum range (`nearDistance`), + * - for non-hits beyond a maximum range (`farDistance`). + * Concurrently, it computes the RGL_FIELD_XYZ_VEC3_F32 field for these non-hit scenarios based on these distances, along with ray origin and direction. + * @param node RaytraceNode to modify. + * @param nearDistance Distance value for non-hits closer than minimum range. + * @param farDistance Distance value for non-hits beyond maximum range. */ -RGL_API rgl_status_t rgl_node_raytrace_with_distortion(rgl_node_t* node, rgl_scene_t scene, const rgl_vec3f* linear_velocity, - const rgl_vec3f* angular_velocity); +RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float nearDistance, float farDistance); /** * Creates or modifies FormatPointsNode. @@ -558,7 +741,20 @@ RGL_API rgl_status_t rgl_node_points_yield(rgl_node_t* node, const rgl_field_t* * Graph output: point cloud (compacted) * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified. */ -RGL_API rgl_status_t rgl_node_points_compact(rgl_node_t* node); +RGL_API [[deprecated("Use rgl_node_points_compact_by_field(rgl_node_t* node, rgl_field_t field) instead.")]] rgl_status_t +rgl_node_points_compact(rgl_node_t* node); + +/** + * Creates or modifies CompactPointsByFieldNode. + * The Node removes points if the given field is set to a non-zero value. + * Currently supported fields are RGL_FIELD_IS_HIT_I32 and RGL_FIELD_IS_GROUND_I32. + * In other words, it converts a point cloud into a dense one. + * Graph input: point cloud + * Graph output: point cloud (compacted) + * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified. + * @param field Field by which points will be removed. Has to be RGL_FIELD_IS_HIT_I32 or RGL_FIELD_IS_GROUND_I32. + */ +RGL_API rgl_status_t rgl_node_points_compact_by_field(rgl_node_t* node, rgl_field_t field); /** * Creates or modifies SpatialMergePointsNode. @@ -611,6 +807,47 @@ RGL_API rgl_status_t rgl_node_points_temporal_merge(rgl_node_t* node, const rgl_ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* points, int32_t points_count, const rgl_field_t* fields, int32_t field_count); +/** + * Creates or modifies RadarPostprocessPointsNode. + * The Node processes point cloud to create radar-like output. + * The point cloud is reduced by clustering input based on hit-point attributes: distance, radial speed and azimuth. + * Some radar parameters may vary for different distance ranges, as radars may employ multiple frequency bands. + * For this reason, the configuration allows the definition of multiple radar scopes of parameters on the assumption that: + * - in case of scopes that overlap, the first matching one will be used + * - if the point is not within any of the radar scopes, it will be rejected + * The output consists of the collection of one point per cluster (the closest to the azimuth and elevation center). + * Graph input: point cloud + * Graph output: point cloud + * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified. + * @param radar_scopes Array of radar scopes of parameters. See `rgl_radar_scope_t` for more details. + * @param radar_scopes_count Number of elements in the `radar_scopes` array. + * @param ray_azimuth_step The azimuth step between rays (in radians). + * @param ray_elevation_step The elevation step between rays (in radians). + * @param frequency The operating frequency of the radar (in Hz). + * @param power_transmitted The power transmitted by the radar (in dBm). + * @param cumulative_device_gain The gain of the radar's antennas and any other gains of the device (in dBi). + * @param received_noise_mean The mean of the received noise (in dB). + * @param received_noise_st_dev The standard deviation of the received noise (in dB). + */ +RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, + int32_t radar_scopes_count, float ray_azimuth_step, + float ray_elevation_step, float frequency, float power_transmitted, + float cumulative_device_gain, float received_noise_mean, + float received_noise_st_dev); + +/** + * Creates or modifies FilterGroundPointsNode. + * The Node adds RGL_FIELD_IS_GROUND_I32 which indicates the point is on the ground. Points are not removed. + * Ground points are defined as those located below the sensor with a normal vector pointing upwards at an angle smaller than the threshold. + * Graph input: point cloud + * Graph output: point cloud + * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified. + * @param sensor_up_vector Pointer to single Vec3 describing up vector of depended frame. + * @param ground_angle_threshold The maximum angle between the sensor's ray and the normal vector of the hit point in radians. + */ +RGL_API rgl_status_t rgl_node_points_filter_ground(rgl_node_t* node, const rgl_vec3f* sensor_up_vector, + float ground_angle_threshold); + /** * Creates or modifies GaussianNoiseAngularRaysNode. * Applies angular noise to the rays before raycasting. @@ -658,6 +895,14 @@ RGL_API rgl_status_t rgl_node_gaussian_noise_angular_hitpoint(rgl_node_t* node, RGL_API rgl_status_t rgl_node_gaussian_noise_distance(rgl_node_t* node, float mean, float st_dev_base, float st_dev_rise_per_meter); +/** + * Assigns value true to out_alive if the given node is known and has not been destroyed, + * assigns value false otherwise. + * @param node Node to check if alive + * @param out_alive Boolean set to indicate if alive + */ +RGL_API rgl_status_t rgl_node_is_alive(rgl_node_t node, bool* out_alive); + /******************************** GRAPH ********************************/ /** diff --git a/install_deps.py b/install_deps.py new file mode 100755 index 000000000..c3391cbc1 --- /dev/null +++ b/install_deps.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +import os +import sys +import subprocess + + +class Config: + # Paths relative to project root + SPDLOG_DIR = os.path.join("external", "spdlog") + SPDLOG_VERSION = "v1.9.2" + + CMAKE_GIT_VERSION_TRACKING_DIR = os.path.join("external", "cmake_git_version_tracking") + CMAKE_GIT_VERSION_TRACKING_VERSION = "904dbda1336ba4b9a1415a68d5f203f576b696bb" + + YAML_CPP_DIR = os.path.join("external", "yaml-cpp") + YAML_CPP_VERSION = "yaml-cpp-0.7.0" + + GOOGLETEST_DIR = os.path.join("external", "googletest") + GOOGLETEST_VERSION = "release-1.11.0" + + +def install_deps(): + cfg = Config() + + # Go to script directory + os.chdir(sys.path[0]) + + if not os.path.isdir(cfg.SPDLOG_DIR): + run_subprocess_command( + f"git clone -b {cfg.SPDLOG_VERSION} --single-branch --depth 1 https://github.com/gabime/spdlog.git {cfg.SPDLOG_DIR}") + + if not os.path.isdir(cfg.CMAKE_GIT_VERSION_TRACKING_DIR): + run_subprocess_command( + f"git clone --single-branch https://github.com/andrew-hardin/cmake-git-version-tracking.git {cfg.CMAKE_GIT_VERSION_TRACKING_DIR}") + run_subprocess_command( + f"cd {cfg.CMAKE_GIT_VERSION_TRACKING_DIR} && git reset --hard {cfg.CMAKE_GIT_VERSION_TRACKING_VERSION}") + os.chdir(sys.path[0]) # Back to script directory + + if not os.path.isdir(cfg.YAML_CPP_DIR): + run_subprocess_command( + f"git clone -b {cfg.YAML_CPP_VERSION} --single-branch --depth 1 https://github.com/jbeder/yaml-cpp.git {cfg.YAML_CPP_DIR}") + + if not os.path.isdir(cfg.GOOGLETEST_DIR): + run_subprocess_command( + f"git clone -b {cfg.GOOGLETEST_VERSION} --single-branch --depth 1 https://github.com/google/googletest {cfg.GOOGLETEST_DIR}") + + print("RGL deps installed successfully") + + +def are_deps_installed() -> bool: + cfg = Config() + return os.path.isdir(cfg.SPDLOG_DIR) \ + and os.path.isdir(cfg.CMAKE_GIT_VERSION_TRACKING_DIR) \ + and os.path.isdir(cfg.YAML_CPP_DIR) \ + and os.path.isdir(cfg.GOOGLETEST_DIR) + + +def run_subprocess_command(command: str, shell=True, stderr=sys.stderr, stdout=sys.stdout): + print(f"Executing command: '{command}'") + process = subprocess.Popen(command, shell=shell, stderr=stderr, stdout=stdout) + process.wait() + if process.returncode != 0: + raise RuntimeError(f"Failed to execute command: '{command}'") + + +if __name__ == "__main__": + sys.exit(install_deps()) diff --git a/ros2_standalone/CMakeLists.txt b/ros2_standalone/CMakeLists.txt index 4b99247d5..d823c89c2 100644 --- a/ros2_standalone/CMakeLists.txt +++ b/ros2_standalone/CMakeLists.txt @@ -16,7 +16,7 @@ set(REQ_STANDALONE_LIBS "") set(REQ_THIRD_PARTY_STANDALONE_LIBS "") set(REQ_STANDALONE_DLLS "") -set(INSTALL_DESTINATION_DIR "ros2_standalone") +set(INSTALL_DESTINATION_DIR "lib/ros2_standalone") # Extend REQ_THIRD_PARTY_STANDALONE_LIBS with _library_name third party dependencies macro(get_standalone_third_party_dependencies _library_name) @@ -104,6 +104,17 @@ macro(get_standalone_dependencies _library_name) endif() endif() + # If library is msgs, fetch all targets to get libraries for dynamic type support + # Those libraries are not listed in _LIBRARIES (which stands for libraries to link against to use ) + if(${_library_name} MATCHES ".*\_msgs$") + foreach(entry ${${_library_name}_TARGETS}) + fetch_target_lib(${entry}) + string(REGEX REPLACE "::" "_" entry_normalized ${entry}) + list(APPEND REQ_STANDALONE_LIBS + ${${entry_normalized}_LIB_PATH}) + endforeach() + endif() + # Get spdlog and dependency if(UNIX AND "${_library_name}" STREQUAL "spdlog") include(${${_library_name}_CONFIG}) @@ -148,8 +159,9 @@ macro(install_standalone_dependencies) string(REGEX REPLACE "\.lib$" ".dll" dll_path ${bin_path}) list(APPEND REQ_STANDALONE_DLLS ${dll_path}) endforeach() + list(REMOVE_DUPLICATES REQ_STANDALONE_DLLS) install(FILES ${REQ_STANDALONE_DLLS} - DESTINATION ${INSTALL_DESTINATION_DIR} + DESTINATION ${INSTALL_DESTINATION_DIR} ) elseif(UNIX) set(_resolvedFiles "") @@ -160,7 +172,7 @@ macro(install_standalone_dependencies) endforeach() install(FILES ${_resolvedFiles} - DESTINATION ${INSTALL_DESTINATION_DIR} + DESTINATION ${INSTALL_DESTINATION_DIR} ) # Fix soversion files @@ -199,6 +211,7 @@ set(ros2_standalone_libs rclcpp sensor_msgs visualization_msgs + radar_msgs rmw_implementation rmw_dds_common FastRTPS diff --git a/setup.py b/setup.py index d9f21f855..f29f55260 100755 --- a/setup.py +++ b/setup.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 +import sys +sys.dont_write_bytecode = True import os import platform import sys @@ -7,6 +9,10 @@ import shutil import argparse +import install_deps as core_deps +from extensions.ros2 import install_deps as ros2_deps +from extensions.pcl import install_deps as pcl_deps + class Config: # Default values for Linux @@ -14,24 +20,17 @@ class Config: CUDA_MIN_VER_MINOR = 7 CUDA_MIN_VER_PATCH = 0 CMAKE_GENERATOR = "'Unix Makefiles'" - VCPKG_INSTALL_DIR = os.path.join("external", "vcpkg") - VCPKG_TAG = "2023.08.09" - VCPKG_EXEC = "vcpkg" - VCPKG_BOOTSTRAP = "bootstrap-vcpkg.sh" - VCPKG_TRIPLET = "x64-linux" + + RGL_BLOBS_DIR = os.path.join("external", "rgl_blobs") + RGL_BLOBS_REPO = "git@github.com:RobotecAI/RGL-blobs.git" + RGL_BLOBS_BRANCH = "main" def __init__(self): # Platform-dependent configuration - if inside_docker(): - self.VCPKG_INSTALL_DIR = os.path.join("/rgldep", "vcpkg") - if on_windows(): self.CUDA_MIN_VER_MINOR = 4 self.CUDA_MIN_VER_PATCH = 152 # patch for CUDA 11.4 Update 4 self.CMAKE_GENERATOR = "Ninja" - self.VCPKG_EXEC = "vcpkg.exe" - self.VCPKG_BOOTSTRAP = "bootstrap-vcpkg.bat" - self.VCPKG_TRIPLET = "x64-windows" def main(): @@ -40,8 +39,14 @@ def main(): parser = argparse.ArgumentParser(description="Helper script to build RGL.") parser.add_argument("--build-dir", type=str, default="build", help="Path to build directory. Default: 'build'") + parser.add_argument("--install-deps", action='store_true', + help="Install dependencies for RGL and exit") parser.add_argument("--install-pcl-deps", action='store_true', help="Install dependencies for PCL extension and exit") + parser.add_argument("--install-ros2-deps", action='store_true', + help="Install dependencies for ROS2 extension and exit") + parser.add_argument("--fetch-rgl-blobs", action='store_true', + help="Fetch RGL blobs and exit (repo used for storing closed-source testing data)") parser.add_argument("--clean-build", action='store_true', help="Remove build directory before cmake") parser.add_argument("--with-pcl", action='store_true', @@ -52,6 +57,8 @@ def main(): help="Build RGL with ROS2 extension and install all dependent ROS2 libraries additionally") parser.add_argument("--with-udp", action='store_true', help="Build RGL with UDP extension (closed-source extension)") + parser.add_argument("--with-snow", action='store_true', + help="Build RGL with snow simulation extension (closed-source extension)") parser.add_argument("--cmake", type=str, default="", help="Pass arguments to cmake. Usage: --cmake=\"args...\"") if on_linux(): @@ -59,58 +66,61 @@ def main(): help="Pass arguments to make. Usage: --make=\"args...\". Defaults to \"-j \"") parser.add_argument("--lib-rpath", type=str, nargs='*', help="Add run-time search path(s) for RGL library. $ORIGIN (actual library path) is added by default.") + parser.add_argument("--build-taped-test", action='store_true', + help="Build taped test (requires RGL blobs repo in runtime)") if on_windows(): parser.add_argument("--ninja", type=str, default=f"-j{os.cpu_count()}", dest="build_args", help="Pass arguments to ninja. Usage: --ninja=\"args...\". Defaults to \"-j \"") args = parser.parse_args() + # ROS2 standalone obviously implies ROS2 + if args.with_ros2_standalone: + args.with_ros2 = True + + # Go to script directory + os.chdir(sys.path[0]) + + # Install RGL dependencies + if args.install_deps: + core_deps.install_deps() + return 0 + # Install dependencies for PCL extension if args.install_pcl_deps: - # Clone vcpkg - if not os.path.isdir(cfg.VCPKG_INSTALL_DIR): - if on_linux() and not inside_docker(): # Inside docker already installed - print("Installing dependencies for vcpkg...") - run_system_command("sudo apt update") - run_system_command("sudo apt install git curl zip unzip tar freeglut3-dev libglew-dev libglfw3-dev") - run_subprocess_command(f"git clone -b {cfg.VCPKG_TAG} --single-branch --depth 1 https://github.com/microsoft/vcpkg {cfg.VCPKG_INSTALL_DIR}") - # Bootstrap vcpkg - if not os.path.isfile(os.path.join(cfg.VCPKG_INSTALL_DIR, cfg.VCPKG_EXEC)): - run_subprocess_command(f"{os.path.join(cfg.VCPKG_INSTALL_DIR, cfg.VCPKG_BOOTSTRAP)}") - - # Install dependencies via vcpkg - run_subprocess_command(f"{os.path.join(cfg.VCPKG_INSTALL_DIR, cfg.VCPKG_EXEC)} install --clean-after-build pcl[core,visualization]:{cfg.VCPKG_TRIPLET}") + pcl_deps.install_deps() + return 0 + + # Install dependencies for ROS2 extension + if args.install_ros2_deps: + ros2_deps.install_deps() + return 0 + + # Install dependencies for ROS2 extension + if args.fetch_rgl_blobs: + fetch_rgl_blobs_repo(cfg) return 0 # Check CUDA - def is_cuda_version_ok(): - nvcc_process = subprocess.run("nvcc --version", shell=True, stdout=subprocess.PIPE) - nvcc_ver_match = re.search("V[0-9]+.[0-9]+.[0-9]+", nvcc_process.stdout.decode("utf-8")) - if not nvcc_ver_match: - raise RuntimeError("CUDA not found") - major = int(nvcc_ver_match[0].split(".")[0][1:]) # [1:] to remove char 'v' - minor = int(nvcc_ver_match[0].split(".")[1]) - patch = int(nvcc_ver_match[0].split(".")[2]) - print(f"Found CUDA {major}.{minor}.{patch}") - for (actual, expected) in [(major, cfg.CUDA_MIN_VER_MAJOR), (minor, cfg.CUDA_MIN_VER_MINOR), (patch, cfg.CUDA_MIN_VER_PATCH)]: - if actual > expected: - return True - if actual < expected: - return False - return True - - if not is_cuda_version_ok(): - raise RuntimeError(f"CUDA version not supported! Get CUDA {cfg.CUDA_MIN_VER_MAJOR}.{cfg.CUDA_MIN_VER_MINOR}.{cfg.CUDA_MIN_VER_PATCH}+") + if not is_cuda_version_ok(cfg): + raise RuntimeError( + f"CUDA version not supported! Get CUDA {cfg.CUDA_MIN_VER_MAJOR}.{cfg.CUDA_MIN_VER_MINOR}.{cfg.CUDA_MIN_VER_PATCH}+") # Check OptiX_INSTALL_DIR if os.environ["OptiX_INSTALL_DIR"] == "": raise RuntimeError("OptiX not found! Make sure you have exported environment variable OptiX_INSTALL_DIR") - # Check extension requirements - if args.with_pcl and not os.path.isdir(cfg.VCPKG_INSTALL_DIR): - raise RuntimeError("PCL extension requires dependencies to be installed: run this script with --install-pcl-deps flag") + # Check if dependencies are installed + if not core_deps.are_deps_installed(): + raise RuntimeError( + "RGL requires dependencies to be installed: run this script with --install-deps flag") - # Go to script directory - os.chdir(sys.path[0]) + if args.with_pcl and not pcl_deps.are_deps_installed(): + raise RuntimeError( + "PCL extension requires dependencies to be installed: run this script with --install-pcl-deps flag") + + if args.with_ros2 and not ros2_deps.are_deps_installed(): + raise RuntimeError( + "ROS2 extension requires radar_msgs to be built: run this script with --install-ros2-deps flag") # Prepare build directory if args.clean_build and os.path.isdir(args.build_dir): @@ -122,15 +132,22 @@ def is_cuda_version_ok(): if on_windows(): os.environ["Path"] = os.environ["Path"] + ";" + os.path.join(os.getcwd(), args.build_dir) + if args.with_ros2: + cfg_ros2 = ros2_deps.Config() + # Source environment for additional packages + # ROS2 itself must be sourced by the user, because its location is unknown to this script + ros2_deps.check_ros2_version() + setup = "setup.bat" if on_windows() else "setup.sh" + source_environment(os.path.join(os.getcwd(), cfg_ros2.RADAR_MSGS_INSTALL_DIR, setup)) + # Build - if args.with_ros2_standalone: - args.with_ros2 = True cmake_args = [ - f"-DCMAKE_TOOLCHAIN_FILE={os.path.join(cfg.VCPKG_INSTALL_DIR, 'scripts', 'buildsystems', 'vcpkg.cmake') if args.with_pcl else ''}", - f"-DVCPKG_TARGET_TRIPLET={cfg.VCPKG_TRIPLET if args.with_pcl else ''}", + f"-DCMAKE_TOOLCHAIN_FILE={os.path.join(pcl_deps.Config().VCPKG_DIR, 'scripts', 'buildsystems', 'vcpkg.cmake') if args.with_pcl else ''}", + f"-DVCPKG_TARGET_TRIPLET={pcl_deps.Config().VCPKG_TRIPLET if args.with_pcl else ''}", f"-DRGL_BUILD_PCL_EXTENSION={'ON' if args.with_pcl else 'OFF'}", f"-DRGL_BUILD_ROS2_EXTENSION={'ON' if args.with_ros2 else 'OFF'}", f"-DRGL_BUILD_UDP_EXTENSION={'ON' if args.with_udp else 'OFF'}", + f"-DRGL_BUILD_SNOW_EXTENSION={'ON' if args.with_snow else 'OFF'}" ] if on_linux(): @@ -141,6 +158,8 @@ def is_cuda_version_ok(): rpath = rpath.replace("$ORIGIN", "\\$ORIGIN") # cmake should not treat this as variable linker_rpath_flags.append(f"-Wl,-rpath={rpath}") cmake_args.append(f"-DCMAKE_SHARED_LINKER_FLAGS=\"{' '.join(linker_rpath_flags)}\"") + # Taped test + cmake_args.append(f"-DRGL_BUILD_TAPED_TESTS={'ON' if args.build_taped_test else 'OFF'}") # Append user args, possibly overwriting cmake_args.append(args.cmake) @@ -150,11 +169,13 @@ def is_cuda_version_ok(): run_subprocess_command(f"cmake --build {args.build_dir} -- {args.build_args}") if args.with_ros2_standalone: - # Build RobotecGPULidar_ros2_standalone project to find and install all dependent ROS2 libraries and their dependencies - # It cannot be added as a subdirectory of RobotecGPULidar project because there is a conflict in the same libraries required by RGL and ROS2 - # RGL takes them from vcpkg as statically linked objects while ROS2 standalone required them as a shared objects + # Build RobotecGPULidar_ros2_standalone project to find and install all dependent ROS2 libraries and their + # dependencies. It cannot be added as a subdirectory of RobotecGPULidar project because there is a conflict in + # the same libraries required by RGL and ROS2 RGL takes them from vcpkg as statically linked objects while + # ROS2 standalone required them as a shared objects ros2_standalone_cmake_args = f"-DCMAKE_INSTALL_PREFIX={os.path.join(os.getcwd(), args.build_dir)}" - run_subprocess_command(f"cmake ros2_standalone -B {args.build_dir}/ros2_standalone -G {cfg.CMAKE_GENERATOR} {ros2_standalone_cmake_args}") + run_subprocess_command( + f"cmake ros2_standalone -B {args.build_dir}/ros2_standalone -G {cfg.CMAKE_GENERATOR} {ros2_standalone_cmake_args}") run_subprocess_command(f"cmake --install {args.build_dir}/ros2_standalone") @@ -166,14 +187,6 @@ def on_windows(): return platform.system() == "Windows" -def inside_docker(): - path = "/proc/self/cgroup" - return ( - os.path.exists("/.dockerenv") or - os.path.isfile(path) and any("docker" in line for line in open(path)) - ) - - def run_subprocess_command(command: str, shell=True, stderr=sys.stderr, stdout=sys.stdout): print(f"Executing command: '{command}'") process = subprocess.Popen(command, shell=shell, stderr=stderr, stdout=stdout) @@ -182,10 +195,92 @@ def run_subprocess_command(command: str, shell=True, stderr=sys.stderr, stdout=s raise RuntimeError(f"Failed to execute command: '{command}'") -def run_system_command(command: str): - print(f"Executing command: '{command}'") - if os.system(command) != 0: - raise RuntimeError(f"Failed to execute command: '{command}'") +def is_cuda_version_ok(cfg): + nvcc_process = subprocess.run("nvcc --version", shell=True, stdout=subprocess.PIPE) + nvcc_ver_match = re.search("V[0-9]+.[0-9]+.[0-9]+", nvcc_process.stdout.decode("utf-8")) + if not nvcc_ver_match: + raise RuntimeError("CUDA not found") + major = int(nvcc_ver_match[0].split(".")[0][1:]) # [1:] to remove char 'v' + minor = int(nvcc_ver_match[0].split(".")[1]) + patch = int(nvcc_ver_match[0].split(".")[2]) + print(f"Found CUDA {major}.{minor}.{patch}") + for (actual, expected) in [(major, cfg.CUDA_MIN_VER_MAJOR), (minor, cfg.CUDA_MIN_VER_MINOR), + (patch, cfg.CUDA_MIN_VER_PATCH)]: + if actual > expected: + return True + if actual < expected: + return False + return True + + +# Returns a dict with env variables visible for a command after running in a system shell +# Used to capture effects of sourcing file such as ros2 setup +def capture_environment(command="cd ."): + env = "set" if on_windows() else "env" + command = f"{command} && {env}" + + # Run the command and capture the output + proc = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) + output, _ = proc.communicate() + + return {key: value for key, _, value in (line.partition('=') + for line in output.decode().splitlines())} + + +# Updates environment of this python process with env variables added/modified after sourcing given file +def source_environment(filepath): + print(f"Sourcing {filepath}") + # Store the original environment variables + source = "call" if on_windows() else "." + original_env = capture_environment() # No-op working on both Windows and Linux + new_env = capture_environment(f"{source} {filepath}") + + for new_key, new_value in new_env.items(): + if new_key not in original_env: + print(f"Added environment variable: {new_key}={new_env[new_key]}") + if new_key in original_env and original_env[new_key] != new_value: + print(f"Modified environment variable: {new_key}={new_env[new_key]}") + os.environ[new_key] = new_env[new_key] + + +def is_command_available(command): + process = subprocess.Popen(f"{command}", shell=True, stderr=subprocess.DEVNULL, stdout=subprocess.DEVNULL) + process.wait() + return process.returncode == 0 + + +def fetch_rgl_blobs_repo(cfg): + def is_up_to_date(branch): + result = subprocess.Popen("git fetch --dry-run --verbose", shell=True, stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + stdout, _ = result.communicate() + return f"[up to date] {branch}" in stdout.decode() + + def ensure_git_lfs_installed(): + + if not is_command_available("git-lfs --help"): + print("Installing git-lfs...") + run_subprocess_command( + "curl -s https://packagecloud.io/install/repositories/github/git-lfs/script.deb.sh | sudo bash") + run_subprocess_command("sudo apt install git-lfs") + + ensure_git_lfs_installed() + if not os.path.isdir(cfg.RGL_BLOBS_DIR): + print("Cloning rgl blobs repository...") + run_subprocess_command( + f"git clone -b {cfg.RGL_BLOBS_BRANCH} --single-branch --depth 1 {cfg.RGL_BLOBS_REPO} {cfg.RGL_BLOBS_DIR}") + os.chdir(cfg.RGL_BLOBS_DIR) + # Set up git-lfs for this repository + run_subprocess_command("git-lfs install && git-lfs pull") + print("RGL blobs repo cloned successfully") + return + + print("Checking for updates in rgl blobs repository...") + os.chdir(cfg.RGL_BLOBS_DIR) + if not is_up_to_date(cfg.RGL_BLOBS_BRANCH): + print("Updating rgl blobs repository...") + run_subprocess_command("git pull && git-lfs pull") + print("RGL blobs repo fetched successfully") if __name__ == "__main__": diff --git a/src/Logger.cpp b/src/Logger.cpp index b84e2a261..5270e580f 100644 --- a/src/Logger.cpp +++ b/src/Logger.cpp @@ -49,12 +49,11 @@ void Logger::configure(rgl_log_level_t logLevel, std::optional("RGL", sinkList.begin(), sinkList.end()); - mainLogger->set_level(spdlog::level::info); // Print logging configuration as INFO + mainLogger->set_level(static_cast(logLevel)); mainLogger->set_pattern("[%c]: %v"); mainLogger->info("Logging configured: level={}, file={}, stdout={}", spdlog::level::to_string_view(static_cast(logLevel)), logFilePath.has_value() ? logFilePath.value().string() : "(disabled)", useStdout); // https://spdlog.docsforge.com/master/3.custom-formatting/#pattern-flags mainLogger->set_pattern("[%T][%6i us][%l]: %v"); - mainLogger->set_level(static_cast(logLevel)); } diff --git a/src/Optix.cpp b/src/Optix.cpp index 0699aa0eb..bf687d824 100644 --- a/src/Optix.cpp +++ b/src/Optix.cpp @@ -52,8 +52,6 @@ static CUcontext getCurrentDeviceContext() const char* error = nullptr; CUresult status; - cudaFree(nullptr); // Force CUDA runtime initialization - CUdevice device; status = cuDeviceGet(&device, 0); if (status != CUDA_SUCCESS) { @@ -156,6 +154,13 @@ Optix::Optix() Optix::~Optix() { + // On Windows, when program is terminated, CUDA gets unloaded before OptiX. + // If that happens, OptiX fails to de-initialize and crashes program with an error 0xc0000409 STATUS_STACK_BUFFER_OVERRUN + // Therefore, if CUDA is unloaded, we give up de-initializing OptiX, hoping that CUDA de-initialization did it. + if (cudaFree(nullptr) == cudaErrorCudartUnloading) { + return; + } + if (pipeline) { optixPipelineDestroy(pipeline); } diff --git a/src/RGLFields.hpp b/src/RGLFields.hpp index 04f7ea56f..5a119ddec 100644 --- a/src/RGLFields.hpp +++ b/src/RGLFields.hpp @@ -19,32 +19,88 @@ #include #include +#include #include /* * Definition of RGL PointCloud Fields is in the API header. * This file defines some internal properties, such as byte sizes and C++ types. + * + * Important: If you change available fields, you must also update: + * - test/include/helpers/fieldGenerators.hpp: add/delete fieldGenerator + * - test/include/helpers/testPointCloud.hpp: add/delete fieldGenerator inside fieldGenerators */ -#define NON_HIT_VALUE FLT_MAX - typedef unsigned char TextureTexelFormat; // Shorter versions to avoid long type names #define XYZ_VEC3_F32 RGL_FIELD_XYZ_VEC3_F32 #define IS_HIT_I32 RGL_FIELD_IS_HIT_I32 +#define IS_GROUND_I32 RGL_FIELD_IS_GROUND_I32 #define RAY_IDX_U32 RGL_FIELD_RAY_IDX_U32 #define ENTITY_ID_I32 RGL_FIELD_ENTITY_ID_I32 #define INTENSITY_F32 RGL_FIELD_INTENSITY_F32 +#define LASER_RETRO_F32 RGL_FIELD_LASER_RETRO_F32 #define RING_ID_U16 RGL_FIELD_RING_ID_U16 #define AZIMUTH_F32 RGL_FIELD_AZIMUTH_F32 +#define ELEVATION_F32 RGL_FIELD_ELEVATION_F32 #define DISTANCE_F32 RGL_FIELD_DISTANCE_F32 #define RETURN_TYPE_U8 RGL_FIELD_RETURN_TYPE_U8 #define TIME_STAMP_F64 RGL_FIELD_TIME_STAMP_F64 +#define ABSOLUTE_VELOCITY_VEC3_F32 RGL_FIELD_ABSOLUTE_VELOCITY_VEC3_F32 +#define RELATIVE_VELOCITY_VEC3_F32 RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32 +#define RADIAL_SPEED_F32 RGL_FIELD_RADIAL_SPEED_F32 +#define POWER_F32 RGL_FIELD_POWER_F32 +#define RCS_F32 RGL_FIELD_RCS_F32 +#define NOISE_F32 RGL_FIELD_NOISE_F32 +#define SNR_F32 RGL_FIELD_SNR_F32 +#define NORMAL_VEC3_F32 RGL_FIELD_NORMAL_VEC3_F32 +#define INCIDENT_ANGLE_F32 RGL_FIELD_INCIDENT_ANGLE_F32 +#define RAY_POSE_MAT3x4_F32 RGL_FIELD_RAY_POSE_MAT3x4_F32 #define PADDING_8 RGL_FIELD_PADDING_8 #define PADDING_16 RGL_FIELD_PADDING_16 #define PADDING_32 RGL_FIELD_PADDING_32 +inline const std::set& getAllRealFields() +{ + static std::set allRealFields = { + XYZ_VEC3_F32, + IS_HIT_I32, + IS_GROUND_I32, + RAY_IDX_U32, + ENTITY_ID_I32, + INTENSITY_F32, + LASER_RETRO_F32, + RING_ID_U16, + AZIMUTH_F32, + ELEVATION_F32, + DISTANCE_F32, + RETURN_TYPE_U8, + TIME_STAMP_F64, + ABSOLUTE_VELOCITY_VEC3_F32, + RELATIVE_VELOCITY_VEC3_F32, + RADIAL_SPEED_F32, + POWER_F32, + RCS_F32, + NOISE_F32, + SNR_F32, + NORMAL_VEC3_F32, + INCIDENT_ANGLE_F32, + RAY_POSE_MAT3x4_F32, + }; + return allRealFields; +} + +inline const std::set& getAllPaddings() +{ + static std::set allPaddings = { + PADDING_8, + PADDING_16, + PADDING_32, + }; + return allPaddings; +} + template struct Field {}; @@ -61,15 +117,28 @@ FIELD(XYZ_VEC3_F32, Vec3f); FIELD(RAY_IDX_U32, uint32_t); // PCL uses uint32_t FIELD(ENTITY_ID_I32, int32_t); FIELD(INTENSITY_F32, float); -FIELD(IS_HIT_I32, int32_t); // Signed may be faster +FIELD(LASER_RETRO_F32, float); +FIELD(IS_HIT_I32, int32_t); // Signed may be faster +FIELD(IS_GROUND_I32, int32_t); // Signed may be faster FIELD(DISTANCE_F32, float); FIELD(AZIMUTH_F32, float); +FIELD(ELEVATION_F32, float); FIELD(RING_ID_U16, uint16_t); FIELD(RETURN_TYPE_U8, uint8_t); FIELD(TIME_STAMP_F64, double); FIELD(PADDING_8, uint8_t); FIELD(PADDING_16, uint16_t); FIELD(PADDING_32, uint32_t); +FIELD(ABSOLUTE_VELOCITY_VEC3_F32, Vec3f); +FIELD(RELATIVE_VELOCITY_VEC3_F32, Vec3f); +FIELD(RADIAL_SPEED_F32, float); +FIELD(POWER_F32, float); +FIELD(RCS_F32, float); +FIELD(NOISE_F32, float); +FIELD(SNR_F32, float); +FIELD(NORMAL_VEC3_F32, Vec3f); +FIELD(INCIDENT_ANGLE_F32, float); +FIELD(RAY_POSE_MAT3x4_F32, Mat3x4f); inline std::size_t getFieldSize(rgl_field_t type) { @@ -78,12 +147,25 @@ inline std::size_t getFieldSize(rgl_field_t type) case RAY_IDX_U32: return Field::size; case ENTITY_ID_I32: return Field::size; case IS_HIT_I32: return Field::size; + case IS_GROUND_I32: return Field::size; case INTENSITY_F32: return Field::size; + case LASER_RETRO_F32: return Field::size; case RING_ID_U16: return Field::size; case AZIMUTH_F32: return Field::size; + case ELEVATION_F32: return Field::size; case DISTANCE_F32: return Field::size; case RETURN_TYPE_U8: return Field::size; case TIME_STAMP_F64: return Field::size; + case ABSOLUTE_VELOCITY_VEC3_F32: return Field::size; + case RELATIVE_VELOCITY_VEC3_F32: return Field::size; + case RADIAL_SPEED_F32: return Field::size; + case POWER_F32: return Field::size; + case RCS_F32: return Field::size; + case NOISE_F32: return Field::size; + case SNR_F32: return Field::size; + case NORMAL_VEC3_F32: return Field::size; + case INCIDENT_ANGLE_F32: return Field::size; + case RAY_POSE_MAT3x4_F32: return Field::size; case PADDING_8: return Field::size; case PADDING_16: return Field::size; case PADDING_32: return Field::size; @@ -124,12 +206,27 @@ inline std::shared_ptr createArray(rgl_field_t type, Args&&... args) case RAY_IDX_U32: return Subclass::type>::create(std::forward(args)...); case ENTITY_ID_I32: return Subclass::type>::create(std::forward(args)...); case INTENSITY_F32: return Subclass::type>::create(std::forward(args)...); + case LASER_RETRO_F32: return Subclass::type>::create(std::forward(args)...); case RING_ID_U16: return Subclass::type>::create(std::forward(args)...); case AZIMUTH_F32: return Subclass::type>::create(std::forward(args)...); + case ELEVATION_F32: return Subclass::type>::create(std::forward(args)...); case DISTANCE_F32: return Subclass::type>::create(std::forward(args)...); case RETURN_TYPE_U8: return Subclass::type>::create(std::forward(args)...); case TIME_STAMP_F64: return Subclass::type>::create(std::forward(args)...); case IS_HIT_I32: return Subclass::type>::create(std::forward(args)...); + case IS_GROUND_I32: return Subclass::type>::create(std::forward(args)...); + case ABSOLUTE_VELOCITY_VEC3_F32: + return Subclass::type>::create(std::forward(args)...); + case RELATIVE_VELOCITY_VEC3_F32: + return Subclass::type>::create(std::forward(args)...); + case RADIAL_SPEED_F32: return Subclass::type>::create(std::forward(args)...); + case POWER_F32: return Subclass::type>::create(std::forward(args)...); + case RCS_F32: return Subclass::type>::create(std::forward(args)...); + case NOISE_F32: return Subclass::type>::create(std::forward(args)...); + case SNR_F32: return Subclass::type>::create(std::forward(args)...); + case NORMAL_VEC3_F32: return Subclass::type>::create(std::forward(args)...); + case INCIDENT_ANGLE_F32: return Subclass::type>::create(std::forward(args)...); + case RAY_POSE_MAT3x4_F32: return Subclass::type>::create(std::forward(args)...); } throw std::invalid_argument(fmt::format("createArray: unknown RGL field {}", type)); } @@ -139,14 +236,27 @@ inline std::string toString(rgl_field_t type) switch (type) { case XYZ_VEC3_F32: return "XYZ_VEC3_F32"; case IS_HIT_I32: return "IS_HIT_I32"; + case IS_GROUND_I32: return "IS_GROUND_I32"; case RAY_IDX_U32: return "RAY_IDX_U32"; case ENTITY_ID_I32: return "ENTITY_ID_I32"; case INTENSITY_F32: return "INTENSITY_F32"; + case LASER_RETRO_F32: return "LASER_RETRO_F32"; case RING_ID_U16: return "RING_ID_U16"; case AZIMUTH_F32: return "AZIMUTH_F32"; + case ELEVATION_F32: return "ELEVATION_F32"; case DISTANCE_F32: return "DISTANCE_F32"; case RETURN_TYPE_U8: return "RETURN_TYPE_U8"; case TIME_STAMP_F64: return "TIME_STAMP_F64"; + case ABSOLUTE_VELOCITY_VEC3_F32: return "ABSOLUTE_VELOCITY_VEC3_F32"; + case RELATIVE_VELOCITY_VEC3_F32: return "RELATIVE_VELOCITY_VEC3_F32"; + case RADIAL_SPEED_F32: return "RADIAL_SPEED_F32"; + case POWER_F32: return "POWER_F32"; + case RCS_F32: return "RCS_F32"; + case NOISE_F32: return "NOISE_F32"; + case SNR_F32: return "SNR_F32"; + case NORMAL_VEC3_F32: return "NORMAL_VEC3_F32"; + case INCIDENT_ANGLE_F32: return "INCIDENT_ANGLE_F32"; + case RAY_POSE_MAT3x4_F32: return "RAY_POSE_MAT3x4_F32"; case PADDING_8: return "PADDING_8"; case PADDING_16: return "PADDING_16"; case PADDING_32: return "PADDING_32"; @@ -164,14 +274,41 @@ inline std::vector toRos2Fields(rgl_field_t type) return {sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32}; case IS_HIT_I32: return {sensor_msgs::msg::PointField::INT32}; + case IS_GROUND_I32: return {sensor_msgs::msg::PointField::INT32}; case RAY_IDX_U32: return {sensor_msgs::msg::PointField::UINT32}; case ENTITY_ID_I32: return {sensor_msgs::msg::PointField::INT32}; case INTENSITY_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case LASER_RETRO_F32: return {sensor_msgs::msg::PointField::FLOAT32}; case RING_ID_U16: return {sensor_msgs::msg::PointField::UINT16}; case AZIMUTH_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case ELEVATION_F32: return {sensor_msgs::msg::PointField::FLOAT32}; case DISTANCE_F32: return {sensor_msgs::msg::PointField::FLOAT32}; case RETURN_TYPE_U8: return {sensor_msgs::msg::PointField::UINT8}; case TIME_STAMP_F64: return {sensor_msgs::msg::PointField::FLOAT64}; + case ABSOLUTE_VELOCITY_VEC3_F32: + return {sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32}; + case RELATIVE_VELOCITY_VEC3_F32: + return {sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32}; + case RADIAL_SPEED_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case POWER_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case RCS_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case NOISE_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case SNR_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case NORMAL_VEC3_F32: + return {sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32}; + case INCIDENT_ANGLE_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case RAY_POSE_MAT3x4_F32: + return { + sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + sensor_msgs::msg::PointField::FLOAT32, sensor_msgs::msg::PointField::FLOAT32, + }; case PADDING_8: return {}; case PADDING_16: return {}; case PADDING_32: return {}; @@ -184,14 +321,27 @@ inline std::vector toRos2Names(rgl_field_t type) switch (type) { case XYZ_VEC3_F32: return {"x", "y", "z"}; case IS_HIT_I32: return {"is_hit"}; + case IS_GROUND_I32: return {"is_ground"}; case ENTITY_ID_I32: return {"entity_id"}; case RAY_IDX_U32: return {"ray_idx"}; case INTENSITY_F32: return {"intensity"}; + case LASER_RETRO_F32: return {"laser_retro"}; case RING_ID_U16: return {"ring"}; case AZIMUTH_F32: return {"azimuth"}; + case ELEVATION_F32: return {"elevation"}; case DISTANCE_F32: return {"distance"}; case RETURN_TYPE_U8: return {"return_type"}; case TIME_STAMP_F64: return {"time_stamp"}; + case ABSOLUTE_VELOCITY_VEC3_F32: return {"abs_vx", "abs_vy", "abs_vz"}; + case RELATIVE_VELOCITY_VEC3_F32: return {"rel_vx", "rel_vy", "rel_vz"}; + case RADIAL_SPEED_F32: return {"radial_speed"}; + case POWER_F32: return {"power"}; + case RCS_F32: return {"rcs"}; + case NOISE_F32: return {"noise"}; + case SNR_F32: return {"snr"}; + case NORMAL_VEC3_F32: return {"nx", "ny", "nz"}; + case INCIDENT_ANGLE_F32: return {"incident_angle"}; + case RAY_POSE_MAT3x4_F32: return {"m00", "m01", "m02", "m03", "m10", "m11", "m12", "m13", "m20", "m21", "m22", "m23"}; case PADDING_8: return {}; case PADDING_16: return {}; case PADDING_32: return {}; @@ -202,20 +352,17 @@ inline std::vector toRos2Names(rgl_field_t type) inline std::vector toRos2Sizes(rgl_field_t type) { switch (type) { + // Add here all vector fields case XYZ_VEC3_F32: return {sizeof(float), sizeof(float), sizeof(float)}; - case IS_HIT_I32: return {getFieldSize(type)}; - case RAY_IDX_U32: return {getFieldSize(type)}; - case ENTITY_ID_I32: return {getFieldSize(type)}; - case INTENSITY_F32: return {getFieldSize(type)}; - case RING_ID_U16: return {getFieldSize(type)}; - case AZIMUTH_F32: return {getFieldSize(type)}; - case DISTANCE_F32: return {getFieldSize(type)}; - case RETURN_TYPE_U8: return {getFieldSize(type)}; - case TIME_STAMP_F64: return {getFieldSize(type)}; - case PADDING_8: return {getFieldSize(type)}; - case PADDING_16: return {getFieldSize(type)}; - case PADDING_32: return {getFieldSize(type)}; + case ABSOLUTE_VELOCITY_VEC3_F32: return {sizeof(float), sizeof(float), sizeof(float)}; + case RELATIVE_VELOCITY_VEC3_F32: return {sizeof(float), sizeof(float), sizeof(float)}; + case NORMAL_VEC3_F32: return {sizeof(float), sizeof(float), sizeof(float)}; + case RAY_POSE_MAT3x4_F32: + return { + sizeof(float), sizeof(float), sizeof(float), sizeof(float), sizeof(float), sizeof(float), + sizeof(float), sizeof(float), sizeof(float), sizeof(float), sizeof(float), sizeof(float), + }; + default: return {getFieldSize(type)}; } - throw std::invalid_argument(fmt::format("toRos2Sizes: unknown RGL field {}", type)); } #endif diff --git a/src/Tape.cpp b/src/Tape.cpp deleted file mode 100644 index 700960ed4..000000000 --- a/src/Tape.cpp +++ /dev/null @@ -1,249 +0,0 @@ -// Copyright 2022 Robotec.AI -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "Tape.hpp" - -using namespace std::placeholders; -namespace fs = std::filesystem; - -std::optional tapeRecorder; - -TapeRecorder::TapeRecorder(const fs::path& path) -{ - std::string pathYaml = fs::path(path).concat(YAML_EXTENSION).string(); - std::string pathBin = fs::path(path).concat(BIN_EXTENSION).string(); - - fileBin = fopen(pathBin.c_str(), "wb"); - if (nullptr == fileBin) { - throw InvalidFilePath(fmt::format("rgl_tape_record_begin: could not open binary file '{}' " - "due to the error: {}", - pathBin, std::strerror(errno))); - } - fileYaml.open(pathYaml); - if (fileYaml.fail()) { - throw InvalidFilePath(fmt::format("rgl_tape_record_begin: could not open yaml file '{}' " - "due to the error: {}", - pathYaml, std::strerror(errno))); - } - TapeRecorder::recordRGLVersion(yamlRoot); - yamlRecording = yamlRoot["recording"]; - beginTimestamp = std::chrono::steady_clock::now(); -} - -TapeRecorder::~TapeRecorder() -{ - // TODO(prybicki): SIOF with Logger !!! - fileYaml << yamlRoot; - fileYaml.close(); - if (fileYaml.fail()) { - RGL_WARN("rgl_tape_record_end: failed to close yaml file due to the error: {}", std::strerror(errno)); - } - if (fclose(fileBin)) { - RGL_WARN("rgl_tape_record_end: failed to close binary file due to the error: {}", std::strerror(errno)); - } -} - -void TapeRecorder::recordRGLVersion(YAML::Node& node) -{ - YAML::Node rglVersion; - rglVersion["major"] = RGL_VERSION_MAJOR; - rglVersion["minor"] = RGL_VERSION_MINOR; - rglVersion["patch"] = RGL_VERSION_PATCH; - node[RGL_VERSION] = rglVersion; -} - - -TapePlayer::TapePlayer(const char* path) -{ - tapeFunctions = { - { "rgl_get_version_info",std::bind(&TapePlayer::tape_get_version_info, this, _1) }, - { "rgl_get_extension_info", std::bind(&TapePlayer::tape_get_extension_info, this, _1)}, - { "rgl_configure_logging", std::bind(&TapePlayer::tape_configure_logging, this, _1)}, - { "rgl_cleanup", std::bind(&TapePlayer::tape_cleanup, this, _1)}, - { "rgl_mesh_create", std::bind(&TapePlayer::tape_mesh_create, this, _1)}, - { "rgl_mesh_destroy", std::bind(&TapePlayer::tape_mesh_destroy, this, _1)}, - { "rgl_mesh_update_vertices", std::bind(&TapePlayer::tape_mesh_update_vertices, this, _1)}, - { "rgl_mesh_set_texture_coords", std::bind(&TapePlayer::tape_mesh_set_texture_coords, this, _1)}, - { "rgl_texture_create", std::bind(&TapePlayer::tape_texture_create, this, _1)}, - { "rgl_texture_destroy", std::bind(&TapePlayer::tape_texture_destroy, this, _1)}, - { "rgl_entity_create", std::bind(&TapePlayer::tape_entity_create, this, _1)}, - { "rgl_entity_destroy", std::bind(&TapePlayer::tape_entity_destroy, this, _1)}, - { "rgl_entity_set_pose", std::bind(&TapePlayer::tape_entity_set_pose, this, _1)}, - { "rgl_entity_set_id", std::bind(&TapePlayer::tape_entity_set_id, this, _1)}, - { "rgl_entity_set_intensity_texture", std::bind(&TapePlayer::tape_entity_set_intensity_texture, this, _1)}, - { "rgl_scene_set_time", std::bind(&TapePlayer::tape_scene_set_time, this, _1)}, - { "rgl_graph_run", std::bind(&TapePlayer::tape_graph_run, this, _1)}, - { "rgl_graph_destroy", std::bind(&TapePlayer::tape_graph_destroy, this, _1)}, - { "rgl_graph_get_result_size", std::bind(&TapePlayer::tape_graph_get_result_size, this, _1)}, - { "rgl_graph_get_result_data", std::bind(&TapePlayer::tape_graph_get_result_data, this, _1)}, - { "rgl_graph_node_add_child", std::bind(&TapePlayer::tape_graph_node_add_child, this, _1)}, - { "rgl_graph_node_remove_child", std::bind(&TapePlayer::tape_graph_node_remove_child, this, _1)}, - { "rgl_graph_node_set_priority", std::bind(&TapePlayer::tape_graph_node_set_priority, this, _1)}, - { "rgl_graph_node_get_priority", std::bind(&TapePlayer::tape_graph_node_get_priority, this, _1)}, - { "rgl_node_rays_from_mat3x4f", std::bind(&TapePlayer::tape_node_rays_from_mat3x4f, this, _1)}, - { "rgl_node_rays_set_range", std::bind(&TapePlayer::tape_node_rays_set_range, this, _1)}, - { "rgl_node_rays_set_ring_ids", std::bind(&TapePlayer::tape_node_rays_set_ring_ids, this, _1)}, - { "rgl_node_rays_set_time_offsets", std::bind(&TapePlayer::tape_node_rays_set_time_offsets, this, _1)}, - { "rgl_node_rays_transform", std::bind(&TapePlayer::tape_node_rays_transform, this, _1)}, - { "rgl_node_points_transform", std::bind(&TapePlayer::tape_node_points_transform, this, _1)}, - { "rgl_node_raytrace", std::bind(&TapePlayer::tape_node_raytrace, this, _1)}, - { "rgl_node_raytrace_with_distortion", std::bind(&TapePlayer::tape_node_raytrace_with_distortion, this, _1)}, - { "rgl_node_points_format", std::bind(&TapePlayer::tape_node_points_format, this, _1)}, - { "rgl_node_points_yield", std::bind(&TapePlayer::tape_node_points_yield, this, _1)}, - { "rgl_node_points_compact", std::bind(&TapePlayer::tape_node_points_compact, this, _1)}, - { "rgl_node_points_spatial_merge", std::bind(&TapePlayer::tape_node_points_spatial_merge, this, _1)}, - { "rgl_node_points_temporal_merge", std::bind(&TapePlayer::tape_node_points_temporal_merge, this, _1)}, - { "rgl_node_points_from_array", std::bind(&TapePlayer::tape_node_points_from_array, this, _1)}, - { "rgl_node_gaussian_noise_angular_ray", std::bind(&TapePlayer::tape_node_gaussian_noise_angular_ray, this, _1)}, - {"rgl_node_gaussian_noise_angular_hitpoint", - std::bind(&TapePlayer::tape_node_gaussian_noise_angular_hitpoint, this, _1) }, - { "rgl_node_gaussian_noise_distance", std::bind(&TapePlayer::tape_node_gaussian_noise_distance, this, _1)}, - -#if RGL_BUILD_PCL_EXTENSION - { "rgl_graph_write_pcd_file", std::bind(&TapePlayer::tape_graph_write_pcd_file, this, _1)}, - { "rgl_node_points_downsample", std::bind(&TapePlayer::tape_node_points_downsample, this, _1)}, - { "rgl_node_points_visualize", std::bind(&TapePlayer::tape_node_points_visualize, this, _1)}, -#endif - -#if RGL_BUILD_ROS2_EXTENSION - { "rgl_node_points_ros2_publish", std::bind(&TapePlayer::tape_node_points_ros2_publish, this, _1)}, - { "rgl_node_points_ros2_publish_with_qos", std::bind(&TapePlayer::tape_node_points_ros2_publish_with_qos, this, _1)}, -#endif - }; - - std::string pathYaml = fs::path(path).concat(YAML_EXTENSION).string(); - std::string pathBin = fs::path(path).concat(BIN_EXTENSION).string(); - - mmapInit(pathBin.c_str()); - yamlRoot = YAML::LoadFile(pathYaml); - yamlRecording = yamlRoot["recording"]; - - if (yamlRoot[RGL_VERSION]["major"].as() != RGL_VERSION_MAJOR || - yamlRoot[RGL_VERSION]["minor"].as() != RGL_VERSION_MINOR) { - throw RecordError("recording version does not match rgl version"); - } - - nextCall = 0; -} - -std::optional TapePlayer::findFirst(std::set fnNames) -{ - for (APICallIdx idx = 0; idx < yamlRecording.size(); ++idx) { - if (fnNames.contains(yamlRecording[idx]["name"].as())) { - return idx; - } - } - return std::nullopt; -} - -std::optional TapePlayer::findLast(std::set fnNames) -{ - for (APICallIdx idx = 0; idx < yamlRecording.size(); ++idx) { - auto rIdx = yamlRecording.size() - 1 - idx; - if (fnNames.contains(yamlRecording[rIdx]["name"].as())) { - return rIdx; - } - } - return std::nullopt; -} - -std::vector TapePlayer::findAll(std::set fnNames) -{ - std::vector result; - for (APICallIdx idx = 0; idx < yamlRecording.size(); ++idx) { - if (fnNames.contains((yamlRecording[idx]["name"].as()))) { - result.push_back(idx); - } - } - return result; -} - -void TapePlayer::playThrough(APICallIdx last) -{ - assert(last < yamlRecording.size()); - for (; nextCall <= last; ++nextCall) { - playThis(nextCall); - } -} - -void TapePlayer::playUntil(std::optional breakpoint) -{ - assert(!breakpoint.has_value() || nextCall < breakpoint.value()); - auto end = breakpoint.value_or(yamlRecording.size()); - assert(end <= yamlRecording.size()); - for (; nextCall < end; ++nextCall) { - playThis(nextCall); - } -} - -void TapePlayer::rewindTo(APICallIdx nextCall) { this->nextCall = nextCall; } - -void TapePlayer::playThis(APICallIdx idx) -{ - const YAML::Node& node = yamlRecording[idx]; - std::string functionName = node["name"].as(); - - if (!tapeFunctions.contains(functionName)) { - throw RecordError(fmt::format("unknown function to play: {}", functionName)); - } - - tapeFunctions[functionName](node); -} - -#include -void TapePlayer::playRealtime() -{ - auto beginTimestamp = std::chrono::steady_clock::now(); - for (; nextCall < yamlRecording.size(); ++nextCall) { - auto nextCallNs = std::chrono::nanoseconds(yamlRecording[nextCall]["timestamp"].as()); - auto elapsed = std::chrono::steady_clock::now() - beginTimestamp; - std::this_thread::sleep_for(nextCallNs - elapsed); - playThis(nextCall); - } -} - -TapePlayer::~TapePlayer() -{ - if (munmap(fileMmap, mmapSize) == -1) { - RGL_WARN("rgl_tape_play: failed to remove binary mappings due to {}", std::strerror(errno)); - } -} - -void TapePlayer::mmapInit(const char* path) -{ - int fd = open(path, O_RDONLY); - if (fd < 0) { - throw InvalidFilePath(fmt::format("rgl_tape_play: could not open binary file: '{}' " - " due to the error: {}", - path, std::strerror(errno))); - } - - struct stat staticBuffer - {}; - int err = fstat(fd, &staticBuffer); - if (err < 0) { - throw RecordError("rgl_tape_play: couldn't read bin file length"); - } - - fileMmap = (uint8_t*) mmap(nullptr, staticBuffer.st_size, PROT_READ, MAP_PRIVATE, fd, 0); - mmapSize = staticBuffer.st_size; - if (fileMmap == MAP_FAILED) { - throw InvalidFilePath(fmt::format("rgl_tape_play: could not mmap binary file: {}", path)); - } - if (close(fd)) { - RGL_WARN("rgl_tape_play: failed to close binary file: '{}' " - "due to the error: {}", - path, std::strerror(errno)); - } -} diff --git a/src/Tape.hpp b/src/Tape.hpp deleted file mode 100644 index c07e6be19..000000000 --- a/src/Tape.hpp +++ /dev/null @@ -1,280 +0,0 @@ -// Copyright 2022 Robotec.AI -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -// Hack to complete compilation on Windows. In runtime it is never used. -#ifdef _WIN32 -#include -#define PROT_READ 1 -#define MAP_PRIVATE 1 -#define MAP_FAILED nullptr -static int munmap(void* addr, size_t length) { return -1; } -static void* mmap(void* start, size_t length, int prot, int flags, int fd, size_t offset) { return nullptr; } -#else -#include -#include -#include -#endif // _WIN32 - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#define BIN_EXTENSION ".bin" -#define YAML_EXTENSION ".yaml" -#define RGL_VERSION "rgl_version" - -#ifdef _WIN32 -#define TAPE_HOOK(...) -#else -#define TAPE_HOOK(...) \ - do \ - if (tapeRecorder.has_value()) { \ - tapeRecorder->recordApiCall(__func__ __VA_OPT__(, ) __VA_ARGS__); \ - } \ - while (0) -#endif // _WIN32 - -#define TAPE_ARRAY(data, count) std::make_pair(data, count) -#define FWRITE(source, elemSize, elemCount, file) \ - do \ - if (fwrite(source, elemSize, elemCount, file) != elemCount) { \ - throw RecordError(fmt::format("Failed to write data to binary file")); \ - } \ - while (0) - -// Type used as a key in TapePlayer object registry -using TapeAPIObjectID = size_t; - -class TapeRecorder -{ - YAML::Node yamlRoot; // Represents the whole yaml file - YAML::Node yamlRecording; // The sequence of API calls - - FILE* fileBin; - std::ofstream fileYaml; - - size_t currentBinOffset = 0; - std::chrono::time_point beginTimestamp; - - static void recordRGLVersion(YAML::Node& node); - - template - size_t writeToBin(const T* source, size_t elemCount) - { - size_t elemSize = sizeof(T); - uint8_t remainder = (elemSize * elemCount) % 16; - uint8_t bytesToAdd = (16 - remainder) % 16; - - FWRITE(source, elemSize, elemCount, fileBin); - if (remainder != 0) { - uint8_t zeros[16]; - FWRITE(zeros, sizeof(uint8_t), bytesToAdd, fileBin); - } - - size_t outBinOffest = currentBinOffset; - currentBinOffset += elemSize * elemCount + bytesToAdd; - return outBinOffest; - } - - template - void recordApiArguments(YAML::Node& node, int currentArgIdx, T currentArg, Args... remainingArgs) - { - node[currentArgIdx] = valueToYaml(currentArg); - - if constexpr (sizeof...(remainingArgs) > 0) { - recordApiArguments(node, ++currentArgIdx, remainingArgs...); - return; - } - } - - //// value to yaml converters - template - T valueToYaml(T value) - { - return value; - } - - uintptr_t valueToYaml(rgl_node_t value) { return (uintptr_t) value; } - uintptr_t valueToYaml(rgl_node_t* value) { return (uintptr_t) *value; } - - uintptr_t valueToYaml(rgl_entity_t value) { return (uintptr_t) value; } - uintptr_t valueToYaml(rgl_entity_t* value) { return (uintptr_t) *value; } - - uintptr_t valueToYaml(rgl_mesh_t value) { return (uintptr_t) value; } - uintptr_t valueToYaml(rgl_mesh_t* value) { return (uintptr_t) *value; } - - uintptr_t valueToYaml(rgl_texture_t value) { return (uintptr_t) value; } - uintptr_t valueToYaml(rgl_texture_t* value) { return (uintptr_t) *value; } - - uintptr_t valueToYaml(rgl_scene_t value) { return (uintptr_t) value; } - uintptr_t valueToYaml(rgl_scene_t* value) { return (uintptr_t) *value; } - - uintptr_t valueToYaml(void* value) { return (uintptr_t) value; } - - int valueToYaml(int32_t* value) { return *value; } - int valueToYaml(rgl_field_t value) { return (int) value; } - int valueToYaml(rgl_log_level_t value) { return (int) value; } - int valueToYaml(rgl_axis_t value) { return (int) value; } - int32_t valueToYaml(rgl_extension_t value) { return static_cast(value); } - int valueToYaml(rgl_qos_policy_reliability_t value) { return (int) value; } - int valueToYaml(rgl_qos_policy_durability_t value) { return (int) value; } - int valueToYaml(rgl_qos_policy_history_t value) { return (int) value; } - - size_t valueToYaml(const rgl_mat3x4f* value) { return writeToBin(value, 1); } - size_t valueToYaml(const rgl_vec3f* value) { return writeToBin(value, 1); } - - // TAPE_ARRAY - template - size_t valueToYaml(std::pair value) - { - return writeToBin(value.first, value.second); - } - - template - size_t valueToYaml(std::pair value) - { - return writeToBin(static_cast(value.first), value.second); - } - -public: - explicit TapeRecorder(const std::filesystem::path& path); - - ~TapeRecorder(); - - template - void recordApiCall(std::string fnName, Args... args) - { - YAML::Node apiCallNode; - apiCallNode["name"] = fnName; - apiCallNode["timestamp"] = - std::chrono::duration_cast(std::chrono::steady_clock::now() - beginTimestamp).count(); - if constexpr (sizeof...(args) > 0) { - recordApiArguments(apiCallNode, 0, args...); - } - yamlRecording.push_back(apiCallNode); - } -}; - -struct TapePlayer -{ - using APICallIdx = int32_t; - explicit TapePlayer(const char* path); - - std::optional findFirst(std::set fnNames); - std::optional findLast(std::set fnNames); - std::vector findAll(std::set fnNames); - - void playThis(APICallIdx idx); - void playThrough(APICallIdx last); - void playUntil(std::optional breakpoint = std::nullopt); - void playRealtime(); - - void rewindTo(APICallIdx nextCall = 0); - - rgl_node_t getNodeHandle(TapeAPIObjectID key) { return tapeNodes.at(key); } - - template - T getCallArg(APICallIdx idx, int arg) - { - return yamlRecording[idx][arg].as(); - } - - ~TapePlayer(); - -private: - YAML::Node yamlRoot{}; - YAML::Node yamlRecording{}; - APICallIdx nextCall{}; - uint8_t* fileMmap{}; - size_t mmapSize{}; - - std::unordered_map tapeMeshes; - std::unordered_map tapeEntities; - std::unordered_map tapeTextures; - std::unordered_map tapeNodes; - - std::map> tapeFunctions; - -private: - void mmapInit(const char* path); - - void tape_get_version_info(const YAML::Node& yamlNode); - void tape_get_extension_info(const YAML::Node& yamlNode); - void tape_configure_logging(const YAML::Node& yamlNode); - void tape_cleanup(const YAML::Node& yamlNode); - void tape_mesh_create(const YAML::Node& yamlNode); - void tape_mesh_destroy(const YAML::Node& yamlNode); - void tape_mesh_update_vertices(const YAML::Node& yamlNode); - void tape_mesh_set_texture_coords(const YAML::Node& yamlNode); - void tape_texture_create(const YAML::Node& yamlNode); - void tape_texture_destroy(const YAML::Node& yamlNode); - void tape_entity_create(const YAML::Node& yamlNode); - void tape_entity_destroy(const YAML::Node& yamlNode); - void tape_entity_set_pose(const YAML::Node& yamlNode); - void tape_entity_set_id(const YAML::Node& yamlNode); - void tape_entity_set_intensity_texture(const YAML::Node& yamlNode); - void tape_scene_set_time(const YAML::Node& yamlNode); - void tape_graph_run(const YAML::Node& yamlNode); - void tape_graph_destroy(const YAML::Node& yamlNode); - void tape_graph_get_result_size(const YAML::Node& yamlNode); - void tape_graph_get_result_data(const YAML::Node& yamlNode); - void tape_graph_node_add_child(const YAML::Node& yamlNode); - void tape_graph_node_remove_child(const YAML::Node& yamlNode); - void tape_graph_node_set_priority(const YAML::Node& yamlNode); - void tape_graph_node_get_priority(const YAML::Node& yamlNode); - void tape_node_rays_from_mat3x4f(const YAML::Node& yamlNode); - void tape_node_rays_set_range(const YAML::Node& yamlNode); - void tape_node_rays_set_ring_ids(const YAML::Node& yamlNode); - void tape_node_rays_set_time_offsets(const YAML::Node& yamlNode); - void tape_node_rays_transform(const YAML::Node& yamlNode); - void tape_node_points_transform(const YAML::Node& yamlNode); - void tape_node_raytrace(const YAML::Node& yamlNode); - void tape_node_raytrace_with_distortion(const YAML::Node& yamlNode); - void tape_node_points_format(const YAML::Node& yamlNode); - void tape_node_points_yield(const YAML::Node& yamlNode); - void tape_node_points_compact(const YAML::Node& yamlNode); - void tape_node_points_spatial_merge(const YAML::Node& yamlNode); - void tape_node_points_temporal_merge(const YAML::Node& yamlNode); - void tape_node_points_from_array(const YAML::Node& yamlNode); - void tape_node_gaussian_noise_angular_ray(const YAML::Node& yamlNode); - void tape_node_gaussian_noise_angular_hitpoint(const YAML::Node& yamlNode); - void tape_node_gaussian_noise_distance(const YAML::Node& yamlNode); - -#if RGL_BUILD_PCL_EXTENSION - void tape_graph_write_pcd_file(const YAML::Node& yamlNode); - void tape_node_points_downsample(const YAML::Node& yamlNode); - void tape_node_points_visualize(const YAML::Node& yamlNode); -#endif - -#if RGL_BUILD_ROS2_EXTENSION - void tape_node_points_ros2_publish(const YAML::Node& yamlNode); - void tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode); -#endif -}; - -extern std::optional tapeRecorder; diff --git a/src/Time.hpp b/src/Time.hpp index 4c34edd99..8fc155e54 100644 --- a/src/Time.hpp +++ b/src/Time.hpp @@ -22,6 +22,10 @@ struct Time { + friend Time operator-(const Time& lhs, const Time& rhs) noexcept; + // TODO: We can use std::strong_ordering if we upgrade to CUDA 12 + friend bool operator==(const Time& lhs, const Time& rhs) noexcept; + static Time zero() { return Time(0); } static Time seconds(double seconds) { return Time(static_cast(seconds * 1.0e9)); } static Time nanoseconds(uint64_t nanoseconds) { return Time(nanoseconds); } @@ -30,7 +34,7 @@ struct Time HostDevFn uint64_t asNanoseconds() const { return timeNs; } #if RGL_BUILD_ROS2_EXTENSION - builtin_interfaces::msg::Time asRos2Msg() + builtin_interfaces::msg::Time asRos2Msg() const { auto msg = builtin_interfaces::msg::Time(); msg.sec = timeNs / static_cast(1e9); @@ -44,3 +48,6 @@ struct Time uint64_t timeNs; }; + +inline Time operator-(const Time& lhs, const Time& rhs) noexcept { return Time::nanoseconds(lhs.timeNs - rhs.timeNs); } +inline bool operator==(const Time& lhs, const Time& rhs) noexcept { return lhs.timeNs == rhs.timeNs; } diff --git a/src/api/apiCommon.hpp b/src/api/apiCommon.hpp index c915d413b..685c176b7 100644 --- a/src/api/apiCommon.hpp +++ b/src/api/apiCommon.hpp @@ -28,7 +28,8 @@ #include #include -#include +#include + #include #include diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp index 799c893f9..0069140a4 100644 --- a/src/api/apiCore.cpp +++ b/src/api/apiCore.cpp @@ -17,6 +17,8 @@ #include +#include + #include #include #include @@ -62,7 +64,7 @@ RGL_API rgl_status_t rgl_get_version_info(int32_t* out_major, int32_t* out_minor return status; } -void TapePlayer::tape_get_version_info(const YAML::Node& yamlNode) +void TapeCore::tape_get_version_info(const YAML::Node& yamlNode, PlaybackState& state) { int32_t out_major, out_minor, out_patch; rgl_get_version_info(&out_major, &out_minor, &out_patch); @@ -86,6 +88,7 @@ RGL_API rgl_status_t rgl_get_extension_info(rgl_extension_t extension, int32_t* case RGL_EXTENSION_PCL: *out_available = RGL_BUILD_PCL_EXTENSION; break; case RGL_EXTENSION_ROS2: *out_available = RGL_BUILD_ROS2_EXTENSION; break; case RGL_EXTENSION_UDP: *out_available = RGL_BUILD_UDP_EXTENSION; break; + case RGL_EXTENSION_SNOW: *out_available = RGL_BUILD_SNOW_EXTENSION; break; default: throw std::invalid_argument(fmt::format("queried unknown RGL extension: {}", extension)); } }); @@ -93,7 +96,7 @@ RGL_API rgl_status_t rgl_get_extension_info(rgl_extension_t extension, int32_t* return status; } -void TapePlayer::tape_get_extension_info(const YAML::Node& yamlNode) +void TapeCore::tape_get_extension_info(const YAML::Node& yamlNode, PlaybackState& state) { int32_t out_available; int32_t recorded_available = yamlNode[1].as(); @@ -115,7 +118,7 @@ RGL_API rgl_status_t rgl_configure_logging(rgl_log_level_t log_level, const char return status; } -void TapePlayer::tape_configure_logging(const YAML::Node& yamlNode) +void TapeCore::tape_configure_logging(const YAML::Node& yamlNode, PlaybackState& state) { rgl_configure_logging((rgl_log_level_t) yamlNode[0].as(), yamlNode[1].as().c_str(), yamlNode[2].as()); @@ -137,7 +140,14 @@ RGL_API rgl_status_t rgl_cleanup(void) while (!Node::instances.empty()) { auto node = Node::instances.begin()->second; if (node->hasGraphRunCtx()) { - node->getGraphRunCtx()->detachAndDestroy(); + try { + // This iterates over all nodes and may trigger pending exceptions + node->getGraphRunCtx()->detachAndDestroy(); + } + catch (std::exception& e) { + RGL_WARN("rgl_cleanup: caught pending exception in Node {}: {}", node->getName(), e.what()); + continue; // Some node has thrown exception so this graph has not been detached, try again + } } auto connectedNodes = node->disconnectConnectedNodes(); for (auto&& nodeToRelease : connectedNodes) { @@ -153,12 +163,10 @@ RGL_API rgl_status_t rgl_cleanup(void) return status; } -void TapePlayer::tape_cleanup(const YAML::Node& yamlNode) +void TapeCore::tape_cleanup(const YAML::Node& yamlNode, PlaybackState& state) { rgl_cleanup(); - tapeMeshes.clear(); - tapeEntities.clear(); - tapeNodes.clear(); + state.clear(); } RGL_API rgl_status_t rgl_mesh_create(rgl_mesh_t* out_mesh, const rgl_vec3f* vertices, int32_t vertex_count, @@ -181,12 +189,12 @@ RGL_API rgl_status_t rgl_mesh_create(rgl_mesh_t* out_mesh, const rgl_vec3f* vert return status; } -void TapePlayer::tape_mesh_create(const YAML::Node& yamlNode) +void TapeCore::tape_mesh_create(const YAML::Node& yamlNode, PlaybackState& state) { rgl_mesh_t mesh = nullptr; - rgl_mesh_create(&mesh, reinterpret_cast(fileMmap + yamlNode[1].as()), yamlNode[2].as(), - reinterpret_cast(fileMmap + yamlNode[3].as()), yamlNode[4].as()); - tapeMeshes.insert(std::make_pair(yamlNode[0].as(), mesh)); + rgl_mesh_create(&mesh, state.getPtr(yamlNode[1]), yamlNode[2].as(), + state.getPtr(yamlNode[3]), yamlNode[4].as()); + state.meshes.insert(std::make_pair(yamlNode[0].as(), mesh)); } RGL_API rgl_status_t rgl_mesh_set_texture_coords(rgl_mesh_t mesh, const rgl_vec2f* uvs, int32_t uv_count) @@ -203,10 +211,9 @@ RGL_API rgl_status_t rgl_mesh_set_texture_coords(rgl_mesh_t mesh, const rgl_vec2 return status; } -void TapePlayer::tape_mesh_set_texture_coords(const YAML::Node& yamlNode) +void TapeCore::tape_mesh_set_texture_coords(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_mesh_set_texture_coords(tapeMeshes.at(yamlNode[0].as()), - reinterpret_cast(fileMmap + yamlNode[1].as()), + rgl_mesh_set_texture_coords(state.meshes.at(yamlNode[0].as()), state.getPtr(yamlNode[1]), yamlNode[2].as()); } @@ -222,11 +229,11 @@ RGL_API rgl_status_t rgl_mesh_destroy(rgl_mesh_t mesh) return status; } -void TapePlayer::tape_mesh_destroy(const YAML::Node& yamlNode) +void TapeCore::tape_mesh_destroy(const YAML::Node& yamlNode, PlaybackState& state) { auto meshId = yamlNode[0].as(); - rgl_mesh_destroy(tapeMeshes.at(meshId)); - tapeMeshes.erase(meshId); + rgl_mesh_destroy(state.meshes.at(meshId)); + state.meshes.erase(meshId); } RGL_API rgl_status_t rgl_mesh_update_vertices(rgl_mesh_t mesh, const rgl_vec3f* vertices, int32_t vertex_count) @@ -243,13 +250,22 @@ RGL_API rgl_status_t rgl_mesh_update_vertices(rgl_mesh_t mesh, const rgl_vec3f* return status; } -void TapePlayer::tape_mesh_update_vertices(const YAML::Node& yamlNode) +void TapeCore::tape_mesh_update_vertices(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_mesh_update_vertices(tapeMeshes.at(yamlNode[0].as()), - reinterpret_cast(fileMmap + yamlNode[1].as()), + rgl_mesh_update_vertices(state.meshes.at(yamlNode[0].as()), state.getPtr(yamlNode[1]), yamlNode[2].as()); } +rgl_status_t rgl_mesh_is_alive(rgl_mesh_t mesh, bool* out_alive) +{ + auto status = rglSafeCall([&]() { + CHECK_ARG(out_alive != nullptr); + *out_alive = Mesh::instances.contains(mesh); + }); + TAPE_HOOK(mesh, out_alive); + return status; +} + RGL_API rgl_status_t rgl_entity_create(rgl_entity_t* out_entity, rgl_scene_t scene, rgl_mesh_t mesh) { auto status = rglSafeCall([&]() { @@ -264,11 +280,11 @@ RGL_API rgl_status_t rgl_entity_create(rgl_entity_t* out_entity, rgl_scene_t sce return status; } -void TapePlayer::tape_entity_create(const YAML::Node& yamlNode) +void TapeCore::tape_entity_create(const YAML::Node& yamlNode, PlaybackState& state) { rgl_entity_t entity = nullptr; - rgl_entity_create(&entity, nullptr, tapeMeshes.at(yamlNode[2].as())); - tapeEntities.insert(std::make_pair(yamlNode[0].as(), entity)); + rgl_entity_create(&entity, nullptr, state.meshes.at(yamlNode[2].as())); + state.entities.insert(std::make_pair(yamlNode[0].as(), entity)); } RGL_API rgl_status_t rgl_entity_destroy(rgl_entity_t entity) @@ -285,11 +301,11 @@ RGL_API rgl_status_t rgl_entity_destroy(rgl_entity_t entity) return status; } -void TapePlayer::tape_entity_destroy(const YAML::Node& yamlNode) +void TapeCore::tape_entity_destroy(const YAML::Node& yamlNode, PlaybackState& state) { auto entityId = yamlNode[0].as(); - rgl_entity_destroy(tapeEntities.at(entityId)); - tapeEntities.erase(entityId); + rgl_entity_destroy(state.entities.at(entityId)); + state.entities.erase(entityId); } RGL_API rgl_status_t rgl_entity_set_pose(rgl_entity_t entity, const rgl_mat3x4f* transform) @@ -306,10 +322,9 @@ RGL_API rgl_status_t rgl_entity_set_pose(rgl_entity_t entity, const rgl_mat3x4f* return status; } -void TapePlayer::tape_entity_set_pose(const YAML::Node& yamlNode) +void TapeCore::tape_entity_set_pose(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_entity_set_pose(tapeEntities.at(yamlNode[0].as()), - reinterpret_cast(fileMmap + yamlNode[1].as())); + rgl_entity_set_pose(state.entities.at(yamlNode[0].as()), state.getPtr(yamlNode[1])); } RGL_API rgl_status_t rgl_entity_set_id(rgl_entity_t entity, int32_t id) @@ -325,9 +340,9 @@ RGL_API rgl_status_t rgl_entity_set_id(rgl_entity_t entity, int32_t id) return status; } -void TapePlayer::tape_entity_set_id(const YAML::Node& yamlNode) +void TapeCore::tape_entity_set_id(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_entity_set_id(tapeEntities.at(yamlNode[0].as()), yamlNode[1].as::type>()); + rgl_entity_set_id(state.entities.at(yamlNode[0].as()), yamlNode[1].as::type>()); } RGL_API rgl_status_t rgl_entity_set_intensity_texture(rgl_entity_t entity, rgl_texture_t texture) @@ -344,10 +359,37 @@ RGL_API rgl_status_t rgl_entity_set_intensity_texture(rgl_entity_t entity, rgl_t return status; } -void TapePlayer::tape_entity_set_intensity_texture(const YAML::Node& yamlNode) +void TapeCore::tape_entity_set_intensity_texture(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_entity_set_intensity_texture(tapeEntities.at(yamlNode[0].as()), - tapeTextures.at(yamlNode[1].as())); + rgl_entity_set_intensity_texture(state.entities.at(yamlNode[0].as()), + state.textures.at(yamlNode[1].as())); +} + +RGL_API rgl_status_t rgl_entity_set_laser_retro(rgl_entity_t entity, float retro) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_entity_set_laser_retro(entity={}, retro={})", (void*) entity, retro); + CHECK_ARG(entity != nullptr); + Entity::validatePtr(entity)->setLaserRetro(retro); + }); + TAPE_HOOK(entity, retro); + return status; +} + +void TapeCore::tape_entity_set_laser_retro(const YAML::Node& yamlNode, PlaybackState& state) +{ + rgl_entity_set_laser_retro(state.entities.at(yamlNode[0].as()), + yamlNode[1].as::type>()); +} + +rgl_status_t rgl_entity_is_alive(rgl_entity_t entity, bool* out_alive) +{ + auto status = rglSafeCall([&]() { + CHECK_ARG(out_alive != nullptr); + *out_alive = Entity::instances.contains(entity); + }); + TAPE_HOOK(entity, out_alive); + return status; } RGL_API rgl_status_t rgl_texture_create(rgl_texture_t* out_texture, const void* texels, int32_t width, int32_t height) @@ -365,14 +407,13 @@ RGL_API rgl_status_t rgl_texture_create(rgl_texture_t* out_texture, const void* return status; } -void TapePlayer::tape_texture_create(const YAML::Node& yamlNode) +void TapeCore::tape_texture_create(const YAML::Node& yamlNode, PlaybackState& state) { rgl_texture_t texture = nullptr; - rgl_texture_create(&texture, reinterpret_cast(fileMmap + yamlNode[1].as()), yamlNode[2].as(), - yamlNode[3].as()); + rgl_texture_create(&texture, state.getPtr(yamlNode[1]), yamlNode[2].as(), yamlNode[3].as()); - tapeTextures.insert(std::make_pair(yamlNode[0].as(), texture)); + state.textures.insert(std::make_pair(yamlNode[0].as(), texture)); } RGL_API rgl_status_t rgl_texture_destroy(rgl_texture_t texture) @@ -387,11 +428,21 @@ RGL_API rgl_status_t rgl_texture_destroy(rgl_texture_t texture) return status; } -void TapePlayer::tape_texture_destroy(const YAML::Node& yamlNode) +void TapeCore::tape_texture_destroy(const YAML::Node& yamlNode, PlaybackState& state) { auto textureId = yamlNode[0].as(); - rgl_texture_destroy(tapeTextures.at(textureId)); - tapeTextures.erase(textureId); + rgl_texture_destroy(state.textures.at(textureId)); + state.textures.erase(textureId); +} + +rgl_status_t rgl_texture_is_alive(rgl_texture_t texture, bool* out_alive) +{ + auto status = rglSafeCall([&]() { + CHECK_ARG(out_alive != nullptr); + *out_alive = Texture::instances.contains(texture); + }); + TAPE_HOOK(texture, out_alive); + return status; } RGL_API rgl_status_t rgl_scene_set_time(rgl_scene_t scene, uint64_t nanoseconds) @@ -407,7 +458,10 @@ RGL_API rgl_status_t rgl_scene_set_time(rgl_scene_t scene, uint64_t nanoseconds) return status; } -void TapePlayer::tape_scene_set_time(const YAML::Node& yamlNode) { rgl_scene_set_time(nullptr, yamlNode[1].as()); } +void TapeCore::tape_scene_set_time(const YAML::Node& yamlNode, PlaybackState& state) +{ + rgl_scene_set_time(nullptr, yamlNode[1].as()); +} RGL_API rgl_status_t rgl_graph_run(rgl_node_t raw_node) { @@ -425,7 +479,10 @@ RGL_API rgl_status_t rgl_graph_run(rgl_node_t raw_node) return status; } -void TapePlayer::tape_graph_run(const YAML::Node& yamlNode) { rgl_graph_run(tapeNodes.at(yamlNode[0].as())); } +void TapeCore::tape_graph_run(const YAML::Node& yamlNode, PlaybackState& state) +{ + rgl_graph_run(state.nodes.at(yamlNode[0].as())); +} RGL_API rgl_status_t rgl_graph_destroy(rgl_node_t raw_node) { @@ -445,14 +502,14 @@ RGL_API rgl_status_t rgl_graph_destroy(rgl_node_t raw_node) return status; } -void TapePlayer::tape_graph_destroy(const YAML::Node& yamlNode) +void TapeCore::tape_graph_destroy(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_node_t userNode = tapeNodes.at(yamlNode[0].as()); + rgl_node_t userNode = state.nodes.at(yamlNode[0].as()); std::set graph = Node::validatePtr(userNode)->getConnectedComponentNodes(); std::set graphNodeIds; for (auto const& graphNode : graph) { - for (auto const& [key, val] : tapeNodes) { + for (auto const& [key, val] : state.nodes) { auto tapeNodePtr = Node::validatePtr(val).get(); if (graphNode.get() == tapeNodePtr) { graphNodeIds.insert(key); @@ -465,7 +522,7 @@ void TapePlayer::tape_graph_destroy(const YAML::Node& yamlNode) while (!graphNodeIds.empty()) { TapeAPIObjectID nodeId = *graphNodeIds.begin(); - tapeNodes.erase(nodeId); + state.nodes.erase(nodeId); graphNodeIds.erase(nodeId); } } @@ -494,11 +551,11 @@ RGL_API rgl_status_t rgl_graph_get_result_size(rgl_node_t node, rgl_field_t fiel return status; } -void TapePlayer::tape_graph_get_result_size(const YAML::Node& yamlNode) +void TapeCore::tape_graph_get_result_size(const YAML::Node& yamlNode, PlaybackState& state) { int32_t out_count, out_size_of; - rgl_graph_get_result_size(tapeNodes.at(yamlNode[0].as()), (rgl_field_t) yamlNode[1].as(), &out_count, - &out_size_of); + rgl_graph_get_result_size(state.nodes.at(yamlNode[0].as()), (rgl_field_t) yamlNode[1].as(), + &out_count, &out_size_of); auto tape_count = yamlNode[2].as(); auto tape_size_of = yamlNode[3].as(); @@ -565,9 +622,9 @@ RGL_API rgl_status_t rgl_graph_get_result_data(rgl_node_t node, rgl_field_t fiel return status; } -void TapePlayer::tape_graph_get_result_data(const YAML::Node& yamlNode) +void TapeCore::tape_graph_get_result_data(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_node_t node = tapeNodes.at(yamlNode[0].as()); + rgl_node_t node = state.nodes.at(yamlNode[0].as()); rgl_field_t field = (rgl_field_t) yamlNode[1].as(); int32_t out_count, out_size_of; rgl_graph_get_result_size(node, field, &out_count, &out_size_of); @@ -590,9 +647,10 @@ RGL_API rgl_status_t rgl_graph_node_add_child(rgl_node_t parent, rgl_node_t chil return status; } -void TapePlayer::tape_graph_node_add_child(const YAML::Node& yamlNode) +void TapeCore::tape_graph_node_add_child(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_graph_node_add_child(tapeNodes.at(yamlNode[0].as()), tapeNodes.at(yamlNode[1].as())); + rgl_graph_node_add_child(state.nodes.at(yamlNode[0].as()), + state.nodes.at(yamlNode[1].as())); } RGL_API rgl_status_t rgl_graph_node_remove_child(rgl_node_t parent, rgl_node_t child) @@ -608,10 +666,10 @@ RGL_API rgl_status_t rgl_graph_node_remove_child(rgl_node_t parent, rgl_node_t c return status; } -void TapePlayer::tape_graph_node_remove_child(const YAML::Node& yamlNode) +void TapeCore::tape_graph_node_remove_child(const YAML::Node& yamlNode, PlaybackState& state) { - rgl_graph_node_remove_child(tapeNodes.at(yamlNode[0].as()), - tapeNodes.at(yamlNode[1].as())); + rgl_graph_node_remove_child(state.nodes.at(yamlNode[0].as()), + state.nodes.at(yamlNode[1].as())); } RGL_API rgl_status_t rgl_graph_node_set_priority(rgl_node_t node, int32_t priority) @@ -630,10 +688,10 @@ RGL_API rgl_status_t rgl_graph_node_set_priority(rgl_node_t node, int32_t priori return status; } -void TapePlayer::tape_graph_node_set_priority(const YAML::Node& yamlNode) +void TapeCore::tape_graph_node_set_priority(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.at(nodeId); + rgl_node_t node = state.nodes.at(nodeId); auto priority = yamlNode[1].as(); rgl_graph_node_set_priority(node, priority); } @@ -653,10 +711,10 @@ RGL_API rgl_status_t rgl_graph_node_get_priority(rgl_node_t node, int32_t* out_p return status; } -void TapePlayer::tape_graph_node_get_priority(const YAML::Node& yamlNode) +void TapeCore::tape_graph_node_get_priority(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.at(nodeId); + rgl_node_t node = state.nodes.at(nodeId); auto tape_priority = yamlNode[1].as(); int32_t out_priority; rgl_graph_node_get_priority(node, &out_priority); @@ -679,13 +737,12 @@ RGL_API rgl_status_t rgl_node_rays_from_mat3x4f(rgl_node_t* node, const rgl_mat3 return status; } -void TapePlayer::tape_node_rays_from_mat3x4f(const YAML::Node& yamlNode) +void TapeCore::tape_node_rays_from_mat3x4f(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_rays_from_mat3x4f(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_rays_from_mat3x4f(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_rays_set_ring_ids(rgl_node_t* node, const int32_t* ring_ids, int32_t ring_ids_count) @@ -701,13 +758,12 @@ RGL_API rgl_status_t rgl_node_rays_set_ring_ids(rgl_node_t* node, const int32_t* return status; } -void TapePlayer::tape_node_rays_set_ring_ids(const YAML::Node& yamlNode) +void TapeCore::tape_node_rays_set_ring_ids(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_rays_set_ring_ids(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_rays_set_ring_ids(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_rays_set_range(rgl_node_t* node, const rgl_vec2f* ranges, int32_t ranges_count) @@ -723,13 +779,12 @@ RGL_API rgl_status_t rgl_node_rays_set_range(rgl_node_t* node, const rgl_vec2f* return status; } -void TapePlayer::tape_node_rays_set_range(const YAML::Node& yamlNode) +void TapeCore::tape_node_rays_set_range(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_rays_set_range(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_rays_set_range(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_rays_set_time_offsets(rgl_node_t* node, const float* offsets, int32_t offsets_count) @@ -745,13 +800,12 @@ RGL_API rgl_status_t rgl_node_rays_set_time_offsets(rgl_node_t* node, const floa return status; } -void TapePlayer::tape_node_rays_set_time_offsets(const YAML::Node& yamlNode) +void TapeCore::tape_node_rays_set_time_offsets(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_rays_set_time_offsets(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_rays_set_time_offsets(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_rays_transform(rgl_node_t* node, const rgl_mat3x4f* transform) @@ -767,12 +821,12 @@ RGL_API rgl_status_t rgl_node_rays_transform(rgl_node_t* node, const rgl_mat3x4f return status; } -void TapePlayer::tape_node_rays_transform(const YAML::Node& yamlNode) +void TapeCore::tape_node_rays_transform(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_rays_transform(&node, reinterpret_cast(fileMmap + yamlNode[1].as())); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_rays_transform(&node, state.getPtr(yamlNode[1])); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_transform(rgl_node_t* node, const rgl_mat3x4f* transform) @@ -788,12 +842,12 @@ RGL_API rgl_status_t rgl_node_points_transform(rgl_node_t* node, const rgl_mat3x return status; } -void TapePlayer::tape_node_points_transform(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_transform(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_points_transform(&node, reinterpret_cast(fileMmap + yamlNode[1].as())); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_transform(&node, state.getPtr(yamlNode[1])); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_raytrace(rgl_node_t* node, rgl_scene_t scene) @@ -804,47 +858,82 @@ RGL_API rgl_status_t rgl_node_raytrace(rgl_node_t* node, rgl_scene_t scene) CHECK_ARG(scene == nullptr); // TODO: remove once rgl_scene_t param is removed createOrUpdateNode(node); - // Clear velocity that could be set by rgl_node_raytrace_with_distortion - Node::validatePtr(*node)->setVelocity(nullptr, nullptr); + auto raytraceNode = Node::validatePtr(*node); }); TAPE_HOOK(node, scene); return status; } -void TapePlayer::tape_node_raytrace(const YAML::Node& yamlNode) +void TapeCore::tape_node_raytrace(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_raytrace(&node, nullptr); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); } -RGL_API rgl_status_t rgl_node_raytrace_with_distortion(rgl_node_t* node, rgl_scene_t scene, const rgl_vec3f* linear_velocity, - const rgl_vec3f* angular_velocity) +RGL_API rgl_status_t rgl_node_raytrace_configure_velocity(rgl_node_t node, const rgl_vec3f* linear_velocity, + const rgl_vec3f* angular_velocity) { auto status = rglSafeCall([&]() { - RGL_API_LOG("rgl_node_raytrace_with_distortion(node={}, scene={}, linear_velocity={}, angular_velocity={})", repr(node), - (void*) scene, repr(linear_velocity), repr(angular_velocity)); + RGL_API_LOG("rgl_node_raytrace_configure_velocity(node={}, linear_velocity={}, angular_velocity={})", repr(node), + repr(linear_velocity), repr(angular_velocity)); CHECK_ARG(node != nullptr); CHECK_ARG(linear_velocity != nullptr); CHECK_ARG(angular_velocity != nullptr); - CHECK_ARG(scene == nullptr); + RaytraceNode::Ptr raytraceNode = Node::validatePtr(node); + raytraceNode->setVelocity(*reinterpret_cast(linear_velocity), + *reinterpret_cast(angular_velocity)); + }); + TAPE_HOOK(node, linear_velocity, angular_velocity); + return status; +} - createOrUpdateNode(node); - Node::validatePtr(*node)->setVelocity(reinterpret_cast(linear_velocity), - reinterpret_cast(angular_velocity)); +void TapeCore::tape_node_raytrace_configure_velocity(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.at(nodeId); + rgl_node_raytrace_configure_velocity(state.nodes.at(nodeId), state.getPtr(yamlNode[1]), + state.getPtr(yamlNode[2])); +} + +RGL_API rgl_status_t rgl_node_raytrace_configure_distortion(rgl_node_t node, bool enable) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_node_raytrace_configure_distortion(node={}, enable={})", repr(node), enable); + CHECK_ARG(node != nullptr); + RaytraceNode::Ptr raytraceNode = Node::validatePtr(node); + raytraceNode->enableRayDistortion(enable); + }); + TAPE_HOOK(node, enable); + return status; +} + +void TapeCore::tape_node_raytrace_configure_distortion(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.at(nodeId); + rgl_node_raytrace_configure_distortion(node, yamlNode[1].as()); +} + +RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float nearDistance, float farDistance) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_node_raytrace_configure_non_hits(node={}, nearDistance={}, farDistance={})", repr(node), nearDistance, + farDistance); + CHECK_ARG(node != nullptr); + RaytraceNode::Ptr raytraceNode = Node::validatePtr(node); + raytraceNode->setNonHitDistanceValues(nearDistance, farDistance); }); - TAPE_HOOK(node, scene, linear_velocity, angular_velocity); + TAPE_HOOK(node, nearDistance, farDistance); return status; } -void TapePlayer::tape_node_raytrace_with_distortion(const YAML::Node& yamlNode) +void TapeCore::tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_raytrace_with_distortion(&node, nullptr, reinterpret_cast(fileMmap + yamlNode[2].as()), - reinterpret_cast(fileMmap + yamlNode[3].as())); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.at(nodeId); + rgl_node_raytrace_configure_non_hits(node, yamlNode[1].as(), yamlNode[2].as()); } RGL_API rgl_status_t rgl_node_points_format(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count) @@ -861,13 +950,12 @@ RGL_API rgl_status_t rgl_node_points_format(rgl_node_t* node, const rgl_field_t* return status; } -void TapePlayer::tape_node_points_format(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_format(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_points_format(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_format(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_yield(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count) @@ -884,13 +972,12 @@ RGL_API rgl_status_t rgl_node_points_yield(rgl_node_t* node, const rgl_field_t* return status; } -void TapePlayer::tape_node_points_yield(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_yield(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_points_yield(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_yield(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_compact(rgl_node_t* node) @@ -899,18 +986,40 @@ RGL_API rgl_status_t rgl_node_points_compact(rgl_node_t* node) RGL_API_LOG("rgl_node_points_compact(node={})", repr(node)); CHECK_ARG(node != nullptr); - createOrUpdateNode(node); + createOrUpdateNode(node, RGL_FIELD_IS_HIT_I32); }); TAPE_HOOK(node); return status; } -void TapePlayer::tape_node_points_compact(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_compact(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_points_compact(&node); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); +} + +RGL_API rgl_status_t rgl_node_points_compact_by_field(rgl_node_t* node, rgl_field_t field) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_node_points_compact_by_field(node={}, field={})", repr(node), field); + CHECK_ARG(node != nullptr); + CHECK_ARG(field == IS_HIT_I32 || field == IS_GROUND_I32); + + createOrUpdateNode(node, field); + }); + TAPE_HOOK(node, field); + return status; +} + +void TapeCore::tape_node_points_compact_by_field(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_field_t field = (rgl_field_t) yamlNode[1].as(); + rgl_node_points_compact_by_field(&node, field); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_spatial_merge(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count) @@ -927,13 +1036,12 @@ RGL_API rgl_status_t rgl_node_points_spatial_merge(rgl_node_t* node, const rgl_f return status; } -void TapePlayer::tape_node_points_spatial_merge(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_spatial_merge(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes[nodeId] : nullptr; - rgl_node_points_spatial_merge(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; + rgl_node_points_spatial_merge(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_temporal_merge(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count) @@ -950,13 +1058,12 @@ RGL_API rgl_status_t rgl_node_points_temporal_merge(rgl_node_t* node, const rgl_ return status; } -void TapePlayer::tape_node_points_temporal_merge(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_temporal_merge(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes[nodeId] : nullptr; - rgl_node_points_temporal_merge(&node, reinterpret_cast(fileMmap + yamlNode[1].as()), - yamlNode[2].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr; + rgl_node_points_temporal_merge(&node, state.getPtr(yamlNode[1]), yamlNode[2].as()); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* points, int32_t points_count, @@ -981,14 +1088,90 @@ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* po return status; } -void TapePlayer::tape_node_points_from_array(const YAML::Node& yamlNode) +void TapeCore::tape_node_points_from_array(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; - rgl_node_points_from_array( - &node, reinterpret_cast(fileMmap + yamlNode[1].as()), yamlNode[2].as(), - reinterpret_cast(fileMmap + yamlNode[3].as()), yamlNode[4].as()); - tapeNodes.insert({nodeId, node}); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_from_array(&node, state.getPtr(yamlNode[1]), yamlNode[2].as(), + state.getPtr(yamlNode[3]), yamlNode[4].as()); + state.nodes.insert({nodeId, node}); +} + +RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, + int32_t radar_scopes_count, float ray_azimuth_step, + float ray_elevation_step, float frequency, float power_transmitted, + float cumulative_device_gain, float received_noise_mean, + float received_noise_st_dev) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_node_points_radar_postprocess(node={}, radar_scopes={}, ray_azimuth_step={}, ray_elevation_step={}, " + "frequency={}, power_transmitted={}, cumulative_device_gain={}, received_noise_mean={}, " + "received_noise_st_dev={})", + repr(node), repr(radar_scopes, radar_scopes_count), ray_azimuth_step, ray_elevation_step, frequency, + power_transmitted, cumulative_device_gain, received_noise_mean, received_noise_st_dev); + CHECK_ARG(radar_scopes != nullptr); + CHECK_ARG(radar_scopes_count > 0); + CHECK_ARG(ray_azimuth_step > 0); + CHECK_ARG(ray_elevation_step > 0); + CHECK_ARG(frequency > 0); + CHECK_ARG(power_transmitted > 0); + CHECK_ARG(cumulative_device_gain > 0); + CHECK_ARG(received_noise_st_dev > 0); + + for (int i = 0; i < radar_scopes_count; ++i) { + CHECK_ARG(radar_scopes[i].begin_distance >= 0); + CHECK_ARG(radar_scopes[i].end_distance >= 0); + CHECK_ARG(radar_scopes[i].distance_separation_threshold >= 0); + CHECK_ARG(radar_scopes[i].radial_speed_separation_threshold >= 0); + CHECK_ARG(radar_scopes[i].azimuth_separation_threshold >= 0); + CHECK_ARG(radar_scopes[i].end_distance >= radar_scopes[i].begin_distance); + } + + createOrUpdateNode( + node, std::vector{radar_scopes, radar_scopes + radar_scopes_count}, ray_azimuth_step, + ray_elevation_step, frequency, power_transmitted, cumulative_device_gain, received_noise_mean, + received_noise_st_dev); + }); + TAPE_HOOK(node, TAPE_ARRAY(radar_scopes, radar_scopes_count), radar_scopes_count, ray_azimuth_step, ray_elevation_step, + frequency, power_transmitted, cumulative_device_gain, received_noise_mean, received_noise_st_dev); + return status; +} + +void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_radar_postprocess(&node, state.getPtr(yamlNode[1]), yamlNode[2].as(), + yamlNode[3].as(), yamlNode[4].as(), yamlNode[5].as(), + yamlNode[6].as(), yamlNode[7].as(), yamlNode[8].as(), + yamlNode[9].as()); + state.nodes.insert({nodeId, node}); +} + +RGL_API rgl_status_t rgl_node_points_filter_ground(rgl_node_t* node, const rgl_vec3f* sensor_up_vector, + float ground_angle_threshold) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_node_points_filter_ground(node={}, sensor_up_vector={}, ground_angle_threshold={})", repr(node), + repr(sensor_up_vector, 1), ground_angle_threshold); + CHECK_ARG(node != nullptr); + CHECK_ARG(ground_angle_threshold >= 0); + + createOrUpdateNode(node, *reinterpret_cast(sensor_up_vector), + ground_angle_threshold); + }); + TAPE_HOOK(node, sensor_up_vector, ground_angle_threshold); + return status; +} + +void TapeCore::tape_node_points_filter_ground(const YAML::Node& yamlNode, PlaybackState& state) +{ + auto nodeId = yamlNode[0].as(); + auto sensor_up_vector = state.getPtr(yamlNode[1]); + auto ground_angle_threshold = yamlNode[2].as(); + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; + rgl_node_points_filter_ground(&node, sensor_up_vector, ground_angle_threshold); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_gaussian_noise_angular_ray(rgl_node_t* node, float mean, float st_dev, rgl_axis_t rotation_axis) @@ -1006,13 +1189,13 @@ RGL_API rgl_status_t rgl_node_gaussian_noise_angular_ray(rgl_node_t* node, float return status; } -void TapePlayer::tape_node_gaussian_noise_angular_ray(const YAML::Node& yamlNode) +void TapeCore::tape_node_gaussian_noise_angular_ray(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_gaussian_noise_angular_ray(&node, yamlNode[1].as(), yamlNode[2].as(), (rgl_axis_t) yamlNode[3].as()); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_gaussian_noise_angular_hitpoint(rgl_node_t* node, float mean, float st_dev, @@ -1031,13 +1214,13 @@ RGL_API rgl_status_t rgl_node_gaussian_noise_angular_hitpoint(rgl_node_t* node, return status; } -void TapePlayer::tape_node_gaussian_noise_angular_hitpoint(const YAML::Node& yamlNode) +void TapeCore::tape_node_gaussian_noise_angular_hitpoint(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_gaussian_noise_angular_hitpoint(&node, yamlNode[1].as(), yamlNode[2].as(), (rgl_axis_t) yamlNode[3].as()); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); } RGL_API rgl_status_t rgl_node_gaussian_noise_distance(rgl_node_t* node, float mean, float st_dev_base, @@ -1056,12 +1239,22 @@ RGL_API rgl_status_t rgl_node_gaussian_noise_distance(rgl_node_t* node, float me return status; } -void TapePlayer::tape_node_gaussian_noise_distance(const YAML::Node& yamlNode) +void TapeCore::tape_node_gaussian_noise_distance(const YAML::Node& yamlNode, PlaybackState& state) { auto nodeId = yamlNode[0].as(); - rgl_node_t node = tapeNodes.contains(nodeId) ? tapeNodes.at(nodeId) : nullptr; + rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_gaussian_noise_distance(&node, yamlNode[1].as(), yamlNode[2].as(), yamlNode[3].as()); - tapeNodes.insert({nodeId, node}); + state.nodes.insert({nodeId, node}); +} + +rgl_status_t rgl_node_is_alive(rgl_node_t node, bool* out_alive) +{ + auto status = rglSafeCall([&]() { + CHECK_ARG(out_alive != nullptr); + *out_alive = Node::instances.contains(node); + }); + TAPE_HOOK(node, out_alive); + return status; } RGL_API rgl_status_t rgl_tape_record_begin(const char* path) @@ -1133,7 +1326,7 @@ RGL_API rgl_status_t rgl_tape_play(const char* path) throw RecordError("rgl_tape_play: recording active"); } else { TapePlayer player(path); - player.playRealtime(); + player.playApproximatelyRealtime(); } }); #endif //_WIN32 diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp index 0652929fe..7c95a0711 100644 --- a/src/gpu/RaytraceRequestContext.hpp +++ b/src/gpu/RaytraceRequestContext.hpp @@ -24,6 +24,9 @@ struct RaytraceRequestContext Vec3f sensorAngularVelocityRPY; bool doApplyDistortion; + float nearNonHitDistance; + float farNonHitDistance; + const Mat3x4f* rays; size_t rayCount; @@ -40,6 +43,7 @@ struct RaytraceRequestContext OptixTraversableHandle scene; double sceneTime; + float sceneDeltaTime; // Output Field::type* xyz; @@ -48,7 +52,15 @@ struct RaytraceRequestContext Field::type* ringIdx; Field::type* distance; Field::type* intensity; + Field::type* laserRetro; Field::type* timestamp; Field::type* entityId; + Field::type* pointAbsVelocity; + Field::type* pointRelVelocity; + Field::type* radialSpeed; + Field::type* azimuth; + Field::type* elevation; + Field::type* normal; + Field::type* incidentAngle; }; static_assert(std::is_trivially_copyable::value); diff --git a/src/gpu/ShaderBindingTableTypes.h b/src/gpu/ShaderBindingTableTypes.h index 7220b9cbe..49f09b012 100644 --- a/src/gpu/ShaderBindingTableTypes.h +++ b/src/gpu/ShaderBindingTableTypes.h @@ -1,9 +1,14 @@ #pragma once #include +#include #include -struct TriangleMeshSBTData +/** + * This struct is a part of HitGroupRecord in SBT (Shader Binding Table). + * This data is available in raytracing callbacks. + */ +struct EntitySBTData { const Vec3f* vertex; const Vec3i* index; @@ -13,7 +18,16 @@ struct TriangleMeshSBTData const Vec2f* textureCoords; size_t textureCoordsCount; cudaTextureObject_t texture; + float laserRetro; + + // Info about the previous frame: + Mat3x4f prevFrameLocalToWorld; // Must not be used if !hasPrevFrameLocalToWorld + bool hasPrevFrameLocalToWorld; // False, if the previous frame pose is not available + + const Vec3f* vertexDisplacementSincePrevFrame; // May be nullptr; }; +static_assert(std::is_trivially_copyable::value); +static_assert(std::is_trivially_constructible::value); struct __align__(OPTIX_SBT_RECORD_ALIGNMENT) RaygenRecord { char header[OPTIX_SBT_RECORD_HEADER_SIZE]; }; @@ -23,5 +37,5 @@ struct __align__(OPTIX_SBT_RECORD_ALIGNMENT) MissRecord { char header[OPTIX_SBT_ struct __align__(OPTIX_SBT_RECORD_ALIGNMENT) HitgroupRecord { char header[OPTIX_SBT_RECORD_HEADER_SIZE]; - TriangleMeshSBTData data; + EntitySBTData data; }; diff --git a/src/gpu/gaussianNoiseKernels.cu b/src/gpu/gaussianNoiseKernels.cu index 28e9b1db7..581d1b186 100644 --- a/src/gpu/gaussianNoiseKernels.cu +++ b/src/gpu/gaussianNoiseKernels.cu @@ -61,7 +61,7 @@ __global__ void kAddGaussianNoiseDistance(size_t pointCount, float mean, float s Field::type pointInRayOriginTransform = lookAtOriginTransform * inPoints[tid]; outPoints[tid] = lookAtOriginTransform.inverse() * - (pointInRayOriginTransform + pointInRayOriginTransform.normalize() * distanceError); + (pointInRayOriginTransform + pointInRayOriginTransform.normalized() * distanceError); outDistances[tid] = inDistances[tid] + distanceError; } diff --git a/src/gpu/helpersKernels.cu b/src/gpu/helpersKernels.cu index 827fbf9b6..5e4b0cf98 100644 --- a/src/gpu/helpersKernels.cu +++ b/src/gpu/helpersKernels.cu @@ -30,4 +30,20 @@ void gpuSetupRandomNumberGenerator(cudaStream_t stream, size_t elementsCount, un curandStatePhilox4_32_10_t* outPHILOXStates) { run(kSetupRandomNumberGenerator, stream, elementsCount, seed, outPHILOXStates); -} \ No newline at end of file +} +// Updates vertices and calculates their displacement. +// Input: newVertices and oldVertices +// Output: verticesDisplacement and newVertices +__global__ void kUpdateVertices(size_t vertexCount, Vec3f* newVerticesToDisplacement, Vec3f* oldToNewVertices) +{ + LIMIT(vertexCount); + // See Mesh::updateVertices to understand the logic here. + Vec3f newVertex = newVerticesToDisplacement[tid]; + newVerticesToDisplacement[tid] -= oldToNewVertices[tid]; + oldToNewVertices[tid] = newVertex; +} + +void gpuUpdateVertices(cudaStream_t stream, size_t vertexCount, Vec3f* newVerticesToDisplacement, Vec3f* oldToNewVertices) +{ + run(kUpdateVertices, stream, vertexCount, newVerticesToDisplacement, oldToNewVertices); +} diff --git a/src/gpu/helpersKernels.hpp b/src/gpu/helpersKernels.hpp index a5ccd54fc..85b384080 100644 --- a/src/gpu/helpersKernels.hpp +++ b/src/gpu/helpersKernels.hpp @@ -16,5 +16,9 @@ #include #include +#include + void gpuSetupRandomNumberGenerator(cudaStream_t stream, size_t elementsCount, unsigned int seed, - curandStatePhilox4_32_10_t* outPHILOXStates); \ No newline at end of file + curandStatePhilox4_32_10_t* outPHILOXStates); + +void gpuUpdateVertices(cudaStream_t stream, size_t vertexCount, Vec3f* newVerticesToDisplacement, Vec3f* oldToNewVertices); diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index 6f5e4e092..63a2e98b0 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -20,6 +20,7 @@ #include #include +#include __global__ void kFormatSoaToAos(size_t pointCount, size_t pointSize, size_t fieldCount, const GPUFieldDesc* soaInData, char* aosOutData) @@ -54,7 +55,7 @@ __global__ void kTransformPoints(size_t pointCount, const Field::t outPoints[tid] = transform * inPoints[tid]; } -__global__ void kApplyCompaction(size_t pointCount, size_t fieldSize, const Field::type* shouldWrite, +__global__ void kApplyCompaction(size_t pointCount, size_t fieldSize, const int32_t* shouldWrite, const CompactionIndexType* writeIndex, char* dst, const char* src) { LIMIT(pointCount); @@ -78,12 +79,157 @@ __global__ void kFilter(size_t count, const Field::type* indices, c memcpy(dst + tid * fieldSize, src + indices[tid] * fieldSize, fieldSize); } -void gpuFindCompaction(cudaStream_t stream, size_t pointCount, const Field::type* isHit, +__device__ Vec3f reflectPolarization(const Vec3f& pol, const Vec3f& hitNormal, const Vec3f& rayDir) +{ + // Normal incidence + if (abs(rayDir.dot(hitNormal)) == 1) { + return -pol; + } + + const auto diffCrossNormal = rayDir.cross(hitNormal); + const auto polU = diffCrossNormal.normalized(); + const auto polR = rayDir.cross(polU).normalized(); + + const auto refDir = (rayDir - hitNormal * (2 * rayDir.dot(hitNormal))).normalized(); + const auto refPolU = -1.0f * polU; + const auto refPolR = refDir.cross(refPolU); + + const auto polCompU = pol.dot(polU); + const auto polCompR = pol.dot(polR); + + return -polCompR * refPolR + polCompU * refPolU; +} + +__global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq, + Mat3x4f lookAtOriginTransform, const Field::type* rayPose, + const Field::type* hitDist, const Field::type* hitNorm, + const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor) +{ + LIMIT(count); + + constexpr bool log = false; + + constexpr float c0 = 299792458.0f; + constexpr float reflectionCoef = 1.0f; // TODO + const float waveLen = c0 / freq; + const float waveNum = 2.0f * static_cast(M_PI) / waveLen; + const thrust::complex i = {0, 1.0}; + const Vec3f dirX = {1, 0, 0}; + const Vec3f dirY = {0, 1, 0}; + const Vec3f dirZ = {0, 0, 1}; + + const Mat3x4f rayPoseLocal = lookAtOriginTransform * rayPose[tid]; + // const Vec3f hitPosLocal = lookAtOriginTransform * hitPos[tid]; + const Vec3f rayDir = rayPoseLocal * Vec3f{0, 0, 1}; + const Vec3f rayPol = rayPoseLocal * Vec3f{1, 0, 0}; // UP, perpendicular to ray + const Vec3f hitNormalLocal = lookAtOriginTransform.rotation() * hitNorm[tid]; + const float hitDistance = hitDist[tid]; + const float rayArea = hitDistance * hitDistance * sinf(rayElevationStepRad) * rayAzimuthStepRad; + + if (log) + printf("rayDir: (%.4f %.4f %.4f) rayPol: (%.4f %.4f %.4f) hitNormal: (%.4f %.4f %.4f)\n", rayDir.x(), rayDir.y(), + rayDir.z(), rayPol.x(), rayPol.y(), rayPol.z(), hitNormalLocal.x(), hitNormalLocal.y(), hitNormalLocal.z()); + + const float phi = atan2(rayDir[1], rayDir[2]); + const float the = acos(rayDir[0] / rayDir.length()); + + // Consider unit vector of the ray direction, these are its projections: + const float cp = cosf(phi); // X-dir component + const float sp = sinf(phi); // Y-dir component + const float ct = cosf(the); // Z-dir component + const float st = sinf(the); // XY-plane component + + const Vec3f dirP = {0, cp, -sp}; + const Vec3f dirT = {-st, sp * ct, cp * ct}; + const Vec3f vecK = waveNum * ((dirZ * cp + dirY * sp) * st + dirX * ct); + + if (log) + printf("phi: %.2f [dirP: (%.4f %.4f %.4f)] theta: %.2f [dirT: (%.4f %.4f %.4f)] vecK=(%.2f, %.2f, %.2f)\n", phi, + dirP.x(), dirP.y(), dirP.z(), the, dirT.x(), dirT.y(), dirT.z(), vecK.x(), vecK.y(), vecK.z()); + + const Vec3f reflectedDir = (rayDir - hitNormalLocal * (2 * rayDir.dot(hitNormalLocal))).normalized(); + const Vec3f reflectedPol = reflectPolarization(rayPol, hitNormalLocal, rayDir); + const Vector<3, thrust::complex> reflectedPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()}; + const float kr = waveNum * hitDistance; + + if (log) + printf("reflectedDir: (%.4f %.4f %.4f) reflectedPol: (%.4f %.4f %.4f)\n", reflectedDir.x(), reflectedDir.y(), + reflectedDir.z(), reflectedPol.x(), reflectedPol.y(), reflectedPol.z()); + + const Vector<3, thrust::complex> apE = reflectionCoef * exp(i * kr) * reflectedPolCplx; + const Vector<3, thrust::complex> apH = -apE.cross(reflectedDir); + + if (log) + printf("apE: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", apE.x().real(), apE.x().imag(), apE.y().real(), + apE.y().imag(), apE.z().real(), apE.z().imag()); + if (log) + printf("apH: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", apH.x().real(), apH.x().imag(), apH.y().real(), + apH.y().imag(), apH.z().real(), apH.z().imag()); + + const Vector<3, thrust::complex> BU1 = apE.cross(-dirP); + const Vector<3, thrust::complex> BU2 = apH.cross(dirT); + const Vector<3, thrust::complex> refField1 = (-(BU1 + BU2)); + + if (log) + printf("BU1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "BU2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "refField1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", + BU1.x().real(), BU1.x().imag(), BU1.y().real(), BU1.y().imag(), BU1.z().real(), BU1.z().imag(), BU2.x().real(), + BU2.x().imag(), BU2.y().real(), BU2.y().imag(), BU2.z().real(), BU2.z().imag(), refField1.x().real(), + refField1.x().imag(), refField1.y().real(), refField1.y().imag(), refField1.z().real(), refField1.z().imag()); + + const Vector<3, thrust::complex> BR1 = apE.cross(dirT); + const Vector<3, thrust::complex> BR2 = apH.cross(dirP); + const Vector<3, thrust::complex> refField2 = (-(BR1 + BR2)); + + if (log) + printf("BR1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "BR2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "refField2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", + BR1.x().real(), BR1.x().imag(), BR1.y().real(), BR1.y().imag(), BR1.z().real(), BR1.z().imag(), BR2.x().real(), + BR2.x().imag(), BR2.y().real(), BR2.y().imag(), BR2.z().real(), BR2.z().imag(), refField2.x().real(), + refField2.x().imag(), refField2.y().real(), refField2.y().imag(), refField2.z().real(), refField2.z().imag()); + + const thrust::complex BU = refField1.dot(reflectedDir); + const thrust::complex BR = refField2.dot(reflectedDir); + // const thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * static_cast(M_PI)))) * + // exp(-i * waveNum * hitDistance); + const thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea * reflectedDir.dot(hitNormalLocal)) / + (4.0f * static_cast(M_PI)))) * + exp(-i * waveNum * hitDistance); + // const thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * static_cast(M_PI)))) * + // exp(-i * vecK.dot(hitPosLocal)); + + const auto BUf = BU * factor; + const auto BRf = BR * factor; + + if (log) + printf("BU: (%.2f + %.2fi) BR: (%.2f + %.2fi) factor: (%.2f + %.2fi) [BUf: (%.2f + %.2fi) BRf: %.2f + %.2fi]\n", + BU.real(), BU.imag(), BR.real(), BR.imag(), factor.real(), factor.imag(), BUf.real(), BUf.imag(), BRf.real(), + BRf.imag()); + + outBUBRFactor[tid] = {BU, BR, factor}; +} + +__global__ void kFilterGroundPoints(size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold, + const Field::type* inPoints, const Field::type* inNormalsPtr, + Field::type* outNonGround, Mat3x4f lidarTransform) +{ + LIMIT(pointCount); + + // Check if normal is pointing up within given threshold. + const float normalUpAngle = acosf(fabs(inNormalsPtr[tid].dot(sensor_up_vector))); + + outNonGround[tid] = normalUpAngle > ground_angle_threshold; +} + + +void gpuFindCompaction(cudaStream_t stream, size_t pointCount, const int32_t* shouldCompact, CompactionIndexType* hitCountInclusive, size_t* outHitCount) { // beg and end could be used as const pointers, however thrust does not support it - auto beg = thrust::device_ptr(isHit); - auto end = thrust::device_ptr(isHit + pointCount); + auto beg = thrust::device_ptr(shouldCompact); + auto end = thrust::device_ptr(shouldCompact + pointCount); auto dst = thrust::device_ptr(hitCountInclusive); // Note: this will compile only in a .cu file @@ -109,7 +255,7 @@ void gpuTransformRays(cudaStream_t stream, size_t rayCount, const Mat3x4f* inRay run(kTransformRays, stream, rayCount, inRays, outRays, transform); }; -void gpuApplyCompaction(cudaStream_t stream, size_t pointCount, size_t fieldSize, const Field::type* shouldWrite, +void gpuApplyCompaction(cudaStream_t stream, size_t pointCount, size_t fieldSize, const int* shouldWrite, const CompactionIndexType* writeIndex, char* dst, const char* src) { run(kApplyCompaction, stream, pointCount, fieldSize, shouldWrite, writeIndex, dst, src); @@ -132,3 +278,20 @@ void gpuFilter(cudaStream_t stream, size_t count, const Field::type { run(kFilter, stream, count, indices, dst, src, fieldSize); } + +void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold, + const Field::type* inPoints, const Field::type* inNormalsPtr, + Field::type* outNonGround, Mat3x4f lidarTransform) +{ + run(kFilterGroundPoints, stream, pointCount, sensor_up_vector, ground_angle_threshold, inPoints, inNormalsPtr, outNonGround, + lidarTransform); +} + +void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq, + Mat3x4f lookAtOriginTransform, const Field::type* rayPose, + const Field::type* hitDist, const Field::type* hitNorm, + const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor) +{ + run(kRadarComputeEnergy, stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, lookAtOriginTransform, rayPose, + hitDist, hitNorm, hitPos, outBUBRFactor); +} diff --git a/src/gpu/nodeKernels.hpp b/src/gpu/nodeKernels.hpp index 0d5cded9d..f05cfd602 100644 --- a/src/gpu/nodeKernels.hpp +++ b/src/gpu/nodeKernels.hpp @@ -21,6 +21,7 @@ #include #include #include +#include /* * The following functions are asynchronous! @@ -29,17 +30,24 @@ // This could be defined in CompactNode, however such include here causes mess because nvcc does not support C++20. using CompactionIndexType = int32_t; -void gpuFindCompaction(cudaStream_t, size_t pointCount, const Field::type* isHit, - CompactionIndexType* hitCountInclusive, size_t* outHitCount); +void gpuFindCompaction(cudaStream_t, size_t pointCount, const int32_t* shouldCompact, CompactionIndexType* hitCountInclusive, + size_t* outHitCount); void gpuFormatSoaToAos(cudaStream_t, size_t pointCount, size_t pointSize, size_t fieldCount, const GPUFieldDesc* soaInData, char* aosOutData); void gpuFormatAosToSoa(cudaStream_t, size_t pointCount, size_t pointSize, size_t fieldCount, const char* aosInData, const GPUFieldDesc* soaOutData); void gpuTransformRays(cudaStream_t, size_t rayCount, const Mat3x4f* inRays, Mat3x4f* outRays, Mat3x4f transform); -void gpuApplyCompaction(cudaStream_t, size_t pointCount, size_t fieldSize, const Field::type* shouldWrite, +void gpuApplyCompaction(cudaStream_t, size_t pointCount, size_t fieldSize, const int* shouldWrite, const CompactionIndexType* writeIndex, char* dst, const char* src); void gpuTransformPoints(cudaStream_t, size_t pointCount, const Field::type* inPoints, Field::type* outPoints, Mat3x4f transform); void gpuCutField(cudaStream_t, size_t pointCount, char* dst, const char* src, size_t offset, size_t stride, size_t fieldSize); void gpuFilter(cudaStream_t, size_t count, const Field::type* indices, char* dst, const char* src, size_t fieldSize); +void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_axis, float ground_angle_threshold, + const Field::type* inPoints, const Field::type* inNormalsPtr, + Field::type* outNonGround, Mat3x4f lidarTransform); +void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq, + Mat3x4f lookAtOriginTransform, const Field::type* rayPose, + const Field::type* hitDist, const Field::type* hitNorm, + const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor); \ No newline at end of file diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index 838020d6a..30020692f 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -26,6 +26,8 @@ #include +static constexpr float toDeg = (180.0f / M_PI); + extern "C" static __constant__ RaytraceRequestContext ctx; struct Vec3fPayload @@ -54,13 +56,14 @@ __forceinline__ __device__ Vec3f decodePayloadVec3f(const Vec3fPayload& src) } template -__forceinline__ __device__ void saveRayResult(const Vec3f* xyz = nullptr, float distance = NON_HIT_VALUE, float intensity = 0, - const int objectID = RGL_ENTITY_INVALID_ID) +__forceinline__ __device__ void saveRayResult(const Vec3f& xyz, float distance, float intensity, const int objectID, + const Vec3f& absVelocity, const Vec3f& relVelocity, float radialSpeed, + const Vec3f& normal, float incidentAngle, float laserRetro) { const int rayIdx = optixGetLaunchIndex().x; if (ctx.xyz != nullptr) { // Return actual XYZ of the hit point or infinity vector. - ctx.xyz[rayIdx] = isFinite ? *xyz : Vec3f{NON_HIT_VALUE, NON_HIT_VALUE, NON_HIT_VALUE}; + ctx.xyz[rayIdx] = xyz; } if (ctx.isHit != nullptr) { ctx.isHit[rayIdx] = isFinite; @@ -83,27 +86,67 @@ __forceinline__ __device__ void saveRayResult(const Vec3f* xyz = nullptr, float if (ctx.entityId != nullptr) { ctx.entityId[rayIdx] = isFinite ? objectID : RGL_ENTITY_INVALID_ID; } + if (ctx.pointAbsVelocity != nullptr) { + ctx.pointAbsVelocity[rayIdx] = absVelocity; + } + if (ctx.pointRelVelocity != nullptr) { + ctx.pointRelVelocity[rayIdx] = relVelocity; + } + if (ctx.radialSpeed != nullptr) { + ctx.radialSpeed[rayIdx] = radialSpeed; + } + if (ctx.normal != nullptr) { + ctx.normal[rayIdx] = normal; + } + if (ctx.incidentAngle != nullptr) { + ctx.incidentAngle[rayIdx] = incidentAngle; + } + if (ctx.laserRetro != nullptr) { + ctx.laserRetro[rayIdx] = laserRetro; + } +} + +__forceinline__ __device__ void saveNonHitRayResult(float nonHitDistance) +{ + Mat3x4f ray = ctx.rays[optixGetLaunchIndex().x]; + Vec3f origin = ray * Vec3f{0, 0, 0}; + Vec3f dir = ray * Vec3f{0, 0, 1} - origin; + Vec3f displacement = dir.normalized() * nonHitDistance; + displacement = {isnan(displacement.x()) ? 0 : displacement.x(), isnan(displacement.y()) ? 0 : displacement.y(), + isnan(displacement.z()) ? 0 : displacement.z()}; + Vec3f xyz = origin + displacement; + saveRayResult(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.0f, Vec3f{NAN}, NAN, 0.0f); } extern "C" __global__ void __raygen__() { if (ctx.scene == 0) { - saveRayResult(); + saveNonHitRayResult(ctx.farNonHitDistance); return; } const int rayIdx = optixGetLaunchIndex().x; Mat3x4f ray = ctx.rays[rayIdx]; + const Mat3x4f rayLocal = ctx.rayOriginToWorld.inverse() * ray; + + // Assuming up vector is Y, forward vector is Z (true for Unity). + // TODO(msz-rai): allow to define up and forward vectors in RGL + // Assuming rays are generated in left-handed coordinate system with the rotation applied in ZXY order. + // TODO(msz-rai): move ray generation to RGL to unify rotations + if (ctx.azimuth != nullptr) { + ctx.azimuth[rayIdx] = rayLocal.toRotationYOrderZXYLeftHandRad(); + } + if (ctx.elevation != nullptr) { + ctx.elevation[rayIdx] = rayLocal.toRotationXOrderZXYLeftHandRad(); + } if (ctx.doApplyDistortion) { - static const float toDeg = (180.0f / M_PI); - // Velocities are in the local frame. Need to transform rays. - ray = ctx.rayOriginToWorld.inverse() * ray; + // Velocities are in the local frame. Need to operate on rays in local frame. // Ray time offsets are in milliseconds, velocities are in unit per seconds. // In order to not lose numerical precision, first multiply values and then convert to proper unit. ray = Mat3x4f::TRS((ctx.rayTimeOffsets[rayIdx] * ctx.sensorLinearVelocityXYZ) * 0.001f, (ctx.rayTimeOffsets[rayIdx] * (ctx.sensorAngularVelocityRPY * toDeg)) * 0.001f) * - ray; + rayLocal; // Back to the global frame. ray = ctx.rayOriginToWorld * ray; } @@ -120,26 +163,27 @@ extern "C" __global__ void __raygen__() extern "C" __global__ void __closesthit__() { - const TriangleMeshSBTData& sbtData = *(const TriangleMeshSBTData*) optixGetSbtDataPointer(); + const EntitySBTData& entityData = *(const EntitySBTData*) optixGetSbtDataPointer(); const int primID = optixGetPrimitiveIndex(); - assert(primID < sbtData.indexCount); - const Vec3i index = sbtData.index[primID]; + assert(primID < entityData.indexCount); + const Vec3i triangleIndices = entityData.index[primID]; const float u = optixGetTriangleBarycentrics().x; const float v = optixGetTriangleBarycentrics().y; - assert(index.x() < sbtData.vertexCount); - assert(index.y() < sbtData.vertexCount); - assert(index.z() < sbtData.vertexCount); + assert(triangleIndices.x() < entityData.vertexCount); + assert(triangleIndices.y() < entityData.vertexCount); + assert(triangleIndices.z() < entityData.vertexCount); - const Vec3f& A = sbtData.vertex[index.x()]; - const Vec3f& B = sbtData.vertex[index.y()]; - const Vec3f& C = sbtData.vertex[index.z()]; + const Vec3f& A = entityData.vertex[triangleIndices.x()]; + const Vec3f& B = entityData.vertex[triangleIndices.y()]; + const Vec3f& C = entityData.vertex[triangleIndices.z()]; Vec3f hitObject = Vec3f((1 - u - v) * A + u * B + v * C); Vec3f hitWorld = optixTransformPointFromObjectToWorldSpace(hitObject); - int objectID = optixGetInstanceId(); + const int objectID = optixGetInstanceId(); + const float laserRetro = entityData.laserRetro; Vec3f origin = decodePayloadVec3f({optixGetPayload_0(), optixGetPayload_1(), optixGetPayload_2()}); @@ -149,7 +193,7 @@ extern "C" __global__ void __closesthit__() float minRange = ctx.rayRangesCount == 1 ? ctx.rayRanges[0].x() : ctx.rayRanges[optixGetLaunchIndex().x].x(); if (distance < minRange) { - saveRayResult(); + saveNonHitRayResult(ctx.nearNonHitDistance); return; } @@ -162,24 +206,81 @@ extern "C" __global__ void __closesthit__() hitWorld = undistortedOrigin + undistortedDir * distance; } + // Normal vector and incident angle + Vec3f rayDir = (hitWorld - origin).normalized(); + const Vec3f wA = optixTransformPointFromObjectToWorldSpace(A); + const Vec3f wB = optixTransformPointFromObjectToWorldSpace(B); + const Vec3f wC = optixTransformPointFromObjectToWorldSpace(C); + const Vec3f wAB = wB - wA; + const Vec3f wCA = wC - wA; + const Vec3f wNormal = wAB.cross(wCA).normalized(); + const float incidentAngle = acosf(fabs(wNormal.dot(rayDir))); + float intensity = 0; - if (sbtData.textureCoords != nullptr && sbtData.texture != 0) { - assert(index.x() < sbtData.textureCoordsCount); - assert(index.y() < sbtData.textureCoordsCount); - assert(index.z() < sbtData.textureCoordsCount); + bool isIntensityRequested = ctx.intensity != nullptr; + if (isIntensityRequested && entityData.textureCoords != nullptr && entityData.texture != 0) { + assert(triangleIndices.x() < entityData.textureCoordsCount); + assert(triangleIndices.y() < entityData.textureCoordsCount); + assert(triangleIndices.z() < entityData.textureCoordsCount); - const Vec2f& uvA = sbtData.textureCoords[index.x()]; - const Vec2f& uvB = sbtData.textureCoords[index.y()]; - const Vec2f& uvC = sbtData.textureCoords[index.z()]; + const Vec2f& uvA = entityData.textureCoords[triangleIndices.x()]; + const Vec2f& uvB = entityData.textureCoords[triangleIndices.y()]; + const Vec2f& uvC = entityData.textureCoords[triangleIndices.z()]; Vec2f uv = (1 - u - v) * uvA + u * uvB + v * uvC; - intensity = tex2D(sbtData.texture, uv[0], uv[1]); + intensity = tex2D(entityData.texture, uv[0], uv[1]); + } + + Vec3f absPointVelocity{NAN}; + Vec3f relPointVelocity{NAN}; + float radialSpeed{NAN}; + bool isVelocityRequested = ctx.pointAbsVelocity != nullptr || ctx.pointRelVelocity != nullptr || ctx.radialSpeed != nullptr; + if (ctx.sceneDeltaTime > 0 && isVelocityRequested) { + Vec3f displacementFromTransformChange = {0, 0, 0}; + if (entityData.hasPrevFrameLocalToWorld) { + // Computing hit point velocity in simple words: + // From raytracing, we get hit point in Entity's coordinate frame (hitObject). + // Think of it as a marker dot on the Entity. + // Having access to Entity's previous pose, we can compute (entityData.prevFrameLocalToWorld * hitObject), + // where the marker dot would be in the previous raytracing frame (displacementVectorOrigin). + // Then, we can connect marker dot in previous raytracing frame with its current position and obtain displacementFromTransformChange vector + // Dividing displacementFromTransformChange by time elapsed from the previous raytracing frame yields velocity vector. + Vec3f displacementVectorOrigin = entityData.prevFrameLocalToWorld * hitObject; + displacementFromTransformChange = hitWorld - displacementVectorOrigin; + } + + // Some entities may have skinned meshes - in this case entity.vertexDisplacementSincePrevFrame will be non-null + Vec3f displacementFromSkinning = {0, 0, 0}; + bool wasSkinned = entityData.vertexDisplacementSincePrevFrame != nullptr; + if (wasSkinned) { + Mat3x4f objectToWorld; + optixGetObjectToWorldTransformMatrix(reinterpret_cast(objectToWorld.rc)); + const Vec3f& vA = entityData.vertexDisplacementSincePrevFrame[triangleIndices.x()]; + const Vec3f& vB = entityData.vertexDisplacementSincePrevFrame[triangleIndices.y()]; + const Vec3f& vC = entityData.vertexDisplacementSincePrevFrame[triangleIndices.z()]; + displacementFromSkinning = objectToWorld.scaleVec() * Vec3f((1 - u - v) * vA + u * vB + v * vC); + } + + absPointVelocity = (displacementFromTransformChange + displacementFromSkinning) / + static_cast(ctx.sceneDeltaTime); + + // Relative point velocity is a sum of linear velocities difference (between sensor and hit-point) + // and impact of sensor angular velocity + Vec3f absPointVelocityInSensorFrame = ctx.rayOriginToWorld.rotation().inverse() * absPointVelocity; + Vec3f relPointVelocityBasedOnSensorLinearVelocity = absPointVelocityInSensorFrame - ctx.sensorLinearVelocityXYZ; + + Vec3f hitRays = ctx.rayOriginToWorld.inverse() * hitWorld; + Vec3f relPointVelocityBasedOnSensorAngularVelocity = Vec3f(.0f) - ctx.sensorAngularVelocityRPY.cross(hitRays); + relPointVelocity = relPointVelocityBasedOnSensorLinearVelocity + relPointVelocityBasedOnSensorAngularVelocity; + + radialSpeed = hitRays.normalized().dot(relPointVelocity); } - saveRayResult(&hitWorld, distance, intensity, objectID); + saveRayResult(hitWorld, distance, intensity, objectID, absPointVelocity, relPointVelocity, radialSpeed, wNormal, + incidentAngle, laserRetro); } -extern "C" __global__ void __miss__() { saveRayResult(); } +extern "C" __global__ void __miss__() { saveNonHitRayResult(ctx.farNonHitDistance); } extern "C" __global__ void __anyhit__() {} diff --git a/src/graph/CompactPointsNode.cpp b/src/graph/CompactByFieldPointsNode.cpp similarity index 74% rename from src/graph/CompactPointsNode.cpp rename to src/graph/CompactByFieldPointsNode.cpp index b7f88c0d9..f268bfa85 100644 --- a/src/graph/CompactPointsNode.cpp +++ b/src/graph/CompactByFieldPointsNode.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Robotec.AI +// Copyright 2024 Robotec.AI // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,7 +18,9 @@ #include #include -void CompactPointsNode::validateImpl() +void CompactByFieldPointsNode::setParameters(rgl_field_t field) { this->fieldToCompactBy = field; } + +void CompactByFieldPointsNode::validateImpl() { IPointsNodeSingleInput::validateImpl(); // Needed to clear cache because fields in the pipeline may have changed @@ -27,14 +29,18 @@ void CompactPointsNode::validateImpl() cacheManager.clear(); } -void CompactPointsNode::enqueueExecImpl() + +void CompactByFieldPointsNode::enqueueExecImpl() { cacheManager.trigger(); inclusivePrefixSum->resize(input->getHeight() * input->getWidth(), false, false); size_t pointCount = input->getWidth() * input->getHeight(); - const auto* isHit = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + + auto requestedFieldData = input->getFieldData(fieldToCompactBy); + auto typedRequestedFieldDataPtr = requestedFieldData->asTyped()->asSubclass()->getReadPtr(); + if (pointCount > 0) { - gpuFindCompaction(getStreamHandle(), pointCount, isHit, inclusivePrefixSum->getWritePtr(), &width); + gpuFindCompaction(getStreamHandle(), pointCount, typedRequestedFieldDataPtr, inclusivePrefixSum->getWritePtr(), &width); } // getFieldData may be called in client's thread from rgl_graph_get_result_data @@ -50,7 +56,7 @@ void CompactPointsNode::enqueueExecImpl() } } -IAnyArray::ConstPtr CompactPointsNode::getFieldData(rgl_field_t field) +IAnyArray::ConstPtr CompactByFieldPointsNode::getFieldData(rgl_field_t field) { std::lock_guard lock{getFieldDataMutex}; @@ -65,18 +71,21 @@ IAnyArray::ConstPtr CompactPointsNode::getFieldData(rgl_field_t field) if (width > 0) { char* outPtr = static_cast(fieldData->getRawWritePtr()); if (!isDeviceAccessible(input->getFieldData(field)->getMemoryKind())) { - auto msg = fmt::format("CompactPointsNode requires its input to be device-accessible, {} is not", field); + auto msg = fmt::format("CompactByFieldPointsNode requires its input to be device-accessible, {} is not", field); throw InvalidPipeline(msg); } const char* inputPtr = static_cast(input->getFieldData(field)->getRawReadPtr()); - const auto* isHitPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + + auto requestedFieldData = input->getFieldData(fieldToCompactBy); + auto typedRequestedFieldDataPtr = requestedFieldData->asTyped()->asSubclass()->getReadPtr(); + const CompactionIndexType* indices = inclusivePrefixSum->getReadPtr(); - gpuApplyCompaction(getStreamHandle(), input->getPointCount(), getFieldSize(field), isHitPtr, indices, outPtr, + gpuApplyCompaction(getStreamHandle(), input->getPointCount(), getFieldSize(field), typedRequestedFieldDataPtr, indices, outPtr, inputPtr); bool calledFromEnqueue = graphRunCtx.value()->isThisThreadGraphThread(); if (!calledFromEnqueue) { // This is a special case, where API calls getFieldData for this field for the first time - // We did not enqueued compcation in enqueueExecImpl, yet, we are asked for results. + // We did not enqueued compaction in enqueueExecImpl, yet, we are asked for results. // This operation was enqueued in the graph stream, but API won't wait for whole graph stream. // Therefore, we need a manual sync here. // TODO: remove this cancer. @@ -89,7 +98,7 @@ IAnyArray::ConstPtr CompactPointsNode::getFieldData(rgl_field_t field) return std::const_pointer_cast(cacheManager.getValue(field)); } -size_t CompactPointsNode::getWidth() const +size_t CompactByFieldPointsNode::getWidth() const { this->synchronize(); return width; diff --git a/src/graph/FilterGroundPointsNode.cpp b/src/graph/FilterGroundPointsNode.cpp new file mode 100644 index 000000000..53d2759c8 --- /dev/null +++ b/src/graph/FilterGroundPointsNode.cpp @@ -0,0 +1,45 @@ +// Copyright 2024 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +void FilterGroundPointsNode::setParameters(const Vec3f& sensor_up_vector, float ground_angle_threshold) +{ + this->sensor_up_vector = sensor_up_vector; + this->ground_angle_threshold = ground_angle_threshold; +} + +void FilterGroundPointsNode::validateImpl() { IPointsNodeSingleInput::validateImpl(); } + +void FilterGroundPointsNode::enqueueExecImpl() +{ + auto pointCount = input->getPointCount(); + outNonGround->resize(pointCount, false, false); + + const auto* inXyzPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + const auto* inNormalsPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + auto lidarTransform = input->getLookAtOriginTransform(); + + gpuFilterGroundPoints(getStreamHandle(), pointCount, sensor_up_vector, ground_angle_threshold, inXyzPtr, inNormalsPtr, + outNonGround->getWritePtr(), lidarTransform); +} + +IAnyArray::ConstPtr FilterGroundPointsNode::getFieldData(rgl_field_t field) +{ + if (field == IS_GROUND_I32) { + return outNonGround; + } + + return input->getFieldData(field); +} diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 0d46d37b3..22e59c1fb 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -68,17 +68,17 @@ struct FormatPointsNode : IPointsNodeSingleInput GPUFieldDescBuilder gpuFieldDescBuilder; }; -struct CompactPointsNode : IPointsNodeSingleInput +struct CompactByFieldPointsNode : IPointsNodeSingleInput { - using Ptr = std::shared_ptr; - void setParameters() {} + using Ptr = std::shared_ptr; + void setParameters(rgl_field_t field); // Node void validateImpl() override; void enqueueExecImpl() override; // Node requirements - std::vector getRequiredFieldList() const override { return {IS_HIT_I32}; } + std::vector getRequiredFieldList() const override { return {IS_HIT_I32, IS_GROUND_I32}; } // Point cloud description bool isDense() const override { return true; } @@ -89,6 +89,7 @@ struct CompactPointsNode : IPointsNodeSingleInput IAnyArray::ConstPtr getFieldData(rgl_field_t field) override; private: + rgl_field_t fieldToCompactBy; size_t width = {0}; DeviceAsyncArray::Ptr inclusivePrefixSum = DeviceAsyncArray::create(arrayMgr); CacheManager cacheManager; @@ -115,19 +116,27 @@ struct RaytraceNode : IPointsNode // Data getters IAnyArray::ConstPtr getFieldData(rgl_field_t field) override { + if (field == RAY_POSE_MAT3x4_F32) { + return raysNode->getRays(); + } return std::const_pointer_cast(fieldData.at(field)); } // RaytraceNode specific - void setVelocity(const Vec3f* linearVelocity, const Vec3f* angularVelocity); + void setVelocity(const Vec3f& linearVelocity, const Vec3f& angularVelocity); + void enableRayDistortion(bool enabled) { doApplyDistortion = enabled; } + void setNonHitDistanceValues(float nearDistance, float farDistance); private: IRaysNode::Ptr raysNode; DeviceAsyncArray::Ptr defaultRange = DeviceAsyncArray::create(arrayMgr); bool doApplyDistortion{false}; - Vec3f sensorLinearVelocityXYZ; - Vec3f sensorAngularVelocityRPY; + Vec3f sensorLinearVelocityXYZ{0, 0, 0}; + Vec3f sensorAngularVelocityRPY{0, 0, 0}; + + float nearNonHitDistance{std::numeric_limits::infinity()}; + float farNonHitDistance{std::numeric_limits::infinity()}; HostPinnedArray::Ptr requestCtxHst = HostPinnedArray::create(); DeviceAsyncArray::Ptr requestCtxDev = DeviceAsyncArray::create(arrayMgr); @@ -360,7 +369,11 @@ struct FromArrayPointsNode : IPointsNode, INoInputNode void enqueueExecImpl() override {} // Point cloud description - bool isDense() const override { return false; } + bool isDense() const override + { + // If point cloud doesn't contain IS_HIT field we assume all points are hits. + return !fieldData.contains(RGL_FIELD_IS_HIT_I32); + } bool hasField(rgl_field_t field) const override { return fieldData.contains(field); } size_t getWidth() const override { return width; } size_t getHeight() const override { return 1; } @@ -457,3 +470,109 @@ struct GaussianNoiseDistanceNode : IPointsNodeSingleInput DeviceAsyncArray::type>::Ptr outDistance = DeviceAsyncArray::type>::create( arrayMgr); }; + +struct RadarPostprocessPointsNode : IPointsNodeSingleInput +{ + using Ptr = std::shared_ptr; + + void setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, float rayElevationStepRad, + float frequency, float powerTransmitted, float cumulativeDeviceGain, float receivedNoiseMean, + float receivedNoiseStDev); + + // Node + void validateImpl() override; + void enqueueExecImpl() override; + + // Node requirements + std::vector getRequiredFieldList() const override; + + // Point cloud description + size_t getWidth() const override; + size_t getHeight() const override { return 1; } + + // Data getters + IAnyArray::ConstPtr getFieldData(rgl_field_t field) override; + +private: + // Data containers + std::vector::type> filteredIndicesHost; + DeviceAsyncArray::type>::Ptr filteredIndices = DeviceAsyncArray::type>::create( + arrayMgr); + HostPinnedArray::type>::Ptr distanceInputHost = HostPinnedArray::type>::create(); + HostPinnedArray::type>::Ptr azimuthInputHost = HostPinnedArray::type>::create(); + HostPinnedArray::type>::Ptr radialSpeedInputHost = + HostPinnedArray::type>::create(); + HostPinnedArray::type>::Ptr elevationInputHost = HostPinnedArray::type>::create(); + DeviceAsyncArray>>::Ptr outBUBRFactorDev = + DeviceAsyncArray>>::create(arrayMgr); + HostPinnedArray>>::Ptr outBUBRFactorHost = + HostPinnedArray>>::create(); + + HostPageableArray::type>::Ptr clusterRcsHost = HostPageableArray::type>::create(); + HostPageableArray::type>::Ptr clusterPowerHost = HostPageableArray::type>::create(); + HostPageableArray::type>::Ptr clusterNoiseHost = HostPageableArray::type>::create(); + HostPageableArray::type>::Ptr clusterSnrHost = HostPageableArray::type>::create(); + + DeviceAsyncArray::type>::Ptr clusterRcsDev = DeviceAsyncArray::type>::create(arrayMgr); + DeviceAsyncArray::type>::Ptr clusterPowerDev = DeviceAsyncArray::type>::create(arrayMgr); + DeviceAsyncArray::type>::Ptr clusterNoiseDev = DeviceAsyncArray::type>::create(arrayMgr); + DeviceAsyncArray::type>::Ptr clusterSnrDev = DeviceAsyncArray::type>::create(arrayMgr); + + float rayAzimuthStepRad; + float rayElevationStepRad; + float frequencyHz; + float powerTransmittedDbm; + float cumulativeDeviceGainDbi; + float receivedNoiseMeanDb; + float receivedNoiseStDevDb; + + std::vector radarScopes; + + std::random_device randomDevice; + + // RGL related members + std::mutex getFieldDataMutex; + mutable CacheManager cacheManager; + + struct RadarCluster + { + RadarCluster(Field::type index, float distance, float azimuth, float radialSpeed, float elevation); + RadarCluster(RadarCluster&& other) noexcept = default; + RadarCluster& operator=(RadarCluster&& other) noexcept = default; + + void addPoint(Field::type index, float distance, float azimuth, float radialSpeed, float elevation); + inline bool isCandidate(float distance, float azimuth, float radialSpeed, const rgl_radar_scope_t& separations) const; + inline bool canMergeWith(const RadarCluster& other, const std::vector& radarScopes) const; + void takeIndicesFrom(RadarCluster&& other); + Field::type findDirectionalCenterIndex(const Field::type* azimuths, + const Field::type* elevations) const; + + std::vector::type> indices; + Vector<2, Field::type> minMaxDistance; + Vector<2, Field::type> minMaxAzimuth; + Vector<2, Field::type> minMaxRadialSpeed; + Vector<2, Field::type> minMaxElevation; // For finding directional center only + }; +}; + +struct FilterGroundPointsNode : IPointsNodeSingleInput +{ + using Ptr = std::shared_ptr; + void setParameters(const Vec3f& sensor_up_vector, float ground_angle_threshold); + + // Node + void validateImpl() override; + void enqueueExecImpl() override; + + // Node requirements + std::vector getRequiredFieldList() const override { return {XYZ_VEC3_F32, NORMAL_VEC3_F32}; }; + + // Data getters + IAnyArray::ConstPtr getFieldData(rgl_field_t field) override; + +private: + Vec3f sensor_up_vector; + float ground_angle_threshold; + DeviceAsyncArray::type>::Ptr outNonGround = DeviceAsyncArray::type>::create( + arrayMgr); +}; diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp new file mode 100644 index 000000000..d39cf64be --- /dev/null +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -0,0 +1,363 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include + +inline static std::optional getRadarScopeWithinDistance(const std::vector& radarScopes, + Field::type distance) +{ + for (auto&& scope : radarScopes) { + if (scope.begin_distance <= distance && distance <= scope.end_distance) { + return scope; + } + } + return std::nullopt; +} + +void RadarPostprocessPointsNode::setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, + float rayElevationStepRad, float frequency, float powerTransmitted, + float cumulativeDeviceGain, float receivedNoiseMean, float receivedNoiseStDev) +{ + this->rayAzimuthStepRad = rayAzimuthStepRad; + this->rayElevationStepRad = rayElevationStepRad; + this->frequencyHz = frequency; + this->radarScopes = radarScopes; + this->powerTransmittedDbm = powerTransmitted; + this->cumulativeDeviceGainDbi = cumulativeDeviceGain; + this->receivedNoiseMeanDb = receivedNoiseMean; + this->receivedNoiseStDevDb = receivedNoiseStDev; +} + +void RadarPostprocessPointsNode::validateImpl() +{ + IPointsNodeSingleInput::validateImpl(); + + if (!input->isDense()) { + throw InvalidPipeline("RadarComputeEnergyPointsNode requires dense input"); + } + + // Needed to clear cache because fields in the pipeline may have changed + // In fact, the cache manager is no longer useful here + // To be kept/removed in some future refactor (when resolving comment in the `enqueueExecImpl`) + cacheManager.clear(); +} + +void RadarPostprocessPointsNode::enqueueExecImpl() +{ + cacheManager.trigger(); + + auto raysPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + auto distancePtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + auto normalPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + auto xyzPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); + outBUBRFactorDev->resize(input->getPointCount(), false, false); + gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, frequencyHz, + input->getLookAtOriginTransform(), raysPtr, distancePtr, normalPtr, xyzPtr, + outBUBRFactorDev->getWritePtr()); + outBUBRFactorHost->copyFrom(outBUBRFactorDev); + CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); + + if (input->getPointCount() == 0) { + filteredIndices->resize(0, false, false); + return; + } + + distanceInputHost->copyFrom(input->getFieldData(DISTANCE_F32)); + azimuthInputHost->copyFrom(input->getFieldData(AZIMUTH_F32)); + radialSpeedInputHost->copyFrom(input->getFieldData(RADIAL_SPEED_F32)); + elevationInputHost->copyFrom(input->getFieldData(ELEVATION_F32)); + + std::vector clusters; + for (int i = 0; i < input->getPointCount(); ++i) { + const auto distance = distanceInputHost->at(i); + const auto azimuth = azimuthInputHost->at(i); + const auto radialSpeed = radialSpeedInputHost->at(i); + const auto elevation = elevationInputHost->at(i); + bool isPointClustered = false; + const auto radarScope = getRadarScopeWithinDistance(radarScopes, distance); + if (!radarScope.has_value()) { + continue; + } + for (auto&& cluster : clusters) { + if (cluster.isCandidate(distance, azimuth, radialSpeed, radarScope.value())) { + cluster.addPoint(i, distance, azimuth, radialSpeed, elevation); + isPointClustered = true; + break; + } + } + + if (!isPointClustered) { + // Create a new cluster + clusters.emplace_back(i, distance, azimuth, radialSpeed, elevation); + } + } + + // Merge clusters if are close enough + bool allClustersGood = false; + while (clusters.size() > 1 && !allClustersGood) { + allClustersGood = true; + for (int i = 0; i < clusters.size(); ++i) { + for (int j = i + 1; j < clusters.size(); ++j) { + if (clusters[i].canMergeWith(clusters[j], radarScopes)) { + clusters[i].takeIndicesFrom(std::move(clusters[j])); + clusters.erase(clusters.begin() + j); + allClustersGood = false; + break; + } + } + if (!allClustersGood) { + break; + } + } + } + + filteredIndicesHost.clear(); + for (auto&& cluster : clusters) { + filteredIndicesHost.push_back( + cluster.findDirectionalCenterIndex(azimuthInputHost->getReadPtr(), elevationInputHost->getReadPtr())); + } + + filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); + + const auto lambda = 299'792'458.0f / frequencyHz; + const auto lambdaSqrtDbsm = 10.0f * log10f(lambda * lambda); + + // Compute per-cluster properties + clusterRcsHost->resize(filteredIndicesHost.size(), false, false); + clusterPowerHost->resize(filteredIndicesHost.size(), false, false); + clusterNoiseHost->resize(filteredIndicesHost.size(), false, false); + clusterSnrHost->resize(filteredIndicesHost.size(), false, false); + std::normal_distribution gaussianNoise(receivedNoiseMeanDb, receivedNoiseStDevDb); + for (int clusterIdx = 0; clusterIdx < clusters.size(); ++clusterIdx) { + std::complex AU = 0; + std::complex AR = 0; + auto&& cluster = clusters[clusterIdx]; + + for (const auto pointInCluster : cluster.indices) { + std::complex BU = {outBUBRFactorHost->at(pointInCluster)[0].real(), + outBUBRFactorHost->at(pointInCluster)[0].imag()}; + std::complex BR = {outBUBRFactorHost->at(pointInCluster)[1].real(), + outBUBRFactorHost->at(pointInCluster)[1].imag()}; + std::complex factor = {outBUBRFactorHost->at(pointInCluster)[2].real(), + outBUBRFactorHost->at(pointInCluster)[2].imag()}; + AU += BU * factor; + AR += BR * factor; + } + + // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation + const auto rcsDbsm = 10.0f * log10f(4.0f * std::numbers::pi_v * (powf(abs(AU), 2) + powf(abs(AR), 2))); + + // TODO: Handle nans in RCS. + if (std::isnan(rcsDbsm)) { + throw InvalidPipeline("RCS is NaN"); + } + + const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); + const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); + + const auto powerReceived = powerTransmittedDbm + cumulativeDeviceGainDbi + cumulativeDeviceGainDbi + rcsDbsm + + lambdaSqrtDbsm - multiplier; + + clusterRcsHost->at(clusterIdx) = rcsDbsm; + clusterNoiseHost->at(clusterIdx) = gaussianNoise(randomDevice); + clusterPowerHost->at(clusterIdx) = powerReceived + clusterNoiseHost->at(clusterIdx); // power received + noise + clusterSnrHost->at(clusterIdx) = clusterPowerHost->at(clusterIdx) - clusterNoiseHost->at(clusterIdx); + } + + clusterPowerDev->copyFrom(clusterPowerHost); + clusterRcsDev->copyFrom(clusterRcsHost); + clusterNoiseDev->copyFrom(clusterNoiseHost); + clusterSnrDev->copyFrom(clusterSnrHost); + + // getFieldData may be called in client's thread from rgl_graph_get_result_data + // Doing job there would be: + // - unexpected (job was supposed to be done asynchronously) + // - hard to implement: + // - to avoid blocking on yet-running graph stream, we would need do it in copy stream, which would require + // temporary rebinding DAAs to copy stream, which seems like nightmarish idea + // Therefore, once we know what fields are requested, we compute them eagerly + // This is supposed to be removed in some future refactor (e.g. when introducing LayeredSoA) + for (auto&& field : cacheManager.getKeys()) { + getFieldData(field); + } +} + +size_t RadarPostprocessPointsNode::getWidth() const +{ + this->synchronize(); + return filteredIndices->getCount(); +} + +IAnyArray::ConstPtr RadarPostprocessPointsNode::getFieldData(rgl_field_t field) +{ + std::lock_guard lock{getFieldDataMutex}; + + if (field == POWER_F32) { + return clusterPowerDev->asAny(); + } + if (field == RCS_F32) { + return clusterRcsDev->asAny(); + } + if (field == NOISE_F32) { + return clusterNoiseDev->asAny(); + } + if (field == SNR_F32) { + return clusterSnrDev->asAny(); + } + + if (!cacheManager.contains(field)) { + auto fieldData = createArray(field, arrayMgr); + fieldData->resize(filteredIndices->getCount(), false, false); + cacheManager.insert(field, fieldData, true); + } + + if (!cacheManager.isLatest(field)) { + auto fieldData = cacheManager.getValue(field); + fieldData->resize(filteredIndices->getCount(), false, false); + char* outPtr = static_cast(fieldData->getRawWritePtr()); + auto fieldArray = input->getFieldData(field); + if (!isDeviceAccessible(fieldArray->getMemoryKind())) { + auto msg = fmt::format("RadarPostprocessPoints requires its input to be device-accessible, {} is not", field); + throw InvalidPipeline(msg); + } + const char* inputPtr = static_cast(fieldArray->getRawReadPtr()); + gpuFilter(getStreamHandle(), filteredIndices->getCount(), filteredIndices->getReadPtr(), outPtr, inputPtr, + getFieldSize(field)); + CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); + cacheManager.setUpdated(field); + } + + return cacheManager.getValue(field); +} + +std::vector RadarPostprocessPointsNode::getRequiredFieldList() const +{ + return {DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32, RAY_POSE_MAT3x4_F32, NORMAL_VEC3_F32, XYZ_VEC3_F32}; +} + +// RadarCluster methods implementation + +RadarPostprocessPointsNode::RadarCluster::RadarCluster(Field::type index, float distance, float azimuth, + float radialSpeed, float elevation) +{ + indices.emplace_back(index); + minMaxDistance = {distance, distance}; + minMaxAzimuth = {azimuth, azimuth}; + minMaxRadialSpeed = {radialSpeed, radialSpeed}; + minMaxElevation = {elevation, elevation}; +} + +void RadarPostprocessPointsNode::RadarCluster::addPoint(Field::type index, float distance, float azimuth, + float radialSpeed, float elevation) +{ + indices.emplace_back(index); + minMaxDistance[0] = std::min(minMaxDistance[0], distance); + minMaxDistance[1] = std::max(minMaxDistance[1], distance); + minMaxAzimuth[0] = std::min(minMaxAzimuth[0], azimuth); + minMaxAzimuth[1] = std::max(minMaxAzimuth[1], azimuth); + minMaxRadialSpeed[0] = std::min(minMaxRadialSpeed[0], radialSpeed); + minMaxRadialSpeed[1] = std::max(minMaxRadialSpeed[1], radialSpeed); + minMaxElevation[0] = std::min(minMaxElevation[0], elevation); + minMaxElevation[1] = std::max(minMaxElevation[1], elevation); +} + +inline bool RadarPostprocessPointsNode::RadarCluster::isCandidate(float distance, float azimuth, float radialSpeed, + const rgl_radar_scope_t& radarScope) const +{ + const auto isWithinDistanceLowerBound = distance + radarScope.distance_separation_threshold >= minMaxDistance[0]; + const auto isWithinDistanceUpperBound = distance - radarScope.distance_separation_threshold <= minMaxDistance[1]; + const auto isWithinAzimuthLowerBound = azimuth + radarScope.azimuth_separation_threshold >= minMaxAzimuth[0]; + const auto isWithinAzimuthUpperBound = azimuth - radarScope.azimuth_separation_threshold <= minMaxAzimuth[1]; + const auto isWithinRadialSpeedLowerBound = radialSpeed + radarScope.radial_speed_separation_threshold >= + minMaxRadialSpeed[0]; + const auto isWithinRadialSpeedUpperBound = radialSpeed - radarScope.radial_speed_separation_threshold <= + minMaxRadialSpeed[1]; + return isWithinDistanceLowerBound && isWithinDistanceUpperBound && isWithinAzimuthLowerBound && isWithinAzimuthUpperBound && + isWithinRadialSpeedLowerBound && isWithinRadialSpeedUpperBound; +} + +inline bool RadarPostprocessPointsNode::RadarCluster::canMergeWith(const RadarCluster& other, + const std::vector& radarScopes) const +{ + // Helper functions + auto doRangesOverlap = [](const Vec2f& a, const Vec2f& b) { + return (b[0] <= a[1] && b[1] >= a[0]) || (a[0] <= b[1] && a[1] >= b[0]); + }; + auto areRangesWithinThreshold = [](const Vec2f& a, const Vec2f& b, float threshold) { + return std::abs(a[1] - b[0]) <= threshold || std::abs(b[1] - a[0]) <= threshold; + }; + + // Find distances that will be compared with each other: + // |---cluster1---| |---cluster2---| + // ^ ^ + const float minDistanceToCompare = std::max(minMaxDistance[0], other.minMaxDistance[0]); + const float maxDistanceToCompare = std::min(minMaxDistance[1], other.minMaxDistance[1]); + const auto radarScope = getRadarScopeWithinDistance(radarScopes, std::max(minDistanceToCompare, maxDistanceToCompare)); + + assert(radarScope.has_value()); // Must have value because it was already checked when creating clusters + + bool isDistanceGood = doRangesOverlap(minMaxDistance, other.minMaxDistance) || + areRangesWithinThreshold(minMaxDistance, other.minMaxDistance, + radarScope->distance_separation_threshold); + + bool isAzimuthGood = doRangesOverlap(minMaxAzimuth, other.minMaxAzimuth) || + areRangesWithinThreshold(minMaxAzimuth, other.minMaxAzimuth, radarScope->azimuth_separation_threshold); + + bool isRadialSpeedGood = doRangesOverlap(minMaxRadialSpeed, other.minMaxRadialSpeed) || + areRangesWithinThreshold(minMaxRadialSpeed, other.minMaxRadialSpeed, + radarScope->radial_speed_separation_threshold); + + return isDistanceGood && isAzimuthGood && isRadialSpeedGood; +} + +void RadarPostprocessPointsNode::RadarCluster::takeIndicesFrom(RadarCluster&& other) +{ + minMaxDistance[0] = std::min(minMaxDistance[0], other.minMaxDistance[0]); + minMaxDistance[1] = std::max(minMaxDistance[1], other.minMaxDistance[1]); + minMaxAzimuth[0] = std::min(minMaxAzimuth[0], other.minMaxAzimuth[0]); + minMaxAzimuth[1] = std::max(minMaxAzimuth[1], other.minMaxAzimuth[1]); + minMaxRadialSpeed[0] = std::min(minMaxRadialSpeed[0], other.minMaxRadialSpeed[0]); + minMaxRadialSpeed[1] = std::max(minMaxRadialSpeed[1], other.minMaxRadialSpeed[1]); + minMaxElevation[0] = std::min(minMaxElevation[0], other.minMaxElevation[0]); + minMaxElevation[1] = std::max(minMaxElevation[1], other.minMaxElevation[1]); + + // Move indices + std::size_t n = indices.size(); + indices.resize(indices.size() + other.indices.size()); + std::move(other.indices.begin(), other.indices.end(), indices.begin() + n); +} + +Field::type RadarPostprocessPointsNode::RadarCluster::findDirectionalCenterIndex( + const Field::type* azimuths, const Field::type* elevations) const +{ + auto meanAzimuth = (minMaxAzimuth[0] + minMaxAzimuth[1]) / 2.0f; + auto meanElevation = (minMaxElevation[0] + minMaxElevation[1]) / 2.0f; + + float minDistance = FLT_MAX; + uint32_t minIndex = indices.front(); + + for (auto&& i : indices) { + float distance = std::abs(azimuths[i] - meanAzimuth) + std::abs(elevations[i] - meanElevation); + if (distance < minDistance) { + minDistance = distance; + minIndex = i; + } + } + return minIndex; +} diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index 7dcd2ae33..17ba2eba0 100644 --- a/src/graph/RaytraceNode.cpp +++ b/src/graph/RaytraceNode.cpp @@ -82,6 +82,8 @@ void RaytraceNode::enqueueExecImpl() .sensorLinearVelocityXYZ = sensorLinearVelocityXYZ, .sensorAngularVelocityRPY = sensorAngularVelocityRPY, .doApplyDistortion = doApplyDistortion, + .nearNonHitDistance = nearNonHitDistance, + .farNonHitDistance = farNonHitDistance, .rays = raysPtr, .rayCount = raysNode->getRayCount(), .rayOriginToWorld = raysNode->getCumulativeRayTransfrom(), @@ -94,14 +96,23 @@ void RaytraceNode::enqueueExecImpl() .rayTimeOffsetsCount = timeOffsets.has_value() ? (*timeOffsets)->getCount() : 0, .scene = sceneAS, .sceneTime = Scene::instance().getTime().value_or(Time::zero()).asSeconds(), + .sceneDeltaTime = static_cast(Scene::instance().getDeltaTime().value_or(Time::zero()).asSeconds()), .xyz = getPtrTo(), .isHit = getPtrTo(), .rayIdx = getPtrTo(), .ringIdx = getPtrTo(), .distance = getPtrTo(), .intensity = getPtrTo(), + .laserRetro = getPtrTo(), .timestamp = getPtrTo(), .entityId = getPtrTo(), + .pointAbsVelocity = getPtrTo(), + .pointRelVelocity = getPtrTo(), + .radialSpeed = getPtrTo(), + .azimuth = getPtrTo(), + .elevation = getPtrTo(), + .normal = getPtrTo(), + .incidentAngle = getPtrTo(), }; requestCtxDev->copyFrom(requestCtxHst); @@ -156,14 +167,14 @@ std::set RaytraceNode::findFieldsToCompute() return outFields; } -void RaytraceNode::setVelocity(const Vec3f* linearVelocity, const Vec3f* angularVelocity) +void RaytraceNode::setVelocity(const Vec3f& linearVelocity, const Vec3f& angularVelocity) { - doApplyDistortion = linearVelocity != nullptr && angularVelocity != nullptr; - - if (!doApplyDistortion) { - return; - } + sensorLinearVelocityXYZ = linearVelocity; + sensorAngularVelocityRPY = angularVelocity; +} - sensorLinearVelocityXYZ = *linearVelocity; - sensorAngularVelocityRPY = *angularVelocity; +void RaytraceNode::setNonHitDistanceValues(float nearDistance, float farDistance) +{ + nearNonHitDistance = nearDistance; + farNonHitDistance = farDistance; } diff --git a/src/math/Mat3x4f.hpp b/src/math/Mat3x4f.hpp index fe39ca2fc..6f5511e76 100644 --- a/src/math/Mat3x4f.hpp +++ b/src/math/Mat3x4f.hpp @@ -33,6 +33,8 @@ struct Mat3x4f static HostDevFn inline Mat3x4f scale(float x, float y, float z) { return {x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0}; } + static HostDevFn inline Mat3x4f scale(Vec3f s) { return Mat3x4f::scale(s.x(), s.y(), s.z()); } + static HostDevFn inline Mat3x4f rotationRad(float x, float y, float z) { // Based on https://github.com/microsoft/DirectXMath/blob/main/Inc/DirectXMathMatrix.inl#L1697 @@ -57,6 +59,8 @@ struct Mat3x4f return m; } + static HostDevFn inline Mat3x4f rotationRad(Vec3f r) { return Mat3x4f::rotationRad(r.x(), r.y(), r.z()); } + static HostDevFn inline Mat3x4f rotationRad(rgl_axis_t axis, float angleRad) { float rotX = axis == RGL_AXIS_X ? angleRad : 0.0f; @@ -65,10 +69,16 @@ struct Mat3x4f return rotationRad(rotX, rotY, rotZ); } - static HostDevFn inline Mat3x4f rotation(float x, float y, float z) + static HostDevFn inline Mat3x4f rotationDeg(float x, float y, float z) + { + return rotationRad(Vec3f{x, y, z} * (static_cast(M_PI) / 180.0f)); + } + + static HostDevFn inline Mat3x4f rotationDeg(Vec3f r) { return Mat3x4f::rotationDeg(r.x(), r.y(), r.z()); } + + static HostDevFn inline Mat3x4f rotationDeg(rgl_axis_t axis, float angleDeg) { - float toRad = (M_PI / 180.0f); - return rotationRad(x * toRad, y * toRad, z * toRad); + return Mat3x4f::rotationRad(axis, angleDeg * (static_cast(M_PI) / 180.0f)); } static HostDevFn inline Mat3x4f translation(float x, float y, float z) { return {1, 0, 0, x, 0, 1, 0, y, 0, 0, 1, z}; } @@ -81,7 +91,7 @@ struct Mat3x4f static HostDevFn inline Mat3x4f TRS(Vec3f t, Vec3f r = {0, 0, 0}, Vec3f s = {1, 1, 1}) { auto T = Mat3x4f::translation(t.x(), t.y(), t.z()); - auto R = Mat3x4f::rotation(r.x(), r.y(), r.z()); + auto R = Mat3x4f::rotationDeg(r.x(), r.y(), r.z()); auto S = Mat3x4f::scale(s.x(), s.y(), s.z()); return T * R * S; } @@ -133,6 +143,46 @@ struct Mat3x4f return {rc[0][0], rc[0][1], rc[0][2], 0.0f, rc[1][0], rc[1][1], rc[1][2], 0.0f, rc[2][0], rc[2][1], rc[2][2], 0.0f}; } + HostDevFn inline float toRotationXOrderZXYLeftHandRad() const + { + // Assuming rotation has been applied in the order: z x y in left-handed coordinate system + // Based on: https://www.geometrictools.com/Documentation/EulerAngles.pdf + // Taking transpose of the rotation matrix, because X axis facing opposite direction in left-handed coordinate system + if (rc[1][2] < 1) { + if (rc[1][2] > -1) { + return asinf(rc[1][2]); + } else { + return -static_cast(M_PI) / 2.0f; + } + } else { + return static_cast(M_PI) / 2.0f; + } + } + + HostDevFn inline float toRotationYOrderZXYLeftHandRad() const + { + // Assuming rotation has been applied in the order: z x y in left-handed coordinate system + // Based on: https://www.geometrictools.com/Documentation/EulerAngles.pdf + if (rc[2][1] < 1) { + if (rc[2][1] > -1) { + return atan2f(-rc[2][0], rc[2][2]); + } else { + return 0.0f; + } + } else { + return 0.0f; + } + } + + HostDevFn inline Vec3f scaleVec() const + { + return { + sqrtf(rc[0][0] * rc[0][0] + rc[1][0] * rc[1][0] + rc[2][0] * rc[2][0]), + sqrtf(rc[0][1] * rc[0][1] + rc[1][1] * rc[1][1] + rc[2][1] * rc[2][1]), + sqrtf(rc[0][2] * rc[0][2] + rc[1][2] * rc[1][2] + rc[2][2] * rc[2][2]), + }; + } + // Converts to Matrix 4x4 and performs inverse operation. // If determinant is zero (cannot inverse) it returns Matrix filled with zeros. HostDevFn inline Mat3x4f inverse() const noexcept @@ -243,7 +293,7 @@ struct fmt::formatter }; #endif // __CUDACC__ -static_assert(std::is_trivially_copyable::value); -static_assert(std::is_trivially_constructible::value); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_standard_layout_v); static_assert(sizeof(Mat3x4f) == 12 * sizeof(float)); static_assert(alignof(Mat3x4f) == 4); diff --git a/src/math/Vector.hpp b/src/math/Vector.hpp index 85c599fe4..a36a085ae 100644 --- a/src/math/Vector.hpp +++ b/src/math/Vector.hpp @@ -55,7 +55,7 @@ struct Vector HostDevFn Vector(const Vector& other) { for (int i = 0; i < dim; ++i) { - row[i] = static_cast(other.row[i]); + row[i] = static_cast(other[i]); } } @@ -77,7 +77,7 @@ struct Vector {} template && dim == 3>> - DevFn operator float3() + DevFn operator float3() const { return float3{row[0], row[1], row[2]}; } @@ -126,6 +126,15 @@ struct Vector PIECEWISE_OPERATOR(/, /=) #undef PIECEWISE_OPERATOR + HostDevFn V operator-() const + { + V v; + for (int i = 0; i < dim; ++i) { + v.row[i] = -this->row[i]; + } + return v; + } + HostDevFn T lengthSquared() const { auto sum = static_cast(0); @@ -139,7 +148,7 @@ struct Vector HostDevFn V half() const { return *this / V{static_cast(2)}; } - HostDevFn V normalize() const { return *this / length(); } + HostDevFn V normalized() const { return *this / length(); } HostDevFn T min() const { @@ -159,6 +168,32 @@ struct Vector return value; } + HostDevFn T dot(const V& other) const + { + T value = static_cast(0); + for (int i = 0; i < dim; ++i) { + value += row[i] * other.row[i]; + } + return value; + } + + // Only for Vector with dim == 3 + template = 0> + HostDevFn V cross(const V& other) const + { + V value = V(static_cast(0)); + value[0] = row[1] * other.row[2] - row[2] * other.row[1]; + value[1] = row[2] * other.row[0] - row[0] * other.row[2]; + value[2] = row[0] * other.row[1] - row[1] * other.row[0]; + return value; + } + + template = 0> + HostDevFn V toSpherical() const + { + return {length(), row[0] == 0 && row[1] == 0 ? 0 : atan2(row[1], row[0]), std::acos(row[2] / length())}; + } + private: T row[dim]; }; @@ -198,10 +233,16 @@ using Vec2i = Vector<2, int>; using Vec3i = Vector<3, int>; using Vec4i = Vector<4, int>; -static_assert(std::is_trivially_copyable::value); -static_assert(std::is_trivially_copyable::value); -static_assert(std::is_trivially_copyable::value); - -static_assert(std::is_trivially_copyable::value); -static_assert(std::is_trivially_copyable::value); -static_assert(std::is_trivially_copyable::value); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_standard_layout_v); +static_assert(std::is_standard_layout_v); +static_assert(std::is_standard_layout_v); + +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_standard_layout_v); +static_assert(std::is_standard_layout_v); +static_assert(std::is_standard_layout_v); diff --git a/src/memory/DeviceArray.inl b/src/memory/DeviceArray.inl index a72cad827..e1ddd0621 100644 --- a/src/memory/DeviceArray.inl +++ b/src/memory/DeviceArray.inl @@ -24,17 +24,16 @@ struct DeviceArray : public Array using Ptr = std::shared_ptr>; using ConstPtr = std::shared_ptr>; - // Explanation why: https://isocpp.org/wiki/faq/templates#nondependent-name-lookup-members - // Note: do not repeat this for methods, since it inhibits virtual dispatch mechanism - using Array::data; - using Array::count; - using Array::capacity; - T* getWritePtr() { return data; } const T* getReadPtr() const { return data; } CUdeviceptr getDeviceReadPtr() const { return reinterpret_cast(this->getReadPtr()); } CUdeviceptr getDeviceWritePtr() { return getDeviceReadPtr(); } protected: + // Explanation why: https://isocpp.org/wiki/faq/templates#nondependent-name-lookup-members + // Note: do not repeat this for methods, since it inhibits virtual dispatch mechanism using Array::Array; + using Array::data; + using Array::count; + using Array::capacity; }; diff --git a/src/memory/HostArray.inl b/src/memory/HostArray.inl index 2844146b1..e381ff2e6 100644 --- a/src/memory/HostArray.inl +++ b/src/memory/HostArray.inl @@ -22,12 +22,6 @@ struct HostArray : public Array using Ptr = std::shared_ptr>; using ConstPtr = std::shared_ptr>; - // Explanation why: https://isocpp.org/wiki/faq/templates#nondependent-name-lookup-members - // Note: do not repeat this for methods, since it inhibits virtual dispatch mechanism - using Array::data; - using Array::count; - using Array::capacity; - T* getWritePtr() { return data; } const T* getReadPtr() const { return data; } @@ -50,6 +44,10 @@ struct HostArray : public Array const T& operator[](size_t idx) const { return data[idx]; } protected: + // Explanation why: https://isocpp.org/wiki/faq/templates#nondependent-name-lookup-members + // Note: do not repeat this for methods, since it inhibits virtual dispatch mechanism using Array::Array; + using Array::data; + using Array::count; + using Array::capacity; }; - diff --git a/src/repr.hpp b/src/repr.hpp index a38e85118..abfe21bcb 100644 --- a/src/repr.hpp +++ b/src/repr.hpp @@ -139,3 +139,21 @@ static inline std::string repr(rgl_node_t* node) std::string nodePointee = (node != nullptr) ? fmt::format("->{}", repr(*node)) : ""; return fmt::format("{}{}", (void*) node, nodePointee); } + +template<> +struct fmt::formatter +{ + template + constexpr auto parse(ParseContext& ctx) + { + return ctx.begin(); + } + + template + auto format(const rgl_radar_scope_t& v, FormatContext& ctx) + { + return fmt::format_to(ctx.out(), "(begin={}, end={}, separation_thresholds: [distance={}, speed={}, azimuth={}])", + v.begin_distance, v.end_distance, v.distance_separation_threshold, + v.radial_speed_separation_threshold, v.azimuth_separation_threshold); + } +}; diff --git a/src/scene/Entity.cpp b/src/scene/Entity.cpp index c2c96838b..10744a88a 100644 --- a/src/scene/Entity.cpp +++ b/src/scene/Entity.cpp @@ -27,8 +27,10 @@ Entity::Entity(std::shared_ptr mesh) : mesh(std::move(mesh)) {} void Entity::setTransform(Mat3x4f newTransform) { - transform = newTransform; - Scene::instance().requestASRebuild(); + formerTransformInfo = transformInfo; + transformInfo = {newTransform, Scene::instance().getTime()}; + Scene::instance().requestASRebuild(); // Current transform + Scene::instance().requestSBTRebuild(); // Previous transform } void Entity::setId(int newId) @@ -39,7 +41,13 @@ void Entity::setId(int newId) throw std::invalid_argument(msg); } id = newId; - Scene::instance().requestASRebuild(); + Scene::instance().requestASRebuild(); // Update instanceId field in AS +} + +void Entity::setLaserRetro(float retro) +{ + laserRetro = retro; + Scene::instance().requestSBTRebuild(); } void Entity::setIntensityTexture(std::shared_ptr texture) @@ -48,4 +56,24 @@ void Entity::setIntensityTexture(std::shared_ptr texture) Scene::instance().requestSBTRebuild(); } -Mat3x4f Entity::getVelocity() const { throw std::runtime_error("unimplemented"); } +std::optional Entity::getPreviousFrameLocalToWorldTransform() const +{ + // At the moment of writing, setting Scene time (rgl_scene_set_time) is optional. + // Making it mandatory (e.g. refusing to raytrace without time set) would simplify the code below. + // However, it would be a breaking change, requiring fixing all plugins. + + bool transformWasSetOnlyOnce = transformInfo.time.has_value() && !formerTransformInfo.time.has_value(); + if (transformWasSetOnlyOnce) { + // The first transform set might be the last one (e.g. static objects), + // so let's assume this object was in the same pose in the previous frame to get zero velocity. + return transformInfo.matrix; + } + + bool formerTransformWasSetInPrecedingFrame = formerTransformInfo.time.has_value() && + formerTransformInfo.time == Scene::instance().getPrevTime(); + if (!formerTransformWasSetInPrecedingFrame) { + return std::nullopt; + } + + return formerTransformInfo.matrix; +} diff --git a/src/scene/Entity.hpp b/src/scene/Entity.hpp index b01404ebc..f610d56da 100644 --- a/src/scene/Entity.hpp +++ b/src/scene/Entity.hpp @@ -57,9 +57,17 @@ struct Entity : APIObject void setIntensityTexture(std::shared_ptr texture); /** - * @return Estimated velocity based on current and previous transforms. + * Sets laser retro that will be used as a point attribute LASER_RETRO_F32 when a ray hits this entity. */ - Mat3x4f getVelocity() const; + void setLaserRetro(float retro); + + /** + * Returns Entity's transform such that it is possible to compute meaningful velocity between it and the current transform. + * Most often it will return the previous frame (if Entity is updated on each frame). See source for details. + * NOTE: It is assumed that (current) transform is always valid for the present scene time (even if it was set in the past). + * @return Mat3x4f of the local-to-world transform from the previous frame if available. + */ + std::optional getPreviousFrameLocalToWorldTransform() const; private: /** @@ -72,8 +80,16 @@ struct Entity : APIObject Entity(std::shared_ptr mesh); private: - Mat3x4f transform{Mat3x4f::identity()}; + struct TransformWithTime + { + Mat3x4f matrix; + std::optional