[Do not merge] Visualization of differences between original and master

.github/workflows/SLIDE_SLAM_test.yml

@@ -0,0 +1,48 @@
+name: SLIDE SLAM Tests
+
+on:
+  push:
+    branches: [ "**" ]
+  pull_request:
+    branches: [ "**" ]
+
+jobs:
+  build-and-test:
+    name: "SLIDE_SLAM build-and-test"
+    runs-on: ubuntu-latest
+    timeout-minutes: 180
+
+    steps:
+    - name: Checkout Repository
+      uses: actions/checkout@v4
+
+    - name: Pull Prebuilt Docker Image
+      run: |
+        docker pull xurobotics/slide-slam:latest
+
+    - name: Set Up Workspace and Clone Repo
+      run: |
+        mkdir -p ~/slideslam_docker_ws/src
+        cd ~/slideslam_docker_ws/src
+        git clone https://github.com/lunarlab-gatech/SLIDE_SLAM.git
+
+    - name: Build Docker Image
+      run: |
+        cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker
+        git checkout apriltag
+        ./build_image.sh
+
+    - name: Run Docker Container
+      run: |
+        cd ~/slideslam_docker_ws/src/SLIDE_SLAM/docker
+        export WS_PATH="$(pwd)"
+        chmod +x run_container_github.sh
+        ./run_container_github.sh
+
+    - name: Build SLIDE_SLAM inside Container and run tests
+      run: |
+        docker start slideslam_ros
+        docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/backend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM
+        docker cp ~/slideslam_docker_ws/src/SLIDE_SLAM/frontend slideslam_ros:/opt/slideslam_docker_ws/src/SLIDE_SLAM
+        docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && cd /opt/slideslam_docker_ws && catkin_make && source devel/setup.bash"
+        docker exec slideslam_ros bash -c "source /opt/ros/noetic/setup.bash && source /opt/slideslam_docker_ws/devel/setup.bash && cd /opt/slideslam_docker_ws && catkin_make run_tests && catkin_test_results --verbose"

.gitignore

@@ -15,3 +15,4 @@ scan2shape/script/max_scale.pkl
 *.npy
 .vscode
 *.png
+bags/

README.md

@@ -101,6 +101,7 @@ docker rm slideslam_ros
 ``` 
 before you run the docker image again.
 
+
 **Troubleshoot**:
 - If you do not see your code or bags inside docker, double check `run_slide_slam_docker.sh` file to make sure you have your workspace and BAG folders mapped properly. 
 
@@ -372,6 +373,17 @@ Finally, execute this script
 
 If you want to terminate this program, go to the last terminal window and press `Enter` to kill all the tmux sessions.
 
+# Run Test Cases
+
+Use the following commands to run the test cases for this repository:
+
+```
+catkin build -DCMAKE_BUILD_TYPE=Debug --catkin-make-args tests
+catkin run_tests
+```
+
+Make sure to rebuild with `-DCMAKE_BUILD_TYPE=Release` when you want to run the algorithm as normal.
+
 # Troubleshoot
 **Rate of segmentation:**
 - When running on your own data, we recommend to throttle the segmentation topic (segmented point cloud or images) rate to 2-4 Hz to avoid computation delay in the front end, especially if you’re experiencing performance issues at higher rates. Please also update the `expected_segmentation_frequency` parameter in the corresponding `process_cloud_node_*_params.yaml` file as well as the `desired_frequency` in the `infer_node_params.yaml` to the actual rate of the topic. 

backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD.sh

@@ -0,0 +1,75 @@
+#!/bin/bash
+
+# Set parameters
+SESSION_NAME=slide_slam_nodes
+BAG_PLAY_RATE=0.5
+BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/'
+SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311"
+
+# Set Display Settings
+CURRENT_DISPLAY=${DISPLAY}
+if [ -z ${DISPLAY} ];
+then
+  echo "DISPLAY is not set"
+  CURRENT_DISPLAY=:0
+fi
+
+# Start Tmux Session if not already in one
+if [ -z ${TMUX} ];
+then
+  TMUX= tmux new-session -s $SESSION_NAME -d
+  echo "Starting new session."
+else
+  echo "Already in tmux, leave it first."
+  exit
+fi
+
+# Make mouse useful in copy mode
+tmux setw -g mouse on
+
+# Setup the panes for the main window
+tmux new-window -t $SESSION_NAME -n "Main"
+tmux split-window -h -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.0
+tmux split-window -v -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.3
+tmux split-window -v -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.0
+tmux split-window -h -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.3
+tmux split-window -h -t $SESSION_NAME
+
+# Setup commands for main window
+tmux select-pane -t $SESSION_NAME:1.0
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch scan2shape_launch infer_node.launch param_file:=config/CoPeD/infer_node_params_wilbur.yaml" Enter
+tmux select-pane -t $SESSION_NAME:1.1
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch object_modeller sync_semantic_measurements.launch" Enter
+tmux select-pane -t $SESSION_NAME:1.2
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch sloam single_robot_sloam_test_LiDAR.launch enable_rviz:=true" Enter
+tmux select-pane -t $SESSION_NAME:1.3
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch scan2shape_launch process_cloud_node_outdoor_with_ns.launch  param_file:=config/CoPeD/process_cloud_node_params_wilbur.yaml" Enter
+tmux select-pane -t $SESSION_NAME:1.4
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wilburOusterOS1-64.yaml" Enter
+tmux select-pane -t $SESSION_NAME:1.5
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 18; cd $BAG_DIR && rosbag play FOREST_wilbur_arl_outdoor_2023_05_19_06_2023-05-19-16-27-47.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wilbur/lidar_points /wilbur/imu/data /wilbur/stereo_left/image_rect_color/compressed /wilbur/lidar_points:=/robot0/lidar_points /wilbur/imu/data:=/robot0/imu/data" Enter
+tmux select-layout -t $SESSION_NAME tiled
+
+# Add window for roscore
+tmux new-window -t $SESSION_NAME -n "roscore"
+tmux split-window -h -t $SESSION_NAME
+tmux split-window -v -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:2.0
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter
+tmux select-pane -t $SESSION_NAME:2.1
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; rosparam set /use_sim_time true" Enter
+tmux select-pane -t $SESSION_NAME:2.2
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; rosrun image_transport republish compressed in:=/wilbur/stereo_left/image_rect_color raw out:=/wilbur/stereo_left/image_rect_color/uncompressed _image_transport:=compressed
+" Enter
+
+# Add window to easily kill all processes
+tmux new-window -t $SESSION_NAME -n "Kill"
+tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}"
+
+# Go back to the first window
+tmux select-window -t $SESSION_NAME:1
+tmux -2 attach-session -t $SESSION_NAME

backend/multi_robot_utils_launch/script/CoPeD/tmux_CoPeD_ground_aerial.sh

@@ -0,0 +1,72 @@
+#!/bin/bash
+
+# Set parameters
+SESSION_NAME=slide_slam_nodes
+BAG_PLAY_RATE=0.5
+BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/CoPeD/FOREST/'
+SETUP_ROS_STRING="export ROS_MASTER_URI=http://localhost:11311"
+
+# Set Display Settings
+CURRENT_DISPLAY=${DISPLAY}
+if [ -z ${DISPLAY} ];
+then
+  echo "DISPLAY is not set"
+  CURRENT_DISPLAY=:0
+fi
+
+# Start Tmux Session if not already in one
+if [ -z ${TMUX} ];
+then
+  TMUX= tmux new-session -s $SESSION_NAME -d
+  echo "Starting new session."
+else
+  echo "Already in tmux, leave it first."
+  exit
+fi
+
+# Make mouse useful in copy mode
+tmux setw -g mouse on
+
+# Setup the panes for the main window
+tmux new-window -t $SESSION_NAME -n "Main"
+tmux split-window -h -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.0
+tmux split-window -v -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.3
+tmux split-window -v -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.0
+tmux split-window -h -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:1.3
+tmux split-window -h -t $SESSION_NAME
+
+# Setup commands for main window
+tmux select-pane -t $SESSION_NAME:1.0
+tmux select-pane -t $SESSION_NAME:1.1
+tmux select-pane -t $SESSION_NAME:1.2
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch faster_lio mapping_ouster64.launch param_file:=config/CoPeD/wandaOusterOS1-64.yaml odom_topic:=/robot0/Odometry" Enter
+tmux select-pane -t $SESSION_NAME:1.3
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch relative_meas_gen odomGPSSync.launch gps_topic:=/wanda/gps slop:=0.1 odom_topic:=/robot0/Odometry" Enter
+tmux select-pane -t $SESSION_NAME:1.4
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_wanda_arl_outtdoor_2023_05_19_06_2023-05-19-16-27-46.bag --clock -r $BAG_PLAY_RATE -s 0 --topics /wanda/lidar_points /wanda/imu/data /wanda/gps /wanda/lidar_points:=/robot0/lidar_points /wanda/imu/data:=/robot0/imu/data" Enter
+tmux select-pane -t $SESSION_NAME:1.5
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 10; cd $BAG_DIR && rosbag play FOREST_race1_2023_05_19_04_29_PM_1.bag -r $BAG_PLAY_RATE -s 0" Enter
+tmux select-layout -t $SESSION_NAME tiled
+
+# Add window for roscore
+tmux new-window -t $SESSION_NAME -n "roscore"
+tmux split-window -h -t $SESSION_NAME
+tmux split-window -v -t $SESSION_NAME
+tmux select-pane -t $SESSION_NAME:2.0
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; roscore" Enter
+tmux select-pane -t $SESSION_NAME:2.1
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 1; rosparam set /use_sim_time true" Enter
+tmux select-pane -t $SESSION_NAME:2.2
+
+
+# Add window to easily kill all processes
+tmux new-window -t $SESSION_NAME -n "Kill"
+tmux send-keys -t $SESSION_NAME "tmux kill-session -t ${SESSION_NAME}"
+
+# Go back to the first window
+tmux select-window -t $SESSION_NAME:1
+tmux -2 attach-session -t $SESSION_NAME

backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_forest.sh

@@ -2,9 +2,8 @@
 
 
 SESSION_NAME=multi_robot_nodes
-BAG_PLAY_RATE=2.0
-#BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/forests'
-BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/forests'
+BAG_PLAY_RATE=1.0
+BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/processed_data/forests'
 
 CURRENT_DISPLAY=${DISPLAY}
 if [ -z ${DISPLAY} ];

backend/multi_robot_utils_launch/script/tmux_multi_robot_with_bags_parking_lot.sh

@@ -4,7 +4,7 @@
 SESSION_NAME=multi_robot_nodes
 BAG_PLAY_RATE=2.0
 # BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/parking-lot'
-BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/parking-lot'
+BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/processed_data/parking-lot'
 
 CURRENT_DISPLAY=${DISPLAY}
 if [ -z ${DISPLAY} ];

backend/multi_robot_utils_launch/script/tmux_single_indoor_robot.sh

@@ -6,7 +6,7 @@ BAG_PLAY_RATE=0.5
 #BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/forests'
 # BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/forests'
 # BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/indoor'
-BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/indoor'
+BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/raw_data/indoor/'
 
 CURRENT_DISPLAY=${DISPLAY}
 if [ -z ${DISPLAY} ];
@@ -51,7 +51,7 @@ tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_mo
 tmux select-pane -t $SESSION_NAME:1.1
 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch object_modeller sync_semantic_measurements.launch robot_name:=robot0 odom_topic:=/dragonfly67/quadrotor_ukf/control_odom" Enter
 tmux select-pane -t $SESSION_NAME:1.2
-tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test_f250.launch enable_rviz:=true" Enter
+tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch sloam single_robot_sloam_test.launch enable_rviz:=true" Enter
 tmux select-pane -t $SESSION_NAME:1.3
 tmux send-keys -t $SESSION_NAME "$SETUP_ROS_STRING; sleep 2; roslaunch scan2shape_launch process_cloud_node_rgbd_indoor_with_ns.launch odom_topic:=/dragonfly67/quadrotor_ukf/control_odom robot_name:=robot0" Enter
 tmux select-pane -t $SESSION_NAME:1.4

backend/multi_robot_utils_launch/script/tmux_single_outdoor_robot.sh

@@ -6,7 +6,7 @@ BAG_PLAY_RATE=0.5
 #BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/forests'
 # BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/forests'
 # BAG_DIR='/home/sam/bags/vems-slam-bags/all_slide_slam_public_demos/indoor'
-BAG_DIR='/opt/bags/vems-slam-bags/all_slide_slam_public_demos/outdoor'
+BAG_DIR='/opt/slideslam_docker_ws/src/SLIDE_SLAM/bags/raw_data/outdoor/'
 
 CURRENT_DISPLAY=${DISPLAY}
 if [ -z ${DISPLAY} ];

backend/sloam/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2
   tf
   roscpp
+  roslib
   actionlib
   actionlib_msgs
 )
@@ -99,14 +100,15 @@ cc_library(
 cc_library(
   NAME core
   INCS include/core ${CLIPPER_DIR}/include
-  SRCS "src/core/sloam.cpp" "src/core/robot.cpp" "src/core/databaseManager.cpp" "src/core/place_recognition.cpp"
+  SRCS "src/core/sloam.cpp" "src/core/robot.cpp" "src/core/databaseManager.cpp" "src/core/place_recognition.cpp" "src/core/input.cpp"
   DEPS sloam_log sloam_base sloam_objects sloam_viz sloam_map sloam_factorgraph Eigen3::Eigen gtsam ${catkin_LIBRARIES} -ldw
   DEFINES USE_CLIPPER=${CLIPPER_VALUE}
 )
 
 if (USE_CLIPPER)
   target_link_libraries(${CC_TARGET_PREFIX}_core PRIVATE semantic_clipper)
 endif()
+target_link_libraries(${CC_TARGET_PREFIX}_core PRIVATE m)
 
 cc_binary(
   NAME node
@@ -115,24 +117,18 @@ cc_binary(
   # DEPS sloam_log sloam_base sloam_objects sloam_core sloam_viz sloam_segmentation sloam_factorgraph gtsam ${catkin_LIBRARIES} -ldw #sloam_loop 
 )
 
+# Example usage: ./sloam_ws/devel/lib/sloam/sloam_all_test --gtest_filter=CylinderTest* (optional filter)
 cc_binary(
   NAME place_recognition_test
-  SRCS "src/tests/place_recognition_test.cpp"
+  SRCS "src/test/place_recognition_test.cpp"
   DEPS sloam_base sloam_core)
 
-# TESTS
-# if(BUILD_TESTING)
-# enable_testing()
-# Example usage: ./sloam_ws/devel/lib/sloam/sloam_all_test --gtest_filter=CylinderTest* (optional filter)
-# cc_test(
-  # NAME all_sloam_test
-  # SRCS "src/tests/run_all_test.cpp" "src/tests/plane_test.cpp" "src/tests/cylinder_test.cpp" "src/tests/core_test.cpp"
-  # DEPS sloam_base sloam_objects sloam_core GTest::GTest)
-# cc_test(
-  # NAME fg_test
-  # SRCS "src/tests/cylinder_factor_test.cpp"
-  # DEPS sloam_base sloam_factorgraph GTest::GTest)
-# cc_test(
-    # NAME cube_fg_test
-    # SRCS "src/tests/cube_factor_test.cpp"
-    # DEPS sloam_base sloam_factorgraph GTest::GTest)
+# Add Unit Tests
+catkin_add_gtest(input_test "src/test/input_test.cpp")
+target_link_libraries(input_test sloam_core ${catkin_LIBRARIES})
+
+catkin_add_gtest(utils_test "src/test/utils_test.cpp")
+target_link_libraries(utils_test sloam_core ${catkin_LIBRARIES})
+
+catkin_add_gtest(sloam_test "src/test/sloam_test.cpp")
+target_link_libraries(sloam_test sloam_core ${catkin_LIBRARIES})