diff --git a/.github/actions/install-macos-source-deps/action.yml b/.github/actions/install-macos-source-deps/action.yml index 21dbd3d4ba..855d646e46 100644 --- a/.github/actions/install-macos-source-deps/action.yml +++ b/.github/actions/install-macos-source-deps/action.yml @@ -203,6 +203,47 @@ runs: cmake --build build -j$NPROC DESTDIR="$STAGE" cmake --install build + # ----------------------------- g2o ------------------------------ + - name: Cache g2o + id: cache-g2o + uses: actions/cache@v4 + with: + path: ${{ runner.temp }}/deps-stage/g2o + key: g2o-67cbe15c-${{ inputs.os }}-${{ steps.depver.outputs.hash }} + + - name: Build g2o + if: steps.cache-g2o.outputs.cache-hit != 'true' + shell: bash + run: | + set -e + NPROC=$(sysctl -n hw.logicalcpu) + SRC="${{ runner.temp }}/src-deps/g2o" + STAGE="${{ runner.temp }}/deps-stage/g2o" + rm -rf "$SRC"; mkdir -p "$SRC" "$STAGE" + # g2o pins a commit (not a tag), so clone then checkout. + git clone https://github.com/RainerKuemmerle/g2o.git "$SRC/g2o" + cd "$SRC/g2o" + git checkout 67cbe15c998737ac6705d3cd18201a72be0d073d + # CMAKE_INSTALL_PREFIX=/usr/local so it stages under usr/local like the + # other deps. Homebrew's libomp is keg-only and g2o does not propagate + # OpenMP's include/lib dirs, so with Apple clang isn't found and + # libomp isn't linked; add its include (-I) and lib (-L -lomp) explicitly. + LIBOMP=$(brew --prefix libomp) + cmake -B build \ + -DCMAKE_BUILD_TYPE=${{ inputs.build_type }} \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DCMAKE_PREFIX_PATH="$LIBOMP" \ + -DCMAKE_C_FLAGS="-I$LIBOMP/include" \ + -DCMAKE_CXX_FLAGS="-I$LIBOMP/include" \ + -DCMAKE_EXE_LINKER_FLAGS="-L$LIBOMP/lib -lomp" \ + -DCMAKE_SHARED_LINKER_FLAGS="-L$LIBOMP/lib -lomp" \ + -DG2O_BUILD_APPS=OFF \ + -DG2O_BUILD_EXAMPLES=OFF \ + -DG2O_USE_OPENMP=ON \ + -DG2O_USE_OPENGL=OFF + cmake --build build -j$NPROC + DESTDIR="$STAGE" cmake --install build + # ------------------- install all units into /usr/local ---------- - name: Install source dependencies into /usr/local shell: bash @@ -213,7 +254,7 @@ runs: set -e STAGE="${{ runner.temp }}/deps-stage" found=0 - for unit in gtsam pointmatcher orbbec depthai opengv; do + for unit in gtsam pointmatcher orbbec depthai opengv g2o; do if [ -d "$STAGE/$unit/usr/local" ]; then sudo cp -a "$STAGE/$unit/usr/local/." /usr/local/ found=1 diff --git a/.github/actions/install-windows-cuda-deps/action.yml b/.github/actions/install-windows-cuda-deps/action.yml index bd4bbfcd54..0664fe700b 100644 --- a/.github/actions/install-windows-cuda-deps/action.yml +++ b/.github/actions/install-windows-cuda-deps/action.yml @@ -26,7 +26,7 @@ runs: uses: actions/cache@v4 with: path: ${{ runner.workspace }}/vcpkg_installed - key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-cuda130_v1 + key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-cuda130_v4 - name: Download and Install vcpkg if: steps.cache-vcpkg.outputs.cache-hit != 'true' @@ -35,10 +35,25 @@ runs: $install_dir = "${{ runner.workspace }}\vcpkg_installed" $archivePath = "${{ runner.workspace }}\vcpkg-export.7z" - # The file has been built locally with bundle-windows-deps.bat - $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022-cuda130.7z" - - Invoke-WebRequest -Uri $url -OutFile $archivePath + # The 7z is built locally with bundle_windows_deps_cuda.bat and uploaded to a + # GitHub release. Resolve it by filename via the API (authenticated with the + # built-in token) so it works even from a *draft* release and regardless of + # the (changing) asset id. Requires the workflow to run in-repo (github.token + # can't read introlab drafts from a fork PR). + $repo = "introlab/rtabmap" + $releaseTag = "0.23.8" + $assetName = "vcpkg-export-66c0373d-x64-vs2022-cuda130.7z" + $apiHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/vnd.github+json" } + # Select the release by tag_name (populated on drafts too, and unique), then + # take the asset from THAT release so concurrent releases can't be confused. + $rel = (Invoke-RestMethod -Uri "https://api.github.com/repos/$repo/releases?per_page=100" -Headers $apiHeaders) | + Where-Object { $_.tag_name -eq $releaseTag } | Select-Object -First 1 + if (-not $rel) { throw "Release tagged $releaseTag not found (including drafts)" } + $asset = $rel.assets | Where-Object { $_.name -eq $assetName } | Select-Object -First 1 + if (-not $asset) { throw "Asset $assetName not found in release $releaseTag" } + Write-Host "Downloading $assetName (asset id $($asset.id)) ..." + $dlHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/octet-stream" } + Invoke-WebRequest -Uri $asset.url -Headers $dlHeaders -OutFile $archivePath & 7z x $archivePath "-o$install_dir" -y - name: Add vcpkg to PATH and env variable diff --git a/.github/actions/install-windows-deps/action.yml b/.github/actions/install-windows-deps/action.yml index 4659b17b96..c9837afae2 100644 --- a/.github/actions/install-windows-deps/action.yml +++ b/.github/actions/install-windows-deps/action.yml @@ -13,7 +13,7 @@ runs: uses: actions/cache@v4 with: path: ${{ runner.workspace }}/vcpkg_installed - key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-v4 + key: ${{ runner.os }}-vcpkg-export-66c0373d-x64-vs2022-v6 - name: Download and Install vcpkg if: steps.cache-vcpkg.outputs.cache-hit != 'true' @@ -22,10 +22,25 @@ runs: $install_dir = "${{ runner.workspace }}\vcpkg_installed" $archivePath = "${{ runner.workspace }}\vcpkg-export.7z" - # The file has been built locally with bundle-windows-deps.bat - $url = "https://github.com/introlab/rtabmap/releases/download/0.23.1/vcpkg-export-66c0373d-x64-vs2022.7z" - - Invoke-WebRequest -Uri $url -OutFile $archivePath + # The 7z is built locally with bundle_windows_deps.bat and uploaded to a + # GitHub release. Resolve it by filename via the API (authenticated with the + # built-in token) so it works even from a *draft* release and regardless of + # the (changing) asset id. Requires the workflow to run in-repo (github.token + # can't read introlab drafts from a fork PR). + $repo = "introlab/rtabmap" + $releaseTag = "0.23.8" + $assetName = "vcpkg-export-66c0373d-x64-vs2022.7z" + $apiHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/vnd.github+json" } + # Select the release by tag_name (populated on drafts too, and unique), then + # take the asset from THAT release so concurrent releases can't be confused. + $rel = (Invoke-RestMethod -Uri "https://api.github.com/repos/$repo/releases?per_page=100" -Headers $apiHeaders) | + Where-Object { $_.tag_name -eq $releaseTag } | Select-Object -First 1 + if (-not $rel) { throw "Release tagged $releaseTag not found (including drafts)" } + $asset = $rel.assets | Where-Object { $_.name -eq $assetName } | Select-Object -First 1 + if (-not $asset) { throw "Asset $assetName not found in release $releaseTag" } + Write-Host "Downloading $assetName (asset id $($asset.id)) ..." + $dlHeaders = @{ Authorization = "Bearer ${{ github.token }}"; "User-Agent" = "rtabmap-ci"; Accept = "application/octet-stream" } + Invoke-WebRequest -Uri $asset.url -Headers $dlHeaders -OutFile $archivePath & 7z x $archivePath "-o$install_dir" -y - name: Add vcpkg to PATH and env variable diff --git a/.github/workflows/cmake-macos.yml b/.github/workflows/cmake-macos.yml index ce467a77c8..a22e315220 100644 --- a/.github/workflows/cmake-macos.yml +++ b/.github/workflows/cmake-macos.yml @@ -26,24 +26,19 @@ jobs: include: - build_name: macos-sequoia-intel os: macos-15-intel - extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_ORBBEC_SDK=ON -DWITH_DEPTHAI=ON' - build_name: macos-sequoia-apple-silicon os: macos-15 - extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_ORBBEC_SDK=ON -DWITH_DEPTHAI=ON' - build_name: macos-tahoe-intel os: macos-26-intel - extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_ORBBEC_SDK=ON -DWITH_DEPTHAI=ON' - build_name: macos-tahoe-apple-silicon os: macos-26 - extra_cmake_def: '-DBUILD_AS_BUNDLE=ON -DWITH_ORBBEC_SDK=ON -DWITH_DEPTHAI=ON' - steps: - uses: actions/checkout@v4 - name: Install Brew Dependencies run: | # Update brew and install from Brewfile if present, or specific packages - brew install pcl opencv octomap g2o pdal yaml-cpp librealsense libfreenect libusb zlib + brew install pcl opencv octomap pdal yaml-cpp librealsense libfreenect libusb zlib libomp suite-sparse ceres-solver - name: Install Source Dependencies # Build (and per-dependency cache) the source-only deps not available from @@ -55,7 +50,21 @@ jobs: - name: Configure CMake run: | - cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }} + # Apple clang has no built-in OpenMP; point find_package(OpenMP) at + # Homebrew's keg-only libomp so PCL/g2o/rtabmap enable OpenMP instead of + # repeatedly logging "Could NOT find OpenMP". + LIBOMP=$(brew --prefix libomp) + cmake -B ${{github.workspace}}/build \ + -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} \ + -DBUILD_AS_BUNDLE=ON \ + -DWITH_ORBBEC_SDK=ON \ + -DWITH_DEPTHAI=ON \ + -DWITH_CERES=ON \ + -DOpenMP_C_FLAGS="-Xclang -fopenmp -I$LIBOMP/include" \ + -DOpenMP_C_LIB_NAMES=omp \ + -DOpenMP_CXX_FLAGS="-Xclang -fopenmp -I$LIBOMP/include" \ + -DOpenMP_CXX_LIB_NAMES=omp \ + -DOpenMP_omp_LIBRARY="$LIBOMP/lib/libomp.dylib" - name: Build run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} diff --git a/.github/workflows/cmake-windows.yml b/.github/workflows/cmake-windows.yml index 5c81284334..87513c9be0 100644 --- a/.github/workflows/cmake-windows.yml +++ b/.github/workflows/cmake-windows.yml @@ -74,6 +74,14 @@ jobs: - name: Build run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + - name: Info + # Skipped for CUDA: the binary links ZED (sl_zed64.dll -> nvcuvid/nvEncodeAPI64), + # which need the NVIDIA driver; the GPU-less runner can't load the exe. + if: matrix.build_name != 'windows-2022-cuda' + working-directory: ${{github.workspace}}/build/bin + run: | + ./rtabmap-console --version + - name: Build Windows Package shell: pwsh run: | @@ -88,11 +96,6 @@ jobs: shell: pwsh run: Get-ChildItem -Path "build" -Filter "RTABMap-*" | Rename-Item -NewName { $_.BaseName + "_cuda" + $_.Extension } - - name: Info - working-directory: ${{github.workspace}}/build/bin - run: | - ./rtabmap-console --version - - name: Upload RTABMap Artifacts (ZIP) uses: actions/upload-artifact@v4 with: diff --git a/app/src/CMakeLists.txt b/app/src/CMakeLists.txt index 68fb707bad..462e4c8725 100644 --- a/app/src/CMakeLists.txt +++ b/app/src/CMakeLists.txt @@ -151,14 +151,30 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) IF(NOT OrbbecSDK_BIN_DIR) MESSAGE(FATAL_ERROR "OrbbecSDK.dll not found! Verify your PATH.") ENDIF(NOT OrbbecSDK_BIN_DIR) - MESSAGE(FATAL "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}") + MESSAGE(STATUS "OrbbecSDK_BIN_DIR=${OrbbecSDK_BIN_DIR}") INSTALL(DIRECTORY "${OrbbecSDK_BIN_DIR}/extensions" DESTINATION ${thirdparty_dest_dir} COMPONENT runtime FILES_MATCHING PATTERN "*.lib" EXCLUDE PATTERN "*") - ENDIF(WIN32) + ELSEIF(APPLE) + # OrbbecSDK loads its extensions (frame processor, filters like + # FrameUnpacker) via dlopen relative to libOrbbecSDK. fixup_bundle puts + # libOrbbecSDK in Contents/Frameworks, so the extensions must sit next to + # it in Frameworks/extensions (they reference @rpath/libOrbbecSDK, which + # resolves via the app executable's @executable_path/../Frameworks rpath). + # Without this the camera is detected but frame processing fails. + get_filename_component(OrbbecSDK_LIB_DIR "${OrbbecSDK_DIR}/../.." ABSOLUTE) + INSTALL(DIRECTORY "${OrbbecSDK_LIB_DIR}/extensions" + DESTINATION Frameworks + COMPONENT runtime) + IF(EXISTS "${OrbbecSDK_LIB_DIR}/OrbbecSDKConfig.xml") + INSTALL(FILES "${OrbbecSDK_LIB_DIR}/OrbbecSDKConfig.xml" + DESTINATION Frameworks + COMPONENT runtime) + ENDIF() + ENDIF(WIN32) ENDIF(OrbbecSDK_FOUND) IF(Torch_FOUND) @@ -405,7 +421,17 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) install(CODE " # Glob Qt Plugins file(GLOB_RECURSE ALL_LIBS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/*${CMAKE_SHARED_LIBRARY_SUFFIX}\") - + + # OrbbecSDK loads its filter/frame-processor extensions via dlopen, and they + # link @rpath/libOrbbecSDK. Feed them to fixup_bundle so their reference is + # rewritten to the embedded libOrbbecSDK + file(GLOB ORBBEC_EXT_LIBS + \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/Frameworks/extensions/filters/*${CMAKE_SHARED_LIBRARY_SUFFIX}\" + \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/Frameworks/extensions/frameprocessor/*${CMAKE_SHARED_LIBRARY_SUFFIX}\") + if(ORBBEC_EXT_LIBS) + list(APPEND ALL_LIBS \${ORBBEC_EXT_LIBS}) + endif() + if(NOT \"${python_pyd_dir}\" STREQUAL \"\") file(GLOB_RECURSE PYD_FILES \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${python_pyd_dir}/*.pyd\") if(PYD_FILES) @@ -421,6 +447,16 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32)) set(BU_CHMOD_BUNDLE_ITEMS ON) include(\"BundleUtilities\") + # nvcuvid.dll (NVDEC) and nvEncodeAPI64.dll (NVENC), and the CUDA driver API + # nvcuda.dll, ship with the NVIDIA GPU driver (System32), not the CUDA toolkit, + # so they are absent on driver-less CI runners and must never be bundled (they + # are resolved at runtime from the end user's driver). Mark them 'system' so + # fixup_bundle skips them instead of failing to resolve them. + function(gp_resolved_file_type_override resolved_file type_var) + if(resolved_file MATCHES \"(nvcuvid|nvEncodeAPI64|nvcuda)\") + set(\${type_var} \"system\" PARENT_SCOPE) + endif() + endfunction() fixup_bundle(\"${APPS}\" \"\${ALL_LIBS}\" \"${DIRS}\") " COMPONENT runtime) diff --git a/app/src/main.cpp b/app/src/main.cpp index 914a3496dc..ae43343198 100644 --- a/app/src/main.cpp +++ b/app/src/main.cpp @@ -52,6 +52,10 @@ int main(int argc, char* argv[]) ULogger::setLevel(ULogger::kWarning); #ifdef WIN32 + // STA (single-threaded apartment) is required for the native file dialogs / File Explorer + // (see commit d75cc04, "Fixed File Explorer hanging (Qt 5.12)"). Do NOT switch to MTA + // (CoInitializeEx COINIT_MULTITHREADED): it deadlocks the + // native file dialogs. CoInitialize(nullptr); #endif @@ -60,6 +64,12 @@ int main(int argc, char* argv[]) QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat()); #endif + // Recommended by VTK when using QVTKOpenGLNativeWidget (a QOpenGLWidget): let all VTK + // render widgets share a single OpenGL context, which is needed for correct rendering + // when render widgets live in / move across multiple top-level windows. Must be set + // before QApplication is constructed. + QApplication::setAttribute(Qt::AA_ShareOpenGLContexts); + /* Create tasks */ QApplication * app = new QApplication(argc, argv); app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box diff --git a/bundle_windows_deps_cuda.bat b/bundle_windows_deps_cuda.bat index b80488d12d..8d9bf05235 100644 --- a/bundle_windows_deps_cuda.bat +++ b/bundle_windows_deps_cuda.bat @@ -205,16 +205,15 @@ cmake -S . -B build -GNinja ^ -DBUILD_PERF_TESTS=OFF ^ -DOPENCV_ENABLE_NONFREE=ON ^ -DBUILD_opencv_apps=OFF ^ - -DBUILD_opencv_python3=ON ^ - -DPYTHON3_EXECUTABLE=%FINAL_EXPORT_PATH%/installed/%TRIPLET%/tools/python3/python.exe ^ - -DPYTHON3_PACKAGES_PATH=bin/Lib/site-packages ^ + -DBUILD_opencv_cudacodec=OFF ^ + -DBUILD_opencv_python3=OFF ^ + -DBUILD_opencv_python_bindings_generator=OFF ^ + -DBUILD_opencv_python_tests=OFF ^ -DBUILD_opencv_java_bindings_generator=OFF ^ -DWITH_CUDA=ON ^ -DWITH_VTK=OFF ^ -DWITH_TBB=ON || exit /b !errorlevel! cmake --build build --config Release --target install || exit /b !errorlevel! -:: move cv2 package under tools/python3 -robocopy "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\bin\Lib" "%FINAL_EXPORT_PATH%\installed\%TRIPLET%\tools\python3\Lib" /E /MOVE /NFL /NDL /NJH /NC /NS /NP cd .. diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index ea5a5deecd..cad4f6ccbc 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -344,7 +344,7 @@ class RTABMAP_CORE_EXPORT SensorData void setGPS(const GPS & gps) {gps_ = gps;} const GPS & gps() const {return gps_;} - void setIMU(const IMU & imu) {imu_ = imu; } + void setIMU(const IMU & imu); const IMU & imu() const {return imu_;} void setEnvSensors(const EnvSensors & sensors) {_envSensors = sensors;} diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index b5e9d50e28..f0f8cc72ed 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -49,8 +49,8 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : public: CameraStereoZed( int deviceId, - int resolution = -1, // -1 = AUTO, 0=HD4K 1=HD2K 2=HD1080 3=HD1200 4=HD720 5=SVGA 6=VGA - int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY + int resolution = -1, // -1 = AUTO, 0=HD4K 1=QHDPLUS 2=HD2K 3=HD1536 4=HD1080 5=HD1200 6=HD720 7=SVGA 8=VGA 9=XVGA 10=TXVGA + int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=ULTRA, 4=NEURAL_LIGHT, 5=NEURAL, 6=NEURAL_ULTRA int sensingMode = 0,// 0=STANDARD, 1=FILL int confidenceThr = 100, bool computeOdometry = false, @@ -61,7 +61,7 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : int texturenessConfidenceThr = 90); // introduced with ZED SDK 3 CameraStereoZed( const std::string & svoFilePath, - int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL + int quality = 1, // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=ULTRA, 4=NEURAL_LIGHT, 5=NEURAL, 6=NEURAL_ULTRA int sensingMode = 0,// 0=STANDARD, 1=FILL int confidenceThr = 100, bool computeOdometry = false, @@ -81,6 +81,15 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : void postInterIMUPublic(const IMU & imu, double stamp); void setRightGrayScale(bool enabled = true); + /** + * If the given ZED depth mode (quality: same encoding as the constructor) is a NEURAL mode + * whose AI model isn't yet downloaded/optimized for the GPU, returns a human-readable warning + * that the first start will be slower (the ZED SDK downloads/optimizes the model during + * init()). Returns an empty string when the model is ready, the mode isn't neural, or the + * SDK/build doesn't support the check. + */ + static std::string getNeuralModelWarning(int quality); + protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); diff --git a/corelib/include/rtabmap/core/lidar/LidarVLP16.h b/corelib/include/rtabmap/core/lidar/LidarVLP16.h index cb04d4eb71..3ff94b4192 100644 --- a/corelib/include/rtabmap/core/lidar/LidarVLP16.h +++ b/corelib/include/rtabmap/core/lidar/LidarVLP16.h @@ -29,10 +29,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // Should be first on windows to avoid "WinSock.h has already been included" error #include #include +#include #include #include +#include +#include + +// On Apple, closing pcl::HDLGrabber's UDP socket while its read thread is blocked +// in receive_from returns EBADF, which pcl::HDLGrabber::readPacketsFromSocket() +// rethrows uncaught in its own thread, aborting the process on shutdown. There we +// run our own single-threaded UDP receive loop and own the socket lifecycle. +// Other platforms use the base pcl::VLPGrabber path (which also avoids needing +// boost::asio::io_context, absent from the Boost 1.65 on e.g. Ubuntu Bionic). +// Comment this out to force the base path everywhere (e.g. A/B testing on macOS). +#if defined(__APPLE__) +#define RTABMAP_VLP16_USE_CUSTOM_SOCKET +#endif + namespace rtabmap { struct PointXYZIT { @@ -68,9 +83,28 @@ class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber { void setOrganized(bool enable); +#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET + // Overridden only on Apple: in network mode we run our own UDP receive loop + // instead of pcl::HDLGrabber's, because on macOS/BSD closing the grabber's + // socket while its read thread is blocked in receive_from returns EBADF, a + // code that pcl::HDLGrabber::readPacketsFromSocket() rethrows uncaught (in + // its own thread), aborting the process on shutdown. Owning the socket lets + // us tear it down with the non-throwing receive_from overload. Like + // pcl::HDLGrabber, we bind to the given LOCAL interface address (to scope + // reception on a multi-homed host) and fall back to all interfaces (0.0.0.0) + // when it is unspecified or not a local address. In PCAP mode we delegate to + // the base grabber. On other platforms we inherit pcl::VLPGrabber directly. + virtual void start() override; + virtual void stop() override; + virtual bool isRunning() const override; +#endif + private: void buildTimings(bool dualMode); virtual void toPointClouds (HDLDataPacket *dataPacket) override; +#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET + void readPackets(); +#endif protected: virtual SensorData captureData(SensorCaptureInfo * info = 0) override; @@ -88,6 +122,18 @@ class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber { std::vector > accumulatedScans_; USemaphore scanReady_; UMutex lastScanMutex_; + + // Own UDP receive path (network mode only). + bool networkMode_; + boost::asio::ip::address ipAddress_; + std::uint16_t port_; + bool receivedScan_; // set once the first scan is received +#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET + boost::asio::io_context ioContext_; + boost::asio::ip::udp::socket * socket_; + std::thread * readThread_; + std::atomic terminate_; +#endif }; } /* namespace rtabmap */ diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index ed7e981b68..935233bf99 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -1247,7 +1247,12 @@ void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, Pa { if(RTABMAP_VERSION_COMPARE(<, std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str()))) { - if(configFilePath.find(".rtabmap") != std::string::npos) + // Detect the user's private config - matches both the legacy + // ~/.rtabmap/rtabmap.ini and the Windows %LOCALAPPDATA%/rtabmap/rtabmap.ini + // (".rtabmap/rtabmap.ini" also contains "rtabmap/rtabmap.ini"): downgrade-on-save is fine there. + // Accept both forward and backward slashes (native Windows paths). + if(configFilePath.find("rtabmap/rtabmap.ini") != std::string::npos || + configFilePath.find("rtabmap\\rtabmap.ini") != std::string::npos) { UWARN("Version in the config file \"%s\" is more recent (\"%s\") than " "current RTAB-Map version used (\"%s\"). The config file will be downgraded " diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp index 6414fb9ff3..b9fc7bb7fd 100644 --- a/corelib/src/SensorCaptureThread.cpp +++ b/corelib/src/SensorCaptureThread.cpp @@ -114,7 +114,6 @@ SensorCaptureThread::SensorCaptureThread( _camera(camera), _odomSensor(odomSensor), _lidar(lidar), - _extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()), _odomAsGt(false), _poseTimeOffset(poseTimeOffset), _poseScaleFactor(poseScaleFactor), @@ -153,10 +152,14 @@ SensorCaptureThread::SensorCaptureThread( { if(_camera) { - if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull()) + if(_odomSensor == _camera && extrinsics.isNull()) { _extrinsicsOdomToCamera.setIdentity(); } + else + { + _extrinsicsOdomToCamera = extrinsics * CameraModel::opticalRotation(); + } UASSERT(!_extrinsicsOdomToCamera.isNull()); UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); } @@ -492,20 +495,26 @@ void SensorCaptureThread::mainLoop() } } - // Adjust local transform of the camera based on the pose frame + // Adjust local transform of the camera(s) based on the pose frame. The correction + // and odom->camera extrinsics are frame-level, so apply the same prefix to each + // camera while keeping its own local transform (multi-camera supported). if(!data.cameraModels().empty()) { - UASSERT(data.cameraModels().size()==1); - CameraModel model = data.cameraModels()[0]; - model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); - data.setCameraModel(model); + std::vector models = data.cameraModels(); + for(size_t i=0; i models = data.stereoCameraModels(); + for(size_t i=0; i(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set + if(this->isKilled()) + { + // A kill was requested (e.g. while we were blocked capturing this frame): don't + // publish anything so we never deliver events to handlers that are being torn down. + } + else if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set { postUpdate(&data, &info); info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial(); info.timeTotal = totalTime.ticks(); this->post(new SensorEvent(data, info)); } - else if(!this->isKilled()) + else { + // Not killed but no data: end of stream. Signal consumers once, then stop. UWARN("no more data..."); this->kill(); this->post(new SensorEvent()); diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 8954af0faf..f04dd8e376 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -952,6 +952,22 @@ void SensorData::setFeatures(const std::vector & keypoints, const _descriptors = descriptors; } +void SensorData::setIMU(const IMU & imu) +{ + UASSERT( + uIsFinite(imu.orientation()[0]) && + uIsFinite(imu.orientation()[1]) && + uIsFinite(imu.orientation()[2]) && + uIsFinite(imu.orientation()[3]) && + uIsFinite(imu.angularVelocity()[0]) && + uIsFinite(imu.angularVelocity()[1]) && + uIsFinite(imu.angularVelocity()[2]) && + uIsFinite(imu.linearAcceleration()[0]) && + uIsFinite(imu.linearAcceleration()[1]) && + uIsFinite(imu.linearAcceleration()[2])); + imu_ = imu; +} + unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes { return sizeof(SensorData) + diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index a82b514f42..8b80984f54 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -107,10 +107,14 @@ void CameraRealSense2::close() { if(!_sensor.get_active_streams().empty()) { + std::string sensorName = _sensor.supports(RS2_CAMERA_INFO_NAME) ? _sensor.get_info(RS2_CAMERA_INFO_NAME) : "?"; try { + UDEBUG("stop() sensor \"%s\"...", sensorName.c_str()); _sensor.stop(); + UDEBUG("stop() sensor \"%s\" done; close()...", sensorName.c_str()); _sensor.close(); + UDEBUG("close() sensor \"%s\" done", sensorName.c_str()); } catch(const rs2::error & error) { @@ -119,12 +123,15 @@ void CameraRealSense2::close() } } #ifdef WIN32 + UDEBUG("Windows-only: Hardware reset (to avoid freezing when clearing devices)..."); dev_[i].hardware_reset(); // To avoid freezing on some Windows computers in the following destructor // Don't do this on linux (tested on Ubuntu 18.04, realsense v2.41.0): T265 cannot be restarted + UDEBUG("Windows-only: Hardware reset... done"); #endif } UDEBUG("Clearing devices..."); dev_.clear(); + UDEBUG("Clearing devices... done!"); } catch(const rs2::error & error) { @@ -554,10 +561,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st catch(const rs2::error & error) { #ifdef __APPLE__ - UWARN("%s. Is the camera already used with another app? On macOS, accessing a " + UERROR("%s. Is the camera already used with another app? On macOS, accessing a " "RealSense camera requires root privileges, so try running with sudo.", error.what()); #else - UWARN("%s. Is the camera already used with another app?", error.what()); + UERROR("%s. Is the camera already used with another app?", error.what()); #endif } diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index 2fc57a26ee..d3d5c779eb 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #ifdef RTABMAP_ZED #include @@ -77,7 +78,7 @@ static cv::Mat slMat2cvMat(sl::Mat& input) { #endif } -Transform zedPoseToTransform(const sl::Pose & pose) +static Transform zedPoseToTransform(const sl::Pose & pose) { return Transform( pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3], @@ -86,7 +87,7 @@ Transform zedPoseToTransform(const sl::Pose & pose) } #if ZED_SDK_MAJOR_VERSION < 3 -IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform) +static IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform) { sl::Orientation orientation = imuData.pose_data.getOrientation(); @@ -124,7 +125,7 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform imuLocalTransform); } #else -IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform) +static IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTransform) { sl::Orientation orientation = sensorData.imu.pose.getOrientation(); @@ -161,6 +162,23 @@ IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTr accCov, imuLocalTransform); } + +// sl::SensorsData::imu.is_available only means the camera has an IMU; a returned +// sample can still contain NaN pose/accel/gyro (e.g. before the IMU fusion has +// initialized, or in SVO/STREAM mode). Validate the actual measurements. +static bool isImuValid(const sl::SensorsData & sensorData) +{ + if(!sensorData.imu.is_available) + { + return false; + } + const sl::float3 & acc = sensorData.imu.linear_acceleration; + const sl::float3 & gyr = sensorData.imu.angular_velocity; + const sl::Orientation ori = sensorData.imu.pose.getOrientation(); + return uIsFinite(acc.v[0]) && uIsFinite(acc.v[1]) && uIsFinite(acc.v[2]) && + uIsFinite(gyr.v[0]) && uIsFinite(gyr.v[1]) && uIsFinite(gyr.v[2]) && + uIsFinite(ori.ox) && uIsFinite(ori.oy) && uIsFinite(ori.oz) && uIsFinite(ori.ow); +} #endif class ZedIMUThread: public UThread @@ -217,7 +235,7 @@ class ZedIMUThread: public UThread #else sl::SensorsData sensordata; sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::CURRENT); - if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available) + if(res == sl::ERROR_CODE::SUCCESS && isImuValid(sensordata)) { camera_->postInterIMUPublic(zedIMUtoIMU(sensordata, imuLocalTransform_), double(sensordata.imu.timestamp.getNanoseconds())/10e8); } @@ -251,6 +269,56 @@ int CameraStereoZed::sdkVersion() #endif } +#ifdef RTABMAP_ZED +static void backwardCompatibility(int & resolution, int & quality) +{ + // -1 = AUTO, 0=HD4K 1=QHDPLUS 2=HD2K 3=HD1536 4=HD1080 5=HD1200 6=HD720 7=SVGA 8=VGA 9=XVGA 10=TXVGA +#if ZED_SDK_MAJOR_VERSION < 4 + // Zed3 Supported: 2=HD2K 4=HD1080 6=HD720 8=VGA + if(resolution == 0 || resolution == 1) // 0=HD4K 1=QHDPLUS + { + UWARN("Zed SDK v3 doesn't support HD4K and QHDPLUS, setting HD2K."); + resolution = 2; // 2=HD2K + } + if(resolution == 3 || resolution == 5) // 3=HD1536 5=HD1200 + { + UWARN("Zed SDK v3 doesn't support HD1536 and HD1200, setting HD1080."); + resolution = 4; // 4=HD1080 + } + if(resolution == 7 || resolution >=9) // 7=SVGA 9=XVGA 10=TXVGA + { + UWARN("Zed SDK v3 doesn't support SVGA, XVGA and TXVGA, setting VGA."); + resolution = 8; // 8=VGA + } + if(quality == 4) { // 4=NEURAL_LIGHT 6=NEURAL_PLUS + UWARN("Zed SDK v3 doesn't support NEURAL_LIGHT and NEURAL PLUS, setting NEURAL."); + quality = 5; // NEURAL + } +#else + if(resolution == -1) + { + resolution = int(sl::RESOLUTION::AUTO); // AUTO + } +#if ZED_SDK_MAJOR_VERSION < 5 + // Zed4 Supported: 0=HD4K 1=QHDPLUS 2=HD2K 4=HD1080 5=HD1200 6=HD720 7=SVGA 8=VGA + if(resolution == 3) // 3=HD1536 + { + UWARN("Zed SDK v4 doesn't support HD1536, setting HD1200."); + resolution_ = 5; // 5=HD1200 + } + if(resolution >=9) // 9=XVGA 10=TXVGA + { + UWARN("Zed SDK v4 doesn't support XVGA and TXVGA, setting VGA."); + resolution = 8; // 8=VGA + } + if(quality > 5) { // NEURAL_PLUS + UWARN("Zed SDK v4 doesn't support NEURAL_LIGHT and NEURAL PLUS, setting NEURAL."); + quality = 5; // NEURAL + } +#endif +#endif +} +#endif CameraStereoZed::CameraStereoZed( int deviceId, @@ -285,29 +353,8 @@ CameraStereoZed::CameraStereoZed( { UDEBUG(""); #ifdef RTABMAP_ZED -#if ZED_SDK_MAJOR_VERSION < 4 - if(resolution_ == 1 || resolution_ == 2) // HD2K, HD1080 - { - resolution_ -= 1; // HD2K=0, HD1080=1 - } - if(resolution_ == 3) // HD1200 - { - resolution_ = 1; // HD1080=1 - } - if(resolution_ == 4 || resolution_ == -1) - { - resolution_ = 2; // HD720=2 - } - else if(resolution_ == 5 || resolution_ == 6) // SVGA, VGA - { - resolution_ = 3; // VGA=3 - } -#else // ZED=4 - if(resolution_ == -1) - { - resolution_ = int(sl::RESOLUTION::AUTO); // AUTO - } -#endif + + backwardCompatibility(resolution_, quality_); #if ZED_SDK_MAJOR_VERSION < 3 UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ = sl::RESOLUTION_HD2K && resolution_ = sl::DEPTH_MODE_NONE && quality_ = 5 + sl::DEPTH_MODE depthMode = (sl::DEPTH_MODE)quality; + if(depthMode == sl::DEPTH_MODE::NEURAL_LIGHT || + depthMode == sl::DEPTH_MODE::NEURAL || + depthMode == sl::DEPTH_MODE::NEURAL_PLUS) + { + sl::AI_MODELS aiModel = + depthMode == sl::DEPTH_MODE::NEURAL_LIGHT ? sl::AI_MODELS::NEURAL_LIGHT_DEPTH : + depthMode == sl::DEPTH_MODE::NEURAL_PLUS ? sl::AI_MODELS::NEURAL_PLUS_DEPTH : + sl::AI_MODELS::NEURAL_DEPTH; + sl::AI_Model_status status = sl::checkAIModelStatus(aiModel); + if(!status.downloaded || !status.optimized) + { + return uFormat("The selected ZED NEURAL depth model is not ready yet (downloaded=%s, " + "optimized=%s): the first start may take significantly longer while the " + "ZED SDK downloads and/or optimizes the model for your GPU. Subsequent " + "starts will be faster.", + status.downloaded?"true":"false", status.optimized?"true":"false"); + } + } +#endif + return std::string(); +} + bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); @@ -463,9 +538,35 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str r = zed_->open(param); } +#if ZED_SDK_MAJOR_VERSION >= 3 + // CORRUPTED_SDK_INSTALLATION on open() is typically a NEURAL depth mode whose optional + // neural/TensorRT runtime files aren't installed. Give a clear, actionable error. + if(r == sl::ERROR_CODE::CORRUPTED_SDK_INSTALLATION && +#if ZED_SDK_MAJOR_VERSION >= 5 + param.depth_mode >= sl::DEPTH_MODE::NEURAL_LIGHT) +#else + param.depth_mode >= sl::DEPTH_MODE::NEURAL) +#endif + { + UERROR("ZED open() returned \"%s\": the optional NEURAL/TensorRT runtime files are " + "likely missing. Install ZED SDK %d.%d, or select a non-NEURAL depth mode " + "(e.g. PERFORMANCE).", toString(r).c_str(), ZED_SDK_MAJOR_VERSION, ZED_SDK_MINOR_VERSION); + // Do NOT delete zed_ here: after a CORRUPTED_SDK_INSTALLATION open failure (NEURAL depth + // mode selected but the TensorRT/neural runtime isn't installed) the ZED SDK is left in a + // bad state and ~sl::Camera() crashes inside sl_zed64.dll. Leak the object (one-time, + // terminal error path) so we fail gracefully with the message above instead of crashing. + zed_ = 0; + return false; + } +#endif + if(r!=sl::ERROR_CODE::SUCCESS) { +#if ZED_SDK_MAJOR_VERSION >= 4 + UERROR("Camera initialization failed: \"%s\": %s", toString(r).c_str(), toVerbose(r).c_str()); +#else UERROR("Camera initialization failed: \"%s\"", toString(r).c_str()); +#endif delete zed_; zed_ = 0; return false; @@ -502,7 +603,11 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str #endif if(r!=sl::ERROR_CODE::SUCCESS) { +#if ZED_SDK_MAJOR_VERSION >= 4 + UERROR("Camera tracking initialization failed: \"%s\": %s", toString(r).c_str(), toVerbose(r).c_str()); +#else UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str()); +#endif } } @@ -731,16 +836,24 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) res = zed_->grab(rparam); timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE); - // If the sensor supports IMU, wait IMU to be available before sending data. + // If the sensor supports IMU, wait for IMU to be available before sending data. if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull()) { sl::SensorsData imudatatmp; res = zed_->getSensorsData(imudatatmp, sl::TIME_REFERENCE::IMAGE); - imuReceived = res == sl::ERROR_CODE::SUCCESS && imudatatmp.imu.is_available && imudatatmp.imu.timestamp.getNanoseconds() != 0; + imuReceived = res == sl::ERROR_CODE::SUCCESS && isImuValid(imudatatmp) && imudatatmp.imu.timestamp.getNanoseconds() != 0; } } while(src_ == CameraVideo::kUsbDevice && (res!=sl::ERROR_CODE::SUCCESS || !imuReceived) && timer.elapsed() < 2.0); + // If no valid IMU arrived within the 2 sec startup window, null the IMU transform so + // we don't re-wait 2 sec on every subsequent frame (camera likely has no working IMU). + if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull() && !imuReceived) + { + UWARN("No valid IMU received within 2 sec; ignoring IMU for the rest of this session."); + imuLocalTransform_.setNull(); + } + if(res==sl::ERROR_CODE::SUCCESS) #endif { @@ -794,7 +907,7 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) #endif } - if(imuPublishingThread_ == 0) + if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull()) { #if ZED_SDK_MAJOR_VERSION < 3 sl::IMUData imudata; @@ -803,7 +916,7 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) #else sl::SensorsData imudata; res = zed_->getSensorsData(imudata, sl::TIME_REFERENCE::IMAGE); - if(res == sl::ERROR_CODE::SUCCESS && imudata.imu.is_available) + if(res == sl::ERROR_CODE::SUCCESS && isImuValid(imudata)) #endif { //ZED-Mini @@ -876,7 +989,11 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) } else if(src_ == CameraVideo::kUsbDevice) { +#if ZED_SDK_MAJOR_VERSION >= 4 + UERROR("CameraStereoZed: Failed to grab images after 2 seconds! (%s: %s)", toString(res).c_str(), toVerbose(res).c_str()); +#else UERROR("CameraStereoZed: Failed to grab images after 2 seconds!"); +#endif } else { diff --git a/corelib/src/lidar/LidarVLP16.cpp b/corelib/src/lidar/LidarVLP16.cpp index 2fa758a0ef..a626e65bc4 100644 --- a/corelib/src/lidar/LidarVLP16.cpp +++ b/corelib/src/lidar/LidarVLP16.cpp @@ -34,6 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define VLP_DUAL_MODE 0x39 #endif +// RTABMAP_VLP16_USE_CUSTOM_SOCKET is defined (Apple only, and toggleable) in +// LidarVLP16.h. When set, network mode uses our own single-threaded UDP receive +// loop; otherwise start()/stop()/isRunning() delegate to pcl::VLPGrabber. + namespace rtabmap { /** @brief Function used to check that hour assigned to timestamp in conversion is @@ -92,8 +96,17 @@ LidarVLP16::LidarVLP16( startSweepTimeHost_(0), organized_(organized), useHostTime_(false), - stampLast_(stampLast) + stampLast_(stampLast), + networkMode_(false), + port_(0), + receivedScan_(false) +#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET + ,socket_(0) + ,readThread_(0) + ,terminate_(false) +#endif { + // ipAddress_ unused in PCAP mode (default-constructed / unspecified). UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str()); } LidarVLP16::LidarVLP16( @@ -111,7 +124,19 @@ LidarVLP16::LidarVLP16( startSweepTimeHost_(0), organized_(organized), useHostTime_(useHostTime), - stampLast_(stampLast) + stampLast_(stampLast), + // networkMode_ = live network capture (vs PCAP playback), on all platforms; + // it drives the "no data received" warning below. The custom socket path it + // also selects in start()/stop() is Apple-only (RTABMAP_VLP16_USE_CUSTOM_SOCKET). + networkMode_(true), + ipAddress_(ipAddress), + port_(port), + receivedScan_(false) +#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET + ,socket_(0) + ,readThread_(0) + ,terminate_(false) +#endif { UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port); } @@ -129,6 +154,132 @@ void LidarVLP16::setOrganized(bool enable) organized_ = true; } +#ifdef RTABMAP_VLP16_USE_CUSTOM_SOCKET +void LidarVLP16::start() +{ + if(networkMode_) + { + if(readThread_ != 0) + { + // already running + return; + } + + terminate_ = false; + // ipAddress_ is the LOCAL interface to listen on (as in pcl::HDLGrabber), + // not the sensor's address. Binding to a specific local IP scopes + // reception to that NIC on a multi-homed host. If it is unspecified + // (0.0.0.0) or not a local address (e.g. the sensor's IP was entered by + // mistake), fall back to listening on all interfaces, like pcl::HDLGrabber. + try + { + boost::asio::ip::udp::endpoint endpoint( + ipAddress_.is_unspecified() ? boost::asio::ip::address(boost::asio::ip::address_v4::any()) : ipAddress_, + port_); + // Unlike pcl::HDLGrabber, we read and process packets in a single + // thread instead of a producer/consumer queue. A large receive buffer + // lets the kernel hold a backlog (~800 VLP16 packets here) so a brief + // stall in toPointClouds() doesn't drop packets. + const int receiveBufferSize = 1024 * 1024; // 1 MB + try + { + socket_ = new boost::asio::ip::udp::socket(ioContext_); + socket_->open(boost::asio::ip::udp::v4()); + socket_->set_option(boost::asio::socket_base::reuse_address(true)); + socket_->set_option(boost::asio::socket_base::receive_buffer_size(receiveBufferSize)); + socket_->bind(endpoint); + } + catch(const std::exception & e) + { + UWARN("Could not bind VLP16 socket to local address %s (%s); " + "falling back to listening on all interfaces (0.0.0.0).", + endpoint.address().to_string().c_str(), e.what()); + delete socket_; + socket_ = new boost::asio::ip::udp::socket(ioContext_); + socket_->open(boost::asio::ip::udp::v4()); + socket_->set_option(boost::asio::socket_base::reuse_address(true)); + socket_->set_option(boost::asio::socket_base::receive_buffer_size(receiveBufferSize)); + socket_->bind(boost::asio::ip::udp::endpoint(boost::asio::ip::address_v4::any(), port_)); + } + } + catch(const std::exception & e) + { + UERROR("Failed to bind to VLP16 UDP port %d: %s", (int)port_, e.what()); + delete socket_; + socket_ = 0; + return; + } + receivedScan_ = false; + readThread_ = new std::thread(&LidarVLP16::readPackets, this); + return; + } + // PCAP mode: use the base grabber. + pcl::VLPGrabber::start(); +} + +void LidarVLP16::stop() +{ + if(networkMode_) + { + terminate_ = true; + if(socket_ != 0) + { + // Closing the socket unblocks receive_from in the read thread. Because + // that call uses the non-throwing (error_code) overload, the EBADF that + // BSD/macOS returns here surfaces as an error code instead of an + // exception, so the thread exits cleanly rather than aborting. + boost::system::error_code ec; + socket_->close(ec); + } + if(readThread_ != 0) + { + readThread_->join(); + delete readThread_; + readThread_ = 0; + } + delete socket_; + socket_ = 0; + return; + } + pcl::VLPGrabber::stop(); +} + +bool LidarVLP16::isRunning() const +{ + if(networkMode_) + { + return readThread_ != 0; + } + return pcl::VLPGrabber::isRunning(); +} + +void LidarVLP16::readPackets() +{ + std::uint8_t data[1500]; + boost::asio::ip::udp::endpoint sender; + while(!terminate_) + { + boost::system::error_code ec; + std::size_t length = socket_->receive_from(boost::asio::buffer(data, sizeof(data)), sender, 0, ec); + if(ec) + { + // On stop() the socket is closed under us (bad_descriptor / + // operation_aborted); only warn if it wasn't an expected shutdown. + if(!terminate_) + { + UWARN("VLP16 socket receive error: %s", ec.message().c_str()); + } + break; + } + // VLP-16 data packets are 1206 bytes; ignore anything shorter. + if(length >= 1206) + { + toPointClouds(reinterpret_cast(data)); + } + } +} +#endif // RTABMAP_VLP16_USE_CUSTOM_SOCKET + bool LidarVLP16::init(const std::string &, const std::string &) { UDEBUG("Init lidar"); @@ -365,8 +516,22 @@ SensorData LidarVLP16::captureData(SensorCaptureInfo * info) { data = lastScan_; lastScan_ = SensorData(); + receivedScan_ = true; } } + else if(networkMode_ && !receivedScan_) + { + // No packet has ever arrived: most likely the sensor isn't sending data + // to this computer. Point the user at the configured listening endpoint. + UWARN("Did not receive any VLP16 data packets for the past 5 seconds. " + "This computer is listening on %s:%d (%s). Make sure the LiDAR is " + "powered on and configured to send its data to this computer's IP " + "address on UDP port %d (set the data destination host in the " + "LiDAR's own web/configuration interface).", + ipAddress_.to_string().c_str(), (int)port_, + ipAddress_.is_unspecified() ? "all local interfaces" : "this interface only", + (int)port_); + } else { UWARN("Did not receive any scans for the past 5 seconds."); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 5fef4cba42..0696ca23d5 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -130,6 +130,12 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog virtual QString getIniFilePath() const; virtual QString getTmpIniFilePath() const; + // When running under sudo, Qt's QSettings replaces the ini with a + // root-owned file (write-temp + rename), so later non-root runs can no + // longer save preferences. Restore the invoking user's ownership (from + // SUDO_UID/SUDO_GID) on the config file and its directory. No-op on Windows + // and when not running as root. Best-effort (ignores failures). + static void restoreConfigOwnership(const QString & filePath); void init(const QString & iniFilePath = ""); void setCurrentPanelToSource(); virtual QString getDefaultWorkingDirectory() const; @@ -263,6 +269,9 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog PreferencesDialog::Src getSourceDriver() const; QString getSourceDriverStr() const; QString getSourceDevice() const; + // Non-empty warning to show before opening the selected source when init() is expected to be + // unusually slow; empty otherwise. + QString getSourceInitWarningMsg() const; PreferencesDialog::Src getLidarSourceDriver() const; PreferencesDialog::Src getOdomSourceDriver() const; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 63afea484a..5ef206f6e3 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -39,6 +39,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include +#include #include #include #include @@ -583,12 +586,33 @@ QString DatabaseViewer::getIniFilePath() const { return iniFilePath_; } +#ifdef WIN32 + // Windows: store settings in the standard per-user config location (%LOCALAPPDATA%\rtabmap) + // instead of a Unix-style dotfile in the home root. A fixed "rtabmap" folder (not the app name) + // is used so the main GUI and DatabaseViewer share the same rtabmap.ini. Other platforms keep + // ~/.rtabmap for consistency. + QString privatePath = QStandardPaths::writableLocation(QStandardPaths::GenericConfigLocation) + "/rtabmap"; +#else QString privatePath = QDir::homePath() + "/.rtabmap"; +#endif if(!QDir(privatePath).exists()) { - QDir::home().mkdir(".rtabmap"); + QDir().mkpath(privatePath); } - return privatePath + "/rtabmap.ini"; + QString iniPath = privatePath + "/rtabmap.ini"; +#ifdef WIN32 + // One-time migration: if there's no ini at the new location but the legacy ~/.rtabmap/rtabmap.ini + // exists, copy it over so users keep their existing settings. + if(!QFile::exists(iniPath)) + { + QString legacyIni = QDir::homePath() + "/.rtabmap/rtabmap.ini"; + if(QFile::exists(legacyIni)) + { + QFile::copy(legacyIni, iniPath); + } + } +#endif + return iniPath; } void DatabaseViewer::readSettings() diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 8daf26b23f..10b94fda26 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -78,6 +78,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #include #include #include @@ -958,7 +960,7 @@ bool MainWindow::handleEvent(UEvent* anEvent) else { Q_EMIT cameraInfoReceived(sensorEvent->info()); - if (_odomThread == 0 && (_sensorCapture->odomProvided()) && _preferencesDialog->isRGBDMode()) + if (_odomThread == 0 && _sensorCapture && _sensorCapture->odomProvided() && _preferencesDialog->isRGBDMode()) { OdometryInfo odomInfo; odomInfo.reg.covariance = sensorEvent->info().odomCovariance; @@ -5528,6 +5530,10 @@ void MainWindow::saveConfigGUI() _preferencesDialog->saveSettings(); this->saveFigures(); this->setWindowModified(false); + + // If running under sudo, the above writes recreate rtabmap.ini as root; + // restore ownership so non-root runs can still save preferences. + PreferencesDialog::restoreConfigOwnership(_preferencesDialog->getIniFilePath()); } void MainWindow::newDatabase() @@ -5932,6 +5938,31 @@ void MainWindow::startDetection() Camera * camera = 0; Lidar * lidar = 0; + // Creating the sensors below opens the devices (createLidar/createCamera/createOdomSensor -> + // init(), a few seconds for ZED/RealSense) on the GUI thread; show a busy dialog (min==max==0 + // => indeterminate) so the window isn't just frozen. Hidden once all sensors are created below. + QString startLabel = tr("Starting sensor..."); + QString initWarn = _preferencesDialog->getSourceInitWarningMsg(); + if(!initWarn.isEmpty()) + { + startLabel += "\n\n" + initWarn; + } + QProgressDialog progress(startLabel, QString(), 0, 0, this); + if(!initWarn.isEmpty()) + { + QLabel * wrapLabel = new QLabel(startLabel); + wrapLabel->setWordWrap(true); + progress.setLabel(wrapLabel); // QProgressDialog takes ownership + progress.setMinimumWidth(450); + } + progress.setWindowModality(Qt::ApplicationModal); + progress.setCancelButton(0); + progress.setMinimumDuration(0); + progress.setValue(0); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + if(_preferencesDialog->getLidarSourceDriver() != PreferencesDialog::kSrcUndef) { lidar = _preferencesDialog->createLidar(); @@ -5989,6 +6020,8 @@ void MainWindow::startDetection() } } + progress.hide(); // all sensors created/opened + _sensorCapture = new SensorCaptureThread(lidar, camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, waitTime, parameters); _sensorCapture->setOdomAsGroundTruth(_preferencesDialog->isOdomSensorAsGt()); @@ -6229,6 +6262,24 @@ void MainWindow::stopDetection() } ULOGGER_DEBUG(""); + + // Closing the camera runs on the GUI thread in "delete _sensorCapture" below (via + // ~SensorCaptureThread -> ~Camera::close()) and can block for a while - e.g. the first 2-3 + // RealSense closes per launch stall ~20s in the Motion Module stop() (librealsense warm-up). + // Show a busy dialog (min==max==0 => indeterminate) so the window isn't just frozen. It is + // declared here so it stays visible across the joins/deletes and closes on scope exit. + QProgressDialog progress(tr("Stopping sensor..."), QString(), 0, 0, this); + if(_sensorCapture) + { + progress.setWindowModality(Qt::ApplicationModal); + progress.setCancelButton(0); + progress.setMinimumDuration(0); + progress.setValue(0); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + } + // kill the processes if(_imuThread) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 3a824ed40d..3be2f4d077 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -39,9 +39,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include +#include #include #include +#ifndef _WIN32 +#include // geteuid, chown +#include +#include // atoi, getenv +#include +#include // strerror +#endif + #include #include #include @@ -49,6 +60,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include +#include #include #include #include @@ -437,12 +451,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->comboBox_cameraStereo->setItemData(kSrcStereoZed - kSrcStereo, 0, Qt::UserRole - 1); _ui->comboBox_odom_sensor->setItemData(2, 0, Qt::UserRole - 1); } - else if(CameraStereoZed::sdkVersion() < 4) + else { - _ui->comboBox_stereoZed_resolution->setItemData(1, 0, Qt::UserRole - 1); - _ui->comboBox_stereoZed_resolution->setItemData(4, 0, Qt::UserRole - 1); - _ui->comboBox_stereoZed_resolution->setItemData(6, 0, Qt::UserRole - 1); - _ui->comboBox_stereoZed_quality->setItemData(3, 0, Qt::UserRole - 1); + if(CameraStereoZed::sdkVersion() < 5) + { + // SDK 4.X + _ui->comboBox_stereoZed_quality->setItemData(5, 0, Qt::UserRole - 1); // NEURAL_ULTRA + _ui->comboBox_stereoZed_resolution->setItemData(4, 0, Qt::UserRole - 1); //HD1536 + _ui->comboBox_stereoZed_resolution->setItemData(10, 0, Qt::UserRole - 1); //XVGA + _ui->comboBox_stereoZed_resolution->setItemData(11, 0, Qt::UserRole - 1); //TXGA + } + if(CameraStereoZed::sdkVersion() < 4) + { + // SDK 3.X + _ui->comboBox_stereoZed_quality->setItemData(3, 0, Qt::UserRole - 1); // NEURAL_LIGHT + _ui->comboBox_stereoZed_resolution->setItemData(1, 0, Qt::UserRole - 1); // HD4K + _ui->comboBox_stereoZed_resolution->setItemData(2, 0, Qt::UserRole - 1); // QHDPLUS + _ui->comboBox_stereoZed_resolution->setItemData(6, 0, Qt::UserRole - 1); // HD1200 + _ui->comboBox_stereoZed_resolution->setItemData(8, 0, Qt::UserRole - 1); // SVGA + } + if(CameraStereoZed::sdkVersion() < 3) + { + _ui->comboBox_stereoZed_quality->setItemData(4, 0, Qt::UserRole - 1); // NEURAL + } } if (!CameraStereoTara::available()) { @@ -985,7 +1016,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->lineEdit_vlp16_pcap_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_vlp16_ip1, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_vlp16_ip2, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); - connect(_ui->spinBox_vlp16_ip2, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip3, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_vlp16_ip4, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_vlp16_port, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_vlp16_organized, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -2440,10 +2471,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->lineEdit_lidar_local_transform->setText("0 0 0 0 0 0"); _ui->lineEdit_vlp16_pcap_path->clear(); - _ui->spinBox_vlp16_ip1->setValue(192); - _ui->spinBox_vlp16_ip2->setValue(168); - _ui->spinBox_vlp16_ip3->setValue(1); - _ui->spinBox_vlp16_ip4->setValue(201); + _ui->spinBox_vlp16_ip1->setValue(0); + _ui->spinBox_vlp16_ip2->setValue(0); + _ui->spinBox_vlp16_ip3->setValue(0); + _ui->spinBox_vlp16_ip4->setValue(0); _ui->spinBox_vlp16_port->setValue(2368); _ui->checkBox_vlp16_organized->setChecked(false); _ui->checkBox_vlp16_hostTime->setChecked(true); @@ -2559,12 +2590,33 @@ QString PreferencesDialog::getWorkingDirectory() const QString PreferencesDialog::getIniFilePath() const { +#ifdef WIN32 + // Windows: store settings in the standard per-user config location (%LOCALAPPDATA%\rtabmap) + // instead of a Unix-style dotfile in the home root. A fixed "rtabmap" folder (not the app name) + // is used so the main GUI and DatabaseViewer share the same rtabmap.ini. Other platforms keep + // ~/.rtabmap for consistency. + QString privatePath = QStandardPaths::writableLocation(QStandardPaths::GenericConfigLocation) + "/rtabmap"; +#else QString privatePath = QDir::homePath() + "/.rtabmap"; +#endif if(!QDir(privatePath).exists()) { - QDir::home().mkdir(".rtabmap"); + QDir().mkpath(privatePath); + } + QString iniPath = privatePath + "/rtabmap.ini"; +#ifdef WIN32 + // One-time migration: if there's no ini at the new location but the legacy ~/.rtabmap/rtabmap.ini + // exists, copy it over so users keep their existing settings. + if(!QFile::exists(iniPath)) + { + QString legacyIni = QDir::homePath() + "/.rtabmap/rtabmap.ini"; + if(QFile::exists(legacyIni)) + { + QFile::copy(legacyIni, iniPath); + } } - return privatePath + "/rtabmap.ini"; +#endif + return iniPath; } QString PreferencesDialog::getTmpIniFilePath() const @@ -2572,6 +2624,41 @@ QString PreferencesDialog::getTmpIniFilePath() const return getIniFilePath()+".tmp"; } +void PreferencesDialog::restoreConfigOwnership(const QString & filePath) +{ +#ifndef _WIN32 + // Only relevant when the process is effectively root (e.g. launched with + // sudo). getenv("SUDO_UID"/"SUDO_GID") give the invoking user. + if(geteuid() == 0) + { + const char * sudoUid = getenv("SUDO_UID"); + const char * sudoGid = getenv("SUDO_GID"); + if(sudoUid && sudoGid) + { + uid_t uid = (uid_t)atoi(sudoUid); + gid_t gid = (gid_t)atoi(sudoGid); + // Restore the config file and its containing directory so the user + // can still write preferences without sudo afterwards. + if(!filePath.isEmpty() && QFile::exists(filePath)) + { + if(chown(filePath.toStdString().c_str(), uid, gid) != 0) + { + UWARN("Could not restore ownership of \"%s\" to uid=%d (%s).", + filePath.toStdString().c_str(), (int)uid, strerror(errno)); + } + } + QString dir = QFileInfo(filePath).absolutePath(); + if(!dir.isEmpty()) + { + chown(dir.toStdString().c_str(), uid, gid); + } + } + } +#else + Q_UNUSED(filePath); +#endif +} + void PreferencesDialog::loadConfigFrom() { QString path = QFileDialog::getOpenFileName(this, tr("Load settings..."), this->getWorkingDirectory(), "*.ini"); @@ -3242,6 +3329,9 @@ void PreferencesDialog::writeSettings(const QString & filePath) uInsert(_parameters, _modifiedParameters); // update cached parameters _modifiedParameters.clear(); _obsoletePanels = kPanelDummy; + + // Keep the ini writable by the invoking user even if we are running as root. + restoreConfigOwnership(filePath.isEmpty() ? getIniFilePath() : filePath); } void PreferencesDialog::writeGuiSettings(const QString & filePath) const @@ -6635,6 +6725,16 @@ QString PreferencesDialog::getSourceDriverStr() const return ""; } +QString PreferencesDialog::getSourceInitWarningMsg() const +{ + if(getSourceDriver() == kSrcStereoZed) + { + return QString::fromStdString( + CameraStereoZed::getNeuralModelWarning(_ui->comboBox_stereoZed_quality->currentIndex())); + } + return QString(); +} + QString PreferencesDialog::getSourceDevice() const { return _ui->lineEdit_sourceDevice->text(); @@ -6820,11 +6920,11 @@ double PreferencesDialog::getSourceScanForceGroundNormalsUp() const Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) { return createCamera( - this->getSourceDriver(), - _ui->lineEdit_sourceDevice->text(), - _ui->lineEdit_calibrationFile->text(), - useRawImages, - useColor, + this->getSourceDriver(), + _ui->lineEdit_sourceDevice->text(), + _ui->lineEdit_calibrationFile->text(), + useRawImages, + useColor, false, false); } @@ -7728,7 +7828,30 @@ void PreferencesDialog::setSLAMMode(bool enabled) void PreferencesDialog::testOdometry() { + QString startLabel = tr("Starting camera..."); + QString initWarn = getSourceInitWarningMsg(); + if(!initWarn.isEmpty()) + { + startLabel += "\n\n" + initWarn; + } + QProgressDialog progress(startLabel, QString(), 0, 0, this); + if(!initWarn.isEmpty()) + { + QLabel * wrapLabel = new QLabel(startLabel); + wrapLabel->setWordWrap(true); + progress.setLabel(wrapLabel); // QProgressDialog takes ownership + progress.setMinimumWidth(450); + } + progress.setWindowModality(Qt::ApplicationModal); + progress.setCancelButton(0); + progress.setMinimumDuration(0); + progress.setValue(0); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + Camera * camera = this->createCamera(); + progress.hide(); if(!camera) { return; @@ -7776,12 +7899,13 @@ void PreferencesDialog::testOdometry() _ui->odom_dataBufferSize->value()); odomThread.registerToEventsManager(); + // parent = 0 (not 'this'): see testCamera() - avoids the nested-modality crash. OdometryViewer * odomViewer = new OdometryViewer(10, _ui->spinBox_decimation_odom->value(), 0.0f, _ui->doubleSpinBox_maxDepth_odom->value(), this->getOdomQualityWarnThr(), - this, + 0, this->getAllParameters()); odomViewer->setWindowTitle(tr("Odometry viewer")); odomViewer->resize(1280, 480+QPushButton().minimumHeight()); @@ -7843,25 +7967,79 @@ void PreferencesDialog::testOdometry() } odomViewer->exec(); - delete odomViewer; + UDEBUG("Dialog closed, stopping sensor..."); + + // Tear down the pipes first so no more events are routed to the threads/viewer being + // destroyed, then stop the threads, then delete the viewer. This avoids delivering + // events to a handler that is being torn down. + UEventsManager::removePipe(&cameraThread, &odomThread, "SensorEvent"); + if(imuThread) + { + UEventsManager::removePipe(imuThread, &odomThread, "IMUEvent"); + } + UEventsManager::removePipe(&odomThread, odomViewer, "OdometryEvent"); + UEventsManager::removePipe(odomViewer, &odomThread, "OdometryResetEvent"); if(imuThread) { imuThread->join(true); - delete imuThread; } + + // Reuse the same dialog for the close. The device close() runs in cameraThread's destructor + // at function scope end (not in join()), so 'progress' stays visible across it. On Windows + // the first 2-3 RealSense closes per launch stall ~20s in the Motion Module stop(). + progress.setLabelText(tr("Closing camera...")); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn cameraThread.join(true); odomThread.join(true); + + // deleteLater() (not delete): see testCamera() - avoids a dangling OpenGL platform + // window that crashes in QWindowsWindow::alertWindow when Preferences later closes. + odomViewer->deleteLater(); + if(imuThread) + { + delete imuThread; + } } void PreferencesDialog::testCamera() { - CameraViewer * window = new CameraViewer(this, this->getAllParameters()); + // Not parented to 'this': the Preferences dialog is itself application-modal, and making + // the viewer a modal *child* of it (nested modality) with an OpenGL/VTK native window + // crashes Qt (QWindowsWindow::alertWindow, this==nullptr) when Preferences later closes. + // exec() below still makes the viewer application-modal, so interaction stays blocked. + CameraViewer * window = new CameraViewer(nullptr, this->getAllParameters()); window->setWindowTitle(tr("Camera viewer")); window->resize(1280, 480+QPushButton().minimumHeight()); window->registerToEventsManager(); + QString startLabel = tr("Starting camera..."); + QString initWarn = getSourceInitWarningMsg(); + if(!initWarn.isEmpty()) + { + startLabel += "\n\n" + initWarn; + } + QProgressDialog progress(startLabel, QString(), 0, 0, this); + if(!initWarn.isEmpty()) + { + QLabel * wrapLabel = new QLabel(startLabel); + wrapLabel->setWordWrap(true); + progress.setLabel(wrapLabel); // QProgressDialog takes ownership + progress.setMinimumWidth(450); + } + progress.setWindowModality(Qt::ApplicationModal); + progress.setCancelButton(0); + progress.setMinimumDuration(0); + progress.setValue(0); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + + // createCamera() init()s the device on the GUI thread (required by ZED) and takes a few seconds. Camera * camera = this->createCamera(); + progress.hide(); if(camera) { SensorCaptureThread cameraThread(camera, this->getAllParameters()); @@ -7906,12 +8084,26 @@ void PreferencesDialog::testCamera() cameraThread.start(); window->exec(); - delete window; - cameraThread.join(true); + UDEBUG("Dialog closed, stopping sensor..."); + UEventsManager::removePipe(&cameraThread, window, "SensorEvent"); + + // Reuse the same dialog for the close. The device close() runs in cameraThread's + // destructor at scope end (not in join()), so 'progress' - declared in the outer scope - + // stays visible across it. On Windows the first 2-3 RealSense closes per launch stall + // ~20s in the Motion Module stop() (librealsense warm-up); this keeps the user informed. + progress.setLabelText(tr("Closing camera...")); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + cameraThread.join(true); // cameraThread's destructor (scope end) closes the device + // deleteLater() (not delete): defer destruction to the event loop so Qt finishes + // tearing down the OpenGL widget's context and window-proc subclass and drains + // pending activation messages first. + window->deleteLater(); } else { - delete window; + window->deleteLater(); } } @@ -8377,14 +8569,25 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() void PreferencesDialog::testLidar() { - CameraViewer * window = new CameraViewer(this, this->getAllParameters()); + // Not parented to 'this': see testCamera() - avoids the nested-modality crash. + CameraViewer * window = new CameraViewer(nullptr, this->getAllParameters()); window->setWindowTitle(tr("Lidar viewer")); window->setWindowFlags(Qt::Window); window->resize(1280, 480+QPushButton().minimumHeight()); window->registerToEventsManager(); window->setDecimation(1); + QProgressDialog progress(tr("Starting lidar..."), QString(), 0, 0, this); + progress.setWindowModality(Qt::ApplicationModal); + progress.setCancelButton(0); + progress.setMinimumDuration(0); + progress.setValue(0); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + Lidar * lidar = this->createLidar(); + progress.hide(); if(lidar) { SensorCaptureThread lidarThread(lidar, this->getAllParameters()); @@ -8403,12 +8606,24 @@ void PreferencesDialog::testLidar() lidarThread.start(); window->exec(); - delete window; - lidarThread.join(true); + UDEBUG("Dialog closed, stopping sensor..."); + UEventsManager::removePipe(&lidarThread, window, "SensorEvent"); + + // Reuse the same dialog for the close. The device close() runs in lidarThread's + // destructor at scope end (not in join()), so 'progress' - declared in the outer + // scope - stays visible across it. + progress.setLabelText(tr("Closing sensor...")); + progress.show(); + QApplication::processEvents(); + QApplication::processEvents(); // make sure it is drawn + lidarThread.join(true); // lidarThread's destructor (scope end) closes the device + // deleteLater() (not delete): see testCamera() - avoids a dangling OpenGL platform + // window that crashes in QWindowsWindow::alertWindow when Preferences later closes. + window->deleteLater(); } else { - delete window; + window->deleteLater(); } } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index c2fef56f2d..172c4d9b9c 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -5740,11 +5740,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki ULTRA + + + NEURAL LIGHT + + NEURAL + + + NEURAL PLUS + + @@ -5903,11 +5913,21 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki HD4K + + + QHDPLUS + + HD2K + + + HD1536 + + HD1080 @@ -5933,6 +5953,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki VGA + + + XVGA + + + + + TXVGA + + @@ -9387,9 +9417,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - QAbstractSpinBox::NoButtons @@ -9400,15 +9427,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 255 - 192 + 0 - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - QAbstractSpinBox::NoButtons @@ -9419,15 +9443,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 255 - 168 + 0 - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - QAbstractSpinBox::NoButtons @@ -9438,15 +9459,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 250 - 1 + 0 - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - QAbstractSpinBox::NoButtons @@ -9457,7 +9475,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 250 - 201 + 0 @@ -9498,8 +9516,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + <html><head/><body><p>Local network interface (IP address of <span style=" font-weight:600;">this</span> computer) on which to listen for VLP16 packets. Use 0.0.0.0 to listen on all interfaces (recommended). This is NOT the sensor's IP address; the sensor decides where to send its data (configure its destination host in the sensor's own web interface).</p></body></html> + - IP Address. + Local IP to listen on (0.0.0.0 = all interfaces). See tooltip. true @@ -9554,8 +9575,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + <html><head/><body><p>Local UDP port on this computer to listen for VLP16 data packets (default 2368). The LiDAR must be configured to send its data to this port.</p></body></html> + - Port. + Port to listen on (default 2368). true @@ -9567,9 +9591,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - QAbstractSpinBox::NoButtons diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index adfa5e9c07..1d701b395c 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -97,7 +97,7 @@ void sighandler(int sig) int main(int argc, char * argv[]) { ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kInfo); + ULogger::setLevel(ULogger::kDebug); //ULogger::setPrintTime(false); //ULogger::setPrintWhere(false); @@ -272,7 +272,7 @@ int main(int argc, char * argv[]) UERROR("Not built with ZED sdk support..."); exit(-1); } - camera = new rtabmap::CameraStereoZed(deviceId.empty()?0:uStr2Int(deviceId), -1, 1, 100, false, rate); + camera = new rtabmap::CameraStereoZed(deviceId.empty()?0:uStr2Int(deviceId), -1, 1, 0, 100, false, rate); } else if (driver == 9) { @@ -523,5 +523,6 @@ int main(int argc, char * argv[]) cameraViewer.exec(); cameraThread.join(true); } + printf("Exiting cleanly.\n"); return 0; } diff --git a/tools/DatabaseViewer/main.cpp b/tools/DatabaseViewer/main.cpp index 597655dab9..eed6bc804c 100644 --- a/tools/DatabaseViewer/main.cpp +++ b/tools/DatabaseViewer/main.cpp @@ -53,6 +53,12 @@ int main(int argc, char * argv[]) QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat()); #endif + // Recommended by VTK when using QVTKOpenGLNativeWidget (a QOpenGLWidget): let all VTK + // render widgets share a single OpenGL context, which is needed for correct rendering + // when render widgets live in / move across multiple top-level windows. Must be set + // before QApplication is constructed. + QApplication::setAttribute(Qt::AA_ShareOpenGLContexts); + #ifdef RTABMAP_PYTHON rtabmap::PythonInterface pythonInterface; #endif diff --git a/tools/LidarViewer/main.cpp b/tools/LidarViewer/main.cpp index bf3ef0dfe1..bbb515f17d 100644 --- a/tools/LidarViewer/main.cpp +++ b/tools/LidarViewer/main.cpp @@ -58,7 +58,7 @@ void showUsage() { printf("\nUsage:\n" "rtabmap-lidar_viewer IP PORT driver\n" - " driver Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n"); + " driver Driver number to use: 0=VLP16 (default IP and port are 0.0.0.0 2368)\n"); exit(1); } diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 67c65a0d51..5729ad1ca1 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -404,13 +404,16 @@ int main (int argc, char * argv[]) { printf("The camera is not calibrated! You should calibrate the camera first.\n"); delete camera; + return 1; } } else { printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n"); delete camera; + return 1; } + printf("Exiting cleanly.\n"); return 0; } diff --git a/utilite/src/ULogger.cpp b/utilite/src/ULogger.cpp index 2d09eef7cb..15eea368e5 100644 --- a/utilite/src/ULogger.cpp +++ b/utilite/src/ULogger.cpp @@ -139,10 +139,13 @@ class UFileLogger : public ULogger fileToClear.close(); } + // Open in binary mode: the log formatter already emits "\r\n" line endings explicitly. + // On Windows, text mode ("a") would translate the "\n" of that "\r\n" into "\r\n" again, + // producing "\r\r\n" on disk - i.e. an extra blank line between entries. #ifdef _MSC_VER - fopen_s(&fout_, fileName_.c_str(), "a"); + fopen_s(&fout_, fileName_.c_str(), "ab"); #else - fout_ = fopen(fileName_.c_str(), "a"); + fout_ = fopen(fileName_.c_str(), "ab"); #endif if(!fout_) {