diff --git a/.gitignore b/.gitignore index 06ccd37b..0a1c75f7 100644 --- a/.gitignore +++ b/.gitignore @@ -31,3 +31,4 @@ container_root/profilers/** **container_root/trav_ws/** **traversability_mapping** container_root/ros_env_vars.sh +container_root/.nv/ \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index f75099fd..955b7e5d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "ORB_SLAM3"] path = ORB_SLAM3 url = https://github.com/suchetanrs/ORB_SLAM3 +[submodule "FastTrack"] + path = FastTrack + url = https://github.com/suchetanrs/FastTrack diff --git a/Dockerfile b/Dockerfile index 1f3f77e4..3fa16bb3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,13 @@ # Image taken from https://github.com/turlucode/ros-docker-gui -FROM osrf/ros:humble-desktop-full-jammy +# =============================================================================== +# Default to cpu build. Cuda kernels are built only if `--target nvidia` is specified. +# =============================================================================== +ARG TARGET=cpu + +# =============================================================================== +# Base stage (Common dependencies for CPU and NVIDIA GPU builds) +# =============================================================================== +FROM osrf/ros:humble-desktop-full-jammy AS base ARG USE_CI RUN apt-get update @@ -28,7 +36,6 @@ RUN apt-get install -y \ RUN apt update - # Build OpenCV RUN apt-get install -y python3-dev python3-numpy python2-dev RUN apt-get install -y libavcodec-dev libavformat-dev libswscale-dev @@ -42,13 +49,6 @@ RUN cd /tmp && git clone https://github.com/opencv/opencv.git && \ make -j8 && make install && \ cd / && rm -rf /tmp/opencv -# Build Pangolin -RUN cd /tmp && git clone https://github.com/stevenlovegrove/Pangolin && \ - cd Pangolin && git checkout v0.9.1 && mkdir build && cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-std=c++14 -DCMAKE_INSTALL_PREFIX=/usr/local .. && \ - make -j8 && make install && \ - cd / && rm -rf /tmp/Pangolin - # Build vscode (can be removed later for deployment) COPY ./container_root/shell_scripts/vscode_install.sh /root/ RUN cd /root/ && sudo chmod +x * && ./vscode_install.sh && rm -rf vscode_install.sh @@ -57,6 +57,74 @@ RUN apt-get update && apt-get install ros-humble-pcl-ros tmux -y RUN apt-get install ros-humble-nav2-common x11-apps nano -y RUN apt-get install -y gdb gdbserver ros-humble-rmw-cyclonedds-cpp ros-humble-cv-bridge ros-humble-image-transport ros-humble-image-common ros-humble-vision-opencv + +# =============================================================================== +# NVIDIA GPU image stage (built if `--target nvidia_gpu` is specified) +# =============================================================================== +FROM nvidia/opengl:1.0-glvnd-devel-ubuntu18.04 AS glvnd +FROM base AS nvidia_gpu + +RUN apt-get update && apt-get install -y --no-install-recommends \ + libglvnd0 \ + libgl1 \ + libglx0 \ + libegl1 \ + libgles2 +COPY --from=glvnd /usr/share/glvnd/egl_vendor.d/10_nvidia.json /usr/share/glvnd/egl_vendor.d/10_nvidia.json +ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all} +ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:-all} + +# Basic deps for adding NVIDIA apt repo +RUN apt-get update && apt-get install -y --no-install-recommends \ + wget ca-certificates gnupg + +# Add NVIDIA CUDA repo keyring (Ubuntu 22.04 repo) +RUN wget -q https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/cuda-keyring_1.1-1_all.deb \ + && dpkg -i cuda-keyring_1.1-1_all.deb && rm -rf cuda-keyring_1.1-1_all.deb + +# Install CUDA Toolkit 12.2 (no driver) +RUN apt-get update && apt-get install -y --no-install-recommends \ + cuda-toolkit-12-2 + +# make CUDA visible on PATH +ENV PATH=/usr/local/cuda-12.2/bin:${PATH} +ENV LD_LIBRARY_PATH=/usr/local/cuda-12.2/lib64:${LD_LIBRARY_PATH} +ENV CUDA_CACHE_PATH=/tmp/cuda_cache + +COPY FastTrack/Thirdparty/Pangolin /tmp/Pangolin +RUN cd /tmp/Pangolin && mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-std=c++14 -DCMAKE_INSTALL_PREFIX=/usr/local .. && \ + make -j8 && \ + make install && \ + cd / && rm -rf /tmp/Pangolin && ldconfig + +COPY FastTrack /home/orb/ORB_SLAM3 +COPY orb_slam3_ros2_wrapper /root/colcon_ws/src/orb_slam3_ros2_wrapper +COPY orb_slam3_map_generator /root/colcon_ws/src/orb_slam3_map_generator +COPY slam_msgs /root/colcon_ws/src/slam_msgs + +# Build ORB-SLAM3 with its dependencies. +RUN if [ "$USE_CI" = "true" ]; then \ + . /opt/ros/humble/setup.sh && cd /home/orb/ORB_SLAM3 && mkdir -p build && ./build.sh && \ + . /opt/ros/humble/setup.sh && cd /root/colcon_ws/ && colcon build --symlink-install; \ + fi + +RUN rm -rf /home/orb/ORB_SLAM3 /root/colcon_ws + +# =============================================================================== +# CPU image stage (Default) (ignored if `--target nvidia_gpu` is specified) +# =============================================================================== + +FROM base AS cpu + +# Build Pangolin +RUN cd /tmp && git clone https://github.com/stevenlovegrove/Pangolin && \ + cd Pangolin && git checkout v0.9.1 && mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-std=c++14 -DCMAKE_INSTALL_PREFIX=/usr/local .. && \ + make -j8 && \ + make install && \ + cd / && rm -rf /tmp/Pangolin && ldconfig + COPY ORB_SLAM3 /home/orb/ORB_SLAM3 COPY orb_slam3_ros2_wrapper /root/colcon_ws/src/orb_slam3_ros2_wrapper COPY orb_slam3_map_generator /root/colcon_ws/src/orb_slam3_map_generator @@ -68,4 +136,9 @@ RUN if [ "$USE_CI" = "true" ]; then \ . /opt/ros/humble/setup.sh && cd /root/colcon_ws/ && colcon build --symlink-install; \ fi -RUN rm -rf /home/orb/ORB_SLAM3 /root/colcon_ws \ No newline at end of file +RUN rm -rf /home/orb/ORB_SLAM3 /root/colcon_ws + +# =============================================================================== +# Final stage (Either CPU or NVIDIA GPU based on `--target` flag) +# =============================================================================== +FROM ${TARGET} AS final \ No newline at end of file diff --git a/FastTrack b/FastTrack new file mode 160000 index 00000000..30cab0c1 --- /dev/null +++ b/FastTrack @@ -0,0 +1 @@ +Subproject commit 30cab0c1dabf106dc7d776da8e6c6ad5f36a7ee5 diff --git a/README.md b/README.md index adb5305b..7d5ae33b 100644 --- a/README.md +++ b/README.md @@ -36,12 +36,20 @@ sudo chmod +x container_root/shell_scripts/docker_install.sh 3. ```source ~/.bashrc``` 4. You can see the built images on your machine by running ```sudo docker images```. +### To build NVIDIA CUDA version: + +Replace step 1. with ```sudo docker build --build-arg USE_CI=false --build-arg TARGET=nvidia_gpu -t orb-slam3-humble-nvidia:22.04 .``` + ## 4. Running the container 1. ```cd ORB-SLAM3-ROS2-Docker``` (ignore if you are already in the folder) 2. ```sudo docker compose run orb_slam3_22_humble``` 3. This should take you inside the container. Once you are inside, run the command ```xeyes``` and a pair of eyes should pop-up. If they do, x11 forwarding has correctly been setup on your computer. +### To run the NVIDIA CUDA version: + +Replace step 2. with ```sudo docker compose run orb_slam3_22_nvidia``` + ## 5. Building the ORB-SLAM3 Wrapper Launch the container using steps in (4). @@ -50,6 +58,14 @@ cd /home/orb/ORB_SLAM3/ && sudo chmod +x build.sh && ./build.sh cd /root/colcon_ws/ && colcon build --symlink-install && source install/setup.bash ``` +### To build with CUDA: + +Launch the container using steps in (4). +```bash +cd /home/orb/ORB_SLAM3/ && sudo chmod +x build.sh && ./build.sh +cd /root/colcon_ws/ && rm -rf build && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +``` + ## Launching ORB-SLAM3 Launch the container using steps in (4). diff --git a/container_root/.bash_history b/container_root/.bash_history index b5dc7977..9db4dc6b 100644 --- a/container_root/.bash_history +++ b/container_root/.bash_history @@ -499,6 +499,186 @@ tmux ls ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh +$ lspci | grep -i nvidia +lspci | grep -i nvidia +sudo apt install lspci +hostnamectl +gcc --version +nvidia-smi +clear +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +nvidia-smi +lspci | grep -i nvidia +hostnamectl +# inside the container +apt-get update && apt-get install -y wget ca-certificates gnupg +# set these for Ubuntu 22.04 x86_64 +distro=ubuntu2204 +arch=x86_64 +wget https://developer.download.nvidia.com/compute/cuda/repos/$distro/$arch/cuda-keyring_1.1-1_all.deb +dpkg -i cuda-keyring_1.1-1_all.deb +apt-get update +apt-get install -y cuda-toolkit-12-2 +cd +nvidia-smi +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +cd /home/orb/ORB_SLAM3/ +ls +chmod +x build.sh +sudo ./build.sh +ls +cd /home/orb/ORB_SLAM3/ +ls +cd Thirdparty/ +ls +git clone https://github.com/stevenlovegrove/Pangolin +cd Pangolin && git checkout v0.9.1 && mkdir build && cd build +cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-std=c++14 -DCMAKE_INSTALL_PREFIX=/usr/local .. +make -j8 && make install +clear +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh +clear +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ls +cd /home/orb/ORB_SLAM3/ +ls +cd Examples +ls +cd .. +ls +./run_script $dataset_name 1 +ls +./run_script.sh +./run_script.sh MH_01 1 +cd Thirdparty/ +ls +cd .. +ls +cat run_script.sh +./run_script.sh MH01 1 +ls +cat build.sh +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh +./run_script.sh MH01 1 +ls +cat build.sh +cd Thirdparty/Pangolin/ +ls +cd build/ +ls +cd .. +ls +cd .. +ls +cd DBoW2/ +ls +cd lib/ +ls +cd .. +ls +cd .. +ls +cd Pangolin/build/ +ls +cd ../../.. +ls +./build +./build.sh +cd Thirdparty/Pangolin/build/ +ls +cd ../.. +ls +cd .. +ls +./run_script.sh MH01 0 +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh +clear +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ls +cd +ls +./launch_slam.sh +ls +cd /home/orb/ORB_SLAM3/ +./build.sh +nvidia-smi +ls +clear +ls +cd /root/ +ls +cd /home/orb/ORB_SLAM3/ +ls +cd +ls +cd / +ls +cd +ls +cd /home/ +ls +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ls +cd colcon_ws/ +ls +cd src/ +ls +cd orb_slam3_ros2_wrapper/ +ls +cd scripts/ +ls +nano monitor_cpu_ram.sh +ls +sudo chmod +x monitor_cpu_ram.sh +ls +pwd +cd +ls +clear +ls +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +rviz2 -d orb_slam3.rviz +nvidia-smi +clear +ls +clear +htop +top +clear +sudo apt-get install htop +htop +pidof 1409280 +pidof rgbd +htop -p 1409280 +pidof rgbd +htop -p $(pidof rgbd) +pidof orb_slam3 +pidof rgbd +ps aux | grep orb +htop +ls +cd /home/orb/ORB_SLAM3/ +./build.sh +ls +cd +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ldd /home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Examples/Stereo-Inertial/stereo_inertial_euroc | egrep -i 'not found|pango|ORB_SLAM3|opencv' || true +ldd build/orb_slam3_ros2_wrapper/rgbd | egrep -i 'not found|pango|ORB_SLAM3|opencv' || true +export LD_LIBRARY_PATH=/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/lib:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/Pangolin/build:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/DBoW2/lib:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/g2o/lib:${LD_LIBRARY_PATH:-} +ldd build/orb_slam3_ros2_wrapper/rgbd | egrep -i 'not found|pango|ORB_SLAM3|opencv' || true +export LD_LIBRARY_PATH=/home/orb/ORB_SLAM3/lib:/home/orb/ORB_SLAM3/Thirdparty/Pangolin/build:/home/orb/ORB_SLAM3/Thirdparty/DBoW2/lib:/home/orb/ORB_SLAM3/Thirdparty/g2o/lib:${LD_LIBRARY_PATH:-} +ldd build/orb_slam3_ros2_wrapper/rgbd | egrep -i 'not found|pango|ORB_SLAM3|opencv' || true +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +tmux +ls clear cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash ls @@ -516,6 +696,88 @@ cd ls ./launch_slam.sh ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +export LD_LIBRARY_PATH=/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/lib:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/Pangolin/build:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/DBoW2/lib:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/g2o/lib:${LD_LIBRARY_PATH:-} +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +export LD_LIBRARY_PATH=/home/orb/ORB_SLAM3/lib:/home/orb/ORB_SLAM3/Thirdparty/Pangolin/build:/home/orb/ORB_SLAM3/Thirdparty/DBoW2/lib:/home/orb/ORB_SLAM3/Thirdparty/g2o/lib:${LD_LIBRARY_PATH:-} +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /home/orb/ORB_SLAM3/ +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +export LD_LIBRARY_PATH=/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/lib:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/Pangolin/build:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/DBoW2/lib:/home/suchetan/ORB-SLAM3-ROS2-Docker/ORB_SLAM3/Thirdparty/g2o/lib:${LD_LIBRARY_PATH:-} +ls +cd +ls +./launch_slam.sh +ls +nvidia-smi +clear +ls +clear +ls +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ls +cd /home/orb/ORB_SLAM3/ +ls +cd +cd /usr/include/ +ls +cd .. +cd local/ +ls +cd include/ +ls +cd pangolin/ +ls +cd windowing/ +ls +cd ../.. +ls +cd .. +ls +cd share/ +ls +cd .. +ls +clear +cd +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +rviz2 -d orb_slam3.rviz +clear +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +nvcc --version +python3 +clear +ls +cd +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +cd +ls +clear +ls +./launch_slam.sh +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +clear +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ls +cd +ls +./launch_slam.sh +ls +cd /home/orb/ORB_SLAM3/ +ls +git status ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py --show-args ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash @@ -532,6 +794,56 @@ ls cd colcon_ws/ ls rm -rf build/ install/ log/ +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ls +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd build/ +ls +cd orb_slam3_ros2_wrapper/ +ls +ldd rgbd | egrep -i 'not found|pango|ORB_SLAM3|opencv' || true +cd +ls +nautilus . +cd /usr/local/ +ls +cd bin/ +ls +cd .. +ls +cd share/ +ls +cd .. +ls +cd lib/ +ls +cd +ldconfig +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +lc +ls +clear +ls +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ldconfig +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ls +rviz2 -d orb_slam3.rviz +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ros2 topic list +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +ls +./launch_slam.sh +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +clear +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +rm -rf ~/.python_history +ls ls cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash ros2 topic hz /imu @@ -542,8 +854,119 @@ clear ls cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash ls +cd +ls +./launch_slam.sh +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +ls +cd /home/orb/ORB_SLAM3/ +ls +cd /home/orb/ORB_SLAM3/ +ls +cd +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +clear +ls +cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +nvidia-smi +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +cd /home/orb/ORB_SLAM3/ && sudo chmod +x build.sh && ./build.sh +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +nvcc --version +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +cd /home/orb/ORB_SLAM3/ && sudo chmod +x build.sh && ./build.sh +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ls +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +cd /home/orb/ORB_SLAM3/ && sudo chmod +x build.sh && ./build.sh +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +grep -R "ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA" build/orb_slam3_ros2_wrapper/CMakeCache.txt +rm -rf build/ install/ log/ +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +clear +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +rm -rf build/ install/ log/ +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +grep -R "ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA" build/orb_slam3_ros2_wrapper/CMakeCache.txt +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +grep -R "ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA" build/orb_slam3_ros2_wrapper/CMakeCache.txt +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +grep -R "ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA" build/orb_slam3_ros2_wrapper/CMakeCache.txt +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +clear +ls +cd +ls +./launch_slam.sh +ls +nvidia-smi +ls +cd colcon_ws/ +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ls +cd +cd /home/orb/ORB_SLAM3/ +ls +./build.sh +clear +ls clear ls +clear +cd +ls +cd colcon_ws/ +ls +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +source install/setup.bash +clear +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +cd +cd /home/orb/ORB_SLAM3/ +ls +./build.sh +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +nvidia-smi +clear +ls +./launch_slam.sh +ls +cd /home/orb/ORB_SLAM3/ +ls +./build.sh +cd +ls +cd colcon_ws/ +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py +cd /tmp/ +ls +cd +ls +clear +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=stereo +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=stereo +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=stereo +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=stereo +cd /home/orb/ORB_SLAM3/ +./build.sh +cd +cd colcon_ws/ +cd /root/colcon_ws/ && colcon build --symlink-install --cmake-args -DORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=ON && source install/setup.bash +ls cd ls ./launch_slam.sh diff --git a/docker-compose.yml b/docker-compose.yml index 56a1e10c..5833dc69 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,6 +1,21 @@ version: "3" + +x-orb-slam3-common: &orb_slam3_common + privileged: true + network_mode: host + ipc: host + pid: host + stdin_open: true + tty: true + environment: + - DISPLAY + - TERM + - QT_X11_NO_MITSHM=1 + entrypoint: /bin/bash + services: orb_slam3_22_humble: + <<: *orb_slam3_common image: orb-slam3-humble:22.04 volumes: #full dev @@ -17,12 +32,30 @@ services: - ./orb_slam3_map_generator/:/root/colcon_ws/src/orb_slam3_map_generator/ - ./slam_msgs/:/root/colcon_ws/src/slam_msgs/ - ./ORB_SLAM3/:/home/orb/ORB_SLAM3/ - privileged: true - network_mode: host - ipc: host - pid: host - stdin_open: true - tty: true - environment: - - DISPLAY - entrypoint: /bin/bash + + orb_slam3_22_humble_nvidia: + <<: *orb_slam3_common + image: orb-slam3-humble-nvidia:22.04 + volumes: + #full dev + - /dev:/dev:rw + #full media + - /media:/media:rw + #Time + - /etc/timezone:/etc/timezone:rw + - /etc/localtime:/etc/localtime:rw + #ros-workspaces + - ./container_root/:/root/ + - ./ros_env_vars.sh:/root/ros_env_vars.sh + - ./orb_slam3_ros2_wrapper/:/root/colcon_ws/src/orb_slam3_ros2_wrapper/ + - ./orb_slam3_map_generator/:/root/colcon_ws/src/orb_slam3_map_generator/ + - ./slam_msgs/:/root/colcon_ws/src/slam_msgs/ + - ./FastTrack/:/home/orb/ORB_SLAM3/ + runtime: nvidia + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all + capabilities: [gpu] diff --git a/orb_slam3_ros2_wrapper/CMakeLists.txt b/orb_slam3_ros2_wrapper/CMakeLists.txt index 38161df5..feb9158a 100644 --- a/orb_slam3_ros2_wrapper/CMakeLists.txt +++ b/orb_slam3_ros2_wrapper/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(orb_slam3_ros2_wrapper) +project(orb_slam3_ros2_wrapper LANGUAGES CXX) IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE Release) @@ -45,12 +45,24 @@ find_package(pcl_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(std_srvs REQUIRED) +option(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA "Enable CUDA support if available" OFF) + +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + include(CheckLanguage) + enable_language(CUDA) + message(WARNING "CUDA support requested for ORB_SLAM3_ROS2_WRAPPER") +endif() + include_directories( include ${ORB_SLAM3_ROOT_DIR}/include ${ORB_SLAM3_ROOT_DIR}/include/CameraModels ) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + include_directories(${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}) +endif() + link_directories( include ) @@ -79,6 +91,9 @@ add_library(orb_slam3_ros2_wrapper_common SHARED src/slam_node_base.cpp ) ament_target_dependencies(orb_slam3_ros2_wrapper_common ${dependencies}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + target_compile_definitions(orb_slam3_ros2_wrapper_common PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() add_executable(rgbd @@ -87,6 +102,10 @@ add_executable(rgbd ) ament_target_dependencies(rgbd ${dependencies}) target_link_libraries(rgbd orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + set_target_properties(rgbd PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + target_compile_definitions(rgbd PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() add_executable(rgbd_imu @@ -95,6 +114,10 @@ add_executable(rgbd_imu ) ament_target_dependencies(rgbd_imu ${dependencies}) target_link_libraries(rgbd_imu orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + set_target_properties(rgbd_imu PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + target_compile_definitions(rgbd_imu PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() add_executable(mono_imu @@ -103,6 +126,11 @@ add_executable(mono_imu ) ament_target_dependencies(mono_imu ${dependencies}) target_link_libraries(mono_imu orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + set_target_properties(mono_imu PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + target_compile_definitions(mono_imu PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() + add_executable(stereo src/stereo/stereo-slam-node.cpp @@ -110,6 +138,11 @@ add_executable(stereo ) ament_target_dependencies(stereo ${dependencies}) target_link_libraries(stereo orb_slam3_ros2_wrapper_common ${PCL_LIBRARIES}) +if(ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA) + set_target_properties(stereo PROPERTIES CUDA_SEPARABLE_COMPILATION ON) + target_compile_definitions(stereo PRIVATE ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA=1) +endif() + install(TARGETS orb_slam3_ros2_wrapper_common rgbd rgbd_imu mono_imu stereo DESTINATION lib/${PROJECT_NAME}) diff --git a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp index 129f0d5e..dded6278 100644 --- a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp +++ b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/orb_slam3_interface.hpp @@ -37,6 +37,10 @@ #include "Frame.h" #include "Map.h" #include "Atlas.h" + +#ifdef ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA +#include "Kernels/KernelController.h" +#endif #include "orb_slam3_ros2_wrapper/type_conversion.hpp" #include "orb_slam3_ros2_wrapper/time_profiler.hpp" diff --git a/orb_slam3_ros2_wrapper/launch/stereo.launch.py b/orb_slam3_ros2_wrapper/launch/stereo.launch.py index 9c43efc3..97f85888 100644 --- a/orb_slam3_ros2_wrapper/launch/stereo.launch.py +++ b/orb_slam3_ros2_wrapper/launch/stereo.launch.py @@ -36,10 +36,10 @@ def generate_launch_description(): def all_nodes_launch(context, robot_namespace): params_file = LaunchConfiguration('params_file') vocabulary_file_path = "/home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt" - config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_stereo.yaml" + config_file_path = "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo.yaml" declare_params_file_cmd = DeclareLaunchArgument( 'params_file', - default_value=os.path.join(orb_wrapper_pkg, 'params', 'ros_params', 'gazebo-stereo-ros-params.yaml'), + default_value=os.path.join(orb_wrapper_pkg, 'params', 'ros_params', 'euroc-stereo-ros-params.yaml'), description='Full path to the ROS2 parameters file to use for all launched nodes') base_frame = "" @@ -79,3 +79,4 @@ def all_nodes_launch(context, robot_namespace): robot_namespace_arg, opaque_function ]) + diff --git a/orb_slam3_ros2_wrapper/launch/unirobot.launch.py b/orb_slam3_ros2_wrapper/launch/unirobot.launch.py index 4fc7305c..53b6c783 100644 --- a/orb_slam3_ros2_wrapper/launch/unirobot.launch.py +++ b/orb_slam3_ros2_wrapper/launch/unirobot.launch.py @@ -1,11 +1,12 @@ import os from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration def generate_launch_description(): @@ -55,8 +56,29 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(["'", sensor_config, "' == 'stereo'"])), ) + monitor_enabled_arg = DeclareLaunchArgument( + "monitor_enabled", + default_value="true", + description="Enable lightweight CPU/RAM monitor for the selected sensor configuration.", + ) + + # Print to screen: monitor should log to stdout + monitor_process = ExecuteProcess( + condition=IfCondition(LaunchConfiguration("monitor_enabled")), + cmd=[ + "/root/colcon_ws/src/orb_slam3_ros2_wrapper/scripts/monitor_cpu_ram.sh", + "--name", sensor_config, + "--hz", "0.3", + # no --out: stdout goes to screen via `output="screen"` + ], + output="screen", + shell=False, + ) + return LaunchDescription([ sensor_config_arg, + monitor_enabled_arg, + monitor_process, rgbd_launch, rgbd_imu_launch, mono_imu_launch, diff --git a/orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_stereo.yaml b/orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo.yaml similarity index 100% rename from orb_slam3_ros2_wrapper/params/orb_slam3_params/gazebo_stereo.yaml rename to orb_slam3_ros2_wrapper/params/orb_slam3_params/euroc_stereo.yaml diff --git a/orb_slam3_ros2_wrapper/params/ros_params/gazebo-stereo-ros-params.yaml b/orb_slam3_ros2_wrapper/params/ros_params/euroc-stereo-ros-params.yaml similarity index 100% rename from orb_slam3_ros2_wrapper/params/ros_params/gazebo-stereo-ros-params.yaml rename to orb_slam3_ros2_wrapper/params/ros_params/euroc-stereo-ros-params.yaml diff --git a/orb_slam3_ros2_wrapper/scripts/monitor_cpu_ram.sh b/orb_slam3_ros2_wrapper/scripts/monitor_cpu_ram.sh new file mode 100755 index 00000000..94959666 --- /dev/null +++ b/orb_slam3_ros2_wrapper/scripts/monitor_cpu_ram.sh @@ -0,0 +1,60 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'EOF' +Usage: + ./script.sh --name [--hz ] [--interval ] + +Output (single line, refreshed): + Cpu(%) , Ram(MB) + +Examples: + ./script.sh --name rgbd --hz 2 + ./script.sh --name rgbd --interval 0.5 +EOF +} + +NAME="" +HZ="" +INTERVAL="" + +while [[ $# -gt 0 ]]; do + case "$1" in + --name) NAME="${2:-}"; shift 2;; + --hz) HZ="${2:-}"; shift 2;; + --interval) INTERVAL="${2:-}"; shift 2;; + -h|--help) usage; exit 0;; + *) echo "Error: Unknown argument: $1"; usage; exit 2;; + esac +done + +[[ -n "$NAME" ]] || { echo "Error: --name is required"; usage; exit 2; } +command -v ps >/dev/null 2>&1 || { echo "Error: 'ps' not found"; exit 1; } + +# Determine sleep seconds (supports fractional) +sleep_s="1" +if [[ -n "$INTERVAL" ]]; then + sleep_s="$INTERVAL" +elif [[ -n "$HZ" ]]; then + sleep_s="$(awk -v hz="$HZ" 'BEGIN{ if(hz<=0) exit 1; printf "%.6f", 1.0/hz }')" \ + || { echo "Error: --hz must be > 0"; exit 2; } +fi + +# Validate sleep_s +awk -v s="$sleep_s" 'BEGIN{ exit !(s ~ /^[0-9]+(\.[0-9]+)?$/ && s > 0) }' \ + || { echo "Error: interval must be a positive number (got: $sleep_s)"; exit 2; } + +while true; do + # Sum %CPU and RSS(KB) across all matching processes + # ps -C matches the "comm" (process name) exactly. + read -r cpu_sum rss_kb_sum < <( + ps -C "$NAME" -o pcpu=,rss= 2>/dev/null \ + | awk '{cpu+=$1; rss+=$2} END{printf "%.2f %d", (cpu+0), (rss+0)}' + ) || true + + ram_mb="$(awk -v kb="${rss_kb_sum:-0}" 'BEGIN{printf "%.2f", kb/1024}')" + + printf "Process name: %s Cpu(%%) %.2f, Ram(MB) %s\n" "$NAME" "${cpu_sum:-0}" "$ram_mb" + sleep "$sleep_s" +done diff --git a/orb_slam3_ros2_wrapper/src/slam_node_base.cpp b/orb_slam3_ros2_wrapper/src/slam_node_base.cpp index 978e4a69..38f1726f 100644 --- a/orb_slam3_ros2_wrapper/src/slam_node_base.cpp +++ b/orb_slam3_ros2_wrapper/src/slam_node_base.cpp @@ -17,6 +17,23 @@ namespace ORB_SLAM3_Wrapper ORB_SLAM3::System::eSensor sensor) : rclcpp::Node(node_name) { +#ifdef ORB_SLAM3_ROS2_WRAPPER_ENABLE_CUDA + bool RunOrbExtractionOnGPU = true; + bool RunStereoMatchOnGPU = true; + bool RunSearchLocalPointsOnGPU = true; + bool RunPoseEstimationOnGPU = true; + bool RunPoseOptimization = false; + + // RunOrbExtractionOnGPU, RunStereoMatchOnGPU, RunSearchLocalPointsOnGPU, RunPoseEstimationOnGPU, RunPoseOptimization + KernelController::setGPURunMode(RunOrbExtractionOnGPU, RunStereoMatchOnGPU, RunSearchLocalPointsOnGPU, RunPoseEstimationOnGPU, RunPoseOptimization); + std::cout << "======================== CUDA CONFIGURATION ========================" << std::endl; + std::cout << "RunOrbExtractionOnGPU: " << (RunOrbExtractionOnGPU ? "True" : "False") << std::endl; + std::cout << "RunStereoMatchOnGPU: " << (RunStereoMatchOnGPU ? "True" : "False") << std::endl; + std::cout << "RunSearchLocalPointsOnGPU: " << (RunSearchLocalPointsOnGPU ? "True" : "False") << std::endl; + std::cout << "RunPoseEstimationOnGPU: " << (RunPoseEstimationOnGPU ? "True" : "False") << std::endl; + std::cout << "RunPoseOptimization: " << (RunPoseOptimization ? "True" : "False") << std::endl; +// # pragma message("ORB_SLAM3_ROS2_WRAPPER: CUDA mode enabled (compiling GPU path))") +#endif // ROS Publishers //---- the following is published when a service is called diff --git a/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp index 11d67fa8..b78975ab 100644 --- a/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp +++ b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.cpp @@ -77,3 +77,4 @@ namespace ORB_SLAM3_Wrapper } } } // namespace ORB_SLAM3_Wrapper + diff --git a/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp index 5ddfae8e..cbf98785 100644 --- a/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp +++ b/orb_slam3_ros2_wrapper/src/stereo/stereo-slam-node.hpp @@ -36,3 +36,4 @@ namespace ORB_SLAM3_Wrapper } // namespace ORB_SLAM3_Wrapper #endif // STEREO_SLAM_NODE_HPP_ + diff --git a/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp b/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp index 579658a0..dcf92949 100644 --- a/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp +++ b/orb_slam3_ros2_wrapper/src/stereo/stereo.cpp @@ -23,3 +23,4 @@ int main(int argc, char **argv) return 0; } +