Baseline hercules

README.md

@@ -73,159 +73,16 @@ Type `exit` to exit the container; you can re-enter using `docker/enter_containe
 **Troubleshoot**:
 - If you do not see your code inside docker, double check `docker/run_container.sh` file to make sure you have your workspace mapped properly. 
 
+## Run HERCULES experiments
 
-## Run Demos
-
-### Download example data
-
-Please download the processed data bags from [this link](https://drive.google.com/drive/folders/125N7srccxXPmn2HAQFjwXNNzYOO7DoqV). This containes compact processed bags for forest and urban outdoor environments. Please use the right data with the right scripts as specified below.
-
-
-### What these demos will do
-- Intermittent communication between robot nodes at a fixed time interval. 
-- Multiple robots running on the same computer, and therefore, the computational load is going to be (num_robots multiplied by the computation load of each robot during actual experiment).
-
-### Run multi-robot demo (based on LiDAR data)
-**First, please refer to the section above and make sure you have everything built.**
-
-**Option 1:** Use our tmux script (recommended)
-
-Source and go to the ' folder inside `multi_robot_utils_launch` package:
-```
-source ~/slideslam_ws/devel/setup.bash
-roscd multi_robot_utils_launch/script
-```
-
-Modify `tmux_multi_robot_with_bags_forest.sh` to set the `BAG_DIR` to where you downloaded the bags
-
-Modify `BAG_PLAY_RATE` to your desired play rate (lower than 1.0 if you have a low-specification CPU)
-
-Then make it executable if needed
-```
-chmod +x tmux_multi_robot_with_bags_forest.sh
-```
-
-Finally, execute this script
-```
-./tmux_multi_robot_with_bags_forest.sh
-```
-
-If you want to terminate this program, go to the last terminal window and press `Enter` to kill all the tmux sessions.
-
-**Option 2:** If you prefer not to use this tmux script, please refer to the `roslaunch` commands inside this tmux script and execute those commands by yourself.
-
-**To run the same above example with urban outdoor data, use the `tmux_multi_robot_with_bags_parking_lot.sh` script and repeat the above steps.**
-
-## Run on raw sensor data (RGBD or LiDAR bags)
-This section will guide you through running our code stack with raw sensor data, which is rosbags containing LiDAR-based or RGBD-based data. Note: size of these raw bags are usually anywhere from 10-100 GB.
-
-### Download example data
-
-Please download the LiDAR demo bags from [this link](https://drive.google.com/drive/folders/1heAnoe6qESp2uQjjwwdR0Q3sUijkcUER). It is present inside the `outdoor` folder.
-
-Please download the RGBD demo bags from [this link](https://drive.google.com/drive/folders/1heAnoe6qESp2uQjjwwdR0Q3sUijkcUER). It is present inside the `indoor` folder.
-
-Please download the KITTI benchmark processed bags from [this link](https://drive.google.com/drive/folders/1heAnoe6qESp2uQjjwwdR0Q3sUijkcUER). It is present inside the `kitti_bags` folder.
-
-Please download our trained RangeNet++ model from [this link](https://drive.google.com/drive/folders/1ignTNFZe3KLh9Fy6fakPwwJLEEnV-0E0). It is currently named `penn_smallest.zip`. Follow the instructions in the `Run our LiDAR data experiments`  section below on how to use this model.
-
-### Run our RGBD data experiments
-
-**Option 1:** Use our tmux script (recommended)
-
-Source and go to the ' folder inside `multi_robot_utils_launch` package:
+Run the following command:
 ```
 source ~/slideslam_original_ws/devel/setup.bash
-roscd multi_robot_utils_launch/script
-```
-
-Modify `tmux_single_indoor_robot.sh` to set the `BAG_DIR` to where you downloaded the bags. Then run the following:
-```
-tmuxp load src/SLIDE_SLAM/backend/multi_robot_utils_launch/tmux/tmux_single_indoor_robot.yaml
-```
-
-
-**IMPORTANT**: If it is your first time to run this script, the front-end instance segmentation network will download the weights from the internet. This may take a while depending on your internet speed. Once this is finished, kill all the tmux sessions (see below) and re-run the script.
-
-If you want to terminate this program, go to the last terminal window and press `Enter` to kill all the tmux sessions.
-
-**Option 2:** If you prefer not to use this tmux script, please refer to the `roslaunch` commands inside this tmux script and execute those commands by yourself, or using the detailed instructions found [here](https://github.com/XuRobotics/SLIDE_SLAM/wiki#run-rgbd-raw-bags-detailed-instructions).
-
-### Run our LiDAR Data experiments
-
-**Download the LiDAR semantic segmentation RangeNet++ model**
-```
-(1) Download the model from the above link.
-(2) Unzip the file and place the model in a location of your choice.
-(3) Open the extracted model folder and make sure that there are no files inside having a .zip extension. If there are, then rename ALL OF THEM to remove the .zip extension. For example backbone.zip should be renamed to backbone
-```
-
-**Option 1:** Use our tmux script (recommended)
-
-Make sure you edit the ```infer_node_params.yaml``` file present inside the ```scan2shape_launch/config``` folder and set the value of ```model_dir``` param to point to the path to the RangeNet++ model you downloaded in the previous step. Make sure to compelte the path with the ```/``` at the end.
-
-Source and go to the ' folder inside `multi_robot_utils_launch` package:
-```
-source ~/slideslam_ws/devel/setup.bash
-roscd multi_robot_utils_launch/script
-```
-
-Modify `tmux_single_outdoor_robot.sh` to set the `BAG_DIR` to where you downloaded the bags
-
-Modify `BAG_PLAY_RATE` to your desired play rate (lower than 1.0 if you have a low-specification CPU)
-
-Then make it executable if needed
-```
-chmod +x tmux_single_outdoor_robot.sh
-```
-
-Finally, execute this script
-```
-./tmux_single_outdoor_robot.sh
+tmuxp load src/SLIDE_SLAM/backend/multi_robot_utils_launch/tmux/hercules.yaml
 ```
 
-If you want to terminate this program, go to the last terminal window and press `Enter` to kill all the tmux sessions.
-
-**Option 2:** If you prefer not to use this tmux script, please refer to the `roslaunch` commands inside this tmux script and execute those commands by yourself, or using the detailed instructions found [here](https://github.com/XuRobotics/SLIDE_SLAM/wiki#run-lidar-raw-bags-detailed-instructions).
-
-### Run KITTI Benchmark experiments
-
-**Option 1:** Use our tmux script
-
-Source and go to the ' folder inside `multi_robot_utils_launch` package:
-```
-source ~/slideslam_ws/devel/setup.bash
-roscd multi_robot_utils_launch/script
-```
-
-Modify `tmux_single_outdoor_kitti.sh` to set the `BAG_DIR` to where you downloaded the bags
-
-Then make it executable if needed
-```
-chmod +x tmux_single_outdoor_kitti.sh
-```
-
-Finally, execute this script
-```
-./tmux_single_outdoor_kitti.sh
-```
-
-If you want to terminate this program, go to the last terminal window and press `Enter` to kill all the tmux sessions.
-
 ## 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. 
 
-# Acknowledgement
-We use GTSAM as the backend. We thank [Guilherme Nardari](linkedin.com/in/guilherme-nardari-23ba91a8) for his contributions to this repository. 
 
-# Citation
-If you find our system or any of its modules useful for your academic work, we would appreciate it if you could cite our work as follows:
-```
-@article{liu2024slideslam,
-  title={Slideslam: Sparse, lightweight, decentralized metric-semantic slam for multi-robot navigation},
-  author={Liu, Xu and Lei, Jiuzhou and Prabhu, Ankit and Tao, Yuezhan and Spasojevic, Igor and Chaudhari, Pratik and Atanasov, Nikolay and Kumar, Vijay},
-  journal={arXiv preprint arXiv:2406.17249},
-  year={2024}
-}
-```

backend/multi_robot_utils_launch/tmux/hercules.yaml

@@ -1,31 +1,54 @@
 session_name: slide_slam_nodes
 environment:
   BAG_PLAY_RATE: "1.0"
-  DATA_DIR: '/home/dbutterfield3/data/Hercules_datasets/V1.6'
-  BAG_DIR: '/home/dbutterfield3/data/Hercules_datasets/V1.6/extract/bags_for_slideslam'
-  ROBOT_NAME: 'Drone1'
+  DATA_DIR: '/home/dbutterfield3/data/Hercules_datasets/V2.1.0'
+  BAG_DIR: '/home/dbutterfield3/data/Hercules_datasets/V2.1.0/extract/bags_for_slideslam'
+  RESULT_DIR: '/home/dbutterfield3/data/Hercules_datasets/V2.1.0/results/slideslam/'
+  ROBOT_NAME_0: 'Husky1'
+  ROBOT_0_T: "[0.0,0.055,0.35]"
+  ROBOT_0_R: "[0.5,-0.5,0.5,-0.5]"
+  ROBOT_NAME_1: 'Drone1'
+  ROBOT_1_T: "[0.35,0.055,-0.2]"
+  ROBOT_1_R: "[0.45451948,-0.45451948,0.54167522,-0.54167522]"
 start_directory: "~/slideslam_original_ws"
 windows:
-  - window_name: Main
+  - window_name: ROBOT_0
     layout: tiled
     shell_command_before:
       - export ROS_MASTER_URI=http://localhost:11311
       - source ~/slideslam_original_ws/devel/setup.bash
       - sleep 2
     panes:
       - shell_command:
-          - roslaunch object_modeller rgb_segmentation_f250_hercules.launch odom_topic:=/${ROBOT_NAME}/odom robot_name:=${ROBOT_NAME} label_color_map_path:=${DATA_DIR}/data/label_color_map.csv
+          - roslaunch object_modeller rgb_segmentation_f250_hercules.launch odom_topic:=/${ROBOT_NAME_0}/odom robot_name:=${ROBOT_NAME_0} label_color_map_path:=${DATA_DIR}/data/label_color_map.csv
       - shell_command:
-          - 
+          - roslaunch scan2shape_launch process_cloud_node_hercules.launch odom_topic:=/${ROBOT_NAME_0}/odom robot_name:=${ROBOT_NAME_0} camera_frame_wrt_odom_frame_T:=${ROBOT_0_T} camera_frame_wrt_odom_frame_R_quat:=${ROBOT_0_R}
       - shell_command:
-          - 
+          - roslaunch object_modeller sync_semantic_measurements.launch robot_name:=${ROBOT_NAME_0} odom_topic:=/${ROBOT_NAME_0}/odom cls_config_path:=/home/dbutterfield3/slideslam_original_ws/src/SLIDE_SLAM/frontend/object_modeller/config/hercules_cls_config.yaml
       - shell_command:
-          - 
+          - roslaunch sloam single_robot_sloam_test_hercules.launch enable_rviz:=true odom_topic:=/${ROBOT_NAME_0}/odom robot_name:=${ROBOT_NAME_0} hostRobot_ID:=0 save_results_dir:=${RESULT_DIR}${ROBOT_NAME_0}
+
+  - window_name: ROBOT_1
+    layout: tiled
+    shell_command_before:
+      - export ROS_MASTER_URI=http://localhost:11311
+      - source ~/slideslam_original_ws/devel/setup.bash
+      - sleep 2
+    panes:
+      - shell_command:
+          - roslaunch object_modeller rgb_segmentation_f250_hercules.launch odom_topic:=/${ROBOT_NAME_1}/odom robot_name:=${ROBOT_NAME_1} label_color_map_path:=${DATA_DIR}/data/label_color_map.csv
+      - shell_command:
+          - roslaunch scan2shape_launch process_cloud_node_hercules.launch odom_topic:=/${ROBOT_NAME_1}/odom robot_name:=${ROBOT_NAME_1} camera_frame_wrt_odom_frame_T:=${ROBOT_1_T} camera_frame_wrt_odom_frame_R_quat:=${ROBOT_1_R}
+      - shell_command:
+          - roslaunch object_modeller sync_semantic_measurements.launch robot_name:=${ROBOT_NAME_1} odom_topic:=/${ROBOT_NAME_1}/odom cls_config_path:=/home/dbutterfield3/slideslam_original_ws/src/SLIDE_SLAM/frontend/object_modeller/config/hercules_cls_config.yaml
+      - shell_command:
+          - roslaunch sloam single_robot_sloam_test_hercules.launch enable_rviz:=false odom_topic:=/${ROBOT_NAME_1}/odom robot_name:=${ROBOT_NAME_1} hostRobot_ID:=1 save_results_dir:=${RESULT_DIR}${ROBOT_NAME_1}
 
   - window_name: roscore
     layout: tiled
     shell_command_before:
       - export ROS_MASTER_URI=http://localhost:11311
+      - source ~/slideslam_original_ws/devel/setup.bash
     panes:
       - shell_command:
           - roscore
@@ -35,9 +58,11 @@ windows:
       - shell_command:
           - sleep 2
           - cd ${BAG_DIR}
-          - rosbag play ${ROBOT_NAME}.bag --clock -r ${BAG_PLAY_RATE} -s 0 /cam0/seg:=/${ROBOT_NAME}/cam0/seg /odom:=/${ROBOT_NAME}/odom /cam0/depth:=/${ROBOT_NAME}/cam0/depth
+          - rosbag play ${ROBOT_NAME_0}.bag --clock -r ${BAG_PLAY_RATE} -s 0 /cam0/seg:=/${ROBOT_NAME_0}/cam0/seg /odom_gt:=/${ROBOT_NAME_0}/odom /cam0/depth:=/${ROBOT_NAME_0}/cam0/depth /odom_gt/path:=/${ROBOT_NAME_0}/odom_gt/path
       - shell_command:
-          - 
+          - sleep 2
+          - cd ${BAG_DIR}
+          - rosbag play ${ROBOT_NAME_1}.bag -r ${BAG_PLAY_RATE} -s 0 /cam0/seg:=/${ROBOT_NAME_1}/cam0/seg /odom_gt:=/${ROBOT_NAME_1}/odom /cam0/depth:=/${ROBOT_NAME_1}/cam0/depth /odom_gt/path:=/${ROBOT_NAME_1}/odom_gt/path
 
   - window_name: kill
     panes:

backend/sloam/clipper_semantic_object/src/semantic_clipper.cpp

@@ -95,6 +95,7 @@ namespace semantic_clipper{
             diff += std::pow(sorted_dist_model[i] - sorted_dist_data[i], 2);
         }
         diff = std::sqrt(diff);
+        std::cout << "Triangle difference: " << diff << std::endl;
 
         // if the difference is less than the threshold, add the matched points to the matched_points_model and matched_points_data
         if (diff < threshold) {
@@ -203,7 +204,7 @@ namespace semantic_clipper{
         }
         // create a clipper::Association object from the matched points
         int number_of_initial_matched_points = matched_points_model.size();
-        // std::cout << "Number of initial matched points: " << number_of_initial_matched_points << std::endl;
+        std::cout << "Number of initial matched points: " << number_of_initial_matched_points << std::endl;
         clipper::Association A = clipper::Association(number_of_initial_matched_points, 2);
         for (int i = 0; i < matched_points_model.size(); i++) {
             A(i, 0) = i;
@@ -250,7 +251,7 @@ namespace semantic_clipper{
         // check if the number of matched points is greater than the minimum number of pairs
         if (clipper_matched_points_model.cols() < min_num_pairs) {
             // std::cout << "Number of matched points is less than the minimum number of pairs" << std::endl;
-            ROS_INFO_STREAM("Number of matched points is less than the minimum number of pairs, number of matched points: " << clipper_matched_points_model.cols());
+            ROS_INFO_STREAM("Number of matched points: " << clipper_matched_points_model.cols());
             return false;
         } else {
             ROS_INFO_STREAM("Number of matched points: " << clipper_matched_points_model.cols());

backend/sloam/include/core/sloamNode.h

@@ -83,15 +83,15 @@ class SLOAMNode : public sloam {
   std::vector<double> inter_loop_closure_time;
   int num_attempts_inter_loop_closure = 0;
   int num_successful_inter_loop_closure = 0;
-  bool save_runtime_analysis = false;
+  bool save_runtime_analysis = true;
+  string save_runtime_analysis_dir_;
   std::string runtime_analysis_file;
 
  private:
   // TODO(xu): load the following four params from rosparam
   bool save_inter_robot_closure_results_ = true;
-  string save_results_dir_ = "/home/sam";
-  bool save_robot_trajectory_as_csv_ = false;
-  string save_runtime_analysis_dir_ = "/home/sam";
+  string save_results_dir_;
+  bool save_robot_trajectory_as_csv_ = true;
 
 
   double inter_robot_place_recognition_frequency_;
@@ -104,7 +104,7 @@ class SLOAMNode : public sloam {
   Cloud::Ptr trellisCloud(
       const std::vector<std::vector<TreeVertex>> &landmarks);
   void publishMap_(const ros::Time stamp);
-  void publishCubeMaps_(const ros::Time stamp);
+  void publishCubeMaps_(const ros::Time stamp, const int &robotID);
 
   bool prepareInputs_(const SE3 relativeMotion, const SE3 prevKeyPose,
                       CloudT::Ptr tree_cloud, CloudT::Ptr ground_cloud,

backend/sloam/include/viz/vizTools.h

@@ -56,7 +56,8 @@ void vizTreeModels(const std::vector<Cylinder> &scanTm,
                    size_t &cylinderId);
 void vizCubeModels(const std::vector<Cube> &cubeModels,
                    visualization_msgs::MarkerArray &tMarkerArray,
-                   size_t &cubeId, const bool &is_global_map);
+                   size_t &cubeId, const bool &is_global_map,
+                   const std::string &frame_id, const int &robotID);
 visualization_msgs::Marker vizGroundModel(const Plane &gplane,
                                           const std::string &frame_id,
                                           const int idx);