diff --git a/ORB_SLAM3 b/ORB_SLAM3 index 9a7d5a0..2e3a130 160000 --- a/ORB_SLAM3 +++ b/ORB_SLAM3 @@ -1 +1 @@ -Subproject commit 9a7d5a061633dd154cf0da3d93b9b2fcd371202b +Subproject commit 2e3a130985e409ce6b673de66a9c0ad6a218e3be diff --git a/README.md b/README.md index 2b609d6..46f6353 100644 --- a/README.md +++ b/README.md @@ -123,6 +123,34 @@ To run the package, these steps can be followed: 3. The bottom-left terminal contains the launch file to start the pointcloud_stitcher node. This should run soon after you launch the SLAM. 4. If you wish to publish the global pointcloud at any point during the SLAM's operation, simply run the python file in the top-right terminal. You should be able to view the global pointcloud in rviz (you can launch RViz with the correct configuration from the bottom-right terminal). +# Localization only mode + +To run this package in localization only mode, first map the environment and save the atlas. + +## Map the environment and save the atlas + +Navigate to `orb_slam3_ros2_docker/params/orb_slam3_params` and choose the param file corresponding to your desired sensor configuration.
+At the end of the file, append the following + +```yaml +System.SaveAtlasToFile: ./my_atlas +``` +Once done mapping, to save the atlas, press `ctrl+c` only once. Wait for the program to clean up completely. Saving the map might take some time. + +## Relaunch ORB_SLAM3 with pre-loaded map + +Navigate to `orb_slam3_ros2_docker/params/orb_slam3_params` and choose the param file corresponding to your desired sensor configuration.
+At the end of the file, remove the parameter used for saving the atlas and append the following + +```yaml +System.LoadAtlasFromFile: ./my_atlas +``` + +You should see the keyframes in the viewer. Select `Localization mode` in the GUI of ORB_SLAM3.
+Then play the bag file. The following should be the result. The blue features represent VIO. The green features are matched with the underlying saved atlas.
+Star-history + + ### Potential issues you may face. The simulation and the wrapper both have their ```ROS_DOMAIN_ID``` set to 55 so they are meant to work out of the box. However, you may face issues if this environment variable is not set properly. Before you start the wrapper, run ```ros2 topic list``` and make sure the topics `/rgb_camera` and `/depth_camera` are visible inside the ORB-SLAM3 container provided the simulation is running alongside. diff --git a/container_root/.bash_history b/container_root/.bash_history deleted file mode 100644 index 9db4dc6..0000000 --- a/container_root/.bash_history +++ /dev/null @@ -1,972 +0,0 @@ -colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2/ -vsc -colcon build --symlink-install -cd -cd /home/orb/ORB_SLAM3/ -./build.sh -cd -ls -cd colcon_ws2/ -colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -cd /home/orb/ORB_SLAM3/ -./build.sh -cd -ls -cd colcon_ws2/ -vsc -colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2/ -vsc -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd- -cd - -source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2/ -vsc -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - -colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -colcon build --symlink-install -kill -9 % -. ../shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -source colcon_ws2/install/setup.bash -./shell_scripts/multi_orb.sh -cd colcon_ws2/ -vsc -colcon build --symlink-install -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - -colcon build --symlink-install -source colcon_ws2/install/setup.bash -source install/setup.bash -. ../shell_scripts/multi_orb.sh -gdb build/orb_slam3_ros2_wrapper/rgbd -apt-get install gdb -gdb build/orb_slam3_ros2_wrapper/rgbd -ls -cd colcon_ws2/ -ls -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - -ls -colcon build --symlink-install -. ../shell_scripts/multi_orb.sh -source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -cd colcon_ws2/ -ls -vsc -source install/setup.bash -gdb build/orb_slam3_ros2_wrapper/rgbd -apt-get install gdb -gdb build/orb_slam3_ros2_wrapper/rgbd -vsc -cd -cd /home/orb/ORB_SLAM3/ -vsc -cd -nano shell_scripts/multi_orb.sh -vi shell_scripts/multi_orb.sh -kill $(jobs -p) -vi shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -cd colcon_ws -vsc -cd -cd colcon_ws2/ -source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -rm -rf build/ install/ log/ -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - -cd colcon_ws2/ -ls -colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -rm -rf build/ install/ log/ -colcon build --symlink-install -source install/setup.bash -nano ~/.bashrc -vi ~/.bashrc -bash -cd colcon_ws2/ -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - -colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -cd colcon_ws -ls -cd .. -nasj -bas -bash -cd colcon_ws2/ -ls -la -cd src/ -ls -la -cd .. -l -ls -la -cd -cd colcon_ws -ls -source install/setup.bash -. ../shell_scripts/multi_orb.sh -export SCOUT_X=1 -export SCOUT_Y=1 -. ../shell_scripts/multi_orb.sh -cd -cd colcon_ws2 -ls -source install/setup.bash -. ../shell_scripts/multi_orb.sh -nano ../shell_scripts/multi_orb.sh -vi ../shell_scripts/multi_orb.sh -vsc -vi ../shell_scripts/multi_orb.sh -. ../shell_scripts/multi_orb.sh -gdb -apt-get install gdb -gdb build/orb_slam3_ros2_wrapper/rgbd -./build/orb_slam3_ros2_wrapper/rgbd "/home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt" "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/scout_v2_rgbd.yaml" -gdb build/orb_slam3_ros2_wrapper/rgbd -./build/orb_slam3_ros2_wrapper/rgbd "/home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt" "/root/colcon_ws/src/orb_slam3_ros2_wrapper/params/scout_v2_rgbd.yaml" -. ../shell_scripts/multi_orb.sh -cd .. -nano shell_scripts/multi_orb.sh -vi shell_scripts/multi_orb.sh -ls -cd colcon_ws2 -ls -rm -rf build/ install/ log/ -colcon build --symlink-install -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -vsc -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -source install/setup.bash -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -source install/setup.bash -rm -rf build/ install/ log/ -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /home/orb/ORB_SLAM3/ -vsc -. ../shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -nano shell_scripts/multi_orb.sh -vi shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -source colcon_ws/install/setup.bash -./shell_scripts/multi_orb.sh -export SCOUT_X=1.0 -./shell_scripts/multi_orb.sh -export SCOUT_Y=1.0 -./shell_scripts/multi_orb.sh -source colcon_ws2/install/setup.bash -./shell_scripts/multi_orb.sh -source colcon_ws2/install/setup.bash -./shell_scripts/multi_orb.sh -ls -cd colcon_ws2/ -ls -source install/setup.bash -ros2 pkg prefix orb_slam3_ros2_wrapper -./shell_scripts/multi_orb.sh -cd colcon_ws2 -vsc -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -source install/setup.bash -./shell_scripts/multi_orb.sh -cd -./shell_scripts/multi_orb.sh -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -ls -rm -rf build/ install/ log/ -cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -ls -rm -rf build/ install/ log/ -ls -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -cd -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -cd -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -vsc -cd -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws -cd -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -vsc -cd -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd -cd colcon_ws -vsc -cd .. -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -cd colcon_ws2/ -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash\ -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -source install/setup.bash -. ../shell_scripts/multi_orb.sh -jobs -nano shell_scripts/multi_orb.sh -vim shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -vim shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -vim shell_scripts/multi_orb.sh -jobs -pgrep orb -cd colcon_ws2/ -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -clear -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2/ -. ../shell_scripts/multi_orb.sh -ls -cd shell_scripts/ -ls -nano multi_orb.sh -vi multi_orb.sh -cd -cd colcon_ws2 -ls -vsc -ls -source install/setup.bash -ls -. ../shell_scripts/multi_orb.sh -cd colcon_ws -cd .. -cd colcon_ws2 -ls -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws -ls -cd -cd colcon_ws2/ -vsc -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws2/ -vsc -. ../shell_scripts/multi_orb.sh -nano shell_scripts/multi_orb.sh -vi shell_scripts/multi_orb.sh -exit 1 -cd /root/colcon_ws2 && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -sleep 4 -vi shell_scripts/multi_orb.sh -cd .. -vi shell_scripts/multi_orb.sh -. ../shell_scripts/multi_orb.sh -cd colcon_ws2/ -. ../shell_scripts/multi_orb.sh -kill -9 %1 -vi ../shell_scripts/multi_orb.sh -. ../shell_scripts/multi_orb.sh -kill -9 %1 -ls -vi shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -kill -9 %1 -vi shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -ls -cd colcon_ws2/ -vsc -cd -ls -xeyes -sudo chmod +x container_root/shell_scripts/launch_orb.sh -sudo chmod +x shell_scripts/launch_orb.sh -ls -rm -rf KeyFrameTrajectory.txt -ls -cd /root/colcon_ws/ -colcon build --symlink-install -source install/setup.bash -colcon build --symlink-install -source install/setup.bash -cd /root/colcon_ws/ -colcon build --symlink-install -source install/setup.bash -sudo chmod +x /root/shell_scripts/launch_orb.sh -./shell_scripts/launch_orb.sh -./root/shell_scripts/launch_orb.sh -. /root/shell_scripts/launch_orb.sh -ls -grep -r "colcon_ws2" . -. /root/shell_scripts/launch_orb.sh -kill -9 %1 -. /root/shell_scripts/launch_orb.sh -. /root/shell_scripts/launch_orb.sh -. /root/shell_scripts/launch_orb.sh -. /root/shell_scripts/launch_orb.sh -. /root/shell_scripts/launch_orb.sh -ps -ef | grep orb -pidof /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd -ps -ef | grep orb -kill 67 -kill 97 -pidof /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd -pidof /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd -ps -ef | grep orb -pidof /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd -pidof /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd /home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt /root/colcon_ws/src/orb_slam3_ros2_wrapper/params/scout_v2_rgbd.yaml --ros-args -r __ns:=/ --params-file /tmp/tmpd6vgf5ou -pgrep /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd /home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt /root/colcon_ws/src/orb_slam3_ros2_wrapper/params/scout_v2_rgbd.yaml --ros-args -r __ns:=/ --params-file /tmp/tmpd6vgf5ou -pgrep orb -ps -ef | grep orb -pgrep /root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd -pgrep /usr/bin/python3 /opt/ros/humble/bin/ros2 launch orb_slam3_ros2_wrapper rgbd.launch.py -ps -ef | grep orb -ps -ef | grep rgbd -pgrep -f /usr/bin/python3 /opt/ros/humble/bin/ros2 launch orb_slam3_ros2_wrapper rgbd.launch.py -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd /home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt /root/colcon_ws/src/orb_slam3_ros2_wrapper/params/scout_v2_rgbd.yaml --ros-args -r __ns:=/ --params-file /tmp/tmpd6vgf5ou" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd /home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt /root/colcon_ws/src/orb_slam3_ros2_wrapper/params/scout_v2_rgbd.yaml" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgb -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -pgrep -f "/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/rgbd" -tmux -. /root/shell_scripts/launch_orb.sh -./shell_scripts/multi_orb.sh -kill -9 %1 -./shell_scripts/multi_orb.sh -ls -cd colcon_ws/ -colcon build --symlink-install -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -ls -cd colcon_ws/ -ls -colcon build --symlink-install -cd -ros2 launch orb_slam3_ros2_wrapper multirobot.launch.py -xeyes -cd /root/colcon_ws/ -colcon build --symlink-install -source install/setup.bash -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -ls -. ../shell_scripts/multi_orb.sh -ls -cd colcon_ws/ -ls -vsc -. ../shell_scripts/multi_orb.sh -cd colcon_ws/2 -cd colcon_ws/ -. ../shell_scripts/multi_orb.sh -cd colcon_ws/ -. ../shell_scripts/multi_orb.sh -./shell_scripts/multi_orb.sh -kill -9 %1 -./shell_scripts/multi_orb.sh -kill -9 %1 -./shell_scripts/multi_orb.sh -kill -9 %1 -cd colcon_ws/ -ls -vsc -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -ls -cd colcon_ws/ -ls -colcon build --symlink-install -ls -vsc -clear -colcon build --symlink-install -clear -colcon build --symlink-install -source install/setup.bash -cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -ls -cd colcon_ws/ -ls -vsc -cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -. ../shell_scripts/multi_orb.sh -cd colcon_ws/ -. ../shell_scripts/multi_orb.sh -cd colcon_ws/ -. ../shell_scripts/multi_orb.sh -cd colcon_ws/l -cd colcon_ws/ -cd colcon_ws/l -. ../shell_scripts/multi_orb.sh -cd colcon_ws/ -. ../shell_scripts/multi_orb.sh -cd colcon_ws/ -vsc -cd -cd shell_scripts/ -vsc -./multi_orb.sh -kill -9 %1 -./multi_orb.sh -kill -9 %1 -./multi_orb.sh -kill -9 %1 -./multi_orb.sh -cd /home/orb/ORB_SLAM3/ -vsc -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -ls -cd src/ -ls -grep -r "Creation of new map with last KF id:" . -grep -r "Atlas::CreateNewMap()" -grep -r "CreateNewMap()" -clear -grep -r "CreateNewMap()" -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -bash -ls -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 -cd /root/colcon_ws && cd /home/orb/ORB_SLAM3/ && ./build.sh && cd - && colcon build --symlink-install && source install/setup.bash -clear -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py--show-args -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 -clwar -clear -ls -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 -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=mono_imu -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 sensor_config:=mono_imu -l -./launch_slam.sh -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=rgbd_imu -ros2 launch orb_slam3_ros2_wrapper unirobot.launch.py sensor_config:=mono_imu -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 -ls -clear -ls -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/media/loc_mode.png b/media/loc_mode.png new file mode 100644 index 0000000..84f0e58 Binary files /dev/null and b/media/loc_mode.png differ 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 dded627..e6acc39 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 @@ -109,6 +109,11 @@ namespace ORB_SLAM3_Wrapper void resetLocalMapping(); + void saveAtlas() + { + mSLAM_->SaveAtlas(ORB_SLAM3::System::FileType::BINARY_FILE); + } + size_t getNumberOfMaps() { return orbAtlas_->GetAllMaps().size(); diff --git a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp index afef4aa..392363c 100644 --- a/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp +++ b/orb_slam3_ros2_wrapper/include/orb_slam3_ros2_wrapper/slam_node_base.hpp @@ -76,6 +76,10 @@ namespace ORB_SLAM3_Wrapper std::shared_ptr request, std::shared_ptr response); + void saveMapSrv(std::shared_ptr request_header, + std::shared_ptr request, + std::shared_ptr response); + private: // ROS Publishers and Subscribers rclcpp::Subscription::SharedPtr odomSub_; @@ -94,6 +98,7 @@ namespace ORB_SLAM3_Wrapper rclcpp::Service::SharedPtr getMapPointsService_; rclcpp::Service::SharedPtr mapPointsService_; rclcpp::Service::SharedPtr resetLocalMapSrv_; + rclcpp::Service::SharedPtr saveMapService_; // ROS Timers rclcpp::TimerBase::SharedPtr mapDataTimer_; rclcpp::CallbackGroup::SharedPtr mapDataCallbackGroup_; diff --git a/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp b/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp index 657ffe2..6853dd2 100644 --- a/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp +++ b/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp @@ -41,7 +41,11 @@ namespace ORB_SLAM3_Wrapper ORBSLAM3Interface::~ORBSLAM3Interface() { std::cout << "Interface destructor" << endl; - mSLAM_->Shutdown(); + // Only shutdown if not already shut down (e.g., if shutdown was called explicitly) + if (mSLAM_ && !mSLAM_->isShutDown()) + { + mSLAM_->Shutdown(); + } mSLAM_.reset(); mapReferencePoses_.clear(); allKFs_.clear(); diff --git a/orb_slam3_ros2_wrapper/src/slam_node_base.cpp b/orb_slam3_ros2_wrapper/src/slam_node_base.cpp index 38f1726..265a0ee 100644 --- a/orb_slam3_ros2_wrapper/src/slam_node_base.cpp +++ b/orb_slam3_ros2_wrapper/src/slam_node_base.cpp @@ -53,6 +53,7 @@ namespace ORB_SLAM3_Wrapper mapPointsCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); mapPointsService_ = this->create_service("orb_slam3/get_all_landmarks_in_map", std::bind(&SlamNodeBase::publishMapPointCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); resetLocalMapSrv_ = this->create_service("orb_slam3/reset_mapping", std::bind(&SlamNodeBase::resetActiveMapSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); + saveMapService_ = this->create_service("orb_slam3/save_map", std::bind(&SlamNodeBase::saveMapSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, mapPointsCallbackGroup_); // TF tfBroadcaster_ = std::make_shared(this); @@ -290,4 +291,11 @@ namespace ORB_SLAM3_Wrapper visibleLandmarksPose_->publish(pose_stamped); } + void SlamNodeBase::saveMapSrv(std::shared_ptr /*request_header*/, + std::shared_ptr request, + std::shared_ptr response) + { + interface_->saveAtlas(); + } + } // namespace ORB_SLAM3_Wrapper