From 10416c58a26ef12acdf10f4d876d6de339b63626 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 26 Jun 2023 14:13:09 -0400 Subject: [PATCH] Feature/remove ros (#171) * drop ros code and flatten * tweak rosinstall files * update readme * update headers a little * tweak section headers more * fix quickstart command * fix repo url * fix kimera semantics branch * last branch tweaks for rosinstall Co-authored-by: Nathan Hughes Co-authored-by: Ubuntu --- hydra/CMakeLists.txt => CMakeLists.txt | 0 README.md | 127 +-- .../nanoflann.CMakeLists.txt.in | 0 .../ade20k_mp3d_typology.yaml | 0 {hydra/config => config}/ade20k_typology.yaml | 0 .../config => config}/d455/d455_typology.yaml | 0 .../d455/dsg_backend_config.yaml | 0 .../d455/dsg_frontend_config.yaml | 0 .../d455/dsg_lcd_config.yaml | 0 .../default_reconstruction_config.yaml | 0 .../default_robot/dsg_backend_config.yaml | 0 .../default_robot/dsg_frontend_config.yaml | 0 .../default_robot/dsg_gnn_lcd_config.yaml | 0 .../default_robot/dsg_lcd_config.yaml | 0 {hydra/config => config}/extrinsics/mp3d.yaml | 0 .../config => config}/extrinsics/sidpac.yaml | 0 .../extrinsics/sparkal1.yaml | 0 .../extrinsics/sparkal2.yaml | 0 .../extrinsics/uhumans2.yaml | 0 .../habitat/dsg_backend_config.yaml | 0 .../habitat/dsg_frontend_config.yaml | 0 .../habitat/dsg_lcd_config.yaml | 0 .../habitat/mp3d_typology.yaml | 0 .../habitat/reconstruction_config.yaml | 0 .../sidpac/dsg_backend_config.yaml | 0 .../sidpac/dsg_frontend_config.yaml | 0 .../sidpac/dsg_gnn_lcd_config.yaml | 0 .../sidpac/dsg_lcd_config.yaml | 0 .../sidpac/reconstruction_config.yaml | 0 .../sidpac/sidpac_typology.yaml | 0 .../simmons_a1/dsg_backend_config.yaml | 0 .../simmons_a1/dsg_frontend_config.yaml | 0 .../simmons_a1/dsg_gnn_lcd_config.yaml | 0 .../simmons_a1/dsg_lcd_config.yaml | 0 .../simmons_a1/reconstruction_config.yaml | 0 .../simmons_jackal/dsg_backend_config.yaml | 0 .../simmons_jackal/dsg_frontend_config.yaml | 0 .../simmons_jackal/dsg_gnn_lcd_config.yaml | 0 .../simmons_jackal/dsg_lcd_config.yaml | 0 .../simmons_jackal/reconstruction_config.yaml | 0 .../uhumans2/apartment_lcd_embeddings.yaml | 0 .../uhumans2/dsg_backend_config.yaml | 0 .../uhumans2/dsg_frontend_config.yaml | 0 .../uhumans2/dsg_gnn_lcd_config.yaml | 0 .../uhumans2/dsg_lcd_config.yaml | 0 .../uhumans2/office_lcd_embeddings.yaml | 0 .../uhumans2/reconstruction_config.yaml | 0 .../uhumans2/subway_lcd_embeddings.yaml | 0 .../uhumans2/uhumans2_apartment_typology.yaml | 0 .../uhumans2/uhumans2_office_typology.yaml | 0 .../uhumans2/uhumans2_subway_typology.yaml | 0 hydra/.gitignore | 6 - hydra/README.md | 32 - ...ild_config.h.in => hydra_build_config.h.in | 0 hydra_msgs/CMakeLists.txt | 25 - hydra_msgs/msg/ActiveLayer.msg | 4 - hydra_msgs/msg/ActiveMesh.msg | 3 - hydra_msgs/msg/DsgUpdate.msg | 6 - hydra_msgs/package.xml | 21 - hydra_msgs/srv/GetDsg.srv | 2 - hydra_msgs/srv/QueryFreespace.srv | 6 - hydra_ros/.gitignore | 6 - hydra_ros/CMakeLists.txt | 127 --- hydra_ros/README.md | 10 - hydra_ros/bin/csv_to_tf | 58 -- hydra_ros/bin/gnn_node | 535 ---------- hydra_ros/bin/odom_to_tf | 47 - hydra_ros/cfg/Colormap.cfg | 16 - hydra_ros/cfg/DynamicLayerVisualizer.cfg | 115 --- hydra_ros/cfg/GvdVisualizer.cfg | 49 - hydra_ros/cfg/LayerVisualizer.cfg | 132 --- hydra_ros/cfg/Visualizer.cfg | 73 -- .../config/graph_visualization_config.yaml | 45 - hydra_ros/config/gvd_validator/uh2.yaml | 57 -- .../config/hydra_visualizer/agent_layer.yaml | 17 - .../hydra_visualizer/buildings_layer.yaml | 24 - .../hydra_visualizer/objects_layer.yaml | 26 - .../config/hydra_visualizer/places_layer.yaml | 24 - .../config/hydra_visualizer/rooms_layer.yaml | 24 - .../config/hydra_visualizer/visualizer.yaml | 25 - .../config/object_visualizer/agent_layer.yaml | 17 - .../object_visualizer/buildings_layer.yaml | 24 - .../object_visualizer/objects_layer.yaml | 25 - .../object_visualizer/places_layer.yaml | 24 - .../config/object_visualizer/rooms_layer.yaml | 24 - .../config/object_visualizer/visualizer.yaml | 25 - hydra_ros/config/overrides.yaml | 0 hydra_ros/config/ros_params.yaml | 9 - hydra_ros/hydra_utils_cmake | 91 -- .../include/hydra_ros/config/ros_parser.h | 200 ---- .../include/hydra_ros/config/ros_utilities.h | 138 --- .../hydra_ros/pipeline/hydra_ros_pipeline.h | 91 -- .../include/hydra_ros/pipeline/ros_backend.h | 122 --- .../include/hydra_ros/pipeline/ros_frontend.h | 130 --- .../hydra_ros/pipeline/ros_lcd_registration.h | 50 - .../hydra_ros/pipeline/ros_reconstruction.h | 126 --- .../hydra_ros/utils/dsg_streaming_interface.h | 95 -- .../include/hydra_ros/utils/node_utilities.h | 72 -- .../hydra_ros/utils/semantic_ros_publishers.h | 99 -- .../hydra_ros/visualizer/colormap_utilities.h | 50 - .../hydra_ros/visualizer/dsg_mesh_plugins.h | 88 -- .../visualizer/dsg_visualizer_plugin.h | 59 -- .../dynamic_scene_graph_visualizer.h | 243 ----- .../visualizer/gvd_visualization_utilities.h | 123 --- .../visualizer/topology_server_visualizer.h | 147 --- .../hydra_ros/visualizer/visualizer_plugins.h | 111 --- .../hydra_ros/visualizer/visualizer_types.h | 64 -- .../visualizer/visualizer_utilities.h | 199 ---- hydra_ros/launch/datasets/mp3d.launch | 34 - hydra_ros/launch/datasets/sidpac.launch | 29 - hydra_ros/launch/datasets/simmons_a1.launch | 37 - .../launch/datasets/simmons_jackal.launch | 35 - hydra_ros/launch/datasets/uhumans2.launch | 36 - hydra_ros/launch/hydra.launch | 166 ---- hydra_ros/launch/hydra_robot.launch | 44 - hydra_ros/launch/hydra_robot_t265.launch | 45 - hydra_ros/launch/misc/dsg_optimizer.launch | 52 - hydra_ros/launch/places/gvd_validator.launch | 22 - hydra_ros/launch/places/hydra_topology.launch | 68 -- .../places/hydra_topology_sidpac.launch | 30 - .../hydra_topology_simmons_jackal.launch | 26 - .../places/hydra_topology_uhumans2.launch | 32 - .../utils/data_sources/a1_pointcloud.launch | 35 - .../utils/data_sources/d455_pointcloud.launch | 35 - .../data_sources/jackal_pointcloud.launch | 36 - .../data_sources/sidpac_pointcloud.launch | 24 - .../data_sources/uhumans2_pointcloud.launch | 24 - .../launch/utils/gt_pose/robot_gt_pose.launch | 20 - .../utils/gt_pose/sidpac_gt_pose.launch | 14 - .../utils/hydra_streaming_visualizer.launch | 55 -- .../launch/utils/hydra_visualizer.launch | 31 - .../launch/utils/includes/a1_static_tfs.xml | 21 - .../includes/a1_t265_camera_static_tfs.xml | 39 - .../utils/includes/a1_t265_static_tfs.xml | 20 - .../utils/includes/hydra_visualizer_base.xml | 32 - .../includes/hydra_visualizer_params.xml | 18 - .../utils/includes/hydra_visualizer_rviz.xml | 10 - .../utils/includes/rgbd_to_pointcloud.xml | 35 - .../utils/includes/sparkal1_static_tfs.xml | 105 -- .../utils/includes/sparkal2_static_tfs.xml | 55 -- .../utils/includes/uhumans2_static_tfs.xml | 15 - hydra_ros/launch/utils/odom_to_tf.launch | 12 - .../utils/publish_rotation_transform.launch | 18 - .../launch/utils/scene_graph_logger.launch | 17 - hydra_ros/nodelet_plugins.xml | 5 - hydra_ros/package.xml | 44 - .../resources/a1_center_mask_roi_640.png | Bin 1733 -> 0 bytes hydra_ros/resources/a1_left_mask_roi.png | Bin 3510 -> 0 bytes hydra_ros/rviz/a1.rviz | 254 ----- hydra_ros/rviz/a1_topdown.rviz | 249 ----- hydra_ros/rviz/dsg_optimizer.rviz | 191 ---- hydra_ros/rviz/gvd_validator.rviz | 189 ---- .../rviz/hydra_streaming_visualizer.rviz | 272 ----- .../hydra_streaming_visualizer_topdown.rviz | 216 ---- hydra_ros/rviz/hydra_visualizer.rviz | 191 ---- hydra_ros/rviz/mp3d.rviz | 255 ----- hydra_ros/rviz/sidpac.rviz | 212 ---- hydra_ros/rviz/sidpac_topdown.rviz | 207 ---- hydra_ros/rviz/sparkal1.rviz | 253 ----- hydra_ros/rviz/sparkal2.rviz | 252 ----- hydra_ros/rviz/uhumans2.rviz | 321 ------ hydra_ros/rviz/uhumans2_topdown.rviz | 314 ------ hydra_ros/scripts/copy_static_tfs.py | 82 -- hydra_ros/scripts/decompress_uhumans2.py | 118 --- hydra_ros/scripts/download_uhumans2.py | 143 --- hydra_ros/scripts/run_robot.sh | 32 - hydra_ros/scripts/run_robot_t265.sh | 32 - hydra_ros/scripts/scrape_kimera_extrinsics.py | 81 -- hydra_ros/src/config/ros_parser.cpp | 74 -- hydra_ros/src/nodes/dsg_optimizer_node.cpp | 155 --- hydra_ros/src/nodes/gvd_validator_node.cpp | 560 ----------- hydra_ros/src/nodes/hydra_node.cpp | 70 -- hydra_ros/src/nodes/hydra_topology_node.cpp | 71 -- hydra_ros/src/nodes/hydra_visualizer_node.cpp | 242 ----- hydra_ros/src/nodes/rotate_tf_node.cpp | 114 --- .../src/nodes/scene_graph_logger_node.cpp | 98 -- hydra_ros/src/pipeline/hydra_ros_pipeline.cpp | 220 ----- hydra_ros/src/pipeline/ros_backend.cpp | 168 ---- hydra_ros/src/pipeline/ros_frontend.cpp | 180 ---- .../src/pipeline/ros_lcd_registration.cpp | 134 --- hydra_ros/src/pipeline/ros_reconstruction.cpp | 304 ------ .../src/utils/dsg_streaming_interface.cpp | 154 --- hydra_ros/src/utils/mask_nodelet.cpp | 102 -- hydra_ros/src/utils/node_utilities.cpp | 141 --- .../src/visualizer/colormap_utilities.cpp | 99 -- hydra_ros/src/visualizer/dsg_mesh_plugins.cpp | 291 ------ .../dynamic_scene_graph_visualizer.cpp | 632 ------------ .../gvd_visualization_utilities.cpp | 735 -------------- .../visualizer/topology_server_visualizer.cpp | 387 -------- .../src/visualizer/visualizer_plugins.cpp | 433 -------- .../src/visualizer/visualizer_utilities.cpp | 933 ------------------ hydra_ros/tests/CMakeLists.txt | 3 - hydra_ros/tests/hydra_ros.test | 15 - hydra_ros/tests/main.cpp | 43 - hydra_ros/tests/resources/map_config.yaml | 10 - .../tests/resources/nested_test_config.yaml | 9 - hydra_ros/tests/resources/test_config.yaml | 17 - hydra_ros/tests/test_ros_config.cpp | 282 ------ .../hydra/backend/backend_config.h | 0 .../hydra/backend/backend_module.h | 0 .../hydra/backend/merge_handler.h | 0 .../hydra/backend/update_functions.h | 0 .../include => include}/hydra/common/common.h | 0 .../hydra/common/dsg_types.h | 0 .../hydra/common/hydra_config.h | 0 .../hydra/common/input_queue.h | 0 .../hydra/common/robot_prefix_config.h | 0 .../hydra/common/shared_module_state.h | 0 .../include => include}/hydra/config/config.h | 0 .../hydra/config/config_formatter.h | 0 .../hydra/config/config_parser.h | 0 .../hydra/config/config_traits.h | 0 .../hydra/config/config_visitor.h | 0 .../hydra/config/eigen_config_types.h | 0 .../hydra/config/ostream_formatter.h | 0 .../hydra/config/yaml_parser.h | 0 .../hydra/frontend/frontend_config.h | 0 .../hydra/frontend/frontend_module.h | 0 .../hydra/frontend/mesh_segmenter.h | 0 .../hydra/gnn/gnn_interface.h | 0 .../hydra/gnn/ort_utilities.h | 0 {hydra/include => include}/hydra/gnn/tensor.h | 0 .../hydra/loop_closure/descriptor_matching.h | 0 .../hydra/loop_closure/detector.h | 0 .../hydra/loop_closure/gnn_descriptors.h | 0 .../hydra/loop_closure/loop_closure_config.h | 0 .../hydra/loop_closure/loop_closure_module.h | 0 .../hydra/loop_closure/registration.h | 0 .../loop_closure/scene_graph_descriptors.h | 0 .../hydra/loop_closure/subgraph_extraction.h | 0 .../places/compression_graph_extractor.h | 0 .../hydra/places/floodfill_graph_extractor.h | 0 .../hydra/places/graph_extractor_config.h | 0 .../hydra/places/graph_extractor_interface.h | 0 .../hydra/places/graph_extractor_types.h | 0 .../hydra/places/graph_extractor_utilities.h | 0 .../hydra/places/gvd_graph.h | 0 .../hydra/places/gvd_integrator.h | 0 .../hydra/places/gvd_integrator_config.h | 0 .../hydra/places/gvd_parent_tracker.h | 0 .../hydra/places/gvd_thinning.h | 0 .../hydra/places/gvd_utilities.h | 0 .../hydra/places/gvd_voxel.h | 0 .../hydra/places/nearest_voxel_utilities.h | 0 .../hydra/places/update_statistics.h | 0 .../hydra/places/vertex_voxel.h | 0 .../hydra/places/voxblox_types.h | 0 .../hydra/reconstruction/combo_integrator.h | 0 .../hydra/reconstruction/configs.h | 0 .../reconstruction/reconstruction_config.h | 0 .../reconstruction/reconstruction_module.h | 0 .../reconstruction/reconstruction_output.h | 0 .../hydra/reconstruction/voxblox_utilities.h | 0 .../voxel_aware_marching_cubes.h | 0 .../voxel_aware_mesh_integrator.h | 0 .../hydra/rooms/graph_clustering.h | 0 .../hydra/rooms/graph_filtration.h | 0 .../hydra/rooms/room_finder.h | 0 .../hydra/rooms/room_finder_config.h | 0 .../hydra/rooms/room_utilities.h | 0 .../hydra/utils/disjoint_set.h | 0 .../hydra/utils/display_utilities.h | 0 .../hydra/utils/log_utilities.h | 0 .../hydra/utils/minimum_spanning_tree.h | 0 .../hydra/utils/nearest_neighbor_utilities.h | 0 .../hydra/utils/timing_utilities.h | 0 install/hydra.rosinstall | 5 +- install/hydra_mit.rosinstall | 4 + {hydra/models => models}/.gitignore | 0 {hydra/models => models}/lcd/.gitignore | 0 {hydra/models => models}/lcd/object_gnn.onnx | Bin {hydra/models => models}/lcd/place_gnn.onnx | Bin {hydra/output => output}/.gitignore | 0 {hydra/output => output}/default/.gitignore | 0 .../default/backend/.gitignore | 0 .../default/frontend/.gitignore | 0 .../output => output}/default/lcd/.gitignore | 0 .../output => output}/default/pgmo/.gitignore | 0 .../default/topology/.gitignore | 0 {hydra/output => output}/sidpac/.gitignore | 0 .../sidpac/backend/.gitignore | 0 .../sidpac/frontend/.gitignore | 0 .../output => output}/sidpac/lcd/.gitignore | 0 .../output => output}/sidpac/pgmo/.gitignore | 0 .../sidpac/topology/.gitignore | 0 {hydra/output => output}/uhumans2/.gitignore | 0 .../uhumans2/backend/.gitignore | 0 .../uhumans2/frontend/.gitignore | 0 .../output => output}/uhumans2/lcd/.gitignore | 0 .../uhumans2/pgmo/.gitignore | 0 .../uhumans2/topology/.gitignore | 0 hydra/package.xml => package.xml | 0 .../export_label_embeddings.py | 0 {hydra/scripts => scripts}/make_colormap.py | 0 {hydra/scripts => scripts}/modify_dgraph.py | 0 {hydra/src => src}/CMakeLists.txt | 0 {hydra/src => src}/backend/backend_module.cpp | 0 {hydra/src => src}/backend/merge_handler.cpp | 0 .../src => src}/backend/update_functions.cpp | 0 {hydra/src => src}/common/hydra_config.cpp | 0 .../common/robot_prefix_config.cpp | 0 .../common/shared_module_state.cpp | 0 {hydra/src => src}/config/yaml_parser.cpp | 0 .../src => src}/frontend/frontend_module.cpp | 0 .../src => src}/frontend/mesh_segmenter.cpp | 0 {hydra/src => src}/gnn/CMakeLists.txt | 0 .../gnn/cmake/ort.CMakeLists.txt.in | 0 {hydra/src => src}/gnn/gnn_interface.cpp | 0 {hydra/src => src}/gnn/gnn_model_reader.cpp | 0 {hydra/src => src}/gnn/ort_utilities.cpp | 0 {hydra/src => src}/gnn/tensor.cpp | 0 .../gnn/tests/resources/make_simple_model.py | 0 .../gnn/tests/resources/simple_model.onnx | Bin .../gnn/tests/utest_gnn_interface.cpp | 0 {hydra/src => src}/gnn/tests/utest_main.cpp | 0 {hydra/src => src}/gnn/tests/utest_tensor.cpp | 0 .../loop_closure/descriptor_matching.cpp | 0 {hydra/src => src}/loop_closure/detector.cpp | 0 .../loop_closure/gnn_descriptors.cpp | 0 .../loop_closure/loop_closure_module.cpp | 0 .../src => src}/loop_closure/registration.cpp | 0 .../loop_closure/scene_graph_descriptors.cpp | 0 .../loop_closure/subgraph_extraction.cpp | 0 .../places/compression_graph_extractor.cpp | 0 .../places/floodfill_graph_extractor.cpp | 0 .../places/graph_extractor_interface.cpp | 0 .../places/graph_extractor_types.cpp | 0 .../places/graph_extractor_utilities.cpp | 0 {hydra/src => src}/places/gvd_graph.cpp | 0 {hydra/src => src}/places/gvd_integrator.cpp | 0 .../src => src}/places/gvd_parent_tracker.cpp | 0 {hydra/src => src}/places/gvd_thinning.cpp | 0 {hydra/src => src}/places/gvd_utilities.cpp | 0 {hydra/src => src}/places/gvd_voxel.cpp | 0 .../places/nearest_voxel_utilities.cpp | 0 .../src => src}/places/update_statistics.cpp | 0 .../reconstruction/reconstruction_config.cpp | 0 .../reconstruction/reconstruction_module.cpp | 0 .../voxel_aware_marching_cubes.cpp | 0 .../voxel_aware_mesh_integrator.cpp | 0 {hydra/src => src}/rooms/graph_clustering.cpp | 0 {hydra/src => src}/rooms/graph_filtration.cpp | 0 {hydra/src => src}/rooms/room_finder.cpp | 0 {hydra/src => src}/rooms/room_utilities.cpp | 0 {hydra/src => src}/utils/disjoint_set.cpp | 0 .../src => src}/utils/display_utilities.cpp | 0 {hydra/src => src}/utils/log_utilities.cpp | 0 .../utils/minimum_spanning_tree.cpp | 0 .../utils/nearest_neighbor_utilities.cpp | 0 {hydra/src => src}/utils/timing_utilities.cpp | 0 {hydra/tests => tests}/CMakeLists.txt | 0 .../backend/test_merge_handler.cpp | 0 .../backend/test_update_functions.cpp | 0 {hydra/tests => tests}/config/test_config.cpp | 0 .../frontend/test_frontend.cpp | 0 .../include/hydra_test/gvd_layer_utils.h | 0 .../include/hydra_test/place_fixtures.h | 0 .../include/hydra_test/resources.h | 0 .../loop_closure/test_descriptor_matching.cpp | 0 .../loop_closure/test_detector.cpp | 0 .../loop_closure/test_gnn_descriptors.cpp | 0 .../loop_closure/test_registration.cpp | 0 .../test_scene_graph_descriptors.cpp | 0 .../loop_closure/test_subgraph_extraction.cpp | 0 {hydra/tests => tests}/main.cpp | 0 .../test_compression_graph_extractor.cpp | 0 .../places/test_esdf_comparison.cpp | 0 .../places/test_floodfill_graph_extractor.cpp | 0 .../places/test_graph_extractor_utilities.cpp | 0 .../places/test_gvd_incremental.cpp | 0 .../places/test_gvd_integrator.cpp | 0 .../places/test_gvd_thinning.cpp | 0 .../places/test_gvd_utilities.cpp | 0 .../reconstruction/test_marching_cubes.cpp | 0 .../test_reconstruction_module.cpp | 0 .../resources/config/map_config.yaml | 0 .../resources/config/missing_config.yaml | 0 .../resources/config/nested_test_config.yaml | 0 .../resources/config/test_config.yaml | 0 .../resources/loop_closure/make_lcd_models.py | 0 .../resources/loop_closure/objects.onnx | Bin .../resources/loop_closure/objects_pos.onnx | Bin .../resources/loop_closure/places.onnx | Bin .../resources/loop_closure/places_pos.onnx | Bin .../loop_closure/test_embeddings.yaml | 0 .../resources/test_semantic_map.csv | 0 .../rooms/test_graph_clustering.cpp | 0 .../rooms/test_graph_filtration.cpp | 0 .../rooms/test_room_finder.cpp | 0 .../rooms/test_room_finder_config.cpp | 0 .../rooms/test_room_utilities.cpp | 0 {hydra/tests => tests}/src/place_fixtures.cpp | 0 {hydra/tests => tests}/src/resources.cpp | 0 .../utils/test_minimum_spanning_tree.cpp | 0 .../utils/test_nearest_neighbor_utilities.cpp | 0 .../utils/test_timing_utilities.cpp | 0 396 files changed, 39 insertions(+), 15974 deletions(-) rename hydra/CMakeLists.txt => CMakeLists.txt (100%) rename {hydra/cmake => cmake}/nanoflann.CMakeLists.txt.in (100%) rename {hydra/config => config}/ade20k_mp3d_typology.yaml (100%) rename {hydra/config => config}/ade20k_typology.yaml (100%) rename {hydra/config => config}/d455/d455_typology.yaml (100%) rename {hydra/config => config}/d455/dsg_backend_config.yaml (100%) rename {hydra/config => config}/d455/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/d455/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/default_reconstruction_config.yaml (100%) rename {hydra/config => config}/default_robot/dsg_backend_config.yaml (100%) rename {hydra/config => config}/default_robot/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/default_robot/dsg_gnn_lcd_config.yaml (100%) rename {hydra/config => config}/default_robot/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/extrinsics/mp3d.yaml (100%) rename {hydra/config => config}/extrinsics/sidpac.yaml (100%) rename {hydra/config => config}/extrinsics/sparkal1.yaml (100%) rename {hydra/config => config}/extrinsics/sparkal2.yaml (100%) rename {hydra/config => config}/extrinsics/uhumans2.yaml (100%) rename {hydra/config => config}/habitat/dsg_backend_config.yaml (100%) rename {hydra/config => config}/habitat/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/habitat/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/habitat/mp3d_typology.yaml (100%) rename {hydra/config => config}/habitat/reconstruction_config.yaml (100%) rename {hydra/config => config}/sidpac/dsg_backend_config.yaml (100%) rename {hydra/config => config}/sidpac/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/sidpac/dsg_gnn_lcd_config.yaml (100%) rename {hydra/config => config}/sidpac/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/sidpac/reconstruction_config.yaml (100%) rename {hydra/config => config}/sidpac/sidpac_typology.yaml (100%) rename {hydra/config => config}/simmons_a1/dsg_backend_config.yaml (100%) rename {hydra/config => config}/simmons_a1/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/simmons_a1/dsg_gnn_lcd_config.yaml (100%) rename {hydra/config => config}/simmons_a1/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/simmons_a1/reconstruction_config.yaml (100%) rename {hydra/config => config}/simmons_jackal/dsg_backend_config.yaml (100%) rename {hydra/config => config}/simmons_jackal/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/simmons_jackal/dsg_gnn_lcd_config.yaml (100%) rename {hydra/config => config}/simmons_jackal/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/simmons_jackal/reconstruction_config.yaml (100%) rename {hydra/config => config}/uhumans2/apartment_lcd_embeddings.yaml (100%) rename {hydra/config => config}/uhumans2/dsg_backend_config.yaml (100%) rename {hydra/config => config}/uhumans2/dsg_frontend_config.yaml (100%) rename {hydra/config => config}/uhumans2/dsg_gnn_lcd_config.yaml (100%) rename {hydra/config => config}/uhumans2/dsg_lcd_config.yaml (100%) rename {hydra/config => config}/uhumans2/office_lcd_embeddings.yaml (100%) rename {hydra/config => config}/uhumans2/reconstruction_config.yaml (100%) rename {hydra/config => config}/uhumans2/subway_lcd_embeddings.yaml (100%) rename {hydra/config => config}/uhumans2/uhumans2_apartment_typology.yaml (100%) rename {hydra/config => config}/uhumans2/uhumans2_office_typology.yaml (100%) rename {hydra/config => config}/uhumans2/uhumans2_subway_typology.yaml (100%) delete mode 100644 hydra/.gitignore delete mode 100644 hydra/README.md rename hydra/hydra_build_config.h.in => hydra_build_config.h.in (100%) delete mode 100644 hydra_msgs/CMakeLists.txt delete mode 100644 hydra_msgs/msg/ActiveLayer.msg delete mode 100644 hydra_msgs/msg/ActiveMesh.msg delete mode 100644 hydra_msgs/msg/DsgUpdate.msg delete mode 100644 hydra_msgs/package.xml delete mode 100644 hydra_msgs/srv/GetDsg.srv delete mode 100644 hydra_msgs/srv/QueryFreespace.srv delete mode 100644 hydra_ros/.gitignore delete mode 100644 hydra_ros/CMakeLists.txt delete mode 100644 hydra_ros/README.md delete mode 100755 hydra_ros/bin/csv_to_tf delete mode 100755 hydra_ros/bin/gnn_node delete mode 100755 hydra_ros/bin/odom_to_tf delete mode 100755 hydra_ros/cfg/Colormap.cfg delete mode 100755 hydra_ros/cfg/DynamicLayerVisualizer.cfg delete mode 100755 hydra_ros/cfg/GvdVisualizer.cfg delete mode 100755 hydra_ros/cfg/LayerVisualizer.cfg delete mode 100755 hydra_ros/cfg/Visualizer.cfg delete mode 100644 hydra_ros/config/graph_visualization_config.yaml delete mode 100644 hydra_ros/config/gvd_validator/uh2.yaml delete mode 100644 hydra_ros/config/hydra_visualizer/agent_layer.yaml delete mode 100644 hydra_ros/config/hydra_visualizer/buildings_layer.yaml delete mode 100644 hydra_ros/config/hydra_visualizer/objects_layer.yaml delete mode 100644 hydra_ros/config/hydra_visualizer/places_layer.yaml delete mode 100644 hydra_ros/config/hydra_visualizer/rooms_layer.yaml delete mode 100644 hydra_ros/config/hydra_visualizer/visualizer.yaml delete mode 100644 hydra_ros/config/object_visualizer/agent_layer.yaml delete mode 100644 hydra_ros/config/object_visualizer/buildings_layer.yaml delete mode 100644 hydra_ros/config/object_visualizer/objects_layer.yaml delete mode 100644 hydra_ros/config/object_visualizer/places_layer.yaml delete mode 100644 hydra_ros/config/object_visualizer/rooms_layer.yaml delete mode 100644 hydra_ros/config/object_visualizer/visualizer.yaml delete mode 100644 hydra_ros/config/overrides.yaml delete mode 100644 hydra_ros/config/ros_params.yaml delete mode 100644 hydra_ros/hydra_utils_cmake delete mode 100644 hydra_ros/include/hydra_ros/config/ros_parser.h delete mode 100644 hydra_ros/include/hydra_ros/config/ros_utilities.h delete mode 100644 hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h delete mode 100644 hydra_ros/include/hydra_ros/pipeline/ros_backend.h delete mode 100644 hydra_ros/include/hydra_ros/pipeline/ros_frontend.h delete mode 100644 hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h delete mode 100644 hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h delete mode 100644 hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h delete mode 100644 hydra_ros/include/hydra_ros/utils/node_utilities.h delete mode 100644 hydra_ros/include/hydra_ros/utils/semantic_ros_publishers.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/visualizer_types.h delete mode 100644 hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h delete mode 100644 hydra_ros/launch/datasets/mp3d.launch delete mode 100644 hydra_ros/launch/datasets/sidpac.launch delete mode 100644 hydra_ros/launch/datasets/simmons_a1.launch delete mode 100644 hydra_ros/launch/datasets/simmons_jackal.launch delete mode 100644 hydra_ros/launch/datasets/uhumans2.launch delete mode 100644 hydra_ros/launch/hydra.launch delete mode 100644 hydra_ros/launch/hydra_robot.launch delete mode 100644 hydra_ros/launch/hydra_robot_t265.launch delete mode 100644 hydra_ros/launch/misc/dsg_optimizer.launch delete mode 100644 hydra_ros/launch/places/gvd_validator.launch delete mode 100644 hydra_ros/launch/places/hydra_topology.launch delete mode 100644 hydra_ros/launch/places/hydra_topology_sidpac.launch delete mode 100644 hydra_ros/launch/places/hydra_topology_simmons_jackal.launch delete mode 100644 hydra_ros/launch/places/hydra_topology_uhumans2.launch delete mode 100644 hydra_ros/launch/utils/data_sources/a1_pointcloud.launch delete mode 100644 hydra_ros/launch/utils/data_sources/d455_pointcloud.launch delete mode 100644 hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch delete mode 100644 hydra_ros/launch/utils/data_sources/sidpac_pointcloud.launch delete mode 100644 hydra_ros/launch/utils/data_sources/uhumans2_pointcloud.launch delete mode 100644 hydra_ros/launch/utils/gt_pose/robot_gt_pose.launch delete mode 100644 hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch delete mode 100644 hydra_ros/launch/utils/hydra_streaming_visualizer.launch delete mode 100644 hydra_ros/launch/utils/hydra_visualizer.launch delete mode 100644 hydra_ros/launch/utils/includes/a1_static_tfs.xml delete mode 100644 hydra_ros/launch/utils/includes/a1_t265_camera_static_tfs.xml delete mode 100644 hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml delete mode 100644 hydra_ros/launch/utils/includes/hydra_visualizer_base.xml delete mode 100644 hydra_ros/launch/utils/includes/hydra_visualizer_params.xml delete mode 100644 hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml delete mode 100644 hydra_ros/launch/utils/includes/rgbd_to_pointcloud.xml delete mode 100644 hydra_ros/launch/utils/includes/sparkal1_static_tfs.xml delete mode 100644 hydra_ros/launch/utils/includes/sparkal2_static_tfs.xml delete mode 100644 hydra_ros/launch/utils/includes/uhumans2_static_tfs.xml delete mode 100644 hydra_ros/launch/utils/odom_to_tf.launch delete mode 100644 hydra_ros/launch/utils/publish_rotation_transform.launch delete mode 100644 hydra_ros/launch/utils/scene_graph_logger.launch delete mode 100644 hydra_ros/nodelet_plugins.xml delete mode 100644 hydra_ros/package.xml delete mode 100644 hydra_ros/resources/a1_center_mask_roi_640.png delete mode 100644 hydra_ros/resources/a1_left_mask_roi.png delete mode 100644 hydra_ros/rviz/a1.rviz delete mode 100644 hydra_ros/rviz/a1_topdown.rviz delete mode 100644 hydra_ros/rviz/dsg_optimizer.rviz delete mode 100644 hydra_ros/rviz/gvd_validator.rviz delete mode 100644 hydra_ros/rviz/hydra_streaming_visualizer.rviz delete mode 100644 hydra_ros/rviz/hydra_streaming_visualizer_topdown.rviz delete mode 100644 hydra_ros/rviz/hydra_visualizer.rviz delete mode 100644 hydra_ros/rviz/mp3d.rviz delete mode 100644 hydra_ros/rviz/sidpac.rviz delete mode 100644 hydra_ros/rviz/sidpac_topdown.rviz delete mode 100644 hydra_ros/rviz/sparkal1.rviz delete mode 100644 hydra_ros/rviz/sparkal2.rviz delete mode 100644 hydra_ros/rviz/uhumans2.rviz delete mode 100644 hydra_ros/rviz/uhumans2_topdown.rviz delete mode 100755 hydra_ros/scripts/copy_static_tfs.py delete mode 100644 hydra_ros/scripts/decompress_uhumans2.py delete mode 100644 hydra_ros/scripts/download_uhumans2.py delete mode 100755 hydra_ros/scripts/run_robot.sh delete mode 100755 hydra_ros/scripts/run_robot_t265.sh delete mode 100755 hydra_ros/scripts/scrape_kimera_extrinsics.py delete mode 100644 hydra_ros/src/config/ros_parser.cpp delete mode 100644 hydra_ros/src/nodes/dsg_optimizer_node.cpp delete mode 100644 hydra_ros/src/nodes/gvd_validator_node.cpp delete mode 100644 hydra_ros/src/nodes/hydra_node.cpp delete mode 100644 hydra_ros/src/nodes/hydra_topology_node.cpp delete mode 100644 hydra_ros/src/nodes/hydra_visualizer_node.cpp delete mode 100644 hydra_ros/src/nodes/rotate_tf_node.cpp delete mode 100644 hydra_ros/src/nodes/scene_graph_logger_node.cpp delete mode 100644 hydra_ros/src/pipeline/hydra_ros_pipeline.cpp delete mode 100644 hydra_ros/src/pipeline/ros_backend.cpp delete mode 100644 hydra_ros/src/pipeline/ros_frontend.cpp delete mode 100644 hydra_ros/src/pipeline/ros_lcd_registration.cpp delete mode 100644 hydra_ros/src/pipeline/ros_reconstruction.cpp delete mode 100644 hydra_ros/src/utils/dsg_streaming_interface.cpp delete mode 100644 hydra_ros/src/utils/mask_nodelet.cpp delete mode 100644 hydra_ros/src/utils/node_utilities.cpp delete mode 100644 hydra_ros/src/visualizer/colormap_utilities.cpp delete mode 100644 hydra_ros/src/visualizer/dsg_mesh_plugins.cpp delete mode 100644 hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp delete mode 100644 hydra_ros/src/visualizer/gvd_visualization_utilities.cpp delete mode 100644 hydra_ros/src/visualizer/topology_server_visualizer.cpp delete mode 100644 hydra_ros/src/visualizer/visualizer_plugins.cpp delete mode 100644 hydra_ros/src/visualizer/visualizer_utilities.cpp delete mode 100644 hydra_ros/tests/CMakeLists.txt delete mode 100644 hydra_ros/tests/hydra_ros.test delete mode 100644 hydra_ros/tests/main.cpp delete mode 100644 hydra_ros/tests/resources/map_config.yaml delete mode 100644 hydra_ros/tests/resources/nested_test_config.yaml delete mode 100644 hydra_ros/tests/resources/test_config.yaml delete mode 100644 hydra_ros/tests/test_ros_config.cpp rename {hydra/include => include}/hydra/backend/backend_config.h (100%) rename {hydra/include => include}/hydra/backend/backend_module.h (100%) rename {hydra/include => include}/hydra/backend/merge_handler.h (100%) rename {hydra/include => include}/hydra/backend/update_functions.h (100%) rename {hydra/include => include}/hydra/common/common.h (100%) rename {hydra/include => include}/hydra/common/dsg_types.h (100%) rename {hydra/include => include}/hydra/common/hydra_config.h (100%) rename {hydra/include => include}/hydra/common/input_queue.h (100%) rename {hydra/include => include}/hydra/common/robot_prefix_config.h (100%) rename {hydra/include => include}/hydra/common/shared_module_state.h (100%) rename {hydra/include => include}/hydra/config/config.h (100%) rename {hydra/include => include}/hydra/config/config_formatter.h (100%) rename {hydra/include => include}/hydra/config/config_parser.h (100%) rename {hydra/include => include}/hydra/config/config_traits.h (100%) rename {hydra/include => include}/hydra/config/config_visitor.h (100%) rename {hydra/include => include}/hydra/config/eigen_config_types.h (100%) rename {hydra/include => include}/hydra/config/ostream_formatter.h (100%) rename {hydra/include => include}/hydra/config/yaml_parser.h (100%) rename {hydra/include => include}/hydra/frontend/frontend_config.h (100%) rename {hydra/include => include}/hydra/frontend/frontend_module.h (100%) rename {hydra/include => include}/hydra/frontend/mesh_segmenter.h (100%) rename {hydra/include => include}/hydra/gnn/gnn_interface.h (100%) rename {hydra/include => include}/hydra/gnn/ort_utilities.h (100%) rename {hydra/include => include}/hydra/gnn/tensor.h (100%) rename {hydra/include => include}/hydra/loop_closure/descriptor_matching.h (100%) rename {hydra/include => include}/hydra/loop_closure/detector.h (100%) rename {hydra/include => include}/hydra/loop_closure/gnn_descriptors.h (100%) rename {hydra/include => include}/hydra/loop_closure/loop_closure_config.h (100%) rename {hydra/include => include}/hydra/loop_closure/loop_closure_module.h (100%) rename {hydra/include => include}/hydra/loop_closure/registration.h (100%) rename {hydra/include => include}/hydra/loop_closure/scene_graph_descriptors.h (100%) rename {hydra/include => include}/hydra/loop_closure/subgraph_extraction.h (100%) rename {hydra/include => include}/hydra/places/compression_graph_extractor.h (100%) rename {hydra/include => include}/hydra/places/floodfill_graph_extractor.h (100%) rename {hydra/include => include}/hydra/places/graph_extractor_config.h (100%) rename {hydra/include => include}/hydra/places/graph_extractor_interface.h (100%) rename {hydra/include => include}/hydra/places/graph_extractor_types.h (100%) rename {hydra/include => include}/hydra/places/graph_extractor_utilities.h (100%) rename {hydra/include => include}/hydra/places/gvd_graph.h (100%) rename {hydra/include => include}/hydra/places/gvd_integrator.h (100%) rename {hydra/include => include}/hydra/places/gvd_integrator_config.h (100%) rename {hydra/include => include}/hydra/places/gvd_parent_tracker.h (100%) rename {hydra/include => include}/hydra/places/gvd_thinning.h (100%) rename {hydra/include => include}/hydra/places/gvd_utilities.h (100%) rename {hydra/include => include}/hydra/places/gvd_voxel.h (100%) rename {hydra/include => include}/hydra/places/nearest_voxel_utilities.h (100%) rename {hydra/include => include}/hydra/places/update_statistics.h (100%) rename {hydra/include => include}/hydra/places/vertex_voxel.h (100%) rename {hydra/include => include}/hydra/places/voxblox_types.h (100%) rename {hydra/include => include}/hydra/reconstruction/combo_integrator.h (100%) rename {hydra/include => include}/hydra/reconstruction/configs.h (100%) rename {hydra/include => include}/hydra/reconstruction/reconstruction_config.h (100%) rename {hydra/include => include}/hydra/reconstruction/reconstruction_module.h (100%) rename {hydra/include => include}/hydra/reconstruction/reconstruction_output.h (100%) rename {hydra/include => include}/hydra/reconstruction/voxblox_utilities.h (100%) rename {hydra/include => include}/hydra/reconstruction/voxel_aware_marching_cubes.h (100%) rename {hydra/include => include}/hydra/reconstruction/voxel_aware_mesh_integrator.h (100%) rename {hydra/include => include}/hydra/rooms/graph_clustering.h (100%) rename {hydra/include => include}/hydra/rooms/graph_filtration.h (100%) rename {hydra/include => include}/hydra/rooms/room_finder.h (100%) rename {hydra/include => include}/hydra/rooms/room_finder_config.h (100%) rename {hydra/include => include}/hydra/rooms/room_utilities.h (100%) rename {hydra/include => include}/hydra/utils/disjoint_set.h (100%) rename {hydra/include => include}/hydra/utils/display_utilities.h (100%) rename {hydra/include => include}/hydra/utils/log_utilities.h (100%) rename {hydra/include => include}/hydra/utils/minimum_spanning_tree.h (100%) rename {hydra/include => include}/hydra/utils/nearest_neighbor_utilities.h (100%) rename {hydra/include => include}/hydra/utils/timing_utilities.h (100%) rename {hydra/models => models}/.gitignore (100%) rename {hydra/models => models}/lcd/.gitignore (100%) rename {hydra/models => models}/lcd/object_gnn.onnx (100%) rename {hydra/models => models}/lcd/place_gnn.onnx (100%) rename {hydra/output => output}/.gitignore (100%) rename {hydra/output => output}/default/.gitignore (100%) rename {hydra/output => output}/default/backend/.gitignore (100%) rename {hydra/output => output}/default/frontend/.gitignore (100%) rename {hydra/output => output}/default/lcd/.gitignore (100%) rename {hydra/output => output}/default/pgmo/.gitignore (100%) rename {hydra/output => output}/default/topology/.gitignore (100%) rename {hydra/output => output}/sidpac/.gitignore (100%) rename {hydra/output => output}/sidpac/backend/.gitignore (100%) rename {hydra/output => output}/sidpac/frontend/.gitignore (100%) rename {hydra/output => output}/sidpac/lcd/.gitignore (100%) rename {hydra/output => output}/sidpac/pgmo/.gitignore (100%) rename {hydra/output => output}/sidpac/topology/.gitignore (100%) rename {hydra/output => output}/uhumans2/.gitignore (100%) rename {hydra/output => output}/uhumans2/backend/.gitignore (100%) rename {hydra/output => output}/uhumans2/frontend/.gitignore (100%) rename {hydra/output => output}/uhumans2/lcd/.gitignore (100%) rename {hydra/output => output}/uhumans2/pgmo/.gitignore (100%) rename {hydra/output => output}/uhumans2/topology/.gitignore (100%) rename hydra/package.xml => package.xml (100%) rename {hydra/scripts => scripts}/export_label_embeddings.py (100%) rename {hydra/scripts => scripts}/make_colormap.py (100%) rename {hydra/scripts => scripts}/modify_dgraph.py (100%) rename {hydra/src => src}/CMakeLists.txt (100%) rename {hydra/src => src}/backend/backend_module.cpp (100%) rename {hydra/src => src}/backend/merge_handler.cpp (100%) rename {hydra/src => src}/backend/update_functions.cpp (100%) rename {hydra/src => src}/common/hydra_config.cpp (100%) rename {hydra/src => src}/common/robot_prefix_config.cpp (100%) rename {hydra/src => src}/common/shared_module_state.cpp (100%) rename {hydra/src => src}/config/yaml_parser.cpp (100%) rename {hydra/src => src}/frontend/frontend_module.cpp (100%) rename {hydra/src => src}/frontend/mesh_segmenter.cpp (100%) rename {hydra/src => src}/gnn/CMakeLists.txt (100%) rename {hydra/src => src}/gnn/cmake/ort.CMakeLists.txt.in (100%) rename {hydra/src => src}/gnn/gnn_interface.cpp (100%) rename {hydra/src => src}/gnn/gnn_model_reader.cpp (100%) rename {hydra/src => src}/gnn/ort_utilities.cpp (100%) rename {hydra/src => src}/gnn/tensor.cpp (100%) rename {hydra/src => src}/gnn/tests/resources/make_simple_model.py (100%) rename {hydra/src => src}/gnn/tests/resources/simple_model.onnx (100%) rename {hydra/src => src}/gnn/tests/utest_gnn_interface.cpp (100%) rename {hydra/src => src}/gnn/tests/utest_main.cpp (100%) rename {hydra/src => src}/gnn/tests/utest_tensor.cpp (100%) rename {hydra/src => src}/loop_closure/descriptor_matching.cpp (100%) rename {hydra/src => src}/loop_closure/detector.cpp (100%) rename {hydra/src => src}/loop_closure/gnn_descriptors.cpp (100%) rename {hydra/src => src}/loop_closure/loop_closure_module.cpp (100%) rename {hydra/src => src}/loop_closure/registration.cpp (100%) rename {hydra/src => src}/loop_closure/scene_graph_descriptors.cpp (100%) rename {hydra/src => src}/loop_closure/subgraph_extraction.cpp (100%) rename {hydra/src => src}/places/compression_graph_extractor.cpp (100%) rename {hydra/src => src}/places/floodfill_graph_extractor.cpp (100%) rename {hydra/src => src}/places/graph_extractor_interface.cpp (100%) rename {hydra/src => src}/places/graph_extractor_types.cpp (100%) rename {hydra/src => src}/places/graph_extractor_utilities.cpp (100%) rename {hydra/src => src}/places/gvd_graph.cpp (100%) rename {hydra/src => src}/places/gvd_integrator.cpp (100%) rename {hydra/src => src}/places/gvd_parent_tracker.cpp (100%) rename {hydra/src => src}/places/gvd_thinning.cpp (100%) rename {hydra/src => src}/places/gvd_utilities.cpp (100%) rename {hydra/src => src}/places/gvd_voxel.cpp (100%) rename {hydra/src => src}/places/nearest_voxel_utilities.cpp (100%) rename {hydra/src => src}/places/update_statistics.cpp (100%) rename {hydra/src => src}/reconstruction/reconstruction_config.cpp (100%) rename {hydra/src => src}/reconstruction/reconstruction_module.cpp (100%) rename {hydra/src => src}/reconstruction/voxel_aware_marching_cubes.cpp (100%) rename {hydra/src => src}/reconstruction/voxel_aware_mesh_integrator.cpp (100%) rename {hydra/src => src}/rooms/graph_clustering.cpp (100%) rename {hydra/src => src}/rooms/graph_filtration.cpp (100%) rename {hydra/src => src}/rooms/room_finder.cpp (100%) rename {hydra/src => src}/rooms/room_utilities.cpp (100%) rename {hydra/src => src}/utils/disjoint_set.cpp (100%) rename {hydra/src => src}/utils/display_utilities.cpp (100%) rename {hydra/src => src}/utils/log_utilities.cpp (100%) rename {hydra/src => src}/utils/minimum_spanning_tree.cpp (100%) rename {hydra/src => src}/utils/nearest_neighbor_utilities.cpp (100%) rename {hydra/src => src}/utils/timing_utilities.cpp (100%) rename {hydra/tests => tests}/CMakeLists.txt (100%) rename {hydra/tests => tests}/backend/test_merge_handler.cpp (100%) rename {hydra/tests => tests}/backend/test_update_functions.cpp (100%) rename {hydra/tests => tests}/config/test_config.cpp (100%) rename {hydra/tests => tests}/frontend/test_frontend.cpp (100%) rename {hydra/tests => tests}/include/hydra_test/gvd_layer_utils.h (100%) rename {hydra/tests => tests}/include/hydra_test/place_fixtures.h (100%) rename {hydra/tests => tests}/include/hydra_test/resources.h (100%) rename {hydra/tests => tests}/loop_closure/test_descriptor_matching.cpp (100%) rename {hydra/tests => tests}/loop_closure/test_detector.cpp (100%) rename {hydra/tests => tests}/loop_closure/test_gnn_descriptors.cpp (100%) rename {hydra/tests => tests}/loop_closure/test_registration.cpp (100%) rename {hydra/tests => tests}/loop_closure/test_scene_graph_descriptors.cpp (100%) rename {hydra/tests => tests}/loop_closure/test_subgraph_extraction.cpp (100%) rename {hydra/tests => tests}/main.cpp (100%) rename {hydra/tests => tests}/places/test_compression_graph_extractor.cpp (100%) rename {hydra/tests => tests}/places/test_esdf_comparison.cpp (100%) rename {hydra/tests => tests}/places/test_floodfill_graph_extractor.cpp (100%) rename {hydra/tests => tests}/places/test_graph_extractor_utilities.cpp (100%) rename {hydra/tests => tests}/places/test_gvd_incremental.cpp (100%) rename {hydra/tests => tests}/places/test_gvd_integrator.cpp (100%) rename {hydra/tests => tests}/places/test_gvd_thinning.cpp (100%) rename {hydra/tests => tests}/places/test_gvd_utilities.cpp (100%) rename {hydra/tests => tests}/reconstruction/test_marching_cubes.cpp (100%) rename {hydra/tests => tests}/reconstruction/test_reconstruction_module.cpp (100%) rename {hydra/tests => tests}/resources/config/map_config.yaml (100%) rename {hydra/tests => tests}/resources/config/missing_config.yaml (100%) rename {hydra/tests => tests}/resources/config/nested_test_config.yaml (100%) rename {hydra/tests => tests}/resources/config/test_config.yaml (100%) rename {hydra/tests => tests}/resources/loop_closure/make_lcd_models.py (100%) rename {hydra/tests => tests}/resources/loop_closure/objects.onnx (100%) rename {hydra/tests => tests}/resources/loop_closure/objects_pos.onnx (100%) rename {hydra/tests => tests}/resources/loop_closure/places.onnx (100%) rename {hydra/tests => tests}/resources/loop_closure/places_pos.onnx (100%) rename {hydra/tests => tests}/resources/loop_closure/test_embeddings.yaml (100%) rename {hydra/tests => tests}/resources/test_semantic_map.csv (100%) rename {hydra/tests => tests}/rooms/test_graph_clustering.cpp (100%) rename {hydra/tests => tests}/rooms/test_graph_filtration.cpp (100%) rename {hydra/tests => tests}/rooms/test_room_finder.cpp (100%) rename {hydra/tests => tests}/rooms/test_room_finder_config.cpp (100%) rename {hydra/tests => tests}/rooms/test_room_utilities.cpp (100%) rename {hydra/tests => tests}/src/place_fixtures.cpp (100%) rename {hydra/tests => tests}/src/resources.cpp (100%) rename {hydra/tests => tests}/utils/test_minimum_spanning_tree.cpp (100%) rename {hydra/tests => tests}/utils/test_nearest_neighbor_utilities.cpp (100%) rename {hydra/tests => tests}/utils/test_timing_utilities.cpp (100%) diff --git a/hydra/CMakeLists.txt b/CMakeLists.txt similarity index 100% rename from hydra/CMakeLists.txt rename to CMakeLists.txt diff --git a/README.md b/README.md index 9a0ebf1b..8c770c66 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,11 @@ -This repository contains code to incrementally build 3D Dynamic Scene Graphs (DSGs) in real-time and is primarily based on the paper ["Hydra: A Real-time Spatial Perception System for 3D Scene Graph Construction and Optimization"](http://www.roboticsproceedings.org/rss18/p050.pdf). If you find this code relevant for your work, please consider citing this paper. A bibtex entry is provided below: +This repository contains code to incrementally build 3D Dynamic Scene Graphs (DSGs) in real-time and is based on the papers: + - ["Hydra: A Real-time Spatial Perception System for 3D Scene Graph Construction and Optimization"](http://www.roboticsproceedings.org/rss18/p050.pdf) + - ["Foundations of Spatial Perception for Robotics: Hierarchical Representations and Real-time Systems"](https://arxiv.org/abs/2305.07154) + +If you find this code relevant for your work, please consider citing one or both of these papers. A bibtex entry is provided below: ``` @article{hughes2022hydra, @@ -15,15 +19,7 @@ This repository contains code to incrementally build 3D Dynamic Scene Graphs (DS pdf={http://www.roboticsproceedings.org/rss18/p050.pdf}, year={2022}, } -``` -**Update (05/20/23):** We recently authored the following paper ["Foundations of Spatial Perception for Robotics: Hierarchical Representations and Real-time Systems"](https://arxiv.org/abs/2305.07154). -Associated updates to Hydra from this paper will be released soon (*current target 06/09/23*). -We also plan to release additional code, most notably for training the room classification networks and GNN-based descriptors as described in the above paper. -This will likely take slightly longer (and is likely to be in a different repository, which will be linked to). -In the meantime, if you find our new paper relevant for your work, please consider also citing this paper. -A bibtex entry is provided below: -``` @article{hughes2023foundations, title={Foundations of Spatial Perception for Robotics: Hierarchical Representations and Real-time Systems}, author={Nathan Hughes and Yun Chang and Siyi Hu and Rajat Talak and Rumaisa Abdulhai and Jared Strader and Luca Carlone}, @@ -34,11 +30,23 @@ A bibtex entry is provided below: } ``` -### Acknowledgements and Disclaimer +#### Acknowledgements -**Acknowledgements:** This work was partially funded by the AIA CRA FA8750-19-2-1000, ARL DCIST CRA W911NF-17-2-0181, and ONR RAIDER N00014-18-1-2828. +This work was partially funded by the AIA CRA FA8750-19-2-1000, ARL DCIST CRA W911NF-17-2-0181, and ONR RAIDER N00014-18-1-2828. -**Disclaimer:** Research was sponsored by the United States Air Force Research Laboratory and the United States Air Force Artificial Intelligence Accelerator and was accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views and conclusions contained in this document are those of the authors and should not be interpreted as representing the official policies, either expressed or implied, of the United States Air Force or the U.S. Government. The U.S. Government is authorized to reproduce and distribute reprints for Government purposes notwithstanding any copyright notation herein. +#### Disclaimer + +Research was sponsored by the United States Air Force Research Laboratory and the United States Air Force Artificial Intelligence Accelerator and was accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views and conclusions contained in this document are those of the authors and should not be interpreted as representing the official policies, either expressed or implied, of the United States Air Force or the U.S. Government. The U.S. Government is authorized to reproduce and distribute reprints for Government purposes notwithstanding any copyright notation herein. + +## News + +**Update (06/26/23):** We've released initial changes from the our newest paper. +We also plan to release additional code, most notably for training the room classification networks and GNN-based descriptors as described in the above paper. +We will link to the new repository once this is done. + +:warning: As part of the this release, we have moved ROS related code to a new repository located [here](https://github.com/MIT-SPARK/Hydra-ROS). This code (and our installation process) still do rely on the ROS ecosystem. + +## Installation and Running ### General Requirements @@ -50,8 +58,6 @@ You can follow the instructions [here](http://wiki.ros.org/ROS/Installation) to Then, make sure you have some general requirements: ``` sudo apt install python3-rosdep python3-catkin-tools python3-vcstool - -# for melodic: sudo apt install python-rosdep python-catkin-tools python3-vcstool ``` Finally, if you haven't set up rosdep yet: @@ -60,6 +66,12 @@ sudo rosdep init rosdep update ``` +### Filing Issues + +:warning: We don't support other platforms. Issues requesting support on other platforms (e.g. Ubuntu 16.04, Windows) will be summarily closed. + +Depending on the nature of the issue, it may be helpful to browse [this](doc/debugging.md) page about debugging Hydra first. + ### Building Hydra To get started: @@ -88,16 +100,8 @@ catkin build Please help us by creating new issues when you run into problems with these instructions! -Finally, if you'd prefer to use the older `wstool` instead of `vcs`, you can do the following instead of `vcs import`: -``` -wstool init -wstool merge hydra/install/hydra.rosinstall -wstool up -``` - -### Running Hydra (Quickstart) +### Quickstart -The only dataset that is supported out-of-the-box is [uHumans2](http://web.mit.edu/sparklab/datasets/uHumans2/). To test Hydra out, you can just download a single scene (the office scene without humans is recommended, and can be found [here](https://drive.google.com/uc?id=1CA_1Awu-bewJKpDrILzWok_H_6cOkGDb). Make sure to decompress the rosbag (`rosbag decompress path/to/bagfile`) before running! @@ -105,7 +109,7 @@ Make sure to decompress the rosbag (`rosbag decompress path/to/bagfile`) before To start Hydra: ``` -roslaunch hydra_dsg_builder_ros uhumans2.launch start_visualizer:=true +roslaunch hydra_ros uhumans2.launch ``` Then, start the rosbag in a separate terminal: @@ -113,75 +117,10 @@ Then, start the rosbag in a separate terminal: rosbag play path/to/rosbag --clock ``` -### Using Kimera-VIO - -:warning: Kimera-VIO functionality is in a pre-release state. Support is limited (and functionality may be brittle). +### Running Hydra -You can configure your workspace to also include Kimera-VIO by: -``` -roscd && cd ../src -vcs import . < hydra/install/vio_public_overlay.rosinstall - -catkin build -``` - -#### Running Using VIO Only - -First, start Kimera: - -``` -roslaunch kimera_vio_ros kimera_vio_ros kimera_vio_ros_uhumans2.launch online:=true viz_type:=1 use_lcd:=false -``` - -and in a separate terminal, run: - -``` -# in separate terminal -roslaunch hydra_dsg_builder_ros uhumans2.launch \ - start_visualizer:=true \ - use_gt_frame:=false -``` - -#### Running Using VIO and External Visual Loop Closures - -First, start Kimera: - -``` -roslaunch kimera_vio_ros kimera_vio_ros kimera_vio_ros_uhumans2.launch online:=true viz_type:=1 \ - use_lcd:=true \ - lcd_no_optimize:=true -``` - -and in a separate terminal, run the same command for Hydra: - -``` -roslaunch hydra_dsg_builder uhumans2.launch \ - start_visualizer:=true \ - use_gt_frame:=false -``` - -:warning: To achieve the best results with Kimera-VIO, you should wait for the LCD vocabulary to finish loading before starting the rosbag. - -#### Running Using VIO and DSG Loop Closures - -First, start Kimera: - -``` -roslaunch kimera_vio_ros kimera_vio_ros kimera_vio_ros_uhumans2.launch online:=true viz_type:=1 \ - use_lcd:=true \ - lcd_no_detection:=true -``` - -and in a separate terminal, run the same command for Hydra: - -``` -roslaunch hydra_dsg_builder_ros uhumans2.launch \ - start_visualizer:=true \ - use_gt_frame:=false \ - enable_dsg_lcd:=true -``` - -:warning: To achieve the best results with Kimera-VIO, you should wait for the LCD vocabulary to finish loading before starting the rosbag. +See [here](https://github.com/MIT-SPARK/Hydra-ROS/blob/main/doc/quickstart.md) for detailed instructions discussing how to run Hydra using ROS. +These also detail how to use Hydra with [Kimera-VIO](https://github.com/MIT-SPARK/Kimera-VIO.git), including how to build Kimera-VIO alongside Hydra. ### Using a Semantic Segmentation Network @@ -206,8 +145,4 @@ catkin build Hydra has a wrapper around config parsing that is documented [here](doc/config_parsing.md). -### Filing Issues -:warning: We don't support other platforms. Issues requesting support on other platforms (e.g. Ubuntu 16.04, Windows) will be summarily closed. - -Depending on the nature of the issue, it may be helpful to browse [this](doc/debugging.md) page about debugging Hydra first. diff --git a/hydra/cmake/nanoflann.CMakeLists.txt.in b/cmake/nanoflann.CMakeLists.txt.in similarity index 100% rename from hydra/cmake/nanoflann.CMakeLists.txt.in rename to cmake/nanoflann.CMakeLists.txt.in diff --git a/hydra/config/ade20k_mp3d_typology.yaml b/config/ade20k_mp3d_typology.yaml similarity index 100% rename from hydra/config/ade20k_mp3d_typology.yaml rename to config/ade20k_mp3d_typology.yaml diff --git a/hydra/config/ade20k_typology.yaml b/config/ade20k_typology.yaml similarity index 100% rename from hydra/config/ade20k_typology.yaml rename to config/ade20k_typology.yaml diff --git a/hydra/config/d455/d455_typology.yaml b/config/d455/d455_typology.yaml similarity index 100% rename from hydra/config/d455/d455_typology.yaml rename to config/d455/d455_typology.yaml diff --git a/hydra/config/d455/dsg_backend_config.yaml b/config/d455/dsg_backend_config.yaml similarity index 100% rename from hydra/config/d455/dsg_backend_config.yaml rename to config/d455/dsg_backend_config.yaml diff --git a/hydra/config/d455/dsg_frontend_config.yaml b/config/d455/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/d455/dsg_frontend_config.yaml rename to config/d455/dsg_frontend_config.yaml diff --git a/hydra/config/d455/dsg_lcd_config.yaml b/config/d455/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/d455/dsg_lcd_config.yaml rename to config/d455/dsg_lcd_config.yaml diff --git a/hydra/config/default_reconstruction_config.yaml b/config/default_reconstruction_config.yaml similarity index 100% rename from hydra/config/default_reconstruction_config.yaml rename to config/default_reconstruction_config.yaml diff --git a/hydra/config/default_robot/dsg_backend_config.yaml b/config/default_robot/dsg_backend_config.yaml similarity index 100% rename from hydra/config/default_robot/dsg_backend_config.yaml rename to config/default_robot/dsg_backend_config.yaml diff --git a/hydra/config/default_robot/dsg_frontend_config.yaml b/config/default_robot/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/default_robot/dsg_frontend_config.yaml rename to config/default_robot/dsg_frontend_config.yaml diff --git a/hydra/config/default_robot/dsg_gnn_lcd_config.yaml b/config/default_robot/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra/config/default_robot/dsg_gnn_lcd_config.yaml rename to config/default_robot/dsg_gnn_lcd_config.yaml diff --git a/hydra/config/default_robot/dsg_lcd_config.yaml b/config/default_robot/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/default_robot/dsg_lcd_config.yaml rename to config/default_robot/dsg_lcd_config.yaml diff --git a/hydra/config/extrinsics/mp3d.yaml b/config/extrinsics/mp3d.yaml similarity index 100% rename from hydra/config/extrinsics/mp3d.yaml rename to config/extrinsics/mp3d.yaml diff --git a/hydra/config/extrinsics/sidpac.yaml b/config/extrinsics/sidpac.yaml similarity index 100% rename from hydra/config/extrinsics/sidpac.yaml rename to config/extrinsics/sidpac.yaml diff --git a/hydra/config/extrinsics/sparkal1.yaml b/config/extrinsics/sparkal1.yaml similarity index 100% rename from hydra/config/extrinsics/sparkal1.yaml rename to config/extrinsics/sparkal1.yaml diff --git a/hydra/config/extrinsics/sparkal2.yaml b/config/extrinsics/sparkal2.yaml similarity index 100% rename from hydra/config/extrinsics/sparkal2.yaml rename to config/extrinsics/sparkal2.yaml diff --git a/hydra/config/extrinsics/uhumans2.yaml b/config/extrinsics/uhumans2.yaml similarity index 100% rename from hydra/config/extrinsics/uhumans2.yaml rename to config/extrinsics/uhumans2.yaml diff --git a/hydra/config/habitat/dsg_backend_config.yaml b/config/habitat/dsg_backend_config.yaml similarity index 100% rename from hydra/config/habitat/dsg_backend_config.yaml rename to config/habitat/dsg_backend_config.yaml diff --git a/hydra/config/habitat/dsg_frontend_config.yaml b/config/habitat/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/habitat/dsg_frontend_config.yaml rename to config/habitat/dsg_frontend_config.yaml diff --git a/hydra/config/habitat/dsg_lcd_config.yaml b/config/habitat/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/habitat/dsg_lcd_config.yaml rename to config/habitat/dsg_lcd_config.yaml diff --git a/hydra/config/habitat/mp3d_typology.yaml b/config/habitat/mp3d_typology.yaml similarity index 100% rename from hydra/config/habitat/mp3d_typology.yaml rename to config/habitat/mp3d_typology.yaml diff --git a/hydra/config/habitat/reconstruction_config.yaml b/config/habitat/reconstruction_config.yaml similarity index 100% rename from hydra/config/habitat/reconstruction_config.yaml rename to config/habitat/reconstruction_config.yaml diff --git a/hydra/config/sidpac/dsg_backend_config.yaml b/config/sidpac/dsg_backend_config.yaml similarity index 100% rename from hydra/config/sidpac/dsg_backend_config.yaml rename to config/sidpac/dsg_backend_config.yaml diff --git a/hydra/config/sidpac/dsg_frontend_config.yaml b/config/sidpac/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/sidpac/dsg_frontend_config.yaml rename to config/sidpac/dsg_frontend_config.yaml diff --git a/hydra/config/sidpac/dsg_gnn_lcd_config.yaml b/config/sidpac/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra/config/sidpac/dsg_gnn_lcd_config.yaml rename to config/sidpac/dsg_gnn_lcd_config.yaml diff --git a/hydra/config/sidpac/dsg_lcd_config.yaml b/config/sidpac/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/sidpac/dsg_lcd_config.yaml rename to config/sidpac/dsg_lcd_config.yaml diff --git a/hydra/config/sidpac/reconstruction_config.yaml b/config/sidpac/reconstruction_config.yaml similarity index 100% rename from hydra/config/sidpac/reconstruction_config.yaml rename to config/sidpac/reconstruction_config.yaml diff --git a/hydra/config/sidpac/sidpac_typology.yaml b/config/sidpac/sidpac_typology.yaml similarity index 100% rename from hydra/config/sidpac/sidpac_typology.yaml rename to config/sidpac/sidpac_typology.yaml diff --git a/hydra/config/simmons_a1/dsg_backend_config.yaml b/config/simmons_a1/dsg_backend_config.yaml similarity index 100% rename from hydra/config/simmons_a1/dsg_backend_config.yaml rename to config/simmons_a1/dsg_backend_config.yaml diff --git a/hydra/config/simmons_a1/dsg_frontend_config.yaml b/config/simmons_a1/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/simmons_a1/dsg_frontend_config.yaml rename to config/simmons_a1/dsg_frontend_config.yaml diff --git a/hydra/config/simmons_a1/dsg_gnn_lcd_config.yaml b/config/simmons_a1/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra/config/simmons_a1/dsg_gnn_lcd_config.yaml rename to config/simmons_a1/dsg_gnn_lcd_config.yaml diff --git a/hydra/config/simmons_a1/dsg_lcd_config.yaml b/config/simmons_a1/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/simmons_a1/dsg_lcd_config.yaml rename to config/simmons_a1/dsg_lcd_config.yaml diff --git a/hydra/config/simmons_a1/reconstruction_config.yaml b/config/simmons_a1/reconstruction_config.yaml similarity index 100% rename from hydra/config/simmons_a1/reconstruction_config.yaml rename to config/simmons_a1/reconstruction_config.yaml diff --git a/hydra/config/simmons_jackal/dsg_backend_config.yaml b/config/simmons_jackal/dsg_backend_config.yaml similarity index 100% rename from hydra/config/simmons_jackal/dsg_backend_config.yaml rename to config/simmons_jackal/dsg_backend_config.yaml diff --git a/hydra/config/simmons_jackal/dsg_frontend_config.yaml b/config/simmons_jackal/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/simmons_jackal/dsg_frontend_config.yaml rename to config/simmons_jackal/dsg_frontend_config.yaml diff --git a/hydra/config/simmons_jackal/dsg_gnn_lcd_config.yaml b/config/simmons_jackal/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra/config/simmons_jackal/dsg_gnn_lcd_config.yaml rename to config/simmons_jackal/dsg_gnn_lcd_config.yaml diff --git a/hydra/config/simmons_jackal/dsg_lcd_config.yaml b/config/simmons_jackal/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/simmons_jackal/dsg_lcd_config.yaml rename to config/simmons_jackal/dsg_lcd_config.yaml diff --git a/hydra/config/simmons_jackal/reconstruction_config.yaml b/config/simmons_jackal/reconstruction_config.yaml similarity index 100% rename from hydra/config/simmons_jackal/reconstruction_config.yaml rename to config/simmons_jackal/reconstruction_config.yaml diff --git a/hydra/config/uhumans2/apartment_lcd_embeddings.yaml b/config/uhumans2/apartment_lcd_embeddings.yaml similarity index 100% rename from hydra/config/uhumans2/apartment_lcd_embeddings.yaml rename to config/uhumans2/apartment_lcd_embeddings.yaml diff --git a/hydra/config/uhumans2/dsg_backend_config.yaml b/config/uhumans2/dsg_backend_config.yaml similarity index 100% rename from hydra/config/uhumans2/dsg_backend_config.yaml rename to config/uhumans2/dsg_backend_config.yaml diff --git a/hydra/config/uhumans2/dsg_frontend_config.yaml b/config/uhumans2/dsg_frontend_config.yaml similarity index 100% rename from hydra/config/uhumans2/dsg_frontend_config.yaml rename to config/uhumans2/dsg_frontend_config.yaml diff --git a/hydra/config/uhumans2/dsg_gnn_lcd_config.yaml b/config/uhumans2/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra/config/uhumans2/dsg_gnn_lcd_config.yaml rename to config/uhumans2/dsg_gnn_lcd_config.yaml diff --git a/hydra/config/uhumans2/dsg_lcd_config.yaml b/config/uhumans2/dsg_lcd_config.yaml similarity index 100% rename from hydra/config/uhumans2/dsg_lcd_config.yaml rename to config/uhumans2/dsg_lcd_config.yaml diff --git a/hydra/config/uhumans2/office_lcd_embeddings.yaml b/config/uhumans2/office_lcd_embeddings.yaml similarity index 100% rename from hydra/config/uhumans2/office_lcd_embeddings.yaml rename to config/uhumans2/office_lcd_embeddings.yaml diff --git a/hydra/config/uhumans2/reconstruction_config.yaml b/config/uhumans2/reconstruction_config.yaml similarity index 100% rename from hydra/config/uhumans2/reconstruction_config.yaml rename to config/uhumans2/reconstruction_config.yaml diff --git a/hydra/config/uhumans2/subway_lcd_embeddings.yaml b/config/uhumans2/subway_lcd_embeddings.yaml similarity index 100% rename from hydra/config/uhumans2/subway_lcd_embeddings.yaml rename to config/uhumans2/subway_lcd_embeddings.yaml diff --git a/hydra/config/uhumans2/uhumans2_apartment_typology.yaml b/config/uhumans2/uhumans2_apartment_typology.yaml similarity index 100% rename from hydra/config/uhumans2/uhumans2_apartment_typology.yaml rename to config/uhumans2/uhumans2_apartment_typology.yaml diff --git a/hydra/config/uhumans2/uhumans2_office_typology.yaml b/config/uhumans2/uhumans2_office_typology.yaml similarity index 100% rename from hydra/config/uhumans2/uhumans2_office_typology.yaml rename to config/uhumans2/uhumans2_office_typology.yaml diff --git a/hydra/config/uhumans2/uhumans2_subway_typology.yaml b/config/uhumans2/uhumans2_subway_typology.yaml similarity index 100% rename from hydra/config/uhumans2/uhumans2_subway_typology.yaml rename to config/uhumans2/uhumans2_subway_typology.yaml diff --git a/hydra/.gitignore b/hydra/.gitignore deleted file mode 100644 index 3a4c6352..00000000 --- a/hydra/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -*.swp -*.idea -*.ccls* -*.vscode/ -*.ipynb_checkpoints/ -*.ipynb diff --git a/hydra/README.md b/hydra/README.md deleted file mode 100644 index 8aff64b4..00000000 --- a/hydra/README.md +++ /dev/null @@ -1,32 +0,0 @@ -# Kimera DSG-Builder - -## Running online - -Steps to running with ground truth for the uhumans2 office dataset: - - 1. Grab the uhumans2 dataset (if you haven't already). You can probably just use the office scene without any humans (`uHumans2_office_s1_00h.bag`) - - 2. Start up the DSG builder - - roslaunch hydra_dsg_builder uhumans2_incremental_dsg.launch start_visualizer:=true - - 3. Start the bag: - - cd /path/to/uhumans2/office/scene - rosbag play uHumans2_office_s1_00h.bag --clock --pause -r 0.5 - -You way want to try the following arguments for the `uhumans2_incremental_dsg` launch file: - - - `use_gt_frame:=false` switches the code to listen for the TF from Kimera-VIO - - `min_glog_level:=0` will enable helpful development output. - - `verbosity:=3` will enable a lot more development output - - `debug:=true` will run the dsg builder in gdb (hopefully not necessary) - -You can find parameters for the front-end [here](config/dsg_frontend_config.yaml) -and for the back-end [here](config/dsg_backend_config.yaml) - -Important parameters: - - - `pgmo/*`: all pgmo settings for the `MeshFrontend` and `KimeraPgmoInterface` (frontend and backend configs respectively) - - `dsg/add_places_to_deformation_graph`: controls place-based factors - - `dsg/optimize_on_lc`: allows disabling optimization diff --git a/hydra/hydra_build_config.h.in b/hydra_build_config.h.in similarity index 100% rename from hydra/hydra_build_config.h.in rename to hydra_build_config.h.in diff --git a/hydra_msgs/CMakeLists.txt b/hydra_msgs/CMakeLists.txt deleted file mode 100644 index 5117ef53..00000000 --- a/hydra_msgs/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -project(hydra_msgs) - -find_package(catkin REQUIRED COMPONENTS std_msgs message_generation voxblox_msgs) - -add_message_files( - FILES - ActiveLayer.msg - ActiveMesh.msg - DsgUpdate.msg -) - -add_service_files(FILES GetDsg.srv QueryFreespace.srv) - -generate_messages(DEPENDENCIES std_msgs voxblox_msgs) - -catkin_package( - CATKIN_DEPENDS - std_msgs - voxblox_msgs - message_runtime - DEPENDS - INCLUDE_DIRS - LIBRARIES -) diff --git a/hydra_msgs/msg/ActiveLayer.msg b/hydra_msgs/msg/ActiveLayer.msg deleted file mode 100644 index 905ad7a5..00000000 --- a/hydra_msgs/msg/ActiveLayer.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -string layer_contents # serialized nodes that are active -int64[] deleted_nodes # nodes that were previously active and removed (rather than archived) -int64[] deleted_edges # edges that were removed diff --git a/hydra_msgs/msg/ActiveMesh.msg b/hydra_msgs/msg/ActiveMesh.msg deleted file mode 100644 index 6c87d366..00000000 --- a/hydra_msgs/msg/ActiveMesh.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -voxblox_msgs/Mesh mesh -voxblox_msgs/Mesh archived_blocks diff --git a/hydra_msgs/msg/DsgUpdate.msg b/hydra_msgs/msg/DsgUpdate.msg deleted file mode 100644 index 599ccad1..00000000 --- a/hydra_msgs/msg/DsgUpdate.msg +++ /dev/null @@ -1,6 +0,0 @@ -Header header -uint8[] layer_contents # serialized nodes that are active -uint64[] deleted_nodes # node ids that were deleted -uint64[] deleted_edges # node ids for edges that were deleted -bool full_update # whether or not the message contains the entire scene graph -int64 sequence_number # update index diff --git a/hydra_msgs/package.xml b/hydra_msgs/package.xml deleted file mode 100644 index 81970cbd..00000000 --- a/hydra_msgs/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - hydra_msgs - 0.0.1 - Messages used for hydra - - Nathan Hughes - Nathan Hughes - - BSD - - catkin - message_generation - message_runtime - std_msgs - voxblox_msgs - - - - - diff --git a/hydra_msgs/srv/GetDsg.srv b/hydra_msgs/srv/GetDsg.srv deleted file mode 100644 index 46abc993..00000000 --- a/hydra_msgs/srv/GetDsg.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -hydra_msgs/DsgUpdate graph diff --git a/hydra_msgs/srv/QueryFreespace.srv b/hydra_msgs/srv/QueryFreespace.srv deleted file mode 100644 index 076f79e4..00000000 --- a/hydra_msgs/srv/QueryFreespace.srv +++ /dev/null @@ -1,6 +0,0 @@ -float64[] x -float64[] y -float64[] z -float64 freespace_distance_m ---- -int8[] in_freespace diff --git a/hydra_ros/.gitignore b/hydra_ros/.gitignore deleted file mode 100644 index 3a4c6352..00000000 --- a/hydra_ros/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -*.swp -*.idea -*.ccls* -*.vscode/ -*.ipynb_checkpoints/ -*.ipynb diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt deleted file mode 100644 index 39502f8f..00000000 --- a/hydra_ros/CMakeLists.txt +++ /dev/null @@ -1,127 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -project(hydra_ros) - -add_compile_options(-Wall -Wextra) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -find_package(Boost REQUIRED COMPONENTS timer) -# find_package(Boost REQUIRED COMPONENTS filesystem) -find_package(spark_dsg REQUIRED) -find_package(GTSAM REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(yaml-cpp REQUIRED) -# TODO(nathan) find hydra independently -find_package( - catkin REQUIRED - COMPONENTS roscpp - dynamic_reconfigure - std_msgs - geometry_msgs - visualization_msgs - hydra_msgs - pcl_ros - tf2_eigen - tf2_ros - kimera_pgmo - voxblox_ros - pose_graph_tools - kimera_semantics_ros - hydra -) - -generate_dynamic_reconfigure_options( - cfg/Colormap.cfg - cfg/DynamicLayerVisualizer.cfg - cfg/GvdVisualizer.cfg - cfg/LayerVisualizer.cfg - cfg/Visualizer.cfg -) - -catkin_package( - CATKIN_DEPENDS - roscpp - dynamic_reconfigure - std_msgs - geometry_msgs - visualization_msgs - hydra_msgs - pcl_ros - tf2_eigen - tf2_ros - kimera_pgmo - voxblox_ros - pose_graph_tools - kimera_semantics_ros - DEPENDS spark_dsg hydra - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -add_library( - ${PROJECT_NAME} - src/config/ros_parser.cpp - src/pipeline/hydra_ros_pipeline.cpp - src/pipeline/ros_backend.cpp - src/pipeline/ros_frontend.cpp - src/pipeline/ros_lcd_registration.cpp - src/pipeline/ros_reconstruction.cpp - src/utils/dsg_streaming_interface.cpp - src/utils/mask_nodelet.cpp - src/utils/node_utilities.cpp - src/visualizer/colormap_utilities.cpp - src/visualizer/dsg_mesh_plugins.cpp - src/visualizer/dynamic_scene_graph_visualizer.cpp - src/visualizer/gvd_visualization_utilities.cpp - src/visualizer/topology_server_visualizer.cpp - src/visualizer/visualizer_plugins.cpp - src/visualizer/visualizer_utilities.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries( - ${PROJECT_NAME} - ${catkin_LIBRARIES} - spark_dsg::spark_dsg - gtsam -) -add_dependencies( - ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} -) - -add_executable(dsg_optimizer_node src/nodes/dsg_optimizer_node.cpp) -target_link_libraries(dsg_optimizer_node ${PROJECT_NAME}) - -add_executable(gvd_validator_node src/nodes/gvd_validator_node.cpp) -target_link_libraries(gvd_validator_node PUBLIC ${PROJECT_NAME} yaml-cpp) - -add_executable(hydra_ros_node src/nodes/hydra_node.cpp) -target_link_libraries(hydra_ros_node PUBLIC ${PROJECT_NAME}) - -add_executable(hydra_topology_node src/nodes/hydra_topology_node.cpp) -target_link_libraries(hydra_topology_node PUBLIC ${PROJECT_NAME} Boost::boost) - -add_executable(hydra_visualizer_node src/nodes/hydra_visualizer_node.cpp) -target_link_libraries(hydra_visualizer_node PUBLIC ${PROJECT_NAME}) - -add_executable(rotate_tf_node src/nodes/rotate_tf_node.cpp) -target_link_libraries(rotate_tf_node PUBLIC ${PROJECT_NAME}) - -add_executable(scene_graph_logger_node src/nodes/scene_graph_logger_node.cpp) -target_link_libraries(scene_graph_logger_node PUBLIC ${PROJECT_NAME}) - -if(CATKIN_ENABLE_TESTING) - add_subdirectory(tests) -endif() - -# TODO(nathan) handle install -install( - TARGETS ${PROJECT_NAME} hydra_ros_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/hydra_ros/README.md b/hydra_ros/README.md deleted file mode 100644 index 83f81c93..00000000 --- a/hydra_ros/README.md +++ /dev/null @@ -1,10 +0,0 @@ -## Hydra-ROS - -Contains ROS code specific to Hydra. - -To build and run tests (assuming you've built the package normally): - -``` -catkin build hydra_utils --catkin-make-args tests -rostest hydra_utils hydra_utils.test -``` diff --git a/hydra_ros/bin/csv_to_tf b/hydra_ros/bin/csv_to_tf deleted file mode 100755 index 22d9a84e..00000000 --- a/hydra_ros/bin/csv_to_tf +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python -"""Node that broadcasts a CSV as a tf.""" -import rospy -import sys -import tf2_ros -import geometry_msgs.msg - - -def main(): - rospy.init_node("csv_to_tf") - br = tf2_ros.TransformBroadcaster() - - trajectory_file = rospy.get_param("~trajectory_file", "") - if trajectory_file == "": - rospy.logfatal("file required!") - sys.exit(1) - - parent_frame = rospy.get_param("~parent_frame", "world") - child_frame = rospy.get_param("~child_frame", "left_cam_pgo") - - delta_t = rospy.Duration(-0.05) - have_first_line = False - - r = rospy.Rate(50) - - with open(trajectory_file, "r") as fin: - for line in fin: - if not have_first_line: - have_first_line = True - continue - - values = [x.strip() for x in line.split(",")] - time_ns = int(values[0]) - pose = [float(x) for x in values[1:]] - - t = geometry_msgs.msg.TransformStamped() - t.header.stamp = rospy.Time(0, time_ns) - t.header.frame_id = parent_frame - t.child_frame_id = child_frame - t.transform.translation.x = pose[0] - t.transform.translation.y = pose[1] - t.transform.translation.z = pose[2] - t.transform.rotation.w = pose[3] - t.transform.rotation.x = pose[4] - t.transform.rotation.y = pose[5] - t.transform.rotation.z = pose[6] - - while not rospy.is_shutdown(): - if rospy.Time.now() - t.header.stamp > delta_t: - break - r.sleep() - - rospy.logdebug("sending " + str(t.header.stamp.to_sec())) - br.sendTransform(t) - - -if __name__ == "__main__": - main() diff --git a/hydra_ros/bin/gnn_node b/hydra_ros/bin/gnn_node deleted file mode 100755 index a8a39272..00000000 --- a/hydra_ros/bin/gnn_node +++ /dev/null @@ -1,535 +0,0 @@ -#!/usr/bin/env python3 -"""Run inference on received scene graphs.""" -import spark_dsg as dsg -import pandas as pd -import numpy as np -import torch_geometric -import gensim.models -import pathlib -import logging -import pickle -import torch -import click -import heapq -import yaml - - -DEFAULT_LAYERS = (dsg.DsgLayers.OBJECTS, dsg.DsgLayers.ROOMS, dsg.DsgLayers.BUILDINGS) - - -def _is_on(G, n1, n2, threshold_on=1.0): - """ - Check whether n1 is "on" n2 or n2 is "on" n1 - - Requires that the n1 center is inside n2 on xy plane, and n1 is above - n2 on z-axis within a threshold (or vice-versa). - """ - pos1 = G.get_position(n1.id.value) - pos2 = G.get_position(n2.id.value) - size1 = _get_size(n1) - size2 = _get_size(n2) - - xy_dist = np.abs(pos1[0:2] - pos2[0:2]) - z_dist = np.abs(pos1[2] - pos2[2]) - n1_above_n2 = pos1[2] > pos2[2] - new_thresh = threshold_on * (size1[2] + size2[2]) / 2 - - if all(xy_dist <= size2[0:2] / 2) and n1_above_n2 and z_dist <= new_thresh: - return True - elif all(xy_dist <= size1[0:2] / 2) and not n1_above_n2 and z_dist <= new_thresh: - return True - else: - return False - - -def _is_above(G, n1, n2, threshold_near=2.0, threshold_on=1.0): - """ - Check whether n1 is "above" n2 or n2 is "above" n1 - - Requires that the n1 center and n2 are nearby on the xy plane, and n1 is above - n2 on z-axis by an amount greater than a provided threshold. - """ - pos1 = G.get_position(n1.id.value) - pos2 = G.get_position(n2.id.value) - size1 = _get_size(n1) - size2 = _get_size(n2) - near_thresh = (size1[0:2] + size2[0:2]) / 2.0 * threshold_near - - xy_dist = np.abs(pos1[0:2] - pos2[0:2]) - z_dist = np.abs(pos1[2] - pos2[2]) - n1_above_n2 = pos1[2] > pos2[2] - dist_thresh = threshold_on * (size1[2] + size2[2]) / 2 - - if all(xy_dist <= near_thresh): - if n1_above_n2 and z_dist > dist_thresh: - return True - if not n1_above_n2 and z_dist > dist_thresh: - return True - - return False - - -def _is_under(G, n1, n2): - """ - Check whether n1 is "under" n2 or n2 is "under" n1 - - Requires that either n1 or n2 is inside the other node on the xy place and - that the positions on the z-axis are distinct. - """ - pos1 = G.get_position(n1.id.value) - pos2 = G.get_position(n2.id.value) - size1 = _get_size(n1) - size2 = _get_size(n2) - - xy_dist = np.abs(pos1[0:2] - pos2[0:2]) - - if all(xy_dist <= size1[0:2] / 2) or all(xy_dist <= size2[0:2] / 2): - if pos1[2] < pos2[2]: - return True - if pos2[2] < pos1[2]: - return True - - return False - - -def _is_near(G, n1, n2, threshold_near=2.0, max_near=2.0): - """ - Check whether n1 is "near" n2 or n2 is "near" n1 - - Requires that either n1 or n2 is inside the other node on the xy place and - that the positions on the z-axis are distinct. - """ - pos1 = G.get_position(n1.id.value) - pos2 = G.get_position(n2.id.value) - size1 = _get_size(n1) - size2 = _get_size(n2) - - avg_size = (size1 + size2) / 2.0 - near_thresh = avg_size * threshold_near - - dist = np.abs(pos1 - pos2) - - # [LocatedNear] - if all(dist <= near_thresh) and all(dist - avg_size <= max_near * np.ones(3)): - return True - - return False - - -def _load_word2vec(model_file): - return gensim.models.KeyedVectors.load_word2vec_format(model_file, binary=True) - - -def _get_embedding(model, label, dim=300): - vec = np.zeros(dim) - try: - vec = np.mean( - [model[s] for s in label.split("_") if s != "of"], - axis=0, - ) - except KeyError: - print(f"{label} cannot be found in pretrained word2vec") - - return torch.from_numpy(vec.astype(np.float32)) - - -def _get_room_labels(room_file): - room_df = pd.read_csv( - room_file, usecols=range(2), names=["char", "room_name"], header=None - ) - return sorted(list(set(room_df["room_name"].to_list()))) - - -def _get_size(node): - return node.attributes.bounding_box.max - node.attributes.bounding_box.min - - -def _dist(G, n1, n2): - return np.linalg.norm(G.get_position(n1) - G.get_position(n2)) - - -def _get_node_masks_and_features( - G, embeddings, layers=DEFAULT_LAYERS, embedding_size=300 -): - num_infer_nodes = sum([G.get_layer(x).num_nodes() for x in layers]) - masks = {x: torch.zeros(num_infer_nodes, dtype=torch.bool) for x in layers} - - features = [] - id_map = {} - ids = [] - for node in G.nodes: - if node.layer not in layers: - continue - - # len is a proxy for index - masks[node.layer][len(features)] = True - id_map[node.id.value] = len(features) - ids.append(node.id.value) - - label = node.attributes.semantic_label - if node.id.category == "O": - embedding = embeddings.get(label, np.zeros(embedding_size)) - else: - embedding = np.zeros(embedding_size) - pos = node.attributes.position - size = _get_size(node) - features.append(np.hstack((pos, size, embedding))) - - id_tensor = torch.tensor(np.array([ids]).T) - return ( - masks, - id_map, - id_tensor, - torch.tensor(np.array(features), dtype=torch.float32), - ) - - -def _get_layer_edge_index(masks, edge_idx, l1, l2=None, undirected=False): - edge_mask = ( - masks[l1][edge_idx].all(0) - if l2 is None - else masks[l1][edge_idx[0, :]] & masks[l2][edge_idx[1, :]] - ) - - layer_idx = edge_idx[:, edge_mask] - if not undirected: - return layer_idx - - if edge_mask.any().item(): - return torch_geometric.utils.to_undirected(layer_idx) - else: - return layer_idx - - -def _add_edge_attr(data): - edge_index = torch.cat( - ( - data.object_edge_index, - data.room_edge_index, - data.object_room_edge_index, - data.room_building_edge_index, - torch.flipud(data.room_building_edge_index), - torch.flipud(data.object_room_edge_index), - ), - 1, - ) - edge_attr_list = [ - torch.tensor([[1, 0, 0, 0, 0, 0]]), - torch.tensor([[0, 1, 0, 0, 0, 0]]), - torch.tensor([[0, 0, 1, 0, 0, 0]]), - torch.tensor([[0, 0, 0, 1, 0, 0]]), - torch.tensor([[0, 0, 0, 0, 1, 0]]), - torch.tensor([[0, 0, 0, 0, 0, 1]]), - ] - edge_attr = torch.cat( - [edge_attr_list[0]] * data.object_edge_index.shape[1] - + [edge_attr_list[1]] * data.room_edge_index.shape[1] - + [edge_attr_list[2]] * data.object_room_edge_index.shape[1] - + [edge_attr_list[3]] * data.room_building_edge_index.shape[1] - + [edge_attr_list[4]] * data.room_building_edge_index.shape[1] - + [edge_attr_list[5]] * data.object_room_edge_index.shape[1], - 0, - ).type(torch.float32) - data.edge_index = edge_index - data.edge_attr = edge_attr - - -def _get_room_parent(node, prefix="R"): - for parent in node.parents(): - if dsg.NodeSymbol(parent).category == prefix: - return parent - - return None - - -def _add_object_connectivity(G, threshold_near=2.0, threshold_on=1.0, max_near=2.0): - room_to_objects = dict() - for node in G.get_layer(dsg.DsgLayers.OBJECTS).nodes: - room_id = _get_room_parent(node) - if room_id is None: - print(f"skipping invalid object {node.id}") - continue - - if room_id not in room_to_objects: - room_to_objects[room_id] = [node] - continue - - cmp_nodes = room_to_objects[room_id] - for cmp_node in cmp_nodes: - is_on = _is_on(G, node, cmp_node, threshold_on=threshold_on) - is_above = _is_above( - G, - node, - cmp_node, - threshold_near=threshold_near, - threshold_on=threshold_on, - ) - is_under = _is_under(G, node, cmp_node) - is_near = _is_near( - G, node, cmp_node, threshold_near=threshold_near, max_near=max_near - ) - - if is_on or is_above or is_under or is_near: - # TODO(nathan) consider getting direction - assert G.insert_edge(node.id.value, cmp_node.id.value) - - room_to_objects[room_id].append(node) - - -def _get_parent_of_closest_neighbor(G, x, max_hop=1): - frontier = [(_dist(G, x.id.value, n), n) for n in x.siblings()] - heapq.heapify(frontier) - - visited = set([x.id.value]) - - while len(frontier) != 0: - dist, curr_id = heapq.heappop(frontier) - visited.add(curr_id) - curr = G.get_node(curr_id) - - curr_parent = curr.get_parent() - if curr_parent is not None: - return curr_parent - - for s in curr.siblings(): - if s in visited: - continue - - heapq.heappush(frontier, (_dist(G, x.id.value, s), s)) - - return None - - -def _show_info(node): - print(f"node: {node.id}") - print(f" - siblings: {[dsg.NodeSymbol(x) for x in node.siblings()]}") - print(f" - parents: {[dsg.NodeSymbol(x) for x in node.parents()]}") - print(f" - children: {[dsg.NodeSymbol(x) for x in node.children()]}") - - -def _get_object_room(G, node): - place_id = node.get_parent() - if place_id is not None: - place = G.get_node(place_id) - place_parent = place.get_parent() - if place_parent is not None: - return place_parent - - closest_place_parent = _get_parent_of_closest_neighbor(G, place) - if closest_place_parent is not None: - return closest_place_parent - - node_pos = G.get_position(node.id.value) - candidate_rooms = [ - x - for x in G.get_layer(dsg.DsgLayers.ROOMS).nodes - if x.attributes.bounding_box.is_inside(node_pos) - ] - if len(candidate_rooms) == 0: - return None - - # TODO(nathan) we should probably pick the closest, but... - return candidate_rooms[0].id.value - - -def preprocess_dsg(G, threshold_near=2.0, threshold_on=1.0, max_near=2.0): - dsg.add_bounding_boxes_to_layer(G, dsg.DsgLayers.ROOMS) - dsg.add_bounding_boxes_to_layer(G, dsg.DsgLayers.BUILDINGS) - - objects = G.get_layer(dsg.DsgLayers.OBJECTS) - invalid_objects = [] - for node in objects.nodes: - room_id = _get_object_room(G, node) - if room_id is None: - num_parents = len(node.parents()) - print(f"failed to find parent for {node.id} ({num_parents} parents)") - invalid_objects.append(node.id.value) - continue - - assert G.insert_edge(node.id.value, room_id) - - for object_id in invalid_objects: - G.remove_node(object_id) - - _add_object_connectivity( - G, threshold_near=threshold_near, max_near=max_near, threshold_on=threshold_on - ) - - -def convert_to_torch_data(G, embedding_dict, add_edge_attr=False): - """Convert a graph to a pytorch tensor.""" - # this automatically maps all rooms and buildings to unknown (zero) embeddings - masks, id_map, id_tensor, x = _get_node_masks_and_features(G, embedding_dict) - - # fill edge_index - edge_index = [] - for edge in G.edges: - if edge.source not in id_map or edge.target not in id_map: - continue - - source_node = G.get_node(edge.source) - target_node = G.get_node(edge.target) - if source_node.layer > target_node.layer: - edge_index.append([id_map[edge.target], id_map[edge.source]]) - else: - edge_index.append([id_map[edge.source], id_map[edge.target]]) - - edge_index = torch.tensor(edge_index, dtype=torch.long).T - - # TODO(nathan) make this more generic / check how sgl uses data fields - R_edges = _get_layer_edge_index( - masks, edge_index, dsg.DsgLayers.ROOMS, undirected=True - ) - O_edges = _get_layer_edge_index( - masks, edge_index, dsg.DsgLayers.OBJECTS, undirected=True - ) - - RB_edges = _get_layer_edge_index( - masks, edge_index, dsg.DsgLayers.ROOMS, dsg.DsgLayers.BUILDINGS - ) - OR_edges = _get_layer_edge_index( - masks, edge_index, dsg.DsgLayers.OBJECTS, dsg.DsgLayers.ROOMS - ) - - torch_data = torch_geometric.data.Data( - x=x, - object_mask=masks[dsg.DsgLayers.OBJECTS], - room_mask=masks[dsg.DsgLayers.ROOMS], - building_mask=masks[dsg.DsgLayers.BUILDINGS], - room_edge_index=R_edges, - object_edge_index=O_edges, - room_building_edge_index=RB_edges, - object_room_edge_index=OR_edges, - edge_index=torch.cat((RB_edges, OR_edges, R_edges, O_edges), dim=1), - ) - - room_ids = id_tensor[masks[dsg.DsgLayers.ROOMS]] - room_ids = [dsg.NodeSymbol(x[0]) for x in room_ids.tolist()] - room_idx_map = {x: idx for idx, x in enumerate(room_ids)} - - if add_edge_attr: - _add_edge_attr(torch_data) - - return torch_data, room_idx_map - - -class GnnModel: - """Class to hold stuff regarding inference.""" - - def __init__(self, model_file, word2vec_file, config_file, load_word2vec=True): - """Load everything.""" - model_path = pathlib.Path(model_file).expanduser().absolute() - with model_path.open("rb") as fin: - self.model = pickle.load(fin) - - word2vec_path = pathlib.Path(word2vec_file).expanduser().absolute() - if load_word2vec: - word2vec = _load_word2vec(word2vec_path) - else: - word2vec = {} - - config_path = pathlib.Path(config_file).expanduser().absolute() - with config_path.open("r") as fin: - self.config = yaml.load(fin.read(), Loader=yaml.SafeLoader) - - self.object_labels = [x for _, x in self.config["object_labels"].items()] - self.object_embeddings = { - idx: _get_embedding(word2vec, label) - for idx, label in enumerate(self.object_labels) - } - - self.building_labels = sorted(self.config["building_labels"]) - self.room_labels = sorted(self.config["room_labels"]) - - self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - self.model.to(self.device) - - def infer(self, G, run_inference=True): - preprocess_dsg(G) - if run_inference: - torch_graph, room_idx_map = convert_to_torch_data( - G, self.object_embeddings, add_edge_attr=True - ) - - with torch.no_grad(): - pred = self.model(torch_graph.to(self.device)) - building_pred = pred[0].argmax(dim=1).cpu() - room_pred = pred[1].argmax(dim=1).cpu() - if len(building_pred) == 1: - building_name = self.building_labels[building_pred[0]] - print(f"building: {building_name}") - pred_room_labels = [self.room_labels[x] for x in room_pred] - pred_room_map = { - room: pred_room_labels[idx] for room, idx in room_idx_map.items() - } - print(f"rooms: {pred_room_map}") - return pred_room_map - - return None - - -@click.group() -def main(): - pass - - -@main.command() -@click.argument("network") -@click.argument("word2vec-file") -@click.argument("config") -@click.option("--recv-url", "-r", default="tcp://127.0.0.1:8001") -@click.option("--send-url", "-s", default="tcp://127.0.0.1:8002") -@click.option("--num-threads", "-t", default=2) -@click.option("--poll-time-ms", default=10) -def run(network, word2vec_file, config, recv_url, send_url, num_threads, poll_time_ms): - """Run everything.""" - logging.basicConfig( - format="[%(asctime)s %(levelname)s] %(message)s", level=logging.DEBUG - ) - - model = GnnModel(network, word2vec_file, config) - - logging.info(f"setting up sender @ {send_url}") - sender = dsg.DsgSender(send_url) - - logging.info(f"setting up receiver @ {recv_url}") - receiver = dsg.DsgReceiver(recv_url) - - while True: - if not receiver.recv(poll_time_ms): - continue - - logging.info(f"curr nodes: {receiver.graph.num_nodes(False)}") - pred_room_map = model.infer(receiver.graph) - - G = dsg.DynamicSceneGraph() - for room_id, label in pred_room_map.items(): - attrs = dsg.RoomNodeAttributes() - attrs.name = f"{room_id}: {label}" - G.add_node(dsg.DsgLayers.ROOMS, room_id.value, attrs) - - sender.send(G) - - -@main.command() -@click.argument("network") -@click.argument("word2vec-file") -@click.argument("config") -@click.argument("dsg_file") -def test(network, word2vec_file, config, dsg_file): - """Run everything.""" - logging.basicConfig( - format="[%(asctime)s %(levelname)s] %(message)s", level=logging.DEBUG - ) - - model = GnnModel(network, word2vec_file, config) - - dsg_path = pathlib.Path(dsg_file).expanduser().absolute() - logging.info(f"loading scene graph from {dsg_path}") - G = dsg.DynamicSceneGraph.load(str(dsg_path)) - - model.infer(G) - - -if __name__ == "__main__": - main() diff --git a/hydra_ros/bin/odom_to_tf b/hydra_ros/bin/odom_to_tf deleted file mode 100755 index 88149f18..00000000 --- a/hydra_ros/bin/odom_to_tf +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python -"""Broadcast a tf from odom.""" -import rospy -import tf2_ros -import nav_msgs.msg -import geometry_msgs.msg - - -def broadcast_tf(msg, arg_tuple): - """Re-broadcast the msg pose as a tf.""" - broadcaster, parent_frame, child_frame = arg_tuple - pose = geometry_msgs.msg.TransformStamped() - pose.header.stamp = msg.header.stamp - pose.header.frame_id = parent_frame - pose.child_frame_id = child_frame - pose.transform.translation.x = msg.pose.pose.position.x - pose.transform.translation.y = msg.pose.pose.position.y - pose.transform.translation.z = msg.pose.pose.position.z - pose.transform.rotation.w = msg.pose.pose.orientation.w - pose.transform.rotation.x = msg.pose.pose.orientation.x - pose.transform.rotation.y = msg.pose.pose.orientation.y - pose.transform.rotation.z = msg.pose.pose.orientation.z - - broadcaster.sendTransform(pose) - - -def main(): - """Do stuff.""" - rospy.init_node("odom_to_tf") - - parent_frame = rospy.get_param("~parent_frame", "world") - child_frame = rospy.get_param("~child_frame", "t265_odom") - broadcaster = tf2_ros.TransformBroadcaster() - - rospy.Subscriber( - "odom", - nav_msgs.msg.Odometry, - broadcast_tf, - (broadcaster, parent_frame, child_frame), - tcp_nodelay=True - ) - - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/hydra_ros/cfg/Colormap.cfg b/hydra_ros/cfg/Colormap.cfg deleted file mode 100755 index b089b60b..00000000 --- a/hydra_ros/cfg/Colormap.cfg +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python -"""Generic HLS colormap dynamic config.""" -PACKAGE = "hydra_ros" - -import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA - -gen = dr_gen.ParameterGenerator() - -gen.add("min_hue", dr_gen.double_t, 0, "min color hue", 0.55, 0.0, 1.0) -gen.add("max_hue", dr_gen.double_t, 0, "max color hue", 0.8, 0.0, 1.0) -gen.add("min_saturation", dr_gen.double_t, 0, "min color saturation", 0.5, 0.0, 1.0) -gen.add("max_saturation", dr_gen.double_t, 0, "max color saturation", 0.7, 0.0, 1.0) -gen.add("min_luminance", dr_gen.double_t, 0, "min color luminance", 0.3, 0.0, 1.0) -gen.add("max_luminance", dr_gen.double_t, 0, "max color luminance", 0.8, 0.0, 1.0) - -exit(gen.generate(PACKAGE, PACKAGE, "Colormap")) diff --git a/hydra_ros/cfg/DynamicLayerVisualizer.cfg b/hydra_ros/cfg/DynamicLayerVisualizer.cfg deleted file mode 100755 index ab1f7cd2..00000000 --- a/hydra_ros/cfg/DynamicLayerVisualizer.cfg +++ /dev/null @@ -1,115 +0,0 @@ -#!/usr/bin/env python -"""Config for the visualization of dynamic layers in the scene graph.""" -PACKAGE = "hydra_ros" - -import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA - -gen = dr_gen.ParameterGenerator() - -gen.add( - "z_offset_scale", - dr_gen.double_t, - 0, - "number of steps of offset to apply", - 0.0, - -5.0, - 10.0, -) -gen.add("visualize", dr_gen.bool_t, 0, "show layer", False) - -# Dynamic Layers - -gen.add("num_colors", dr_gen.int_t, 0, "number of unique colors to use", 20, 2, 255) -gen.add("color_offset", dr_gen.int_t, 0, "offset into colors", 20, 2, 255) -gen.add( - "saturation", - dr_gen.double_t, - 0, - "saturation for layer color", - 0.8, - 0.0, - 1.0, -) -gen.add( - "luminance", - dr_gen.double_t, - 0, - "luminance for layer color", - 0.8, - 0.0, - 1.0, -) -gen.add( - "edge_sl_ratio", - dr_gen.double_t, - 0, - "ratio of saturation and luminance to node marker color", - 0.6, - 0.0, - 1.0, -) -gen.add( - "node_scale", - dr_gen.double_t, - 0, - "size of the node markers", - 0.1, - 0.01, - 2.0, -) -gen.add( - "node_alpha", - dr_gen.double_t, - 0, - "alpha of the node markers", - 1.0, - 0.0, - 1.0, -) -gen.add( - "node_use_sphere", - dr_gen.bool_t, - 0, - "use sphere markers (instead of cubes) for node centroids", - False, -) -gen.add( - "edge_scale", - dr_gen.double_t, - 0, - "layer edge size", - 0.03, - 0.001, - 1.0, -) -gen.add("edge_alpha", dr_gen.double_t, 0, "layer edge alpha", 1.0, 0.0, 1.0) -gen.add( - "label_height", - dr_gen.double_t, - 0, - "height of text label above latest node in layer", - 1.0, - 0.0, - 5.0, -) -gen.add( - "label_scale", - dr_gen.double_t, - 0, - "scale of text label above latest node in layer", - 0.5, - 0.05, - 5.0, -) -gen.add( - "interlayer_edge_insertion_skip", - dr_gen.int_t, - 0, - "Number of edges to skip when drawing interlayer edges", - 0, - 0, - 1000, -) -gen.add("visualize_interlayer_edges", dr_gen.bool_t, 0, "show interlayer edges", False) - -exit(gen.generate(PACKAGE, PACKAGE, "DynamicLayerVisualizer")) diff --git a/hydra_ros/cfg/GvdVisualizer.cfg b/hydra_ros/cfg/GvdVisualizer.cfg deleted file mode 100755 index 703b5dfc..00000000 --- a/hydra_ros/cfg/GvdVisualizer.cfg +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python -"""Config for GVD visualizations.""" -PACKAGE = "hydra_ros" - -import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA - -gen = dr_gen.ParameterGenerator() -mode_enum = gen.enum( - [ - gen.const("DEFAULT", dr_gen.int_t, 0, "default color mode source (distance)"), - gen.const( - "DISTANCE", dr_gen.int_t, 1, "use voxel distance as color mode source" - ), - gen.const( - "BASIS_POINTS", - dr_gen.int_t, - 2, - "use number of basis points as color mode source", - ), - ], - "Enum for GVD visualization mode", -) - -gvd = gen.add_group("GVD") -gvd.add("gvd_alpha", dr_gen.double_t, 0, "alpha of the GVD", 0.6, 0.0, 1.0) -gvd.add("gvd_min_alpha", dr_gen.double_t, 0, "min alpha of the GVD", 0.6, 0.0, 1.0) -gvd.add("gvd_min_distance", dr_gen.double_t, 0, "distance colormap min", 0.2, 0.0, 2.0) -gvd.add("gvd_max_distance", dr_gen.double_t, 0, "distance colormap max", 5.0, 0.0, 5.0) -gvd.add( - "basis_threshold", dr_gen.int_t, 0, "basis threshold for GVD inclusion", 2, 1, 26 -) -gvd.add("min_num_basis", dr_gen.int_t, 0, "basis colormap min", 1, 1, 26) -gvd.add("max_num_basis", dr_gen.int_t, 0, "basis colormap max", 26, 1, 26) -gvd.add( - "gvd_mode", dr_gen.int_t, 0, "visualization mode", 0, 0, 2, edit_method=mode_enum -) -gvd.add("gvd_graph_scale", dr_gen.double_t, 0, "scale for wireframe", 0.005, 0.0, 1.0) - -esdf = gen.add_group("ESDF") -esdf.add("esdf_alpha", dr_gen.double_t, 0, "alpha of the ESDF", 0.6, 0.0, 1.0) -esdf.add("slice_height", dr_gen.double_t, 0, "height of ESDf slice", 0.5, -10.0, 10.0) -esdf.add( - "esdf_min_distance", dr_gen.double_t, 0, "distance colormap min", 0.2, 0.0, 2.0 -) -esdf.add( - "esdf_max_distance", dr_gen.double_t, 0, "distance colormap max", 5.0, 0.0, 5.0 -) - -exit(gen.generate(PACKAGE, "hydra_topology", "GvdVisualizer")) diff --git a/hydra_ros/cfg/LayerVisualizer.cfg b/hydra_ros/cfg/LayerVisualizer.cfg deleted file mode 100755 index edf19032..00000000 --- a/hydra_ros/cfg/LayerVisualizer.cfg +++ /dev/null @@ -1,132 +0,0 @@ -#!/usr/bin/env python -"""Visualization config for static layers in the scene graph.""" -PACKAGE = "hydra_ros" - -import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA - -gen = dr_gen.ParameterGenerator() - -gen.add( - "z_offset_scale", - dr_gen.double_t, - 0, - "number of steps of offset to apply", - 0.0, - -5.0, - 10.0, -) -gen.add("visualize", dr_gen.bool_t, 0, "show layer", False) - -# Nodes -nodes = gen.add_group("nodes", type="tab") -nodes.add( - "marker_scale", dr_gen.double_t, 0, "size of the centroid marker", 0.1, 0.01, 2.0 -) -nodes.add( - "marker_alpha", dr_gen.double_t, 0, "alpha of the centroid marker", 1.0, 0.0, 1.0 -) -nodes.add( - "use_sphere_marker", - dr_gen.bool_t, - 0, - "use sphere markers (instead of cubes)", - False, -) -nodes.add("use_label", dr_gen.bool_t, 0, "add text label", False) -nodes.add( - "label_height", dr_gen.double_t, 0, "height of text label above node", 1.0, 0.0, 5.0 -) -nodes.add( - "label_scale", dr_gen.double_t, 0, "scale of text label above node", 0.5, 0.05, 5.0 -) -nodes.add( - "use_bounding_box", - dr_gen.bool_t, - 0, - "display bounding box", - False, -) -nodes.add( - "collapse_bounding_box", - dr_gen.bool_t, - 0, - "draw bounding box at ground level", - False, -) -nodes.add( - "bbox_wireframe_scale", - dr_gen.double_t, - 0, - "scale of bounding box wireframe", - 0.1, - 0.001, - 1.0, -) -nodes.add( - "bbox_wireframe_edge_scale", - dr_gen.double_t, - 0, - "scale of edges drawn to bbox corners", - 0.01, - 0.001, - 1.0, -) -nodes.add( - "bounding_box_alpha", dr_gen.double_t, 0, "alpha of bounding boxes", 0.5, 0.0, 1.0 -) - -# Edges -edges = gen.add_group("edges", type="tab") -edges.add("use_edge_source", dr_gen.bool_t, 0, "use edge source layer for config", True) - -edges.add( - "interlayer_edge_scale", - dr_gen.double_t, - 0, - "interlayer edge size", - 0.03, - 0.001, - 1.0, -) -edges.add( - "interlayer_edge_alpha", dr_gen.double_t, 0, "interlayer edge alpha", 1.0, 0.0, 1.0 -) -edges.add( - "interlayer_edge_use_color", dr_gen.bool_t, 0, "show interlayer edge in color", True -) -edges.add( - "interlayer_edge_insertion_skip", - dr_gen.int_t, - 0, - "Number of edges to skip when drawing interlayer edges", - 0, - 0, - 1000, -) - -edges.add( - "intralayer_edge_scale", - dr_gen.double_t, - 0, - "intralayer edge size", - 0.03, - 0.001, - 1.0, -) -edges.add( - "intralayer_edge_alpha", dr_gen.double_t, 0, "intralayer edge alpha", 1.0, 0.0, 1.0 -) -edges.add( - "intralayer_edge_use_color", dr_gen.bool_t, 0, "show intralayer edge in color", True -) -edges.add( - "intralayer_edge_insertion_skip", - dr_gen.int_t, - 0, - "Number of edges to skip when drawing intralayer edges", - 0, - 0, - 1000, -) - -exit(gen.generate(PACKAGE, PACKAGE, "LayerVisualizer")) diff --git a/hydra_ros/cfg/Visualizer.cfg b/hydra_ros/cfg/Visualizer.cfg deleted file mode 100755 index 8713fe3d..00000000 --- a/hydra_ros/cfg/Visualizer.cfg +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python -"""Configuration options for the scene graph visualization.""" -PACKAGE = "hydra_ros" - -import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA - -gen = dr_gen.ParameterGenerator() - -gen.add( - "layer_z_step", - dr_gen.double_t, - 0, - "size of the distance between layers", - 5.0, - 0.0, - 50.0, -) -gen.add( - "mesh_edge_break_ratio", - dr_gen.double_t, - 0, - "point at which to break the edge into many edges", - 0.5, - 0.0, - 1.0, -) -gen.add( - "mesh_layer_offset", - dr_gen.double_t, - 0, - "height mesh layer exists at", - 0.0, - -5.0, - 5.0, -) -gen.add( - "draw_mesh_edges", - dr_gen.bool_t, - 0, - "whether or not to draw mesh edges", - False, -) -gen.add( - "collapse_layers", - dr_gen.bool_t, - 0, - "whether or not to show the layers independently", - False, -) -gen.add( - "color_places_by_distance", - dr_gen.bool_t, - 0, - "color the places by distance to nearest obstacle", - False, -) -gen.add( - "color_nodes_by_active_flag", - dr_gen.bool_t, - 0, - "color nodes by whether or not they are active", - False, -) - - -gen.add( - "places_colormap_min_distance", dr_gen.double_t, 0, "min distance", 0.0, 0.0, 100.0 -) -gen.add( - "places_colormap_max_distance", dr_gen.double_t, 0, "max distance", 10.0, 0.0, 100.0 -) - -exit(gen.generate(PACKAGE, PACKAGE, "Visualizer")) diff --git a/hydra_ros/config/graph_visualization_config.yaml b/hydra_ros/config/graph_visualization_config.yaml deleted file mode 100644 index 2d61202b..00000000 --- a/hydra_ros/config/graph_visualization_config.yaml +++ /dev/null @@ -1,45 +0,0 @@ ---- -topology_marker_ns: topology_graph -show_block_outlines: true -use_gvd_block_outlines: false -gvd_visualizer: - visualization_type: 2 # 0 -> NONE, 1 -> ESDF_SLICE, 2 -> GVD - color_nearest_vertices: false - gvd_alpha: 0.6 - gvd_min_distance: 0.2 - gvd_max_distance: 3.0 - basis_threshold: 2 - min_num_basis: 1 - max_num_basis: 10 - gvd_mode: 0 # 0 -> DEFAULT, 1 -> DISTANCE, 2 -> BASIS_POINTs - esdf_alpha: 0.6 - slice_height: 1.0 - esdf_min_distance: 0.2 - esdf_max_distance: 5.0 -graph_visualizer: - z_offset_scale: 0.0 - visualize: true - marker_scale: 0.2 - marker_alpha: 0.9 - use_sphere_marker: true - use_label: true - label_height: 0.7 - label_scale: 0.3 - use_bounding_box: false - bounding_box_alpha: 0.5 - use_edge_source: false - interlayer_edge_scale: 0.1 - interlayer_edge_alpha: 0.4 - interlayer_edge_use_color: true - interlayer_edge_insertion_skip: 0 - intralayer_edge_scale: 0.02 - intralayer_edge_alpha: 0.4 - intralayer_edge_use_color: false - intralayer_edge_insertion_skip: 0 -visualizer_colormap: - min_hue: 0.0 - max_hue: 0.5 - min_saturation: 0.9 - max_saturation: 0.9 - min_luminance: 0.6 - max_luminance: 0.85 diff --git a/hydra_ros/config/gvd_validator/uh2.yaml b/hydra_ros/config/gvd_validator/uh2.yaml deleted file mode 100644 index c7753545..00000000 --- a/hydra_ros/config/gvd_validator/uh2.yaml +++ /dev/null @@ -1,57 +0,0 @@ ---- -color_topic: /tesse/left_cam/rgb/image_raw -depth_topic: /tesse/depth_cam/mono/image_raw -start: -1.0 -duration: 30.0 -intrinsics: [415.69219, 415.69219, 360.0, 240.0] -# gvd integration and graph extraction -max_distance_m: 10.0 -min_distance_m: 0.2 -min_diff_m: 0.0 -min_weight: 1.0e-6 -positive_distance_only: true -min_basis_for_extraction: 2 -num_buckets: 20 -multi_queue: false -extract_graph: true -voronoi_config: - mode: L1_THEN_ANGLE - min_distance_m: 0.40 - parent_l1_separation: 12 - parent_cos_angle_separation: 0.0 -graph_extractor: - min_extra_basis: 2 - min_vertex_basis: 4 - merge_new_nodes: true - node_merge_distance_m: 0.3 - edge_splitting_merge_nodes: true - max_edge_split_iterations: 5 - max_edge_deviation: 4 - add_freespace_edges: true - freespace_active_neighborhood_hops: 1 - freespace_edge_num_neighbors: 3 - freespace_edge_min_clearance_m: 0.2 - add_component_connection_edges: true - connected_component_window: 10 - connected_component_hops: 2 - component_nodes_to_check: 50 - component_nearest_neighbors: 1 - component_max_edge_length_m: 5.0 - component_min_clearance_m: 0.5 - remove_isolated_nodes: true - use_color: true -integrator_threads: 12 -voxel_carving_enabled: true -use_const_weight: false -default_truncation_distance: 0.3 -max_weight: 10000 -min_ray_length_m: 0.1 -max_ray_length_m: 10.0 -allow_clear: true -use_weight_dropoff: true -use_sparsity_compensation_factor: false -integration_order_mode: mixed -enable_anti_grazing: false -start_voxel_subsampling_factor: 2 -max_consecutive_ray_collisions: 2 -clear_checks_every_n_frames: 1 diff --git a/hydra_ros/config/hydra_visualizer/agent_layer.yaml b/hydra_ros/config/hydra_visualizer/agent_layer.yaml deleted file mode 100644 index e4dcdb8f..00000000 --- a/hydra_ros/config/hydra_visualizer/agent_layer.yaml +++ /dev/null @@ -1,17 +0,0 @@ ---- -visualize: true -num_colors: 5 -saturation: 0.9 -luminance: 0.7 -edge_sl_ratio: 0.6 -node_scale: 0.2 -node_alpha: 0.9 -node_use_sphere: true -edge_scale: 0.05 -edge_alpha: 0.9 -label_height: 0.9 -label_scale: 0.8 -color_offset: 3 -interlayer_edge_insertion_skip: 10 -visualize_interlayer_edges: false -z_offset_scale: 0.0 diff --git a/hydra_ros/config/hydra_visualizer/buildings_layer.yaml b/hydra_ros/config/hydra_visualizer/buildings_layer.yaml deleted file mode 100644 index 3c54a71e..00000000 --- a/hydra_ros/config/hydra_visualizer/buildings_layer.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -# general -z_offset_scale: 5.5 -visualize: true -# nodes -marker_scale: 1.0 -marker_alpha: 1.0 -use_sphere_marker: false -use_label: true -label_height: 1.5 -label_scale: 1.25 -use_bounding_box: false -bounding_box_alpha: 0.5 -collapse_bounding_box: false -# edges -use_edge_source: false -interlayer_edge_scale: 0.10 -interlayer_edge_alpha: 0.5 -interlayer_edge_use_color: false -interlayer_edge_insertion_skip: 0 -intralayer_edge_scale: 0.05 -intralayer_edge_alpha: 1.0 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/hydra_visualizer/objects_layer.yaml b/hydra_ros/config/hydra_visualizer/objects_layer.yaml deleted file mode 100644 index 3599728b..00000000 --- a/hydra_ros/config/hydra_visualizer/objects_layer.yaml +++ /dev/null @@ -1,26 +0,0 @@ ---- -# generic -z_offset_scale: 2.0 -visualize: true -# node -marker_scale: 0.40 -marker_alpha: 0.6 -use_sphere_marker: false -use_label: false -label_height: 0.5 -label_scale: 0.45 -use_bounding_box: true -bounding_box_alpha: 0.9 -bbox_wireframe_scale: 0.05 -bbox_wireframe_edge_scale: 0.05 -collapse_bounding_box: true -# edge -use_edge_source: true -interlayer_edge_scale: 0.05 -interlayer_edge_alpha: 0.3 -interlayer_edge_use_color: true -interlayer_edge_insertion_skip: 10 -intralayer_edge_scale: 0.03 -intralayer_edge_alpha: 0.6 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/hydra_visualizer/places_layer.yaml b/hydra_ros/config/hydra_visualizer/places_layer.yaml deleted file mode 100644 index 6e416941..00000000 --- a/hydra_ros/config/hydra_visualizer/places_layer.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -# general -z_offset_scale: 3.0 -visualize: true -# nodes -marker_scale: 0.2 -marker_alpha: 0.9 -use_sphere_marker: true -use_label: false -label_height: 1.0 -label_scale: 0.5 -use_bounding_box: false -bounding_box_alpha: 0.5 -collapse_bounding_box: false -# edges -use_edge_source: false -interlayer_edge_scale: 0.08 -interlayer_edge_alpha: 0.4 -interlayer_edge_use_color: true -interlayer_edge_insertion_skip: 0 -intralayer_edge_scale: 0.01 -intralayer_edge_alpha: 0.5 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/hydra_visualizer/rooms_layer.yaml b/hydra_ros/config/hydra_visualizer/rooms_layer.yaml deleted file mode 100644 index 6142e3d0..00000000 --- a/hydra_ros/config/hydra_visualizer/rooms_layer.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -# general -z_offset_scale: 4.2 -visualize: true -# nodes -marker_scale: 0.6 -marker_alpha: 0.8 -use_sphere_marker: false -use_label: true -label_height: 1.25 -label_scale: 1.0 -use_bounding_box: false -bounding_box_alpha: 0.5 -collapse_bounding_box: false -# edges -use_edge_source: true -interlayer_edge_scale: 0.08 -interlayer_edge_alpha: 0.4 -interlayer_edge_use_color: true -interlayer_edge_insertion_skip: 3 -intralayer_edge_scale: 0.1 -intralayer_edge_alpha: 0.2 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/hydra_visualizer/visualizer.yaml b/hydra_ros/config/hydra_visualizer/visualizer.yaml deleted file mode 100644 index d1c672c8..00000000 --- a/hydra_ros/config/hydra_visualizer/visualizer.yaml +++ /dev/null @@ -1,25 +0,0 @@ ---- -# general options -layer_z_step: 5.5 # unit separation between layers -# ratio of where the split from a single edge to the multiple edges to the mesh occurs. -# 0 -> at the height of the mesh -# 1 -> at the height of the object layer -mesh_edge_break_ratio: 0.5 -# where to draw the mesh (0.0 is recommended) -mesh_layer_offset: 0.0 -# whether or not to apply offsets to each of the layers -collapse_layers: false -# color places by their distance to the nearest obstalce -color_places_by_distance: false -# minimum distance to clip to for places colormap -places_colormap_min_distance: 0.0 -# maximum distance to clip to for places colormap -places_colormap_max_distance: 3.0 - -places_colormap: - min_hue: 0.0 - max_hue: 0.5 - min_saturation: 0.9 - max_saturation: 0.8 - min_luminance: 0.6 - max_luminance: 0.85 diff --git a/hydra_ros/config/object_visualizer/agent_layer.yaml b/hydra_ros/config/object_visualizer/agent_layer.yaml deleted file mode 100644 index ffe94cb0..00000000 --- a/hydra_ros/config/object_visualizer/agent_layer.yaml +++ /dev/null @@ -1,17 +0,0 @@ ---- -visualize: false -num_colors: 5 -saturation: 0.9 -luminance: 0.7 -edge_sl_ratio: 0.6 -node_scale: 0.2 -node_alpha: 0.9 -node_use_sphere: true -edge_scale: 0.05 -edge_alpha: 0.9 -label_height: 0.9 -label_scale: 0.8 -color_offset: 3 -interlayer_edge_insertion_skip: 10 -visualize_interlayer_edges: false -z_offset_scale: 0.0 diff --git a/hydra_ros/config/object_visualizer/buildings_layer.yaml b/hydra_ros/config/object_visualizer/buildings_layer.yaml deleted file mode 100644 index 810aaa21..00000000 --- a/hydra_ros/config/object_visualizer/buildings_layer.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -# general -z_offset_scale: 5.5 -visualize: false -# nodes -marker_scale: 1.0 -marker_alpha: 1.0 -use_sphere_marker: false -use_label: true -label_height: 1.5 -label_scale: 1.25 -use_bounding_box: false -bounding_box_alpha: 0.5 -collapse_bounding_box: false -# edges -use_edge_source: false -interlayer_edge_scale: 0.10 -interlayer_edge_alpha: 0.5 -interlayer_edge_use_color: false -interlayer_edge_insertion_skip: 0 -intralayer_edge_scale: 0.05 -intralayer_edge_alpha: 1.0 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/object_visualizer/objects_layer.yaml b/hydra_ros/config/object_visualizer/objects_layer.yaml deleted file mode 100644 index c7bd76fd..00000000 --- a/hydra_ros/config/object_visualizer/objects_layer.yaml +++ /dev/null @@ -1,25 +0,0 @@ ---- -# generic -z_offset_scale: 1.5 -visualize: true -# node -marker_scale: 0.25 -marker_alpha: 0.9 -use_sphere_marker: true -use_label: true -label_height: 0.7 -label_scale: 0.5 -use_bounding_box: true -bounding_box_alpha: 0.9 -bbox_wireframe_scale: 0.08 -collapse_bounding_box: true -# edge -use_edge_source: true -interlayer_edge_scale: 0.05 -interlayer_edge_alpha: 0.3 -interlayer_edge_use_color: true -interlayer_edge_insertion_skip: 10 -intralayer_edge_scale: 0.03 -intralayer_edge_alpha: 0.6 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/object_visualizer/places_layer.yaml b/hydra_ros/config/object_visualizer/places_layer.yaml deleted file mode 100644 index fb083b29..00000000 --- a/hydra_ros/config/object_visualizer/places_layer.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -# general -z_offset_scale: 2.5 -visualize: false -# nodes -marker_scale: 0.2 -marker_alpha: 0.9 -use_sphere_marker: true -use_label: false -label_height: 1.0 -label_scale: 0.5 -use_bounding_box: false -bounding_box_alpha: 0.5 -collapse_bounding_box: false -# edges -use_edge_source: false -interlayer_edge_scale: 0.08 -interlayer_edge_alpha: 0.4 -interlayer_edge_use_color: true -interlayer_edge_insertion_skip: 0 -intralayer_edge_scale: 0.01 -intralayer_edge_alpha: 0.7 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/object_visualizer/rooms_layer.yaml b/hydra_ros/config/object_visualizer/rooms_layer.yaml deleted file mode 100644 index 135b1e34..00000000 --- a/hydra_ros/config/object_visualizer/rooms_layer.yaml +++ /dev/null @@ -1,24 +0,0 @@ ---- -# general -z_offset_scale: 3.4 -visualize: false -# nodes -marker_scale: 0.6 -marker_alpha: 0.8 -use_sphere_marker: false -use_label: true -label_height: 1.0 -label_scale: 0.75 -use_bounding_box: false -bounding_box_alpha: 0.5 -collapse_bounding_box: false -# edges -use_edge_source: true -interlayer_edge_scale: 0.08 -interlayer_edge_alpha: 0.4 -interlayer_edge_use_color: true -interlayer_edge_insertion_skip: 1 -intralayer_edge_scale: 0.1 -intralayer_edge_alpha: 0.8 -intralayer_edge_use_color: false -intralayer_edge_insertion_skip: 0 diff --git a/hydra_ros/config/object_visualizer/visualizer.yaml b/hydra_ros/config/object_visualizer/visualizer.yaml deleted file mode 100644 index 9400c0d7..00000000 --- a/hydra_ros/config/object_visualizer/visualizer.yaml +++ /dev/null @@ -1,25 +0,0 @@ ---- -# general options -layer_z_step: 4.0 # unit separation between layers -# ratio of where the split from a single edge to the multiple edges to the mesh occurs. -# 0 -> at the height of the mesh -# 1 -> at the height of the object layer -mesh_edge_break_ratio: 0.5 -# where to draw the mesh (0.0 is recommended) -mesh_layer_offset: 0.0 -# whether or not to apply offsets to each of the layers -collapse_layers: false -# color places by their distance to the nearest obstalce -color_places_by_distance: false -# minimum distance to clip to for places colormap -places_colormap_min_distance: 0.0 -# maximum distance to clip to for places colormap -places_colormap_max_distance: 3.0 - -places_colormap: - min_hue: 0.0 - max_hue: 0.5 - min_saturation: 0.9 - max_saturation: 0.8 - min_luminance: 0.6 - max_luminance: 0.85 diff --git a/hydra_ros/config/overrides.yaml b/hydra_ros/config/overrides.yaml deleted file mode 100644 index e69de29b..00000000 diff --git a/hydra_ros/config/ros_params.yaml b/hydra_ros/config/ros_params.yaml deleted file mode 100644 index 6c633ec8..00000000 --- a/hydra_ros/config/ros_params.yaml +++ /dev/null @@ -1,9 +0,0 @@ -dsg: - zmq_num_threads: 2 - zmq_poll_time_ms: 10 -min_mesh_separation_s: 0.5 -frontend_mesh_separation_s: 0.5 -topology_visualizer_ns: "~/topology_visualizer" -enable_reconstruction_output_queue: true -disable_timer_output: true -timing_disabled: false diff --git a/hydra_ros/hydra_utils_cmake b/hydra_ros/hydra_utils_cmake deleted file mode 100644 index df32fb65..00000000 --- a/hydra_ros/hydra_utils_cmake +++ /dev/null @@ -1,91 +0,0 @@ -find_package( - catkin REQUIRED - COMPONENTS dynamic_reconfigure - geometry_msgs - image_transport - kimera_pgmo - nodelet - pose_graph_tools - roscpp - tf2_eigen - tf2_ros - visualization_msgs - glog_catkin -) -find_package(OpenCV REQUIRED) -find_package(cv_bridge REQUIRED) - -catkin_package( - CATKIN_DEPENDS - dynamic_reconfigure - geometry_msgs - image_transport - kimera_pgmo - nodelet - pose_graph_tools - roscpp - tf2_eigen - tf2_ros - visualization_msgs - DEPENDS - INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} - LIBRARIES ${PROJECT_NAME} -) - -target_include_directories( - ${PROJECT_NAME} - PUBLIC include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - PRIVATE ${OpenCV_INCLUDE_DIRS} -) -target_link_libraries( - ${PROJECT_NAME} - PUBLIC yaml-cpp ${catkin_LIBRARIES} spark_dsg::spark_dsg - PRIVATE ${OpenCV_LIBRARIES} gtsam -) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) - -add_executable(hydra_visualizer_node src/hydra_visualizer_node.cpp) -target_link_libraries(hydra_visualizer_node ${PROJECT_NAME}) - -add_executable(scene_graph_logger_node src/scene_graph_logger_node.cpp) -target_link_libraries(scene_graph_logger_node ${PROJECT_NAME}) - -add_executable(rotate_tf_node src/rotate_tf_node.cpp) -target_link_libraries(rotate_tf_node ${catkin_LIBRARIES}) -target_include_directories(rotate_tf_node PUBLIC ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME}_nodelet src/mask_nodelet.cpp) -target_include_directories( - ${PROJECT_NAME}_nodelet PUBLIC ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} - ${cv_bridge_INCLUDE_DIRS} -) -target_link_libraries( - ${PROJECT_NAME}_nodelet - ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} - ${cv_bridge_LIBRARIES} -) - -install( - TARGETS ${PROJECT_NAME} - ${PROJECT_NAME}_nodelet - hydra_visualizer_node - rotate_tf_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - -catkin_install_python( - PROGRAMS - nodes/csv_to_tf - nodes/odom_to_tf - DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/hydra_ros/include/hydra_ros/config/ros_parser.h b/hydra_ros/include/hydra_ros/config/ros_parser.h deleted file mode 100644 index d656788e..00000000 --- a/hydra_ros/include/hydra_ros/config/ros_parser.h +++ /dev/null @@ -1,200 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include - -#include - -namespace config_parser { - -namespace detail { - -template ::value, bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, const std::string& name, T& value) { - return nh.getParam(name, value); -} - -template , - typename std::enable_if::value, bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, - const std::string& name, - std::vector& value) { - return nh.getParam(name, value); -} - -template ::value, bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, - const std::string& name, - std::map& value) { - if (!nh.hasParam(name)) { - return false; - } - - value.clear(); - nh.getParam(name, value); - return true; -} - -template -void convertFromInt(int placeholder, T& value) { - if (sizeof(T) < sizeof(int)) { - // avoid overflow on uint16_t and smaller - constexpr const int lo_value = static_cast(std::numeric_limits::min()); - constexpr const int hi_value = static_cast(std::numeric_limits::max()); - placeholder = std::clamp(placeholder, lo_value, hi_value); - // TODO(nathan) think about warning - } - - value = static_cast(placeholder); -} - -template >, - std::is_integral>::value, - bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, const std::string& name, T& value) { - int placeholder = 0; - if (!nh.getParam(name, placeholder)) { - return false; - } - - convertFromInt(placeholder, value); - return true; -} - -template , - typename std::enable_if>, - std::is_integral>::value, - bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, - const std::string& name, - std::vector& value) { - std::vector placeholders; - if (!nh.getParam(name, placeholders)) { - return false; - } - - for (const int placeholder : placeholders) { - T new_value; - convertFromInt(placeholder, new_value); - value.push_back(new_value); - } - - return true; -} - -template ::value, bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, - const std::string& name, - std::set& value) { - std::vector placeholders; - if (!readRosParam(nh, name, placeholders)) { - return false; - } - - value.clear(); - value.insert(placeholders.begin(), placeholders.end()); - return true; -} - -template ::value, bool>::type = true> -bool readRosParam(const ros::NodeHandle& nh, const std::string& name, T& value) { - std::string placeholder; - if (!readRosParam(nh, name, placeholder)) { - return false; - } - - readConfigEnumFromString(placeholder, value); - return true; -} - -// adl indirection -struct read_ros_param_fn { - template - constexpr auto operator()(const ros::NodeHandle& nh, - const std::string& name, - T& val) const -> decltype(readRosParam(nh, name, val)) { - return readRosParam(nh, name, val); - } -}; - -} // namespace detail - -namespace { - -constexpr const auto& readRosParam = detail::static_const; - -} // namespace - -class RosParserImpl { - public: - RosParserImpl(); - - explicit RosParserImpl(const ros::NodeHandle& nh); - - RosParserImpl(const ros::NodeHandle& nh, const std::string& name); - - RosParserImpl(const RosParserImpl& other) = default; - - RosParserImpl& operator=(const RosParserImpl& other) = default; - - ~RosParserImpl() = default; - - RosParserImpl child(const std::string& new_name) const; - - std::vector children() const; - - inline std::string name() const { return nh_.resolveName(name_); } - - template - bool parse(T& value, const Logger*) const { - return ::config_parser::readRosParam(nh_, name_, value); - } - - private: - ros::NodeHandle nh_; - std::string name_; -}; - -using RosParser = Parser; - -} // namespace config_parser diff --git a/hydra_ros/include/hydra_ros/config/ros_utilities.h b/hydra_ros/include/hydra_ros/config/ros_utilities.h deleted file mode 100644 index 6f4cd619..00000000 --- a/hydra_ros/include/hydra_ros/config/ros_utilities.h +++ /dev/null @@ -1,138 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include - -#include - -#include "hydra_ros/config/ros_parser.h" - -namespace config_parser { - -template -Config load_from_ros(const std::string& ns, Logger::Ptr logger = nullptr) { - RosParser parser(std::make_unique(ros::NodeHandle(ns))); - if (logger) { - parser.setLogger(logger); - } - - Config config; - ConfigVisitor::visit_config(parser, config); - return config; -} - -template -Config load_from_ros_nh(const ros::NodeHandle& nh, - const std::string& ns = "", - Logger::Ptr logger = nullptr) { - RosParser parser(std::make_unique(nh, ns)); - if (logger) { - parser.setLogger(logger); - } - - Config config; - ConfigVisitor::visit_config(parser, config); - return config; -} - -} // namespace config_parser - -namespace hydra { - -struct HydraParamLogger : config_parser::Logger { - inline void log_missing(const std::string& message) const override { - LOG(INFO) << message; - } -}; - -template -struct ConfigStructName { - static std::string get() { return "Unknown Config"; } -}; - -template -Config load_config(const ros::NodeHandle& nh, - const std::string& ns = "", - bool verbose = true, - int dump_verbosity = 5) { - auto logger = std::make_shared(); - auto config = - config_parser::load_from_ros_nh(nh, ns, verbose ? logger : nullptr); - - if (verbose) { - const auto name = ConfigStructName::get(); - std::string filler(name.size() + 50, '='); - VLOG(dump_verbosity) << name << std::endl << filler << std::endl << config; - } - - return config; -} - -} // namespace hydra - -namespace Eigen { - -template -bool readRosParam(const ros::NodeHandle& nh, - const std::string& name, - Matrix& value) { - std::vector raw_values; - config_parser::readRosParam(nh, name, raw_values); - if (raw_values.empty()) { - return false; - } - - if (raw_values.size() != static_cast(N)) { - std::stringstream ss; - ss << "invalid param length: " << raw_values.size() << " != " << N; - throw std::domain_error(ss.str()); - } - - for (int i = 0; i < N; ++i) { - value(i) = raw_values[i]; - } - - return true; -} - -} // namespace Eigen - -#define DECLARE_STRUCT_NAME(name) \ - template <> \ - struct ConfigStructName { \ - static std::string get() { return #name; } \ - } diff --git a/hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h b/hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h deleted file mode 100644 index d5b58981..00000000 --- a/hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h +++ /dev/null @@ -1,91 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include - -#include "hydra_ros/pipeline/ros_backend.h" -#include "hydra_ros/pipeline/ros_frontend.h" -#include "hydra_ros/pipeline/ros_reconstruction.h" - -namespace hydra { - -struct HydraRosConfig { - bool enable_lcd = false; - bool use_ros_backend = false; - bool do_reconstruction = true; - bool enable_frontend_output = true; - double frontend_mesh_separation_s = 0.0; -}; - -struct HydraRosPipeline { - explicit HydraRosPipeline(const ros::NodeHandle& nh, int robot_id = 0); - - void start(); - - void stop(); - - void save(const std::string& output_path); - - void bowCallback(const pose_graph_tools::BowQueries::ConstPtr& msg); - - void sendFrontendOutput(const DynamicSceneGraph& graph, - const BackendInput& backend_input, - uint64_t timestamp_ns); - - void sendFrontendGraph(const DynamicSceneGraph& graph, uint64_t timestamp_ns); - - ros::NodeHandle nh; - - HydraRosConfig config; - RobotPrefixConfig prefix; - SharedDsgInfo::Ptr frontend_dsg; - SharedDsgInfo::Ptr backend_dsg; - SharedModuleState::Ptr shared_state; - - std::shared_ptr reconstruction; - std::shared_ptr frontend; - std::shared_ptr backend; - std::shared_ptr backend_visualizer; - std::shared_ptr lcd; - - std::unique_ptr dsg_sender; - ros::Publisher mesh_graph_pub; - ros::Publisher mesh_update_pub; - ros::Subscriber bow_sub; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/pipeline/ros_backend.h b/hydra_ros/include/hydra_ros/pipeline/ros_backend.h deleted file mode 100644 index 79e55d0e..00000000 --- a/hydra_ros/include/hydra_ros/pipeline/ros_backend.h +++ /dev/null @@ -1,122 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/utils/dsg_streaming_interface.h" - -namespace hydra { - -using kimera_pgmo::KimeraPgmoMesh; -using pose_graph_tools::PoseGraph; - -class RosBackend : public BackendModule { - public: - using Policy = - message_filters::sync_policies::ApproximateTime; - using Sync = message_filters::Synchronizer; - - RosBackend(const ros::NodeHandle& nh, - const RobotPrefixConfig& prefix, - const SharedDsgInfo::Ptr& dsg, - const SharedDsgInfo::Ptr& backend_dsg, - const SharedModuleState::Ptr& state); - - ~RosBackend(); - - void inputCallback(const kimera_pgmo::KimeraPgmoMesh::ConstPtr& mesh, - const pose_graph_tools::PoseGraph::ConstPtr& deformation_graph); - - void poseGraphCallback(const pose_graph_tools::PoseGraph::ConstPtr& msg); - - protected: - void publishOutputs(const pcl::PolygonMesh& mesh, size_t timestamp_ns) const; - - void publishDeformationGraphViz() const; - - void publishPoseGraphViz() const; - - void publishUpdatedMesh(const pcl::PolygonMesh& mesh, size_t timestamp_ns) const; - - protected: - ros::NodeHandle nh_; - std::list pose_graph_queue_; - kimera_pgmo::KimeraPgmoMesh::ConstPtr latest_mesh_msg_; - - ros::Subscriber pose_graph_sub_; - std::unique_ptr> deformation_graph_sub_; - std::unique_ptr> mesh_sub_; - std::unique_ptr sync_; -}; - -class RosBackendVisualizer { - public: - RosBackendVisualizer(const ros::NodeHandle& nh, - const BackendConfig& config, - const RobotPrefixConfig& prefix); - - virtual ~RosBackendVisualizer() = default; - - void publishOutputs(const DynamicSceneGraph& graph, - const kimera_pgmo::DeformationGraph& dgraph, - size_t timestamp_ns); - - protected: - virtual void publishPoseGraph(const DynamicSceneGraph& graph, - const kimera_pgmo::DeformationGraph& dgraph) const; - - virtual void publishDeformationGraphViz(const kimera_pgmo::DeformationGraph& dgraph, - size_t timestamp_ns) const; - - protected: - ros::NodeHandle nh_; - BackendConfig config_; - RobotPrefixConfig prefix_; - - ros::Publisher mesh_mesh_edges_pub_; - ros::Publisher pose_mesh_edges_pub_; - ros::Publisher pose_graph_pub_; - - // Hack for temporary removal of label flickering - size_t last_zmq_pub_time_; - - std::unique_ptr zmq_sender_; - std::unique_ptr dsg_sender_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/pipeline/ros_frontend.h b/hydra_ros/include/hydra_ros/pipeline/ros_frontend.h deleted file mode 100644 index 69ba0585..00000000 --- a/hydra_ros/include/hydra_ros/pipeline/ros_frontend.h +++ /dev/null @@ -1,130 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/utils/dsg_streaming_interface.h" -#include "hydra_ros/utils/semantic_ros_publishers.h" - -namespace hydra { - -using hydra_msgs::ActiveLayer; -using hydra_msgs::ActiveMesh; -using pose_graph_tools::PoseGraph; - -using ObjectCloudPub = SemanticRosPublishers; -using MeshVertexCloud = MeshSegmenter::MeshVertexCloud; -using LabelIndices = MeshSegmenter::LabelIndices; - -struct ROSFrontendConfig { - bool enable_active_mesh_pub = false; - bool enable_segmented_mesh_pub = false; - std::string world_frame = "world"; - std::string sensor_frame = "left_cam"; - bool use_latest_tf = true; - bool use_posegraph_pos = true; -}; - -template -void visit_config(const Visitor& v, ROSFrontendConfig& config) { - v.visit("enable_active_mesh_pub", config.enable_active_mesh_pub); - v.visit("enable_segmented_mesh_pub", config.enable_segmented_mesh_pub); - v.visit("world_frame", config.world_frame); - v.visit("sensor_frame", config.sensor_frame); - v.visit("use_latest_tf", config.use_latest_tf); - v.visit("use_posegraph_pos", config.use_posegraph_pos); -} - -struct RosFrontend : public FrontendModule { - using Policy = - message_filters::sync_policies::ApproximateTime; - using Sync = message_filters::Synchronizer; - - RosFrontend(const ros::NodeHandle& nh, - const RobotPrefixConfig& prefix, - const SharedDsgInfo::Ptr& dsg, - const SharedModuleState::Ptr& state); - - ~RosFrontend(); - - void inputCallback(const ActiveLayer::ConstPtr& places, - const ActiveMesh::ConstPtr& mesh); - - void poseGraphCallback(const PoseGraph::ConstPtr& pose_graph); - - void publishActiveVertices(const MeshSegmenter::MeshVertexCloud& vertices, - const MeshSegmenter::IndicesVector& indices, - const MeshSegmenter::LabelIndices&) const; - - void publishObjectClouds(const MeshSegmenter::MeshVertexCloud& vertices, - const MeshSegmenter::IndicesVector&, - const MeshSegmenter::LabelIndices& label_indices) const; - - std::optional getLatestPosition() const; - - std::optional getLatestPositionTf( - const ros::Time& time_to_use) const; - - ros::NodeHandle nh_; - ROSFrontendConfig ros_config_; - std::list pose_graph_queue_; - - std::unique_ptr> places_sub_; - std::unique_ptr> mesh_sub_; - std::unique_ptr sync_; - - ros::Subscriber pose_graph_sub_; - - tf2_ros::Buffer buffer_; - std::unique_ptr tf_listener_; - - ros::Publisher active_vertices_pub_; - std::unique_ptr segmented_vertices_pub_; -}; - -} // namespace hydra - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, ROSFrontendConfig) diff --git a/hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h b/hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h deleted file mode 100644 index 091bfce3..00000000 --- a/hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h +++ /dev/null @@ -1,50 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include - -namespace hydra { - -struct DsgAgentSolver : lcd::DsgRegistrationSolver { - DsgAgentSolver() = default; - - virtual ~DsgAgentSolver() = default; - - lcd::DsgRegistrationSolution solve(const DynamicSceneGraph& dsg, - const lcd::DsgRegistrationInput& match, - NodeId query_agent_id) const override; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h b/hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h deleted file mode 100644 index eb932299..00000000 --- a/hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h +++ /dev/null @@ -1,126 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/visualizer/topology_server_visualizer.h" - -namespace hydra { - -struct RosReconstructionConfig { - bool visualize_reconstruction = true; - std::string topology_visualizer_ns = "~"; - bool publish_mesh = false; - bool enable_output_queue = false; - double pointcloud_separation_s = 0.1; - double tf_wait_duration_s = 0.1; - std::string kimera_extrinsics_file = ""; - double tf_buffer_size_s = 30.0; -}; - -template -void visit_config(const Visitor& v, RosReconstructionConfig& config) { - v.visit("visualize_reconstruction", config.visualize_reconstruction); - v.visit("topology_visualizer_ns", config.topology_visualizer_ns); - v.visit("publish_reconstruction_mesh", config.publish_mesh); - v.visit("enable_reconstruction_output_queue", config.enable_output_queue); - v.visit("pointcloud_separation_s", config.pointcloud_separation_s); - v.visit("tf_wait_duration_s", config.tf_wait_duration_s); - v.visit("kimera_extrinsics_file", config.kimera_extrinsics_file); - v.visit("tf_buffer_size_s", config.tf_buffer_size_s); -} - -} // namespace hydra - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, RosReconstructionConfig); - -namespace hydra { - -using pose_graph_tools::PoseGraph; - -class RosReconstruction : public ReconstructionModule { - public: - using Pointcloud = pcl::PointCloud; - using PointcloudQueue = InputQueue; - - RosReconstruction(const ros::NodeHandle& nh, - const RobotPrefixConfig& prefix, - const OutputQueue::Ptr& output_queue = nullptr); - - virtual ~RosReconstruction(); - - void handlePointcloud(const Pointcloud::ConstPtr& cloud); - - void handlePoseGraph(const PoseGraph::ConstPtr& pose_graph); - - bool handleFreespaceSrv(hydra_msgs::QueryFreespace::Request& req, - hydra_msgs::QueryFreespace::Response& res); - - protected: - void pointcloudSpin(); - - void visualize(const ReconstructionOutput& output); - - ros::NodeHandle nh_; - RosReconstructionConfig ros_config_; - - ros::Subscriber pcl_sub_; - ros::Subscriber pose_graph_sub_; - std::unique_ptr buffer_; - std::unique_ptr tf_listener_; - - PointcloudQueue pointcloud_queue_; - std::unique_ptr pointcloud_thread_; - std::unique_ptr last_time_received_; - std::mutex pose_graph_mutex_; - std::list pose_graphs_; - - // visualizer - std::unique_ptr visualizer_; - ros::Publisher mesh_pub_; - - // freespace query - ros::ServiceServer freespace_server_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h b/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h deleted file mode 100644 index b79436b6..00000000 --- a/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h +++ /dev/null @@ -1,95 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include - -#include - -namespace hydra { - -class DsgSender { - public: - explicit DsgSender(const ros::NodeHandle& nh, - const std::string& timer_name = "publish_dsg", - bool publish_mesh = false, - double min_mesh_separation_s = 0.0); - - void sendGraph(const DynamicSceneGraph& graph, const ros::Time& stamp) const; - - private: - ros::NodeHandle nh_; - ros::Publisher pub_; - ros::Publisher mesh_pub_; - mutable std::optional last_mesh_time_ns_; - - std::string timer_name_; - bool publish_mesh_; - double min_mesh_separation_s_; -}; - -class DsgReceiver { - public: - using LogCallback = std::function; - - explicit DsgReceiver(const ros::NodeHandle& nh); - - DsgReceiver(const ros::NodeHandle& nh, const LogCallback& cb); - - inline DynamicSceneGraph::Ptr graph() const { return graph_; } - - inline bool updated() const { return has_update_; } - - inline void clearUpdated() { has_update_ = false; } - - private: - void handleUpdate(const hydra_msgs::DsgUpdate::ConstPtr& msg); - - void handleMesh(const mesh_msgs::TriangleMeshStamped::ConstPtr& msg); - - ros::NodeHandle nh_; - ros::Subscriber sub_; - ros::Subscriber mesh_sub_; - - bool has_update_; - DynamicSceneGraph::Ptr graph_; - std::unique_ptr mesh_; - - std::unique_ptr log_callback_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/utils/node_utilities.h b/hydra_ros/include/hydra_ros/utils/node_utilities.h deleted file mode 100644 index d56a0f12..00000000 --- a/hydra_ros/include/hydra_ros/utils/node_utilities.h +++ /dev/null @@ -1,72 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include - -namespace hydra { - -enum class ExitMode { CLOCK, SERVICE, NORMAL }; - -struct ServiceFunctor { - ServiceFunctor() : should_exit(false) {} - - bool callback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - should_exit = true; - return true; - } - - bool should_exit; -}; - -inline bool haveClock() { - return ros::TopicManager::instance()->getNumPublishers("/clock"); -} - -ExitMode getExitMode(const ros::NodeHandle& nh); - -void spinWhileClockPresent(); - -void spinUntilExitRequested(); - -std::string configureTimers(const ros::NodeHandle& nh); - -void saveTimingInformation(const std::string& output_path); - -void spinAndWait(const ros::NodeHandle& nh); - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/utils/semantic_ros_publishers.h b/hydra_ros/include/hydra_ros/utils/semantic_ros_publishers.h deleted file mode 100644 index 9683d0b4..00000000 --- a/hydra_ros/include/hydra_ros/utils/semantic_ros_publishers.h +++ /dev/null @@ -1,99 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -// originally part of kimera_batch_dsg -#include - -#include -#include - -namespace hydra { - -template -class SemanticRosPublishers { - typedef std::unordered_map SemanticRosPubs; - - public: - /** - * @brief SemanticRosPublishers Publishes a msg in a different ROS topic - * defined by the semantic label. This is a convenience class - * to allow for publishing msgs of the same type in different ROS topics. - * @param topic_name String to be prepended to all the ROS topics advertised. - * @param nh_private Node handle to create the ROS publishers. - */ - SemanticRosPublishers(const std::string& topic_name, - const ros::NodeHandle& nh_private) - : topic_name_(topic_name), nh_private_(nh_private), pubs_() {} - virtual ~SemanticRosPublishers() = default; - - /** publish Publishes a given ROS msg for a given semantic_label under the - * same ROS topic. - * If the given semantic_label has not been published before, a new ROS topic - * is advertised with the name given by topic_name_. - */ - void publish(const LabelType& semantic_label, const MsgType& msg) { - // Publish centroids for each semantic label - const auto& pub_it = pubs_.find(semantic_label); - if (pub_it == pubs_.end()) { - pubs_[semantic_label] = nh_private_.advertise( - topic_name_ + "_semantic_label_" + std::to_string(semantic_label), 1, true); - } - // Publish semantic pointcloud - pubs_.at(semantic_label).publish(msg); - } - - /** - * Derive the topic name from the label. - */ - std::string getTopic(const LabelType& semantic_label) { - const auto& pub_it = pubs_.find(semantic_label); - if (pub_it == pubs_.end()) { - pubs_[semantic_label] = nh_private_.advertise( - topic_name_ + "_semantic_label_" + std::to_string(semantic_label), 1, true); - } - return pubs_.at(semantic_label).getTopic(); - } - - std::string get_prefix() { return topic_name_; } - - private: - /// Prepended string to the ROS topic that is to be advertised. - std::string topic_name_; - - ros::NodeHandle nh_private_; - SemanticRosPubs pubs_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h b/hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h deleted file mode 100644 index ef15f08c..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h +++ /dev/null @@ -1,50 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include - -#include "hydra_ros/visualizer/visualizer_types.h" - -namespace hydra { -namespace dsg_utils { - -std_msgs::ColorRGBA makeColorMsg(const NodeColor& color, double alpha = 1.0); - -NodeColor getRgbFromHls(double hue, double luminance, double saturation); - -NodeColor interpolateColorMap(const ColormapConfig& config, double ratio); - -} // namespace dsg_utils -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h b/hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h deleted file mode 100644 index f7902e99..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h +++ /dev/null @@ -1,88 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include - -#include "hydra_ros/visualizer/dsg_visualizer_plugin.h" - -namespace hydra { - -class RvizMeshPlugin : public DsgVisualizerPlugin { - public: - RvizMeshPlugin(const ros::NodeHandle& nh, const std::string& name); - - virtual ~RvizMeshPlugin() = default; - - void draw(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - void reset(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - private: - std::string name_; - ros::Publisher mesh_pub_; - bool published_mesh_; -}; - -class PgmoMeshPlugin : public DsgVisualizerPlugin { - public: - PgmoMeshPlugin(const ros::NodeHandle& nh, const std::string& name); - - virtual ~PgmoMeshPlugin() = default; - - void draw(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - void reset(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - protected: - ros::Publisher mesh_pub_; -}; - -class VoxbloxMeshPlugin : public DsgVisualizerPlugin { - public: - VoxbloxMeshPlugin(const ros::NodeHandle& nh, const std::string& name); - - virtual ~VoxbloxMeshPlugin() = default; - - void draw(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - void reset(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - protected: - ros::Publisher mesh_pub_; - voxblox::BlockIndexList curr_blocks_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h b/hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h deleted file mode 100644 index 6dd573d0..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h +++ /dev/null @@ -1,59 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include - -namespace hydra { - -class DsgVisualizerPlugin { - public: - using Ptr = std::shared_ptr; - - DsgVisualizerPlugin(const ros::NodeHandle& nh, const std::string& name) - : nh_(nh, name) {} - - virtual ~DsgVisualizerPlugin() = default; - - virtual void draw(const std_msgs::Header& header, const DynamicSceneGraph& graph) = 0; - - virtual void reset(const std_msgs::Header& header, - const DynamicSceneGraph& graph) = 0; - - protected: - ros::NodeHandle nh_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h b/hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h deleted file mode 100644 index 7745f2a9..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h +++ /dev/null @@ -1,243 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include - -#include -#include - -#include "hydra_ros/visualizer/dsg_visualizer_plugin.h" -#include "hydra_ros/visualizer/visualizer_types.h" - -namespace hydra { - -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; - -template -class ConfigManager { - public: - using Ptr = std::shared_ptr>; - using Server = dynamic_reconfigure::Server; - - ConfigManager(const ros::NodeHandle& nh, const std::string& ns) - : nh_(nh, ns), changed_(true) { - server_ = std::make_unique(nh_); - server_->setCallback(boost::bind(&ConfigManager::update, this, _1, _2)); - } - - bool hasChange() { return changed_; } - - void clearChangeFlag() { changed_ = false; } - - const Config& get() const { return config_; }; - - private: - void update(Config& config, uint32_t) { - config_ = config; - changed_ = true; - } - - ros::NodeHandle nh_; - - bool changed_; - Config config_; - - std::unique_ptr server_; -}; - -void clearPrevMarkers(const std_msgs::Header& header, - const std::set& curr_nodes, - const std::string& ns, - std::set& prev_nodes, - MarkerArray& msg); - -class DynamicSceneGraphVisualizer { - public: - using DynamicLayerConfigManager = ConfigManager; - using VisualizerConfigManager = ConfigManager; - using LayerConfigManager = ConfigManager; - using ColormapConfigManager = ConfigManager; - - DynamicSceneGraphVisualizer(const ros::NodeHandle& nh, - const DynamicSceneGraph::LayerIds& layer_ids); - - virtual ~DynamicSceneGraphVisualizer() = default; - - void addPlugin(const std::shared_ptr& plugin) { - plugins_.push_back(plugin); - } - - void start(bool periodic_redraw = false); - - bool redraw(); - - inline void setGraphUpdated() { need_redraw_ = true; } - - void setGraph(const DynamicSceneGraph::Ptr& scene_graph, bool need_reset = true); - - void reset(); - - protected: - virtual void resetImpl(const std_msgs::Header& header, MarkerArray& msg); - - virtual void redrawImpl(const std_msgs::Header& header, MarkerArray& msg); - - virtual bool hasConfigChanged() const; - - virtual void clearConfigChangeFlags(); - - virtual void drawLayer(const std_msgs::Header& header, - const SceneGraphLayer& layer, - const LayerConfig& config, - MarkerArray& msg); - - virtual void drawLayerMeshEdges(const std_msgs::Header& header, - LayerId layer_id, - const std::string& ns, - MarkerArray& msg); - - protected: - void deleteMultiMarker(const std_msgs::Header& header, - const std::string& ns, - MarkerArray& msg); - - void addMultiMarkerIfValid(const Marker& marker, MarkerArray& msg); - - void setupConfigs(const DynamicSceneGraph::LayerIds& layer_ids); - - void displayLoop(const ros::WallTimerEvent&); - - void deleteLayer(const std_msgs::Header& header, - const SceneGraphLayer& layer, - MarkerArray& msg); - - inline std::string getDynamicNodeNamespace(char layer_prefix) const { - return dynamic_node_ns_prefix_ + layer_prefix; - } - - inline std::string getDynamicEdgeNamespace(char layer_prefix) const { - return dynamic_edge_ns_prefix_ + layer_prefix; - } - - inline std::string getDynamicLabelNamespace(char layer_prefix) const { - return dynamic_label_ns_prefix_ + layer_prefix; - } - - inline std::string getLayerNodeNamespace(LayerId layer) const { - return node_ns_prefix_ + std::to_string(layer); - } - - inline std::string getLayerEdgeNamespace(LayerId layer) const { - return edge_ns_prefix_ + std::to_string(layer); - } - - inline std::string getLayerLabelNamespace(LayerId layer) const { - return label_ns_prefix_ + std::to_string(layer); - } - - inline std::string getLayerBboxNamespace(LayerId layer) const { - return bbox_ns_prefix_ + std::to_string(layer); - } - - inline std::string getLayerBboxEdgeNamespace(LayerId layer) const { - return bbox_ns_prefix_ + "edges_" + std::to_string(layer); - } - - private: - const DynamicLayerConfig& getConfig(LayerId layer); - - void drawDynamicLayer(const std_msgs::Header& header, - const DynamicSceneGraphLayer& layer, - const DynamicLayerConfig& config, - const VisualizerConfig& viz_config, - size_t viz_idx, - MarkerArray& msg); - - void deleteLabel(const std_msgs::Header& header, char prefix, MarkerArray& msg); - - void deleteDynamicLayer(const std_msgs::Header& header, - char prefix, - MarkerArray& msg); - - void drawDynamicLayers(const std_msgs::Header& header, MarkerArray& msg); - - protected: - DynamicSceneGraph::Ptr scene_graph_; - - ros::NodeHandle nh_; - - bool need_redraw_; - bool periodic_redraw_; - std::string world_frame_; - std::string visualizer_ns_; - std::string visualizer_layer_ns_; - - ros::WallTimer visualizer_loop_timer_; - - const std::string node_ns_prefix_ = "layer_nodes_"; - const std::string edge_ns_prefix_ = "layer_edges_"; - const std::string label_ns_prefix_ = "layer_labels_"; - const std::string bbox_ns_prefix_ = "layer_bounding_boxes_"; - const std::string mesh_edge_ns_ = "mesh_object_connections"; - const std::string interlayer_edge_ns_prefix_ = "interlayer_edges_"; - const LayerId mesh_edge_source_layer_ = DsgLayers::OBJECTS; - const std::string dynamic_node_ns_prefix_ = "dynamic_nodes_"; - const std::string dynamic_edge_ns_prefix_ = "dynamic_edges_"; - const std::string dynamic_label_ns_prefix_ = "dynamic_label_"; - - std::set published_multimarkers_; - std::map> prev_labels_; - std::map> curr_labels_; - - std::map layer_configs_; - VisualizerConfigManager::Ptr visualizer_config_; - ColormapConfigManager::Ptr places_colormap_; - - ros::Publisher dsg_pub_; - - std::map dynamic_configs_; - - std::list> plugins_; - - std::set published_dynamic_labels_; - - ros::Publisher dynamic_layers_viz_pub_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h b/hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h deleted file mode 100644 index 8bcf61d2..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h +++ /dev/null @@ -1,123 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include -#include -#include - -#include "hydra_ros/visualizer/visualizer_types.h" - -namespace hydra { - -using hydra_ros::GvdVisualizerConfig; -using CompressedNodeMap = std::unordered_map; - -class MarkerGroupPub { - public: - using MarkerCallback = std::function; - using ArrayCallback = std::function; - - explicit MarkerGroupPub(const ros::NodeHandle& nh); - - void publish(const std::string& name, const MarkerCallback& marker) const; - - void publish(const std::string& name, const ArrayCallback& marker) const; - - private: - mutable ros::NodeHandle nh_; - mutable std::map pubs_; -}; - -enum class GvdVisualizationMode : int { - DEFAULT = hydra_ros::GvdVisualizer_DEFAULT, - DISTANCE = hydra_ros::GvdVisualizer_DISTANCE, - BASIS_POINTS = hydra_ros::GvdVisualizer_BASIS_POINTS, -}; - -GvdVisualizationMode getModeFromString(const std::string& mode); - -visualization_msgs::Marker makeGvdMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const voxblox::Layer& layer); - -visualization_msgs::Marker makeSurfaceVoxelMarker( - const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const voxblox::Layer& layer); - -visualization_msgs::Marker makeErrorMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const voxblox::Layer& lhs, - const voxblox::Layer& rhs, - double threshold); - -visualization_msgs::Marker makeEsdfMarker( - const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const voxblox::Layer& layer); - -visualization_msgs::Marker makeBlocksMarker( - const voxblox::Layer& layer, double scale); - -visualization_msgs::Marker makeBlocksMarker( - const voxblox::Layer& layer, double scale); - -visualization_msgs::Marker makeMeshBlocksMarker(const voxblox::MeshLayer& layer, - double scale); - -visualization_msgs::MarkerArray makeGvdGraphMarkers(const places::GvdGraph& graph, - const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const std::string& ns, - size_t marker_id = 0); - -visualization_msgs::MarkerArray showGvdClusters( - const places::GvdGraph& graph, - const CompressedNodeMap& clusters, - const std::unordered_map& remapping, - const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const std::string& ns, - size_t marker_id = 0); - -visualization_msgs::MarkerArray makePlaceSpheres(const std_msgs::Header& header, - const SceneGraphLayer& layer, - const std::string& ns, - double alpha = 0.1); - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h b/hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h deleted file mode 100644 index 827a435f..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h +++ /dev/null @@ -1,147 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include -#include - -#include "hydra_ros/visualizer/gvd_visualization_utilities.h" -#include "hydra_ros/visualizer/visualizer_types.h" -#include "hydra_ros/visualizer/visualizer_utilities.h" - -namespace hydra { - -using hydra_ros::GvdVisualizerConfig; -using RqtMutexPtr = std::unique_ptr; - -struct TopologyVisualizerConfig { - std::string world_frame = "world"; - std::string topology_marker_ns = "topology_graph"; - bool show_block_outlines = false; - bool use_gvd_block_outlines = false; - double outline_scale = 0.01; - - ColormapConfig colormap; - GvdVisualizerConfig gvd; - VisualizerConfig graph; - LayerConfig graph_layer; -}; - -template -void visit_config(const Visitor& v, TopologyVisualizerConfig& config) { - v.visit("world_frame", config.world_frame); - v.visit("topology_marker_ns", config.topology_marker_ns); - v.visit("show_block_outlines", config.show_block_outlines); - v.visit("use_gvd_block_outlines", config.use_gvd_block_outlines); - v.visit("outline_scale", config.outline_scale); -} - -class TopologyServerVisualizer { - public: - explicit TopologyServerVisualizer(const std::string& ns); - - virtual ~TopologyServerVisualizer() = default; - - void visualize(const SceneGraphLayer& graph, - const places::GvdGraph& gvd_graph, - const voxblox::Layer& gvd, - const voxblox::Layer& tsdf, - uint64_t timestamp_ns, - const voxblox::MeshLayer* mesh = nullptr); - - void visualizeError(const voxblox::Layer& lhs, - const voxblox::Layer& rhs, - double threshold, - uint64_t timestamp_ns); - - void visualizeExtractor(uint64_t timestamp_ns, - const places::CompressionGraphExtractor& extractor); - - private: - void visualizeGraph(const std_msgs::Header& header, const SceneGraphLayer& graph); - - void visualizeGvd(const std_msgs::Header& header, - const voxblox::Layer& gvd) const; - - void visualizeGvdGraph(const std_msgs::Header& header, - const places::GvdGraph& gvd_graph) const; - - void visualizeBlocks(const std_msgs::Header& header, - const voxblox::Layer& gvd, - const voxblox::Layer& tsdf, - const voxblox::MeshLayer* mesh) const; - - void publishGraphLabels(const std_msgs::Header& header, const SceneGraphLayer& graph); - - void publishFreespace(const std_msgs::Header& header, const SceneGraphLayer& graph); - - void gvdConfigCb(GvdVisualizerConfig& config, uint32_t level); - - void graphConfigCb(LayerConfig& config, uint32_t level); - - void colormapCb(ColormapConfig& config, uint32_t level); - - void setupConfigServers(); - - template - void startRqtServer(const std::string& config_ns, - std::unique_ptr>& server, - const Callback& callback) { - ros::NodeHandle config_nh(nh_, config_ns); - server.reset(new dynamic_reconfigure::Server(config_nh)); - server->setCallback(boost::bind(callback, this, _1, _2)); - } - - private: - ros::NodeHandle nh_; - std::unique_ptr pubs_; - - TopologyVisualizerConfig config_; - std::set previous_labels_; - size_t previous_spheres_; - - mutable bool published_gvd_graph_; - mutable bool published_gvd_clusters_; - - std::unique_ptr> gvd_config_server_; - std::unique_ptr> graph_config_server_; - std::unique_ptr> colormap_server_; -}; - -} // namespace hydra - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, TopologyVisualizerConfig) diff --git a/hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h b/hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h deleted file mode 100644 index fc9f0248..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h +++ /dev/null @@ -1,111 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include - -#include "hydra_ros/visualizer/dsg_visualizer_plugin.h" -#include "hydra_ros/visualizer/visualizer_types.h" - -namespace hydra { - -struct PMGraphPluginConfig { - explicit PMGraphPluginConfig(const ros::NodeHandle& nh); - - double mesh_edge_scale = 0.005; - double mesh_edge_alpha = 0.8; - double mesh_marker_scale = 0.1; - double mesh_marker_alpha = 0.8; - NodeColor leaf_color; - NodeColor interior_color; - NodeColor invalid_color; - LayerConfig layer_config; -}; - -class MeshPlaceConnectionsPlugin : public DsgVisualizerPlugin { - public: - MeshPlaceConnectionsPlugin(const ros::NodeHandle& nh, const std::string& name); - - virtual ~MeshPlaceConnectionsPlugin() = default; - - void draw(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - void reset(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - protected: - ros::Publisher marker_pub_; - PMGraphPluginConfig config_; - bool published_nodes_; - bool published_edges_; -}; - -class PlacesFactorGraphViz { - public: - using Ptr = std::shared_ptr; - - explicit PlacesFactorGraphViz(const ros::NodeHandle& nh); - - virtual ~PlacesFactorGraphViz() = default; - - void draw(char vertex_prefix, - const SceneGraphLayer& places, - const MinimumSpanningTreeInfo& mst_info, - const kimera_pgmo::DeformationGraph& deformations); - - protected: - ros::NodeHandle nh_; - ros::Publisher marker_pub_; - PMGraphPluginConfig config_; -}; - -class PlaceParentsPlugin : public DsgVisualizerPlugin { - public: - PlaceParentsPlugin(const ros::NodeHandle& nh, const std::string& name); - - virtual ~PlaceParentsPlugin() = default; - - void draw(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - void reset(const std_msgs::Header& header, const DynamicSceneGraph& graph) override; - - protected: - ros::Publisher marker_pub_; - PMGraphPluginConfig config_; - bool published_nodes_; - bool published_edges_; -}; - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/visualizer_types.h b/hydra_ros/include/hydra_ros/visualizer/visualizer_types.h deleted file mode 100644 index 00a42914..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/visualizer_types.h +++ /dev/null @@ -1,64 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include -#include -#include -#include - -namespace hydra { - -using LayerConfig = hydra_ros::LayerVisualizerConfig; -using DynamicLayerConfig = hydra_ros::DynamicLayerVisualizerConfig; -using VisualizerConfig = hydra_ros::VisualizerConfig; -using ColormapConfig = hydra_ros::ColormapConfig; -using NodeColor = SemanticNodeAttributes::ColorVector; - -inline double getZOffset(double z_offset_scale, - const VisualizerConfig& visualizer_config) { - if (visualizer_config.collapse_layers) { - return 0.0; - } - - return z_offset_scale * visualizer_config.layer_z_step; -} - -inline double getZOffset(const LayerConfig& config, - const VisualizerConfig& visualizer_config) { - return getZOffset(config.z_offset_scale, visualizer_config); -} - -} // namespace hydra diff --git a/hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h b/hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h deleted file mode 100644 index 5a889c7d..00000000 --- a/hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h +++ /dev/null @@ -1,199 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include -#include - -#include "hydra_ros/visualizer/visualizer_types.h" - -namespace hydra { - -using ColorFunction = std::function; -using EdgeColorFunction = - std::function; - -visualization_msgs::Marker makeDeleteMarker(const std_msgs::Header& header, - size_t id, - const std::string& ns); - -visualization_msgs::Marker makeLayerWireframeBoundingBoxes( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeEdgesToBoundingBoxes( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeBoundingBoxMarker( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphNode& node, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeTextMarker(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphNode& node, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeTextMarkerNoHeight( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphNode& node, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeCentroidMarkers( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeCentroidMarkers( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const ColormapConfig& colors); - -visualization_msgs::Marker makeCentroidMarkers( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const ColorFunction& color_func); - -visualization_msgs::MarkerArray makeGraphEdgeMarkers( - const std_msgs::Header& header, - const DynamicSceneGraph& scene_graph, - const std::map& configs, - const VisualizerConfig& visualizer_config, - const std::string& ns); - -visualization_msgs::Marker makeMeshEdgesMarker( - const std_msgs::Header& header, - const LayerConfig& config, - const VisualizerConfig& visualizer_config, - const DynamicSceneGraph& graph, - const SceneGraphLayer& layer, - const std::string& ns); - -visualization_msgs::MarkerArray makeGvdWireframe( - const std_msgs::Header& header, - const LayerConfig& config, - const VisualizerConfig& visualizer_config, - const SceneGraphLayer& layer, - const std::string& ns, - const ColormapConfig& colors, - size_t marker_id = 0); - -visualization_msgs::MarkerArray makeGvdWireframe(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& gvd, - const std::string& ns, - const ColorFunction& color_func, - size_t marker_id = 0); - -visualization_msgs::Marker makeLayerEdgeMarkers( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const NodeColor& color, - const std::string& ns); - -visualization_msgs::Marker makeLayerEdgeMarkers( - const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const EdgeColorFunction& color_func); - -visualization_msgs::Marker makeDynamicCentroidMarkers( - const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const NodeColor& color, - const std::string& ns, - size_t marker_id = 0); - -visualization_msgs::Marker makeDynamicCentroidMarkers( - const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - double layer_offset_scale, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const ColorFunction& color_func, - size_t marker_id = 0); - -visualization_msgs::MarkerArray makeDynamicGraphEdgeMarkers( - const std_msgs::Header& header, - const DynamicSceneGraph& graph, - const std::map& configs, - const std::map& dynamic_configs, - const VisualizerConfig& visualizer_config, - const std::string& ns_prefix); - -visualization_msgs::Marker makeDynamicEdgeMarkers( - const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const NodeColor& color, - const std::string& ns, - size_t marker_id); - -visualization_msgs::Marker makeDynamicLabelMarker( - const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - size_t marker_id); - -} // namespace hydra diff --git a/hydra_ros/launch/datasets/mp3d.launch b/hydra_ros/launch/datasets/mp3d.launch deleted file mode 100644 index 6a442a68..00000000 --- a/hydra_ros/launch/datasets/mp3d.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/datasets/sidpac.launch b/hydra_ros/launch/datasets/sidpac.launch deleted file mode 100644 index 5dfe51ec..00000000 --- a/hydra_ros/launch/datasets/sidpac.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/datasets/simmons_a1.launch b/hydra_ros/launch/datasets/simmons_a1.launch deleted file mode 100644 index 69f70c8d..00000000 --- a/hydra_ros/launch/datasets/simmons_a1.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/datasets/simmons_jackal.launch b/hydra_ros/launch/datasets/simmons_jackal.launch deleted file mode 100644 index f72c6678..00000000 --- a/hydra_ros/launch/datasets/simmons_jackal.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/datasets/uhumans2.launch b/hydra_ros/launch/datasets/uhumans2.launch deleted file mode 100644 index a35285df..00000000 --- a/hydra_ros/launch/datasets/uhumans2.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/hydra.launch b/hydra_ros/launch/hydra.launch deleted file mode 100644 index c96a3288..00000000 --- a/hydra_ros/launch/hydra.launch +++ /dev/null @@ -1,166 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/hydra_robot.launch b/hydra_ros/launch/hydra_robot.launch deleted file mode 100644 index 680bbc59..00000000 --- a/hydra_ros/launch/hydra_robot.launch +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/hydra_robot_t265.launch b/hydra_ros/launch/hydra_robot_t265.launch deleted file mode 100644 index 68f89d40..00000000 --- a/hydra_ros/launch/hydra_robot_t265.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/misc/dsg_optimizer.launch b/hydra_ros/launch/misc/dsg_optimizer.launch deleted file mode 100644 index 42df1e71..00000000 --- a/hydra_ros/launch/misc/dsg_optimizer.launch +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/places/gvd_validator.launch b/hydra_ros/launch/places/gvd_validator.launch deleted file mode 100644 index cad6ca5d..00000000 --- a/hydra_ros/launch/places/gvd_validator.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/places/hydra_topology.launch b/hydra_ros/launch/places/hydra_topology.launch deleted file mode 100644 index 103dfb6d..00000000 --- a/hydra_ros/launch/places/hydra_topology.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/places/hydra_topology_sidpac.launch b/hydra_ros/launch/places/hydra_topology_sidpac.launch deleted file mode 100644 index 3a5f9ec9..00000000 --- a/hydra_ros/launch/places/hydra_topology_sidpac.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/places/hydra_topology_simmons_jackal.launch b/hydra_ros/launch/places/hydra_topology_simmons_jackal.launch deleted file mode 100644 index cd54d8f4..00000000 --- a/hydra_ros/launch/places/hydra_topology_simmons_jackal.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/places/hydra_topology_uhumans2.launch b/hydra_ros/launch/places/hydra_topology_uhumans2.launch deleted file mode 100644 index d41b47ee..00000000 --- a/hydra_ros/launch/places/hydra_topology_uhumans2.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/data_sources/a1_pointcloud.launch b/hydra_ros/launch/utils/data_sources/a1_pointcloud.launch deleted file mode 100644 index fe45bd70..00000000 --- a/hydra_ros/launch/utils/data_sources/a1_pointcloud.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/data_sources/d455_pointcloud.launch b/hydra_ros/launch/utils/data_sources/d455_pointcloud.launch deleted file mode 100644 index 8f91ad77..00000000 --- a/hydra_ros/launch/utils/data_sources/d455_pointcloud.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch b/hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch deleted file mode 100644 index d017f626..00000000 --- a/hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/data_sources/sidpac_pointcloud.launch b/hydra_ros/launch/utils/data_sources/sidpac_pointcloud.launch deleted file mode 100644 index af39a366..00000000 --- a/hydra_ros/launch/utils/data_sources/sidpac_pointcloud.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/data_sources/uhumans2_pointcloud.launch b/hydra_ros/launch/utils/data_sources/uhumans2_pointcloud.launch deleted file mode 100644 index 7b87c0c7..00000000 --- a/hydra_ros/launch/utils/data_sources/uhumans2_pointcloud.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/gt_pose/robot_gt_pose.launch b/hydra_ros/launch/utils/gt_pose/robot_gt_pose.launch deleted file mode 100644 index b5fb3c1e..00000000 --- a/hydra_ros/launch/utils/gt_pose/robot_gt_pose.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch b/hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch deleted file mode 100644 index 5b3541bc..00000000 --- a/hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/hydra_streaming_visualizer.launch b/hydra_ros/launch/utils/hydra_streaming_visualizer.launch deleted file mode 100644 index 57a82171..00000000 --- a/hydra_ros/launch/utils/hydra_streaming_visualizer.launch +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/hydra_visualizer.launch b/hydra_ros/launch/utils/hydra_visualizer.launch deleted file mode 100644 index a340f9f6..00000000 --- a/hydra_ros/launch/utils/hydra_visualizer.launch +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/a1_static_tfs.xml b/hydra_ros/launch/utils/includes/a1_static_tfs.xml deleted file mode 100644 index 5d820f80..00000000 --- a/hydra_ros/launch/utils/includes/a1_static_tfs.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/a1_t265_camera_static_tfs.xml b/hydra_ros/launch/utils/includes/a1_t265_camera_static_tfs.xml deleted file mode 100644 index b4145ab8..00000000 --- a/hydra_ros/launch/utils/includes/a1_t265_camera_static_tfs.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml b/hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml deleted file mode 100644 index 6d217fcf..00000000 --- a/hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/hydra_visualizer_base.xml b/hydra_ros/launch/utils/includes/hydra_visualizer_base.xml deleted file mode 100644 index 37b93428..00000000 --- a/hydra_ros/launch/utils/includes/hydra_visualizer_base.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/hydra_visualizer_params.xml b/hydra_ros/launch/utils/includes/hydra_visualizer_params.xml deleted file mode 100644 index a3f2e449..00000000 --- a/hydra_ros/launch/utils/includes/hydra_visualizer_params.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml b/hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml deleted file mode 100644 index c26894cc..00000000 --- a/hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/rgbd_to_pointcloud.xml b/hydra_ros/launch/utils/includes/rgbd_to_pointcloud.xml deleted file mode 100644 index 79fb4cf6..00000000 --- a/hydra_ros/launch/utils/includes/rgbd_to_pointcloud.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/sparkal1_static_tfs.xml b/hydra_ros/launch/utils/includes/sparkal1_static_tfs.xml deleted file mode 100644 index 038e0b74..00000000 --- a/hydra_ros/launch/utils/includes/sparkal1_static_tfs.xml +++ /dev/null @@ -1,105 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/sparkal2_static_tfs.xml b/hydra_ros/launch/utils/includes/sparkal2_static_tfs.xml deleted file mode 100644 index 97c8255e..00000000 --- a/hydra_ros/launch/utils/includes/sparkal2_static_tfs.xml +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/includes/uhumans2_static_tfs.xml b/hydra_ros/launch/utils/includes/uhumans2_static_tfs.xml deleted file mode 100644 index 3681ef88..00000000 --- a/hydra_ros/launch/utils/includes/uhumans2_static_tfs.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/odom_to_tf.launch b/hydra_ros/launch/utils/odom_to_tf.launch deleted file mode 100644 index a99eaaf6..00000000 --- a/hydra_ros/launch/utils/odom_to_tf.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/hydra_ros/launch/utils/publish_rotation_transform.launch b/hydra_ros/launch/utils/publish_rotation_transform.launch deleted file mode 100644 index 55670b14..00000000 --- a/hydra_ros/launch/utils/publish_rotation_transform.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - $(arg centroid) - - - diff --git a/hydra_ros/launch/utils/scene_graph_logger.launch b/hydra_ros/launch/utils/scene_graph_logger.launch deleted file mode 100644 index 0efdd896..00000000 --- a/hydra_ros/launch/utils/scene_graph_logger.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/hydra_ros/nodelet_plugins.xml b/hydra_ros/nodelet_plugins.xml deleted file mode 100644 index e40ba611..00000000 --- a/hydra_ros/nodelet_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - nodelet to mask an arbitrary image - - diff --git a/hydra_ros/package.xml b/hydra_ros/package.xml deleted file mode 100644 index 8afd85bf..00000000 --- a/hydra_ros/package.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - hydra_ros - 1.0.0 - hydra ros nodes - - Nathan Hughes - Yun Chang - Nathan Hughes - - BSD - - catkin - roscpp - dynamic_reconfigure - std_msgs - geometry_msgs - visualization_msgs - hydra_msgs - pcl_ros - tf2_eigen - tf2_ros - cv_bridge - nodelet - image_transport - glog_catkin - - gtsam - spark_dsg - pose_graph_tools - kimera_pgmo - kimera_semantics_ros - voxblox_ros - hydra - - image_proc - depth_image_proc - rviz - - - - - - diff --git a/hydra_ros/resources/a1_center_mask_roi_640.png b/hydra_ros/resources/a1_center_mask_roi_640.png deleted file mode 100644 index d0852d37eb7a5049d268ee0962a82a79c5b8c1ad..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1733 zcmeAS@N?(olHy`uVBq!ia0y~yU}|7sV0^#<6krh8)oj7Qz}DmG;uuoF`1YVB(-Sv_ z!ww(*_b+p9w=3CT+Pz97Q{DBJ=ftw0C38oaqv0@`2u3plg(bno5RLi`BDQ*exS#q@ zS@gVR^{IUCi^hk1Pw{(Se0%8Zsr6nz7q{%6db{=bl=)LWJwCKQSdUX*!(RRA_ZII{ z^#b=o>%31LJmm9JJ#opO2~#$+{N4OXJE%|Ki~Q9137f>e*iSw09m%z8SxxjQuM4`T z+L!XM++AH0a>~0&E_9#9Q&mTQ?fH{G^(_#e((hU6enI-w_9c6iUWh)84vG`}()CGK zGoPc%{pYkP<}7;`*La_jZn_t^PxUFQW4`A6NuQb)e4oPaSs8a>_NnqEHu^6@pBe|r ziGSJjNmSFHyDIu;*OYeFz3XdSPAzUyJ7vDaMl)kcjnk>-rnFPyOKj9LR@6A0DsE~! z#lFNwEo0Rj{im9a$xkaZ`8lJsk9$|TU5K-cJ*C{_7r;0DQ^x|~DTkNZD8Ar%nxo0j zVdY{OeTuosFR*V~(f$R=Qx-41qxVALX^Q4{ZYvMV$WwQl`hxhT6>VRzcuMoqJ323P zo+fCTb6fdXhM(HohGOSVda~pWi`rpb&L-Hs`I@dKE-%xjL;WOpj0?Zt+VCj_@dWF@mdKI;Vst0MC)^ Apa1{> diff --git a/hydra_ros/resources/a1_left_mask_roi.png b/hydra_ros/resources/a1_left_mask_roi.png deleted file mode 100644 index b1a71cc9c09525d7d3d73b3af89c716d9b9d3daf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3510 zcmeH~e@qnV8OLY$?X1mB=Jp!XPCNB#ulB+<2SYENDQj-a9x7G5I~G)w6$b2S4;HL! z3meyD1qKg|a&>#!gJh+wf`TFkp4_Q<*)DH~G?$d_Ei5dXM#b40`LU$>X8FURH(fh5 z_Wo$EY5(g#{`1X~&-3JYp7)veeUdp|RJfUbhJA)02)ba)rfmd4>ZWnx%eoTDueF@e z`Qd_1c{@CYzx$zj^e+k|HY%R<=V>xL%MXWRUca@05fuzy3ooPM;iCfd8(#sJ! zZo`dB6ngWi>*+L}-i@cv;puYtf9%7MeYxdcN1RmF)`VgyYSD;)>3{ICW`u9*LSCus462I??#*0K~!?M&KI7gul!6v8HxeaQ|+^XG;zvhU#n#Par9a}Ks zqjU46x#diA2XDKX`VbOY@iOJeEn=D<)ikajKc<(d3uil5UJ{#^{FS#-FF>M72>d=} zfW$l>h@|dAqK^;Eq(YE5$Omqwx*)NcH(v<`IAjP9XFfPfVOMx>)}07TTX#9H8INCDT)ZXN*xQf5( zA8xOkARDjEm1TPK>TjmrhbS`b9chgV+S@BF9G>Fu)F$j~y86)Tj9TD-_#r-XbktE1 zBO8y-iCObP@a;nmM*W80e5E{wuiYa>C8Ij`(_g$X7B5ZrQ<^92yJ{83;=9uWl(r{q zz7`xPXtxh0$}*eus%Pgi9O>?r+bO5uJ;vc4-WE&Q!hx*7wy}o`M%$+4w$i9}JGH`l zJ2(vZ(a^nmr&=f~cZGnQI?j8CI6Uc(ytTr!p)y9@6V4dc{>p|b8MP=}^^x}H$L5xB z)yJC5&gLt<&+GV1rkRoAr>>})=xz7z(Tqg_W2PLjDlUjF)c_JzdZ2FW~W z{bQ=F+LIwF>v-R=#(^Wp&bc=du*JNO}8AxfeKmm{mJmG9JA<{b+;I{F1RLj+Wihs8ea?@GiQ1f=qWyt0Ai7T^ks6*mPjQ*c3%y z5fwXY>87+|u&Y6`RV(jucop3_Nv8XxS0Q@9TFz72ptKR9Cc&lOi6IlWXnYhyFN#VI z-5Dhtr_lGFnCTv=Ob_E-`qQ~?I;n@vpjSlYk>GlhQ9DeZ>L7~Ni{ExCUEHrs&nAsd z*1N2wpSmu%N|?f0Wq|9PK@Rc8fHVSK4sg(& zxN5qelz&36nQ0wkEd$g+!R27?*C-`{uZg$zu$Dn8 zM{tQuVT}^xI;YVg@y101bQOW~)GFaM=(2{p1L_kU zxD)Lr)yLh65|1WZ%UHz*U0Z?HOLg$5$(=Z5Qir%TQDSsSeu2g#Pt50OY!h$v7}9-I z6_3_7B;GZtN4WAyqG`JIWmfS)S3b~+nUmGcwndqyag})EoI&3n89b_NNW5cG+qljN zqG_^~VwEB2$^%*n^K5nV=%Q?(agOK-nbZ-^S_w3f=^8Sm2Pt2i$X<}|3W$Q(L*o|C zDuUp7%C6%@c}hU9L)<{)Q=GLN1TRu%9Vg{60kuKwqH*^Z;Qchd%2~@maDa-;5ZO!e zuzFId^5z*g4`pZmm&V|*sez}%BKZX3-LZ0`#Gxv1bZp7j+62c z0bPc84~_ddYbgl!e*xYFf`e2fMhq><^#WRgxSYn2vp&YY7*V$%?-kHDA>KjbcFtM? zf)^;eju+*P0y+b65siB}>kbgSL`9~Fx+VDs0=fzD8#KPgS+|4W2b6D`sGMlsT+^(N zcb$NkhQxcULP5NR#-!*uZ&LM9Zv);u=3AExp|k%}_~X3)UhZ)hX1%%Olji=V=M`IZ Z>33fFjqj(`|M(#*$S>S9xM5Gr-vOtcJcIxM diff --git a/hydra_ros/rviz/a1.rviz b/hydra_ros/rviz/a1.rviz deleted file mode 100644 index 306852e1..00000000 --- a/hydra_ros/rviz/a1.rviz +++ /dev/null @@ -1,254 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 1246 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /sparkal1/semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Image - Enabled: false - Image Topic: /cam_d455/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Color - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: false - - Class: rviz/Image - Enabled: false - Image Topic: /cam_d455/depth/image_rect_raw - Max Value: 1 - Median window: 3000 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 28.469799041748047 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 9.487153053283691 - Y: -0.244480162858963 - Z: 10.57595157623291 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.2953983545303345 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.668578147888184 - Saved: ~ -Window Geometry: - Color: - collapsed: false - Depth: - collapsed: false - Displays: - collapsed: true - Height: 1543 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001fe00000569fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000569000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e0074006900630073000000003d000002b50000001600fffffffb0000000600520047004200000002cf000000c80000000000000000fb0000000a0044006500700074006800000002ed000000ac0000001600fffffffb0000000600520047004201000002ec000000ad0000000000000000fb0000000a0043006f006c006f0072000000003d000005690000001600fffffffb0000000a0049006d00610067006500000002d0000000c70000000000000000fb0000000a0044006500700074006801000002d0000000c70000000000000000000000010000010f00000569fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000569000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000075f0000056900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1887 - X: 67 - Y: 27 diff --git a/hydra_ros/rviz/a1_topdown.rviz b/hydra_ros/rviz/a1_topdown.rviz deleted file mode 100644 index 67ce0e00..00000000 --- a/hydra_ros/rviz/a1_topdown.rviz +++ /dev/null @@ -1,249 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 402 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /sparkal1/semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /cam_d455/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Color - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /cam_d455/depth/image_rect_raw - Max Value: 1 - Median window: 3000 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Angle: 0.45000120997428894 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 61.3039436340332 - Target Frame: - Value: TopDownOrtho (rviz) - X: 4.109123706817627 - Y: 3.330336332321167 - Saved: ~ -Window Geometry: - Color: - collapsed: false - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1543 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001fe00000569fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000021d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e00740069006300730100000260000001a20000001600fffffffb0000000600520047004200000002cf000000c80000000000000000fb0000000a0044006500700074006800000002ed000000ac0000001600fffffffb0000000600520047004201000002ec000000ad0000000000000000fb0000000a0043006f006c006f007201000004080000019e0000001600fffffffb0000000a0049006d00610067006500000002d0000000c70000000000000000fb0000000a0044006500700074006801000002d0000000c70000000000000000000000010000010f0000056bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000056b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000055b0000056900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1887 - X: 1953 - Y: 27 diff --git a/hydra_ros/rviz/dsg_optimizer.rviz b/hydra_ros/rviz/dsg_optimizer.rviz deleted file mode 100644 index 359e5718..00000000 --- a/hydra_ros/rviz/dsg_optimizer.rviz +++ /dev/null @@ -1,191 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 75 - Name: Displays - Property Tree Widget: - Expanded: - - /Dsg Dynamic Layers1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 696 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /kimera_dsg_visualizer/dynamic_layers_viz - Name: Dsg Dynamic Layers - Namespaces: - dynamic_edges_a: true - dynamic_label_a: true - dynamic_nodes_a: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /incremental_dsg_builder_node/pm_graph_viz/mesh_connection_plugin/places_mesh_connection_viz - Name: Place-Mesh Graph Connections - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /kimera_dsg_visualizer/dsg_markers - Name: Scene Graph - Namespaces: - interlayer_edges_3_2: true - layer_bounding_boxes_2: true - layer_edges_3: true - layer_nodes_2: true - layer_nodes_3: true - mesh_object_connections: true - Queue Size: 100 - Value: true - - Class: rviz_mesh_plugin/TriangleMesh - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: DSG Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /kimera_dsg_visualizer/dsg_mesh - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /dsg_optimizer_node/pgmo/posegraph_viewer/loop_edges - Name: InlierLcEdges - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /dsg_optimizer_node/pgmo/posegraph_viewer/odometry_edges - Name: OdomEdges - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /dsg_optimizer_node/pgmo/posegraph_viewer/rejected_loop_edges - Name: OutlierLcEdges - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /dsg_optimizer_node/pgmo/deformation_graph_mesh_mesh - Name: MeshMeshEdges - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /dsg_optimizer_node/pgmo/deformation_graph_pose_mesh - Name: PoseMeshEdges - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 25.45623779296875 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 7.4949212074279785 - Y: -2.775383234024048 - Z: 2.9567534923553467 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6647979021072388 - Target Frame: left_cam - Value: Orbit (rviz) - Yaw: 2.611844062805176 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1025 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c700000340fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002da000002160000000000000000fb0000000a0049006d0061006700650000000500000002a30000000000000000fb000000100044006900730070006c006100790073010000003d00000340000000c900fffffffb000000160044006500700074006800200049006d006100670065000000019a0000009e0000000000000000fb0000001200520047004200200049006d00610067006500000001f1000000c20000000000000000fb00000024005300650067006d0065006e0074006100740069006f006e00200049006d006100670065000000027c000000fa0000000000000000000000010000025700000774fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000774000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000061fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005700000034000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1853 - X: 67 - Y: 27 diff --git a/hydra_ros/rviz/gvd_validator.rviz b/hydra_ros/rviz/gvd_validator.rviz deleted file mode 100644 index e31c1bb3..00000000 --- a/hydra_ros/rviz/gvd_validator.rviz +++ /dev/null @@ -1,189 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 725 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: VoxbloxMesh - Topic: /mesh_viz - Unreliable: false - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/surface_viz - Name: Surface Voxels - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/gvd_viz - Name: GVD - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/graph_viz - Name: Places - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/graph_label_viz - Name: Places (Labels) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/full/surface_viz - Name: Surface Voxels (Full) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/full/gvd_viz - Name: GVD (Full) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /gvd_validator/full/graph_viz - Name: Places (Full) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /gvd_validator/error_viz - Name: ESDF Error - Namespaces: - error_marker: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 11.935579299926758 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -12.340414047241211 - Y: 25.65089988708496 - Z: 0.7586337924003601 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.2053990662097931 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.1404342651367188 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006801000002a7000000f20000000000000000fb0000000600520047004201000002d6000000c30000000000000000fb0000001200530065006d0061006e007400690063007301000002fa0000009f0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_ros/rviz/hydra_streaming_visualizer.rviz b/hydra_ros/rviz/hydra_streaming_visualizer.rviz deleted file mode 100644 index 580e45b5..00000000 --- a/hydra_ros/rviz/hydra_streaming_visualizer.rviz +++ /dev/null @@ -1,272 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 142 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - cam_d455_accel_frame: - Value: true - cam_d455_accel_optical_frame: - Value: true - cam_d455_color_frame: - Value: true - cam_d455_color_optical_frame: - Value: true - cam_d455_depth_frame: - Value: true - cam_d455_depth_optical_frame: - Value: true - cam_d455_gyro_frame: - Value: true - cam_d455_imu_optical_frame: - Value: true - cam_d455_kalibr: - Value: true - cam_d455_link: - Value: true - cam_t265_accel_frame: - Value: true - cam_t265_accel_optical_frame: - Value: true - cam_t265_gyro_frame: - Value: true - cam_t265_imu_optical_frame: - Value: true - cam_t265_link: - Value: true - cam_t265_odom_frame: - Value: true - cam_t265_pose_frame: - Value: true - left_cam: - Value: true - world: - Value: true - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - cam_t265_odom_frame: - cam_t265_pose_frame: - cam_t265_link: - cam_t265_accel_frame: - cam_t265_accel_optical_frame: - {} - cam_t265_gyro_frame: - cam_t265_imu_optical_frame: - cam_d455_kalibr: - cam_d455_link: - cam_d455_accel_frame: - cam_d455_accel_optical_frame: - {} - cam_d455_color_frame: - cam_d455_color_optical_frame: - {} - cam_d455_depth_frame: - cam_d455_depth_optical_frame: - {} - cam_d455_gyro_frame: - cam_d455_imu_optical_frame: - {} - left_cam: - {} - Update Interval: 0 - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_labels_2: true - layer_labels_4: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /cam_d455/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 35.81574249267578 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 2.397207736968994 - Y: -7.912760257720947 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7353984713554382 - Target Frame: - Yaw: 3.2904016971588135 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f90000039dfc020000000efb0000001200530065006c0065006300740069006f006e000000003e0000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000000cb000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000010f000001610000001600fffffffb0000000a0049006d0061006700650000000235000000b60000000000000000fb0000000a0049006d0061006700650100000276000001650000001600fffffffb0000000a0049006d006100670065010000023d0000019e0000000000000000fb0000000a0049006d00610067006501000001b7000002240000000000000000fb0000000a0049006d006100670065010000021a000001c10000000000000000000000010000010f0000039dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e0000039d000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650000000000000007800000039300fffffffb0000000800540069006d00650100000000000004500000000000000000000005810000039d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 29 diff --git a/hydra_ros/rviz/hydra_streaming_visualizer_topdown.rviz b/hydra_ros/rviz/hydra_streaming_visualizer_topdown.rviz deleted file mode 100644 index 1a9ebb8e..00000000 --- a/hydra_ros/rviz/hydra_streaming_visualizer_topdown.rviz +++ /dev/null @@ -1,216 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 363 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: true - base_link: - Value: true - camera_accel_frame: - Value: true - camera_accel_optical_frame: - Value: true - camera_gyro_frame: - Value: true - camera_imu_optical_frame: - Value: true - camera_link: - Value: true - camera_odom_frame: - Value: true - camera_pose_frame: - Value: true - left_cam: - Value: true - map: - Value: true - world: - Value: true - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - camera_odom_frame: - camera_pose_frame: - camera_link: - camera_accel_frame: - camera_accel_optical_frame: - {} - camera_gyro_frame: - camera_imu_optical_frame: - {} - map: - world: - base_link: - left_cam: - {} - Update Interval: 0 - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_edges_4: true - layer_labels_2: true - layer_labels_4: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Angle: 1.4799994230270386 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 43.006595611572266 - Target Frame: - X: -2.711778402328491 - Y: 15.237112045288086 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1043 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000359fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e00000359000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650000000000000007800000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000780000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 1920 - Y: 0 diff --git a/hydra_ros/rviz/hydra_visualizer.rviz b/hydra_ros/rviz/hydra_visualizer.rviz deleted file mode 100644 index 73103e9d..00000000 --- a/hydra_ros/rviz/hydra_visualizer.rviz +++ /dev/null @@ -1,191 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 363 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: Mesh (Voxblox) - Queue Size: 10 - Topic: /hydra_visualizer/dsg_mesh/voxblox - Unreliable: false - Value: true - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh (TriangleMesh) - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /hydra_visualizer/mesh - Name: Mesh (MarkerBased) - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Mesh Subscribers - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - interlayer_edges_5_4: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_edges_4: true - layer_labels_4: true - layer_labels_5: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - layer_nodes_5: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 78.42807006835938 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -13.64610767364502 - Y: 12.250905990600586 - Z: 20.807790756225586 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: -0.04999962076544762 - Target Frame: - Yaw: 5.390016078948975 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1016 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000359fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e00000359000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000359fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e00000359000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_ros/rviz/mp3d.rviz b/hydra_ros/rviz/mp3d.rviz deleted file mode 100644 index fffbff79..00000000 --- a/hydra_ros/rviz/mp3d.rviz +++ /dev/null @@ -1,255 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /TF1/Frames1 - - /TF1/Tree1 - Splitter Ratio: 0.5 - Tree Height: 277 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: Segmentation Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: false - depth_camera_link: - Value: false - rgb_camera_link: - Value: false - semantic_camera_link: - Value: true - world: - Value: true - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - base_link: - depth_camera_link: - {} - rgb_camera_link: - {} - semantic_camera_link: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Input Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /habitat/agent_0/pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /habitat/agent_0/depth - Max Value: 30 - Median window: 5 - Min Value: 0 - Name: Depth Image - Normalize Range: false - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /habitat/agent_0/rgb - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /habitat/agent_0/semantic - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Segmentation Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Show Trail: false - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph - Namespaces: - interlayer_edges_4_3: true - layer_edges_3: true - layer_edges_4: true - layer_labels_4: true - layer_nodes_3: true - layer_nodes_4: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - - Class: rviz_mesh_plugin/TriangleMesh - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: TriangleMesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: true - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - Enabled: true - Global Options: - Background Color: 30; 30; 30 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 33.361881256103516 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -6.912068843841553 - Y: 3.2137718200683594 - Z: 3.5431013107299805 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6002027988433838 - Target Frame: left_cam - Yaw: 0.10009074956178665 - Saved: ~ -Window Geometry: - Depth Image: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001c700000337fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002da000002160000000000000000fb0000000a0049006d0061006700650000000500000002a30000000000000000fb000000100044006900730070006c006100790073010000003d00000152000000c900fffffffb000000160044006500700074006800200049006d0061006700650100000195000000a80000001600fffffffb0000001200520047004200200049006d0061006700650100000243000000970000001600fffffffb00000024005300650067006d0065006e0074006100740069006f006e00200049006d00610067006501000002e0000000940000001600ffffff000000010000025700000774fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000774000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000061fc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005b30000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB Image: - collapsed: false - Segmentation Image: - collapsed: false - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_ros/rviz/sidpac.rviz b/hydra_ros/rviz/sidpac.rviz deleted file mode 100644 index 728d4d08..00000000 --- a/hydra_ros/rviz/sidpac.rviz +++ /dev/null @@ -1,212 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 80 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Semantics -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_labels_2: true - layer_labels_4: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /azure/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /azure/depth_to_rgb/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 35.61590576171875 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -6.252310276031494 - Y: -5.647854328155518 - Z: 6.616474151611328 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.14539863169193268 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.300398826599121 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000210000003a2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000000d9000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e0074006900630073010000003b0000018b0000001600fffffffb0000000600520047004201000001cc000002110000001600fffffffb0000000a004400650070007400680000000304000000930000001600ffffff000000010000010f0000056bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000056b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004afc0100000002fb0000000800540069006d00650000000000000007800000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000056a000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_ros/rviz/sidpac_topdown.rviz b/hydra_ros/rviz/sidpac_topdown.rviz deleted file mode 100644 index 6d57371c..00000000 --- a/hydra_ros/rviz/sidpac_topdown.rviz +++ /dev/null @@ -1,207 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 397 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Semantics -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_edges_4: true - layer_labels_4: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /azure/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /azure/depth_to_rgb/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Angle: 3.5550038814544678 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 60.421165466308594 - Target Frame: - Value: TopDownOrtho (rviz) - X: -6.289322376251221 - Y: 0.6851611137390137 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1545 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001800000056bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000218000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e0074006900630073010000025b000001a40000001600fffffffb000000060052004700420100000405000001a30000001600fffffffb0000000a004400650070007400680000000304000000930000001600ffffff000000010000010f0000056bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000056b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005d90000056b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1887 - X: 1953 - Y: 27 diff --git a/hydra_ros/rviz/sparkal1.rviz b/hydra_ros/rviz/sparkal1.rviz deleted file mode 100644 index 8dc70d8f..00000000 --- a/hydra_ros/rviz/sparkal1.rviz +++ /dev/null @@ -1,253 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 601 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: RGB -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /sparkal1/oneformer/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /sparkal1/forward/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /sparkal1/forward/depth/image_rect_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /sparkal1/semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: sparkal1/map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 57.95964813232422 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -22.299163818359375 - Y: 10.510017395019531 - Z: -0.9869003295898438 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7147973775863647 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.778567790985107 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1545 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001970000056bfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e00740069006300730100000327000001360000001600fffffffb000000060052004700420100000463000001450000001600fffffffb0000000a0044006500700074006800000002ed000000ac0000001600fffffffb0000000600520047004201000002ec000000ad0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005c20000056b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1887 - X: 67 - Y: 27 diff --git a/hydra_ros/rviz/sparkal2.rviz b/hydra_ros/rviz/sparkal2.rviz deleted file mode 100644 index a45e50cb..00000000 --- a/hydra_ros/rviz/sparkal2.rviz +++ /dev/null @@ -1,252 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /Semantics1 - - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 320 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: false - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /sparkal2/forward/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: compressed - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /sparkal2/forward/depth/image_rect_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/Axes - Enabled: true - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /sparkal1/semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: sparkal2/map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 57.95964813232422 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -22.299163818359375 - Y: 10.510017395019531 - Z: -0.9869003295898438 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7147973775863647 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.778567790985107 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001970000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001c9000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e0074006900630073010000020a000000c00000001600fffffffb0000000600520047004201000002d0000000c90000001600fffffffb0000000a0044006500700074006800000002ed000000ac0000001600fffffffb0000000600520047004201000002ec000000ad0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000005e30000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_ros/rviz/uhumans2.rviz b/hydra_ros/rviz/uhumans2.rviz deleted file mode 100644 index 07faad17..00000000 --- a/hydra_ros/rviz/uhumans2.rviz +++ /dev/null @@ -1,321 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Scene Graph1/Scene Graph (Static)1 - - /Scene Graph1/Mesh (Voxblox)1 - - /Scene Graph1/Mesh (TriangleMesh)1 - - /Input1 - Splitter Ratio: 0.5 - Tree Height: 140 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: Semantics -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - interlayer_edges_5_4: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_edges_4: true - layer_labels_2: true - layer_labels_4: true - layer_labels_5: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - layer_nodes_5: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: Mesh (Voxblox) - Queue Size: 10 - Topic: /hydra_dsg_visualizer/dsg_mesh/voxblox - Unreliable: false - Value: true - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh (TriangleMesh) - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: true - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Input Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/depth_cam/mono/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/seg_cam/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/left_cam/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Name: Input - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /incremental_dsg_builder_node/pgmo/deformation_graph_mesh_mesh - Name: Mesh Edges - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /incremental_dsg_builder_node/pgmo/deformation_graph_pose_mesh - Name: Pose-Mesh Edges - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: false - Name: Factor Graph - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/mesh_block_viz - Name: Mesh Allocated Blocks - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/voxel_block_viz - Name: GVD Allocated Blocks - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/gvd_graph_viz - Name: GVD Graph - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/graph_viz - Name: Sparse GVD Graph - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/gvd_cluster_viz - Name: GVD Clusters - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: true - Name: Topology - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 87.75440979003906 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 8.067428588867188 - Y: 6.879875183105469 - Z: -1.1174240112304688 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6197975873947144 - Target Frame: - Yaw: 2.470393180847168 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000bfb0000001200530065006c0065006300740069006f006e000000003b0000018f0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680100000108000000bd0000001600fffffffb0000001200530065006d0061006e007400690063007301000001cb000000dc0000001600fffffffb0000000600520047004201000002ad000000ec0000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_ros/rviz/uhumans2_topdown.rviz b/hydra_ros/rviz/uhumans2_topdown.rviz deleted file mode 100644 index ae841ea2..00000000 --- a/hydra_ros/rviz/uhumans2_topdown.rviz +++ /dev/null @@ -1,314 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Input1 - - /Input1/Semantics1 - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: RGB -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dsg_markers - Name: Scene Graph (Static) - Namespaces: - interlayer_edges_3_2: true - interlayer_edges_4_3: true - layer_bounding_boxes_2: true - layer_bounding_boxes_edges_2: true - layer_edges_3: true - layer_edges_4: true - layer_labels_2: true - layer_labels_4: true - layer_nodes_2: true - layer_nodes_3: true - layer_nodes_4: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz - Name: Scene Graph (Dynamic) - Namespaces: - "dynamic_edges_\x00": true - "dynamic_label_\x00": true - "dynamic_nodes_\x00": true - Queue Size: 100 - Value: true - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: Mesh (Voxblox) - Topic: /hydra_dsg_visualizer/dsg_mesh/voxblox - Unreliable: false - Value: true - - Class: rviz_mesh_plugin/TriangleMesh - Cull Faces: true - Display Type: - Faces Alpha: 1 - Faces Color: 0; 255; 0 - Value: Vertex Color - Enabled: true - Mesh Buffer Size: 1 - Name: Mesh (TriangleMesh) - Show Normals: - Normals Alpha: 1 - Normals Color: 255; 0; 255 - Normals Scaling Factor: 0.10000000149011612 - Value: true - Show Wireframe: - Value: false - Wireframe Alpha: 1 - Wireframe Color: 0; 0; 0 - Topic: /hydra_dsg_visualizer/dsg_mesh/pgmo - Value: true - Enabled: true - Name: Scene Graph - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Input Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Image - Enabled: false - Image Topic: /tesse/depth_cam/mono/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/left_cam/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Name: Input - - Class: rviz/Group - Displays: - - Class: rviz/Marker - Enabled: true - Marker Topic: /incremental_dsg_builder_node/pgmo/deformation_graph_mesh_mesh - Name: Mesh Edges - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /incremental_dsg_builder_node/pgmo/deformation_graph_pose_mesh - Name: Pose-Mesh Edges - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: false - Name: Factor Graph - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/mesh_block_viz - Name: Mesh Allocated Blocks - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/voxel_block_viz - Name: GVD Allocated Blocks - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/gvd_graph_viz - Name: GVD Graph - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/graph_viz - Name: Sparse GVD Graph - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /incremental_dsg_builder_node/topology_visualizer/gvd_cluster_viz - Name: GVD Clusters - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: true - Name: Topology - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Angle: -4.740005016326904 - Class: rviz/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 60.25772476196289 - Target Frame: - Value: TopDownOrtho (rviz) - X: -1.1125832796096802 - Y: -6.463243007659912 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1543 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001a700000569fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000000c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680000000134000000730000001600fffffffb0000001200530065006d0061006e0074006900630073010000003d000002a70000001600fffffffb0000000600520047004201000002ea000002bc0000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005b20000056900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1887 - X: 1953 - Y: 27 diff --git a/hydra_ros/scripts/copy_static_tfs.py b/hydra_ros/scripts/copy_static_tfs.py deleted file mode 100755 index 7a683657..00000000 --- a/hydra_ros/scripts/copy_static_tfs.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python -# Copyright 2022, Massachusetts Institute of Technology. -# All Rights Reserved -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -# Research was sponsored by the United States Air Force Research Laboratory and -# the United States Air Force Artificial Intelligence Accelerator and was -# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views -# and conclusions contained in this document are those of the authors and should -# not be interpreted as representing the official policies, either expressed or -# implied, of the United States Air Force or the U.S. Government. The U.S. -# Government is authorized to reproduce and distribute reprints for Government -# purposes notwithstanding any copyright notation herein. -# -# -"""Copy static tfs from one uhumans2 bag to a launch file.""" -import rosbag -import argparse - -NODE_PREFIX = ' \n\n") - - with rosbag.Bag(args.bag_file, "r") as bag: - for topic, tf_msg, t in bag.read_messages(topics=["/tf_static"]): - for msg in tf_msg.transforms: - fout.write(NODE_PREFIX) - name = "bag_static_tf_{}".format(tf_idx) - tf_idx += 1 - fout.write(' name="{}"'.format(name)) - fout.write( - TF_STR.format( - x=msg.transform.translation.x, - y=msg.transform.translation.y, - z=msg.transform.translation.z, - qx=msg.transform.rotation.x, - qy=msg.transform.rotation.y, - qz=msg.transform.rotation.z, - qw=msg.transform.rotation.w, - parent=msg.header.frame_id, - child=msg.child_frame_id, - ) - ) - - fout.write("\n") - - -if __name__ == "__main__": - main() diff --git a/hydra_ros/scripts/decompress_uhumans2.py b/hydra_ros/scripts/decompress_uhumans2.py deleted file mode 100644 index 4c510960..00000000 --- a/hydra_ros/scripts/decompress_uhumans2.py +++ /dev/null @@ -1,118 +0,0 @@ -# Copyright 2022, Massachusetts Institute of Technology. -# All Rights Reserved -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -# Research was sponsored by the United States Air Force Research Laboratory and -# the United States Air Force Artificial Intelligence Accelerator and was -# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views -# and conclusions contained in this document are those of the authors and should -# not be interpreted as representing the official policies, either expressed or -# implied, of the United States Air Force or the U.S. Government. The U.S. -# Government is authorized to reproduce and distribute reprints for Government -# purposes notwithstanding any copyright notation herein. -# -# -#!/usr/bin/env python3 - -# Put this in a yaml file perhaps -ids = { # "apartment_scene": - # {"uHumans2_apartment_s1_00h.bag": '1kU_drpyG7glQ8pJyeztpiy214WbBEtbM', - # "uHumans2_apartment_s1_01h.bag": '1jp7HrRsfGbmC-z757wwXDEpgNPHw0SbK', - # "uHumans2_apartment_s1_02h.bag": '1ai2p6QVNFaPJfEFOOec6URp5Anu0MBoo' - # }, - # "office_scene": - # {"uHumans2_office_s1_00h.bag": '1CA_1Awu-bewJKpDrILzWok_H_6cOkGDb', - # "uHumans2_office_s1_06h.bag": '1zECekG47mlGafaJ84vCbwcx3Dz03NuvN', - # "uHumans2_office_s1_12h.bag": '1Of7s_QTE9nL1Hd69SFW1R5uDiJDiQrZr' - # }, - "subway_scene": { - "uHumans2_subway_s1_00h.bag": "1ChL1SW1tfZrCjn5XEf4GJG8nm_Cb5AEm", - "uHumans2_subway_s1_24h.bag": "1ifatqW3hzL9yo8m7Jt3BCqIr-6kzIols", - "uHumans2_subway_s1_36h.bag": "1xFG565R-9LKXC60Rfx-7fruy3BP4TrHl", - }, - "neighborhood_scene": { - "uHumans2_neighborhood_s1_00h.bag": "1p_Uv4RLbl1GtjRxu2tldopFRKmgg_Vsy", - "uHumans2_neighborhood_s1_24h.bag": "1LXloULyuohBzFLumBoScBMlFrT5nRPcE", - "uHumans2_neighborhood_s1_36h.bag": "1AwgGpqe2g12T2Lm4Nilz4EaNm2_OtfHL", - }, -} -url = "https://drive.google.com/uc?id=" - -import os -import errno - - -def run(args): - assert os.path.exists(args.dataset_dir) - for dataset_name in ids.keys(): - assert os.path.exists(os.path.join(args.dataset_dir, dataset_name)) - print("Decompressing Rosbags for dataset: %s" % dataset_name) - for rosbag_name in ids[dataset_name].keys(): - print("Decompressing rosbag: %s" % rosbag_name) - os.system( - "rosbag decompress %s" - % os.path.join(args.dataset_dir, dataset_name, rosbag_name) - ) - print("Done decompressing rosbag: %s" % rosbag_name) - - print("Done decompressing rosbags.") - - return True - - -def parser(): - import argparse - - basic_desc = "Decompress uHumans2 dataset." - - shared_parser = argparse.ArgumentParser( - add_help=True, description="{}".format(basic_desc) - ) - - output_opts = shared_parser.add_argument_group("output options") - output_opts.add_argument( - "--dataset_dir", - type=str, - help="Path to the directory where the datasets are.", - required=True, - ) - - main_parser = argparse.ArgumentParser(description="{}".format(basic_desc)) - sub_parsers = main_parser.add_subparsers(dest="subcommand") - sub_parsers.required = True - return shared_parser - - -import argcomplete -import sys - -if __name__ == "__main__": - parser = parser() - argcomplete.autocomplete(parser) - args = parser.parse_args() - # try: - if run(args): - sys.exit(os.EX_OK) - # except Exception as e: - # print("error: ", e) - # raise Exception("Main evaluation run failed.") diff --git a/hydra_ros/scripts/download_uhumans2.py b/hydra_ros/scripts/download_uhumans2.py deleted file mode 100644 index bae4b2a8..00000000 --- a/hydra_ros/scripts/download_uhumans2.py +++ /dev/null @@ -1,143 +0,0 @@ -# Copyright 2022, Massachusetts Institute of Technology. -# All Rights Reserved -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -# Research was sponsored by the United States Air Force Research Laboratory and -# the United States Air Force Artificial Intelligence Accelerator and was -# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views -# and conclusions contained in this document are those of the authors and should -# not be interpreted as representing the official policies, either expressed or -# implied, of the United States Air Force or the U.S. Government. The U.S. -# Government is authorized to reproduce and distribute reprints for Government -# purposes notwithstanding any copyright notation herein. -# -# -#!/usr/bin/env python3 - -import gdown - -# Put this in a yaml file perhaps -ids = { - "apartment_scene": { - "uHumans2_apartment_s1_00h.bag": "1kU_drpyG7glQ8pJyeztpiy214WbBEtbM", - "uHumans2_apartment_s1_01h.bag": "1jp7HrRsfGbmC-z757wwXDEpgNPHw0SbK", - "uHumans2_apartment_s1_02h.bag": "1ai2p6QVNFaPJfEFOOec6URp5Anu0MBoo", - }, - "office_scene": { - "uHumans2_office_s1_00h.bag": "1CA_1Awu-bewJKpDrILzWok_H_6cOkGDb", - "uHumans2_office_s1_06h.bag": "1zECekG47mlGafaJ84vCbwcx3Dz03NuvN", - "uHumans2_office_s1_12h.bag": "1Of7s_QTE9nL1Hd69SFW1R5uDiJDiQrZr", - }, - "subway_scene": { - "uHumans2_subway_s1_00h.bag": "1ChL1SW1tfZrCjn5XEf4GJG8nm_Cb5AEm", - "uHumans2_subway_s1_24h.bag": "1ifatqW3hzL9yo8m7Jt3BCqIr-6kzIols", - "uHumans2_subway_s1_36h.bag": "1xFG565R-9LKXC60Rfx-7fruy3BP4TrHl", - }, - "neighborhood_scene": { - "uHumans2_neighborhood_s1_00h.bag": "1p_Uv4RLbl1GtjRxu2tldopFRKmgg_Vsy", - "uHumans2_neighborhood_s1_24h.bag": "1LXloULyuohBzFLumBoScBMlFrT5nRPcE", - "uHumans2_neighborhood_s1_36h.bag": "1AwgGpqe2g12T2Lm4Nilz4EaNm2_OtfHL", - }, -} -url = "https://drive.google.com/uc?id=" - -# md5 = 'fa837a88f0c40c513d975104edf3da17' -# gdown.cached_download(url, output, md5=md5, postprocess=gdown.extractall) - -import os -import errno - - -def create_full_path_if_not_exists(filename): - if not os.path.exists(os.path.dirname(filename)): - try: - print("Creating non-existent path: %s" % filename) - os.makedirs(os.path.dirname(filename)) - except OSError as exc: # Guard against race condition - if exc.errno != errno.EEXIST: - print("Could not create inexistent filename: " + filename) - - -def ensure_dir(dir_path): - """Check if the path directory exists: if it does, returns true, - if not creates the directory dir_path and returns if it was successful""" - if not os.path.exists(dir_path): - os.makedirs(dir_path) - return True - - -def run(args): - assert ensure_dir(args.output_dir) - for dataset_name in ids.keys(): - assert ensure_dir(os.path.join(args.output_dir, dataset_name)) - for rosbag_name in ids[dataset_name].keys(): - print("Downloading rosbag: %s" % rosbag_name) - rosbag_url = url + ids[dataset_name][rosbag_name] - gdown.download( - rosbag_url, - os.path.join(args.output_dir, dataset_name, rosbag_name), - quiet=False, - ) - print("Done downloading dataset.") - - return True - - -def parser(): - import argparse - - basic_desc = "Download uHumans2 dataset in google drive." - - shared_parser = argparse.ArgumentParser( - add_help=True, description="{}".format(basic_desc) - ) - - input_opts = shared_parser.add_argument_group("input options") - output_opts = shared_parser.add_argument_group("output options") - - output_opts.add_argument( - "--output_dir", - type=str, - help="Path to the output directory where the datasets will be saved.", - required=True, - ) - - main_parser = argparse.ArgumentParser(description="{}".format(basic_desc)) - sub_parsers = main_parser.add_subparsers(dest="subcommand") - sub_parsers.required = True - return shared_parser - - -import argcomplete -import sys - -if __name__ == "__main__": - parser = parser() - argcomplete.autocomplete(parser) - args = parser.parse_args() - # try: - if run(args): - sys.exit(os.EX_OK) - # except Exception as e: - # print("error: ", e) - # raise Exception("Main evaluation run failed.") diff --git a/hydra_ros/scripts/run_robot.sh b/hydra_ros/scripts/run_robot.sh deleted file mode 100755 index 6fce2211..00000000 --- a/hydra_ros/scripts/run_robot.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -if [[ $# -eq 0 ]] ; then - echo 'Log path required!!' - exit 1 -fi - -if [[ -d $1 ]] ; then - echo 'Log path '$1' exists!' - read -p "Delete '$1' and continue? [y/N]: " -r - echo - - if [[ $REPLY =~ ^[Yy]$ ]] ; then - rm -rf $1 - else - exit 1 - fi -fi - - -# Make Hydra Folders -mkdir -p $1 -mkdir -p $1/frontend -mkdir -p $1/backend -mkdir -p $1/lcd -mkdir -p $1/pgmo -mkdir -p $1/topology - -# Make Log Directory -mkdir -p $1/logs - -roslaunch hydra_dsg_builder_ros hydra_robot.launch dsg_path:="$1" glog_to_file:=true glog_dir:="$1/logs" verbosity:=2 record_bag:=true exit_mode:=SERVICE enable_dsg_lcd:=true use_zmq_interface:=true diff --git a/hydra_ros/scripts/run_robot_t265.sh b/hydra_ros/scripts/run_robot_t265.sh deleted file mode 100755 index 8cd349af..00000000 --- a/hydra_ros/scripts/run_robot_t265.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -if [[ $# -eq 0 ]] ; then - echo 'Log path required!!' - exit 1 -fi - -if [[ -d $1 ]] ; then - echo 'Log path '$1' exists!' - read -p "Delete '$1' and continue? [y/N]: " -r - echo - - if [[ $REPLY =~ ^[Yy]$ ]] ; then - rm -rf $1 - else - exit 1 - fi -fi - - -# Make Hydra Folders -mkdir -p $1 -mkdir -p $1/frontend -mkdir -p $1/backend -mkdir -p $1/lcd -mkdir -p $1/pgmo -mkdir -p $1/topology - -# Make Log Directory -mkdir -p $1/logs - -roslaunch hydra_dsg_builder_ros hydra_robot_t265.launch dsg_path:="$1" glog_to_file:=true glog_dir:="$1/logs" verbosity:=2 record_bag:=true exit_mode:=SERVICE enable_dsg_lcd:=true use_zmq_interface:=true diff --git a/hydra_ros/scripts/scrape_kimera_extrinsics.py b/hydra_ros/scripts/scrape_kimera_extrinsics.py deleted file mode 100755 index 21504ffe..00000000 --- a/hydra_ros/scripts/scrape_kimera_extrinsics.py +++ /dev/null @@ -1,81 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2022, Massachusetts Institute of Technology. -# All Rights Reserved -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# -# Research was sponsored by the United States Air Force Research Laboratory and -# the United States Air Force Artificial Intelligence Accelerator and was -# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views -# and conclusions contained in this document are those of the authors and should -# not be interpreted as representing the official policies, either expressed or -# implied, of the United States Air Force or the U.S. Government. The U.S. -# Government is authorized to reproduce and distribute reprints for Government -# purposes notwithstanding any copyright notation herein. -from scipy.spatial.transform.rotation import Rotation as R -import numpy as np -import pathlib -import yaml -import sys - - -def main(): - """Scrape TF from kimera config and write to hydra format.""" - if len(sys.argv) < 3: - print("missing required arguments!", file=sys.stderr) - print( - "usage: scrape_kimera_extrinsics.py PATH_TO_KIMERA_CONIFG PATH_TO_OUTPUT", - file=sys.stderr, - ) - sys.exit(1) - - input_path = pathlib.Path(sys.argv[1]).expanduser().absolute() - output_path = pathlib.Path(sys.argv[2]).expanduser().absolute() - - with input_path.open("r") as fin: - contents = [x for x in fin] - # skip first line - if "%YAML:1.0" in contents[0]: - contents_str = "\n".join(contents[1:]) - else: - contents_str = "\n".join(contents) - config = yaml.load(contents_str, Loader=yaml.Loader) - - body_T_sensor = np.array([float(x) for x in config["T_BS"]["data"]]).reshape((4, 4)) - body_t_sensor = body_T_sensor[:3, 3] - body_R_sensor = R.from_matrix(body_T_sensor[:3, :3]).as_quat().tolist() - - to_write = { - "body_t_camera": body_t_sensor.tolist(), - "body_R_camera": { - "w": body_R_sensor[3], - "x": body_R_sensor[0], - "y": body_R_sensor[1], - "z": body_R_sensor[2], - }, - } - with output_path.open("w") as fout: - fout.write(yaml.dump(to_write)) - - -if __name__ == "__main__": - main() diff --git a/hydra_ros/src/config/ros_parser.cpp b/hydra_ros/src/config/ros_parser.cpp deleted file mode 100644 index 7c297846..00000000 --- a/hydra_ros/src/config/ros_parser.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/config/ros_parser.h" - -#include - -namespace config_parser { - -RosParserImpl::RosParserImpl(const ros::NodeHandle& nh, const std::string& name) - : nh_(nh), name_(name) {} - -RosParserImpl::RosParserImpl(const ros::NodeHandle& nh) : RosParserImpl(nh, "") {} - -RosParserImpl::RosParserImpl() : RosParserImpl(ros::NodeHandle(), "") {} - -RosParserImpl RosParserImpl::child(const std::string& new_name) const { - // push name onto nodehandle namespace if name isn't empty - ros::NodeHandle new_nh = (name_ == "") ? nh_ : ros::NodeHandle(nh_, name_); - return RosParserImpl(new_nh, new_name); -} - -std::vector RosParserImpl::children() const { - const std::string resolved_name = nh_.resolveName(name_); - if (resolved_name == "") { - return {}; - } - - XmlRpc::XmlRpcValue value; - nh_.getParam(name_, value); - if (value.getType() != XmlRpc::XmlRpcValue::Type::TypeStruct) { - return {}; - } - - std::vector children; - for (const auto& nv_pair : value) { - children.push_back(nv_pair.first); - } - - return children; -} - -} // namespace config_parser diff --git a/hydra_ros/src/nodes/dsg_optimizer_node.cpp b/hydra_ros/src/nodes/dsg_optimizer_node.cpp deleted file mode 100644 index 412851fa..00000000 --- a/hydra_ros/src/nodes/dsg_optimizer_node.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/pipeline/ros_backend.h" -#include "hydra_ros/pipeline/ros_frontend.h" -#include "hydra_ros/visualizer/dsg_mesh_plugins.h" -#include "hydra_ros/visualizer/dynamic_scene_graph_visualizer.h" - -namespace hydra { - -struct DsgOptimizer { - DsgOptimizer(const ros::NodeHandle& node_handle) - : nh(node_handle), reset_backend(false), dsg_output_path("") { - CHECK(nh.getParam("dsg_filepath", dsg_filepath)) << "missing dsg_filepath!"; - CHECK(nh.getParam("dgrf_filepath", dgrf_filepath)) << "missing dgrf_filepath!"; - CHECK(nh.getParam("frontend_filepath", frontend_filepath)) - << "missing frontend_state_filepath"; - nh.getParam("log_path", dsg_output_path); - - const LayerId mesh_layer_id = 1; - const std::map& layer_id_map{{DsgLayers::OBJECTS, 'o'}, - {DsgLayers::PLACES, 'p'}, - {DsgLayers::ROOMS, 'r'}, - {DsgLayers::BUILDINGS, 'b'}}; - - frontend_dsg = std::make_shared(layer_id_map, mesh_layer_id); - backend_dsg = std::make_shared(layer_id_map, mesh_layer_id); - - frontend_dsg->graph = frontend_dsg->graph->load(dsg_filepath); - frontend_dsg->updated = true; - - do_optimize(); - - optimize_service = - nh.advertiseService("optimize", &DsgOptimizer::handle_service, this); - } - - bool handle_service(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - reset_backend = true; - return true; - } - - void do_optimize() { - // TODO(nathan) maybe pull robot id from somewhere - RobotPrefixConfig prefix(0); - auto config = load_config(nh); - auto pgmo_config = load_config(nh, "pgmo"); - SharedModuleState::Ptr state(new SharedModuleState()); - backend.reset(new BackendModule( - prefix, config, pgmo_config, frontend_dsg, backend_dsg, state)); - LOG(ERROR) << "Loading backend state!"; - backend->loadState(frontend_filepath, dgrf_filepath); - LOG(ERROR) << "Loaded backend state!"; - - BackendInput input; - input.deformation_graph.reset(new pose_graph_tools::PoseGraph()); - backend->spinOnce(input, true); - - visualizer->setGraph(backend_dsg->graph); - visualizer->redraw(); - - // backend->visualizePoseGraph(); - // backend->visualizeDeformationGraphEdges(); - } - - void run() { - ros::WallRate r(10); - while (ros::ok()) { - if (reset_backend) { - reset_backend = false; - do_optimize(); - } - - r.sleep(); - ros::spinOnce(); - } - - if (!dsg_output_path.empty()) { - LOG(INFO) << "[DSG Node] Saving scene graph and other stats and logs to " - << dsg_output_path; - backend_dsg->graph->save(dsg_output_path + "/backend/dsg.json", false); - backend_dsg->graph->save(dsg_output_path + "/backend/dsg_with_mesh.json"); - } - } - - ros::NodeHandle nh; - bool reset_backend; - - std::string dsg_filepath; - std::string frontend_filepath; - std::string dgrf_filepath; - std::string dsg_output_path; - - SharedDsgInfo::Ptr frontend_dsg; - SharedDsgInfo::Ptr backend_dsg; - - BackendModule::Ptr backend; - std::unique_ptr visualizer; - - ros::ServiceServer optimize_service; -}; - -} // namespace hydra - -int main(int argc, char* argv[]) { - ros::init(argc, argv, "dsg_optimizer_node"); - - FLAGS_minloglevel = 0; - FLAGS_logtostderr = 1; - FLAGS_colorlogtostderr = 1; - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - - ros::NodeHandle nh("~"); - nh.setParam("dsg/call_update_periodically", false); - hydra::DsgOptimizer optimizer(nh); - optimizer.run(); - - return 0; -} diff --git a/hydra_ros/src/nodes/gvd_validator_node.cpp b/hydra_ros/src/nodes/gvd_validator_node.cpp deleted file mode 100644 index 9062c5e5..00000000 --- a/hydra_ros/src/nodes/gvd_validator_node.cpp +++ /dev/null @@ -1,560 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "hydra_ros/visualizer/topology_server_visualizer.h" - -DEFINE_string(config, "", "gvd integrator yaml config"); -DEFINE_string(output_path, "", "output directory"); -DEFINE_int32(max_updates, 0, "maximum number of updates per bag"); -DEFINE_int32(voxels_per_side, 16, "voxel block size"); -DEFINE_double(voxel_size, 0.1, "voxel size"); - -using config_parser::ConfigVisitor; -using config_parser::YamlParser; -using config_parser::YamlParserImpl; -using hydra::TopologyServerVisualizer; -using hydra::places::ComboIntegrator; -using hydra::places::GvdVoxel; -using sensor_msgs::Image; -using voxblox::BlockIndexList; -using voxblox::Layer; -using voxblox::MeshLayer; -using voxblox::TsdfIntegratorBase; -using voxblox::TsdfIntegratorFactory; -using voxblox::TsdfVoxel; - -using Policy = message_filters::sync_policies::ApproximateTime; -using TimeSync = message_filters::Synchronizer; -using TsdfConfig = voxblox::TsdfIntegratorBase::Config; -using MeshConfig = voxblox::MeshIntegratorConfig; -using GvdConfig = hydra::places::GvdIntegratorConfig; - -struct ComparisonResult { - size_t num_missing_lhs = 0; - size_t num_missing_rhs = 0; - size_t num_observed = 0; - size_t num_unobserved = 0; - size_t num_rhs_seen_lhs_unseen = 0; - size_t num_lhs_seen_rhs_unseen = 0; - size_t num_fixed_different = 0; - size_t num_parent_different = 0; - size_t num_basis_different = 0; - size_t num_surface_different = 0; - double min_error = std::numeric_limits::infinity(); - double max_error = 0.0; -}; - -std::ostream& operator<<(std::ostream& out, const ComparisonResult& r) { - return out << " - " << r.num_missing_lhs << " / " << r.num_missing_rhs - << " unallocated (lhs / rhs) " << std::endl - << " - " << r.num_lhs_seen_rhs_unseen << " / " - << r.num_rhs_seen_lhs_unseen << " unique (lhs / rhs)" << std::endl - << " - observed: " << r.num_observed << std::endl - << " - unobserved: " << r.num_unobserved << std::endl - << " - fixed: " << r.num_fixed_different << std::endl - << " - parent: " << r.num_parent_different << std::endl - << " - basis: " << r.num_basis_different << std::endl - << " - surface: " << r.num_surface_different << std::endl - << " - error: [" << r.min_error << ", " << r.max_error << "]"; -} - -size_t getMissingBlocks(const Layer& layer, - const BlockIndexList blocks, - const Layer& other_layer) { - size_t num_missing = 0; - for (const auto& idx : blocks) { - if (other_layer.hasBlock(idx)) { - continue; - } - - const auto& block = layer.getBlockByIndex(idx); - for (size_t i = 0; i < block.num_voxels(); ++i) { - if (block.getVoxelByLinearIndex(i).observed) { - num_missing++; - } - } - } - return num_missing; -} - -ComparisonResult compareLayers(const Layer& lhs, const Layer& rhs) { - voxblox::BlockIndexList lhs_blocks; - lhs.getAllAllocatedBlocks(&lhs_blocks); - - voxblox::BlockIndexList rhs_blocks; - rhs.getAllAllocatedBlocks(&rhs_blocks); - - ComparisonResult results; - results.num_missing_lhs = getMissingBlocks(rhs, rhs_blocks, lhs); - results.num_missing_rhs = getMissingBlocks(lhs, lhs_blocks, rhs); - - for (const auto& idx : lhs_blocks) { - if (!rhs.hasBlock(idx)) { - continue; - } - - const auto& lhs_block = lhs.getBlockByIndex(idx); - const auto& rhs_block = rhs.getBlockByIndex(idx); - for (size_t i = 0; i < lhs_block.num_voxels(); ++i) { - const GvdVoxel& lhs_voxel = lhs_block.getVoxelByLinearIndex(i); - const GvdVoxel& rhs_voxel = rhs_block.getVoxelByLinearIndex(i); - if (!lhs_voxel.observed && !rhs_voxel.observed) { - results.num_unobserved++; - continue; - } - - results.num_observed++; - - if (!lhs_voxel.observed || !rhs_voxel.observed) { - results.num_rhs_seen_lhs_unseen += (!lhs_voxel.observed ? 1 : 0); - results.num_lhs_seen_rhs_unseen += (!rhs_voxel.observed ? 1 : 0); - continue; - } - - results.num_fixed_different += (lhs_voxel.fixed != rhs_voxel.fixed) ? 1 : 0; - results.num_parent_different += - (lhs_voxel.has_parent != rhs_voxel.has_parent) ? 1 : 0; - results.num_basis_different += - (lhs_voxel.num_extra_basis != rhs_voxel.num_extra_basis) ? 1 : 0; - results.num_surface_different += - (lhs_voxel.on_surface != rhs_voxel.on_surface) ? 1 : 0; - - double error = std::abs(lhs_voxel.distance - rhs_voxel.distance); - results.min_error = std::min(results.min_error, error); - results.max_error = std::max(results.max_error, error); - } - } - - return results; -} - -namespace voxblox { - -template -void visit_config(const Visitor& v, TsdfIntegratorBase::Config& config) { - v.visit("default_truncation_distance", config.default_truncation_distance); - v.visit("max_weight", config.max_weight); - v.visit("voxel_carving_enabled", config.voxel_carving_enabled); - v.visit("min_ray_length_m", config.min_ray_length_m); - v.visit("max_ray_length_m", config.max_ray_length_m); - v.visit("use_const_weight", config.use_const_weight); - v.visit("allow_clear", config.allow_clear); - v.visit("use_weight_dropoff", config.use_weight_dropoff); - v.visit("use_sparsity_compensation_factor", config.use_sparsity_compensation_factor); - v.visit("integrator_threads", config.integrator_threads); - v.visit("integration_order_mode", config.integration_order_mode); - v.visit("enable_anti_grazing", config.enable_anti_grazing); - v.visit("start_voxel_subsampling_factor", config.start_voxel_subsampling_factor); - v.visit("max_consecutive_ray_collisions", config.max_consecutive_ray_collisions); - v.visit("clear_checks_every_n_frames", config.clear_checks_every_n_frames); -} - -} // namespace voxblox - -namespace hydra { - -struct BagConfig { - std::string color_topic; - std::string depth_topic; - double start = -1.0; - double duration = -1.0; - std::vector intrinsics; -}; - -template -void visit_config(const Visitor& v, BagConfig& config) { - v.visit("color_topic", config.color_topic); - v.visit("depth_topic", config.depth_topic); - v.visit("start", config.start); - v.visit("duration", config.duration); - v.visit("intrinsics", config.intrinsics); -} - -} // namespace hydra - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, BagConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(voxblox, TsdfIntegratorBase::Config) -using hydra::BagConfig; - -bool gvdFlagsSame(const GvdVoxel& lhs, const GvdVoxel& rhs) { - return lhs.fixed == rhs.fixed && lhs.has_parent == rhs.has_parent && - lhs.num_extra_basis == rhs.num_extra_basis && lhs.on_surface == rhs.on_surface; -} - -struct GvdValidator { - struct Intrinstics { - float fx; - float fy; - float cx; - float cy; - } intrinsics; - - GvdValidator(const TsdfConfig& tsdf_config, - const MeshConfig& mesh_config, - const GvdConfig& gvd_config, - double voxel_size, - int voxels_per_side) - : gvd_config(gvd_config), - mesh_config(mesh_config), - voxel_size(voxel_size), - voxels_per_side(voxels_per_side) { - // roughly 3000 hours: should be enough to store all the tfs - ros::Duration max_duration; - max_duration.fromSec(1.0e7); - buffer.reset(new tf2::BufferCore(max_duration)); - - visualizer.reset(new TopologyServerVisualizer("~")); - full_visualizer.reset(new TopologyServerVisualizer("~full")); - - tsdf.reset(new Layer(voxel_size, voxels_per_side)); - tsdf_integrator = TsdfIntegratorFactory::create("fast", tsdf_config, tsdf.get()); - - gvd.reset(new Layer(voxel_size, voxels_per_side)); - mesh.reset(new MeshLayer(tsdf->block_size())); - gvd_integrator.reset( - new ComboIntegrator(gvd_config, tsdf.get(), gvd, mesh, &mesh_config)); - } - - void addTfsFromBag(const rosbag::Bag& bag) { - std::vector tf_topic{"/tf"}; - rosbag::View view(bag, rosbag::TopicQuery(tf_topic)); - for (const auto& m : view) { - const auto msg = m.instantiate(); - if (!msg) { - LOG(ERROR) << "Invalid message on /tf topic!"; - continue; - } - - for (const auto& transform : msg->transforms) { - buffer->setTransform(transform, "rosbag"); - } - } - - std::vector static_tf_topic{"/tf_static"}; - rosbag::View static_view(bag, rosbag::TopicQuery(static_tf_topic)); - for (const auto& m : static_view) { - const auto msg = m.instantiate(); - if (!msg) { - LOG(ERROR) << "Invalid message on /tf topic!"; - continue; - } - - for (const auto& transform : msg->transforms) { - buffer->setTransform(transform, "rosbag", true); - } - } - } - - void fillPointCloud(const cv::Mat& color, - const cv::Mat& depth, - voxblox::Pointcloud& xyz, - voxblox::Colors& colors) { - for (int r = 0; r < color.rows; ++r) { - for (int c = 0; c < color.cols; ++c) { - const ssize_t index = r * color.cols + c; - const float u = (c - intrinsics.cx) / intrinsics.fx; - const float v = (r - intrinsics.cy) / intrinsics.fy; - - Eigen::Vector3f& point = xyz[index]; - const float depth_value = depth.at(r, c); - point.x() = u * depth_value; - point.y() = v * depth_value; - point.z() = depth_value; - - auto& color_point = colors[index]; - const auto& pixel = color.at(r, c); - color_point.r = pixel[0]; - color_point.g = pixel[1]; - color_point.b = pixel[2]; - } - } - } - - void handleImages(const sensor_msgs::Image::ConstPtr& rgb_msg, - const sensor_msgs::Image::ConstPtr& depth_msg) { - ++num_updates; - - auto rgb = cv_bridge::toCvCopy(rgb_msg); - if (rgb->image.empty()) { - LOG(ERROR) << "invalid rgb image"; - return; - } - - auto depth_img = cv_bridge::toCvCopy(depth_msg); - - cv::Mat depth; - if (depth_img->image.type() == CV_32FC1) { - depth = depth_img->image; - } else if (depth_img->image.type() == CV_16UC1) { - depth_img->image.convertTo(depth, CV_32FC1, 1.0e-3); - } else { - LOG(FATAL) << "Invalid depth type: " << depth_img->image.type(); - return; - } - - const size_t total = rgb_msg->width * rgb_msg->height; - voxblox::Pointcloud xyz(total); - voxblox::Colors colors(total); - fillPointCloud(rgb->image, depth, xyz, colors); - - const auto pose = buffer->lookupTransform("world", "left_cam", rgb->header.stamp); - - Eigen::Quaterniond world_q_color; - tf2::convert(pose.transform.rotation, world_q_color); - - Eigen::Vector3d world_t_color; - world_t_color << pose.transform.translation.x, pose.transform.translation.y, - pose.transform.translation.z; - - voxblox::Transformation voxblox_transform(world_q_color.cast(), - world_t_color.cast()); - tsdf_integrator->integratePointCloud(voxblox_transform, xyz, colors, false); - - VLOG(2) << ""; - VLOG(2) << "===================================================================="; - VLOG(2) << "Starting Incremental Update"; - VLOG(2) << "===================================================================="; - gvd_integrator->update(rgb_msg->header.stamp.toNSec(), true, true); - - full_gvd.reset(new Layer(voxel_size, voxels_per_side)); - full_mesh.reset(new MeshLayer(tsdf->block_size())); - full_gvd_integrator.reset( - new ComboIntegrator(gvd_config, tsdf.get(), full_gvd, full_mesh, &mesh_config)); - - VLOG(2) << ""; - VLOG(2) << "===================================================================="; - VLOG(2) << "Running Full Update" << std::endl; - VLOG(2) << "===================================================================="; - full_gvd_integrator->update(rgb_msg->header.stamp.toNSec(), false, true); - - auto result = compareLayers(*gvd, *full_gvd); - - LOG(INFO) << std::endl - << "**********************" << std::endl - << "* Comparison Results *" << std::endl - << "**********************" << std::endl - << result; - - if (visualizer) { - const uint64_t timestamp_ns = rgb_msg->header.stamp.toNSec(); - visualizer->visualize(gvd_integrator->getGraph(), - gvd_integrator->getGvdGraph(), - *gvd, - *tsdf, - timestamp_ns); - - visualizer->visualizeError(*gvd, *full_gvd, 0.0, timestamp_ns); - - voxblox_msgs::Mesh mesh_msg; - generateVoxbloxMeshMsg(mesh, voxblox::ColorMode::kLambertColor, &mesh_msg); - mesh_msg.header.frame_id = "world"; - mesh_msg.header.stamp = rgb_msg->header.stamp; - mesh_pub.publish(mesh_msg); - - ros::spinOnce(); - } - - if (full_visualizer) { - const uint64_t timestamp_ns = rgb_msg->header.stamp.toNSec(); - full_visualizer->visualize(full_gvd_integrator->getGraph(), - full_gvd_integrator->getGvdGraph(), - *full_gvd, - *tsdf, - timestamp_ns); - ros::spinOnce(); - } - } - - void readBag(const std::string& bag_path, const BagConfig& config) { - rosbag::Bag bag; - LOG(INFO) << "Opening " << bag_path << " ..."; - bag.open(bag_path, rosbag::bagmode::Read); - LOG(INFO) << "Opened " << bag_path; - LOG(INFO) << "Adding bag tfs to buffer..."; - addTfsFromBag(bag); - LOG(INFO) << "Added bag tfs to buffer"; - - std::vector topics{config.color_topic, config.depth_topic}; - rosbag::View view(bag, rosbag::TopicQuery(topics)); - - bool have_start = false; - ros::Time start; - - TimeSync sync(Policy(10)); - sync.registerCallback(&GvdValidator::handleImages, this); - - for (const auto& m : view) { - if (visualizer && !ros::ok()) { - break; - } - - const auto topic = m.getTopic(); - if (!have_start) { - start = m.getTime(); - if (config.start >= 0.0) { - start += ros::Duration(config.start); - } - have_start = true; - } - - const auto diff_s = (m.getTime() - start).toSec(); - if (diff_s < 0.0) { - VLOG(2) << "Skipping message " << std::abs(diff_s) << " [s] before start"; - continue; - } - - if (config.duration >= 0.0 && diff_s > config.duration) { - LOG(INFO) << "Reached end of duration: " << diff_s << " [s]"; - return; - } - - auto msg = m.instantiate(); - if (!msg) { - LOG(ERROR) << "unable to get image from " << topic; - return; - } - - if (topic == config.color_topic) { - sync.add<0>(ros::MessageEvent(msg, m.getTime())); - } else { - sync.add<1>(ros::MessageEvent(msg, m.getTime())); - } - - if (FLAGS_max_updates > 0 && num_updates >= FLAGS_max_updates) { - break; - } - } - - bag.close(); - } - - GvdConfig gvd_config; - MeshConfig mesh_config; - double voxel_size; - int voxels_per_side; - - int64_t num_updates = 0; - std::unique_ptr buffer; - - Layer::Ptr tsdf; - TsdfIntegratorBase::Ptr tsdf_integrator; - - Layer::Ptr gvd; - MeshLayer::Ptr mesh; - std::unique_ptr gvd_integrator; - std::unique_ptr visualizer; - - Layer::Ptr full_gvd; - MeshLayer::Ptr full_mesh; - std::unique_ptr full_gvd_integrator; - std::unique_ptr full_visualizer; - - ros::Publisher mesh_pub; -}; - -struct ValidatorLogger : public config_parser::Logger { - void log_missing(const std::string& message) const override { LOG(ERROR) << message; } -}; - -int main(int argc, char** argv) { - ros::init(argc, argv, "gvd_validator"); - ros::NodeHandle nh(""); - - FLAGS_minloglevel = 0; - FLAGS_logtostderr = 1; - FLAGS_colorlogtostderr = 1; - FLAGS_v = 1; - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - - if (argc <= 1) { - LOG(FATAL) << "Missing required bag path"; - return 1; - } - - const std::string bag_path(argv[1]); - - YAML::Node node = YAML::LoadFile(FLAGS_config); - - YamlParser parser(std::make_unique(node), - std::make_shared()); - - BagConfig config; - ConfigVisitor::visit_config(parser, config); - - TsdfConfig tsdf_config; - ConfigVisitor::visit_config(parser, tsdf_config); - - MeshConfig mesh_config; - ConfigVisitor::visit_config(parser, mesh_config); - - GvdConfig gvd_config; - ConfigVisitor::visit_config(parser, gvd_config); - - VLOG(1) << "BagConfig:" << std::endl << config; - VLOG(1) << "TsdfConfig:" << std::endl << tsdf_config; - VLOG(1) << "MeshConfig:" << std::endl << mesh_config; - VLOG(1) << "GvdConfig:" << std::endl << gvd_config; - - GvdValidator validator( - tsdf_config, mesh_config, gvd_config, FLAGS_voxel_size, FLAGS_voxels_per_side); - validator.mesh_pub = nh.advertise("mesh_viz", 1, true); - validator.intrinsics.fx = config.intrinsics.at(0); - validator.intrinsics.fy = config.intrinsics.at(1); - validator.intrinsics.cx = config.intrinsics.at(2); - validator.intrinsics.cy = config.intrinsics.at(3); - validator.readBag(bag_path, config); - return 0; -} diff --git a/hydra_ros/src/nodes/hydra_node.cpp b/hydra_ros/src/nodes/hydra_node.cpp deleted file mode 100644 index 4f7ed383..00000000 --- a/hydra_ros/src/nodes/hydra_node.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/pipeline/hydra_ros_pipeline.h" -#include "hydra_ros/utils/node_utilities.h" - -using hydra::timing::ElapsedTimeRecorder; - -int main(int argc, char* argv[]) { - ros::init(argc, argv, "hydra_node"); - - FLAGS_minloglevel = 3; - FLAGS_logtostderr = 1; - FLAGS_colorlogtostderr = 1; - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - - ros::NodeHandle nh("~"); - const auto dsg_output_path = hydra::configureTimers(nh); - - int robot_id = 0; - nh.getParam("robot_id", robot_id); - - hydra::HydraRosPipeline hydra(nh, robot_id); - hydra.start(); - - hydra::spinAndWait(nh); - - hydra.stop(); - hydra.save(dsg_output_path); - - hydra::saveTimingInformation(dsg_output_path); - return 0; -} diff --git a/hydra_ros/src/nodes/hydra_topology_node.cpp b/hydra_ros/src/nodes/hydra_topology_node.cpp deleted file mode 100644 index 090d22ad..00000000 --- a/hydra_ros/src/nodes/hydra_topology_node.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/pipeline/ros_reconstruction.h" - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, LogConfig); -namespace hydra { -DECLARE_STRUCT_NAME(LogConfig); -} // namespace hydra - -int main(int argc, char* argv[]) { - ros::init(argc, argv, "hydra_topology_node"); - - FLAGS_minloglevel = 3; - FLAGS_logtostderr = 1; - FLAGS_colorlogtostderr = 1; - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - - ros::NodeHandle nh("~"); - - const auto log_config = hydra::load_config(nh, "", false); - hydra::LogSetup logs(log_config); - - const hydra::RobotPrefixConfig prefix(nh.param("robot_id", 0)); - hydra::RosReconstruction module(nh, prefix); - module.start(); - - ros::spin(); - - module.stop(); - module.save(logs.getLogDir()); - - return 0; -} diff --git a/hydra_ros/src/nodes/hydra_visualizer_node.cpp b/hydra_ros/src/nodes/hydra_visualizer_node.cpp deleted file mode 100644 index de4eb443..00000000 --- a/hydra_ros/src/nodes/hydra_visualizer_node.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include - -#include - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/utils/dsg_streaming_interface.h" -#include "hydra_ros/visualizer/dsg_mesh_plugins.h" -#include "hydra_ros/visualizer/dynamic_scene_graph_visualizer.h" - -using DsgVisualizer = hydra::DynamicSceneGraphVisualizer; -using spark_dsg::getDefaultLayerIds; - -namespace hydra { - -struct NodeConfig { - enum class MeshPluginType { - VOXBLOX, - RVIZ, - PGMO, - } mesh_plugin_type = MeshPluginType::PGMO; - bool load_graph = false; - std::string scene_graph_filepath = ""; - std::string visualizer_ns = "/hydra_dsg_visualizer"; - std::string mesh_plugin_ns = "dsg_mesh"; - std::string output_path = ""; -}; - -template -void visit_config(const Visitor& v, NodeConfig& config) { - v.visit("mesh_plugin_type", config.mesh_plugin_type); - v.visit("load_graph", config.load_graph); - v.visit("scene_graph_filepath", config.scene_graph_filepath); - v.visit("visualizer_ns", config.visualizer_ns); - v.visit("mesh_plugin_ns", config.mesh_plugin_ns); - v.visit("output_path", config.output_path); -} - -using MeshPluginEnum = NodeConfig::MeshPluginType; - -struct RosParamLogger : config_parser::Logger { - inline void log_missing(const std::string& message) const override { - ROS_INFO_STREAM(message); - } -}; - -} // namespace hydra - -DECLARE_CONFIG_ENUM(hydra, - MeshPluginEnum, - {MeshPluginEnum::VOXBLOX, "VOXBLOX"}, - {MeshPluginEnum::PGMO, "PGMO"}, - {MeshPluginEnum::RVIZ, "RVIZ"}) - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, NodeConfig) - -namespace hydra { - -std::string getSpecificMeshNs(const std::string& mesh_ns, MeshPluginEnum mesh_type) { - switch (mesh_type) { - case MeshPluginEnum::VOXBLOX: - return mesh_ns + "/voxblox"; - case MeshPluginEnum::PGMO: - return mesh_ns + "/pgmo"; - case MeshPluginEnum::RVIZ: - return mesh_ns + "/rviz"; - default: - return mesh_ns; - } -} - -struct VisualizerNode { - VisualizerNode(const ros::NodeHandle& nh) : nh_(nh) { - auto logger = std::make_shared(); - config_ = config_parser::load_from_ros_nh(nh_, "", logger); - ROS_INFO_STREAM("Config: " << std::endl << config_); - - ros::NodeHandle viz_nh(config_.visualizer_ns); - visualizer_.reset(new DsgVisualizer(viz_nh, getDefaultLayerIds())); - - auto mesh_ns = getSpecificMeshNs(config_.mesh_plugin_ns, config_.mesh_plugin_type); - hydra::DsgVisualizerPlugin::Ptr plugin; - switch (config_.mesh_plugin_type) { - case MeshPluginEnum::VOXBLOX: - plugin = std::make_shared(viz_nh, mesh_ns); - break; - case MeshPluginEnum::RVIZ: - plugin = std::make_shared(viz_nh, mesh_ns); - break; - case MeshPluginEnum::PGMO: - default: - plugin = std::make_shared(viz_nh, mesh_ns); - break; - } - - visualizer_->addPlugin(plugin); - - if (!config_.output_path.empty()) { - size_log_file_.reset( - new std::ofstream(config_.output_path + "/dsg_message_sizes.csv")); - *size_log_file_ << "time_ns,bytes" << std::endl; - } - - if (!config_.load_graph || config_.scene_graph_filepath.empty()) { - receiver_.reset(new DsgReceiver(nh_, [&](const ros::Time& stamp, size_t bytes) { - if (size_log_file_) { - *size_log_file_ << stamp.toNSec() << "," << bytes << std::endl; - } - })); - } else { - loadGraph(); - } - } - - ~VisualizerNode() { - std::cout << "timing stats: " - << hydra::timing::ElapsedTimeRecorder::instance().getStats("receive_dsg") - << std::endl; - std::cout << "mesh timing stats: " - << hydra::timing::ElapsedTimeRecorder::instance().getStats("receive_mesh") - << std::endl; - - if (config_.output_path.empty()) { - return; - } - - timing::ElapsedTimeRecorder::instance().logAllElapsed(config_.output_path); - } - - void loadGraph() { - ROS_INFO_STREAM("Loading dsg from: " << config_.scene_graph_filepath); - auto dsg = hydra::DynamicSceneGraph::load(config_.scene_graph_filepath); - ROS_INFO_STREAM("Loaded dsg: " << dsg->numNodes() << " nodes, " << dsg->numEdges() - << " edges, has mesh? " - << (dsg->hasMesh() ? "yes" : "no")); - visualizer_->setGraph(dsg); - } - - bool handleService(std_srvs::Empty::Request&, std_srvs::Empty::Response&) { - loadGraph(); - return true; - } - - void spin() { - ROS_DEBUG("Visualizer running"); - - if (!config_.load_graph) { - bool graph_set = false; - - ros::Rate r(10); - while (ros::ok()) { - ros::spinOnce(); - - if (receiver_ && receiver_->updated()) { - if (!receiver_->graph()) { - r.sleep(); - continue; - } - if (!graph_set) { - visualizer_->setGraph(receiver_->graph()); - graph_set = true; - } else { - visualizer_->setGraphUpdated(); - } - - visualizer_->redraw(); - receiver_->clearUpdated(); - } - - r.sleep(); - } - } else { - reload_service_ = - nh_.advertiseService("reload", &VisualizerNode::handleService, this); - visualizer_->start(); - ros::spin(); - } - } - - ros::NodeHandle nh_; - std::unique_ptr visualizer_; - std::unique_ptr receiver_; - NodeConfig config_; - std::unique_ptr size_log_file_; - ros::ServiceServer reload_service_; -}; - -} // namespace hydra - -int main(int argc, char** argv) { - ros::init(argc, argv, "dsg_visualizer_node"); - - FLAGS_minloglevel = 3; - FLAGS_logtostderr = 1; - FLAGS_colorlogtostderr = 1; - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - - ros::NodeHandle nh("~"); - hydra::VisualizerNode node(nh); - node.spin(); - - return 0; -} diff --git a/hydra_ros/src/nodes/rotate_tf_node.cpp b/hydra_ros/src/nodes/rotate_tf_node.cpp deleted file mode 100644 index 65853893..00000000 --- a/hydra_ros/src/nodes/rotate_tf_node.cpp +++ /dev/null @@ -1,114 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include - -void send_tf(tf2_ros::TransformBroadcaster& br, - const std::string& parent, - const std::string& child, - double radius, - const std::vector& centroid, - double ratio) { - const double theta = ratio * 2 * M_PI; - const double phi = theta + M_PI; - Eigen::Quaterniond q(std::cos(phi / 2.0), 0.0, 0.0, std::sin(phi / 2.0)); - - geometry_msgs::TransformStamped msg; - msg.header.stamp = ros::Time::now(); - msg.header.frame_id = parent; - msg.child_frame_id = child; - msg.transform.translation.x = centroid[0] + radius * std::cos(theta); - msg.transform.translation.y = centroid[1] + radius * std::sin(theta); - msg.transform.translation.z = centroid[2]; - tf2::convert(q, msg.transform.rotation); - - br.sendTransform(msg); -} - -int main(int argc, char* argv[]) { - ros::init(argc, argv, "rotate_tf_node"); - ros::NodeHandle nh; - - std::string parent_frame = "world"; - ros::param::get("~parent_frame", parent_frame); - - std::string child_frame = "rotated_view"; - ros::param::get("~child_frame", child_frame); - - double period_s = 5.0; - ros::param::get("~period_s", period_s); - - bool use_wall_time = true; - ros::param::get("~use_wall_time", use_wall_time); - - double radius = 50.0; - ros::param::get("~radius", radius); - - std::vector centroid{0.0, 0.0, 0.0}; - ros::param::get("~centroid", centroid); - if (centroid.size() != 3) { - ROS_FATAL_STREAM("Invalid centroid size: " << centroid.size() << " != 3"); - return 1; - } - - tf2_ros::TransformBroadcaster br; - - const double timer_period_s = 0.01; - - if (use_wall_time) { - ros::WallTime start_time = ros::WallTime::now(); - ros::WallTimer timer = nh.createWallTimer( - ros::WallDuration(timer_period_s), [&](const ros::WallTimerEvent& event) { - const double elapsed_s = (event.current_real - start_time).toSec(); - const double ratio = elapsed_s / period_s; - send_tf(br, parent_frame, child_frame, radius, centroid, ratio); - }); - ros::spin(); - } else { - ros::Time start_time = ros::Time::now(); - ros::Timer timer = nh.createTimer( - ros::Duration(timer_period_s), [&](const ros::TimerEvent& event) { - const double elapsed_s = (event.current_real - start_time).toSec(); - const double ratio = elapsed_s / period_s; - send_tf(br, parent_frame, child_frame, radius, centroid, ratio); - }); - - ros::spin(); - } - - return 0; -} diff --git a/hydra_ros/src/nodes/scene_graph_logger_node.cpp b/hydra_ros/src/nodes/scene_graph_logger_node.cpp deleted file mode 100644 index 459de875..00000000 --- a/hydra_ros/src/nodes/scene_graph_logger_node.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include - -#include - -#include "hydra_ros/utils/dsg_streaming_interface.h" - -namespace hydra { - -struct SceneGraphLoggerNode { - SceneGraphLoggerNode(const ros::NodeHandle& nh) - : nh_(nh), curr_count_(0), curr_output_count_(0), output_every_num_(1) { - receiver_.reset(new DsgReceiver(nh_)); - if (!nh_.getParam("output_path", output_path_)) { - ROS_FATAL("Failed to get output path parameter"); - throw std::runtime_error("failed to get output path"); - } - - nh_.getParam("output_every_num", output_every_num_); - } - - void spin() { - ros::Rate r(10); - while (ros::ok()) { - ros::spinOnce(); - - if (!receiver_->updated()) { - r.sleep(); - continue; - } - - receiver_->clearUpdated(); - ++curr_count_; - if (output_every_num_ == 1 || curr_count_ % output_every_num_ != 1) { - continue; - } - - std::stringstream ss; - ss << output_path_ << "/dsg_" << std::setfill('0') << std::setw(6) - << curr_output_count_; - ss << ".json"; - receiver_->graph()->save(ss.str(), false); - ++curr_output_count_; - } - } - - ros::NodeHandle nh_; - size_t curr_count_; - size_t curr_output_count_; - int output_every_num_; - std::string output_path_; - std::unique_ptr receiver_; -}; - -} // namespace hydra - -int main(int argc, char** argv) { - ros::init(argc, argv, "scene_graph_logger_node"); - - ros::NodeHandle nh("~"); - hydra::SceneGraphLoggerNode node(nh); - node.spin(); - - return 0; -} diff --git a/hydra_ros/src/pipeline/hydra_ros_pipeline.cpp b/hydra_ros/src/pipeline/hydra_ros_pipeline.cpp deleted file mode 100644 index b722ea9d..00000000 --- a/hydra_ros/src/pipeline/hydra_ros_pipeline.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/pipeline/hydra_ros_pipeline.h" - -#include "hydra_ros/config/ros_utilities.h" -#include "hydra_ros/pipeline/ros_lcd_registration.h" - -namespace hydra { - -template -void visit_config(const Visitor& v, HydraRosConfig& config) { - v.visit("enable_lcd", config.enable_lcd); - v.visit("use_ros_backend", config.use_ros_backend); - v.visit("do_reconstruction", config.do_reconstruction); - v.visit("enable_frontend_output", config.enable_frontend_output); - v.visit("frontend_mesh_separation_s", config.frontend_mesh_separation_s); -} - -DECLARE_STRUCT_NAME(HydraRosConfig); -DECLARE_STRUCT_NAME(FrontendConfig); -DECLARE_STRUCT_NAME(BackendConfig); -DECLARE_STRUCT_NAME(LoopClosureConfig); -DECLARE_STRUCT_NAME(kimera_pgmo::KimeraPgmoConfig); - -} // namespace hydra - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, HydraRosConfig) - -namespace hydra { - -HydraRosPipeline::HydraRosPipeline(const ros::NodeHandle& node_handle, int robot_id) - : nh(node_handle), prefix(robot_id) { - config = load_config(nh); - - // TODO(nathan) parse and use at some point - const LayerId mesh_layer_id = 1; - const std::map layer_id_map{{DsgLayers::OBJECTS, 'o'}, - {DsgLayers::PLACES, 'p'}, - {DsgLayers::ROOMS, 'r'}, - {DsgLayers::BUILDINGS, 'b'}}; - - frontend_dsg.reset(new SharedDsgInfo(layer_id_map, mesh_layer_id)); - backend_dsg.reset(new SharedDsgInfo(layer_id_map, mesh_layer_id)); - shared_state.reset(new SharedModuleState()); - - if (config.do_reconstruction) { - const auto frontend_config = load_config(nh); - frontend = std::make_shared( - prefix, frontend_config, frontend_dsg, shared_state); - reconstruction = - std::make_shared(nh, prefix, frontend->getQueue()); - } else { - frontend = std::make_shared(nh, prefix, frontend_dsg, shared_state); - } - - if (config.enable_frontend_output) { - ros::NodeHandle frontend_nh(nh, "frontend"); - dsg_sender.reset(new DsgSender( - frontend_nh, "frontend", true, config.frontend_mesh_separation_s)); - mesh_graph_pub = - nh.advertise("mesh_graph_incremental", 100, true); - mesh_update_pub = - nh.advertise("full_mesh_update", 100, true); - frontend->addOutputCallback(std::bind(&HydraRosPipeline::sendFrontendOutput, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); - } - - const auto backend_config = load_config(nh); - if (config.use_ros_backend) { - backend = std::make_shared( - nh, prefix, frontend_dsg, backend_dsg, shared_state); - } else { - const auto pgmo_config = load_config(nh, "pgmo"); - backend = std::make_shared( - prefix, backend_config, pgmo_config, frontend_dsg, backend_dsg, shared_state); - } - - backend_visualizer = - std::make_shared(nh, backend_config, prefix); - backend->addOutputCallback(std::bind(&RosBackendVisualizer::publishOutputs, - backend_visualizer.get(), - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); - - if (config.enable_lcd) { - auto lcd_config = load_config(nh, ""); - lcd_config.detector.num_semantic_classes = frontend->maxSemanticLabel(); - VLOG(1) << "Number of classes for LCD: " - << lcd_config.detector.num_semantic_classes; - shared_state->lcd_queue.reset(new InputQueue()); - lcd.reset(new LoopClosureModule(prefix, lcd_config, frontend_dsg, shared_state)); - - bow_sub = nh.subscribe("bow_vectors", 100, &HydraRosPipeline::bowCallback, this); - if (lcd_config.detector.enable_agent_registration) { - lcd->getDetector().setRegistrationSolver(0, std::make_unique()); - } - } -} - -void HydraRosPipeline::bowCallback(const pose_graph_tools::BowQueries::ConstPtr& msg) { - for (const auto& query : msg->queries) { - shared_state->visual_lcd_queue.push( - pose_graph_tools::BowQuery::ConstPtr(new pose_graph_tools::BowQuery(query))); - } -} - -void HydraRosPipeline::start() { - if (reconstruction) { - reconstruction->start(); - } - - if (frontend) { - frontend->start(); - } - - if (backend) { - backend->start(); - } - - if (lcd) { - lcd->start(); - } -} - -void HydraRosPipeline::stop() { - if (reconstruction) { - reconstruction->stop(); - } - - if (frontend) { - frontend->stop(); - } - - if (lcd) { - lcd->stop(); - } - - if (backend) { - backend->stop(); - } -} - -void HydraRosPipeline::save(const std::string& output_path) { - if (!output_path.empty()) { - if (reconstruction) { - reconstruction->save(output_path + "/topology/"); - } - - if (frontend) { - frontend->save(output_path + "/frontend/"); - } - - if (backend) { - backend->save(output_path + "/backend/"); - } - - if (lcd) { - lcd->save(output_path + "/lcd/"); - } - } -} - -void HydraRosPipeline::sendFrontendOutput(const DynamicSceneGraph& graph, - const BackendInput& backend_input, - uint64_t timestamp_ns) { - if (backend_input.deformation_graph) { - mesh_graph_pub.publish(*backend_input.deformation_graph); - } - - if (backend_input.mesh_update) { - mesh_update_pub.publish(backend_input.mesh_update->toRosMsg(timestamp_ns)); - } - - sendFrontendGraph(graph, timestamp_ns); -} - -void HydraRosPipeline::sendFrontendGraph(const DynamicSceneGraph& graph, - uint64_t timestamp_ns) { - ros::Time stamp; - stamp.fromNSec(timestamp_ns); - dsg_sender->sendGraph(graph, stamp); -} - -} // namespace hydra diff --git a/hydra_ros/src/pipeline/ros_backend.cpp b/hydra_ros/src/pipeline/ros_backend.cpp deleted file mode 100644 index e2badbd1..00000000 --- a/hydra_ros/src/pipeline/ros_backend.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/pipeline/ros_backend.h" - -#include "hydra_ros/config/ros_utilities.h" - -namespace hydra { - -using kimera_pgmo::DeformationGraph; -using mesh_msgs::TriangleMeshStamped; -using message_filters::Subscriber; -using visualization_msgs::Marker; - -RosBackend::RosBackend(const ros::NodeHandle& nh, - const RobotPrefixConfig& prefix, - const SharedDsgInfo::Ptr& dsg, - const SharedDsgInfo::Ptr& backend_dsg, - const SharedModuleState::Ptr& state) - : BackendModule(prefix, - load_config(nh), - load_config(nh, "pgmo"), - dsg, - backend_dsg, - state), - nh_(nh) { - pose_graph_sub_ = nh_.subscribe( - "pose_graph_incremental", 10000, &RosBackend::poseGraphCallback, this); - - mesh_sub_.reset(new Subscriber(nh_, "pgmo/ful_mesh", 1)); - deformation_graph_sub_.reset( - new Subscriber(nh_, "pgmo/mesh_graph_incremental", 100)); - sync_.reset(new Sync(Policy(10), *mesh_sub_, *deformation_graph_sub_)); - sync_->registerCallback(boost::bind(&RosBackend::inputCallback, this, _1, _2)); -} - -RosBackend::~RosBackend() {} - -void RosBackend::inputCallback(const KimeraPgmoMesh::ConstPtr& mesh, - const PoseGraph::ConstPtr& deformation_graph) { - latest_mesh_msg_ = mesh; - have_new_mesh_ = true; - - auto input = std::make_shared(); - input->deformation_graph = deformation_graph; - input->timestamp_ns = mesh->header.stamp.toNSec(); - input->pose_graphs = pose_graph_queue_; - pose_graph_queue_.clear(); - - state_->backend_queue.push(input); -} - -void RosBackend::poseGraphCallback(const PoseGraph::ConstPtr& msg) { - pose_graph_queue_.push_back(msg); -} - -// TODO(nathan) copy mesh message into MeshStruct - -RosBackendVisualizer::RosBackendVisualizer(const ros::NodeHandle& nh, - const BackendConfig& config, - const RobotPrefixConfig& prefix) - : nh_(nh), config_(config), prefix_(prefix), last_zmq_pub_time_(0) { - mesh_mesh_edges_pub_ = - nh_.advertise("pgmo/deformation_graph_mesh_mesh", 10, false); - pose_mesh_edges_pub_ = - nh_.advertise("pgmo/deformation_graph_pose_mesh", 10, false); - pose_graph_pub_ = nh_.advertise("pgmo/pose_graph", 10, false); - - double min_mesh_separation_s = 0.0; - nh_.getParam("min_mesh_separation_s", min_mesh_separation_s); - - dsg_sender_.reset(new hydra::DsgSender(nh_, "backend", true, min_mesh_separation_s)); - if (config_.use_zmq_interface) { - zmq_sender_.reset( - new spark_dsg::ZmqSender(config_.zmq_send_url, config_.zmq_num_threads)); - } -} - -void RosBackendVisualizer::publishOutputs(const DynamicSceneGraph& graph, - const DeformationGraph& dgraph, - size_t timestamp_ns) { - ros::Time stamp; - stamp.fromNSec(timestamp_ns); - - // TODO(nathan) consider serializing to bytes before sending - dsg_sender_->sendGraph(graph, stamp); - - if (config_.use_zmq_interface && timestamp_ns - last_zmq_pub_time_ > 9000000000) { - zmq_sender_->send(graph); - last_zmq_pub_time_ = timestamp_ns; - } - - if (pose_graph_pub_.getNumSubscribers() > 0) { - publishPoseGraph(graph, dgraph); - } - - if (mesh_mesh_edges_pub_.getNumSubscribers() > 0 || - pose_mesh_edges_pub_.getNumSubscribers() > 0) { - publishDeformationGraphViz(dgraph, timestamp_ns); - } -} - -void RosBackendVisualizer::publishPoseGraph(const DynamicSceneGraph& graph, - const DeformationGraph& dgraph) const { - const auto& agent = graph.getLayer(DsgLayers::AGENTS, prefix_.key); - - std::map> id_timestamps; - id_timestamps[prefix_.id] = std::vector(); - auto& times = id_timestamps[prefix_.id]; - for (const auto& node : agent.nodes()) { - ros::Time curr_stamp; - curr_stamp.fromNSec(node->timestamp.count()); - times.push_back(curr_stamp); - } - - const auto& pose_graph = dgraph.getPoseGraph(id_timestamps); - pose_graph_pub_.publish(*pose_graph); -} - -void RosBackendVisualizer::publishDeformationGraphViz(const DeformationGraph& dgraph, - size_t timestamp_ns) const { - ros::Time stamp; - stamp.fromNSec(timestamp_ns); - - Marker mm_edges_msg; - Marker pm_edges_msg; - kimera_pgmo::fillDeformationGraphMarkers(dgraph, stamp, mm_edges_msg, pm_edges_msg); - - if (!mm_edges_msg.points.empty()) { - mesh_mesh_edges_pub_.publish(mm_edges_msg); - } - if (!pm_edges_msg.points.empty()) { - pose_mesh_edges_pub_.publish(pm_edges_msg); - } -} - -} // namespace hydra diff --git a/hydra_ros/src/pipeline/ros_frontend.cpp b/hydra_ros/src/pipeline/ros_frontend.cpp deleted file mode 100644 index 14c16b2c..00000000 --- a/hydra_ros/src/pipeline/ros_frontend.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/pipeline/ros_frontend.h" - -#include "hydra_ros/config/ros_utilities.h" - -namespace hydra { - -using message_filters::Subscriber; - -RosFrontend::RosFrontend(const ros::NodeHandle& nh, - const RobotPrefixConfig& prefix, - const SharedDsgInfo::Ptr& dsg, - const SharedModuleState::Ptr& state) - : FrontendModule(prefix, load_config(nh), dsg, state), nh_(nh) { - ros_config_ = load_config(nh); - - pose_graph_sub_ = nh_.subscribe( - "pose_graph_incremental", 100, &RosFrontend::poseGraphCallback, this); - - mesh_sub_.reset(new Subscriber(nh_, "voxblox_mesh", 5)); - places_sub_.reset(new Subscriber(nh_, "active_places", 5)); - sync_.reset(new Sync(Policy(10), *places_sub_, *mesh_sub_)); - sync_->registerCallback(boost::bind(&RosFrontend::inputCallback, this, _1, _2)); - - tf_listener_.reset(new tf2_ros::TransformListener(buffer_)); - - if (ros_config_.enable_active_mesh_pub) { - active_vertices_pub_ = nh_.advertise("active_vertices", 1, true); - segmenter_->addVisualizationCallback( - [this](const auto& cloud, const auto& indices, const auto& labels) { - this->publishActiveVertices(cloud, indices, labels); - }); - } - if (ros_config_.enable_segmented_mesh_pub) { - segmented_vertices_pub_.reset(new ObjectCloudPub("object_vertices", nh_)); - segmenter_->addVisualizationCallback( - [this](const auto& cloud, const auto& indices, const auto& labels) { - this->publishObjectClouds(cloud, indices, labels); - }); - } -} - -RosFrontend::~RosFrontend() { segmented_vertices_pub_.reset(); } - -void RosFrontend::inputCallback(const ActiveLayer::ConstPtr& places, - const ActiveMesh::ConstPtr& mesh) { - VLOG(5) << "Received input @ " << places->header.stamp.toNSec() << " [ns]"; - LOG(FATAL) << "ROS input for frontend not implemented currently"; - - ReconstructionOutput::Ptr input(new ReconstructionOutput()); - - if (ros_config_.use_posegraph_pos && !pose_graph_queue_.empty()) { - const auto latest_position = getLatestPosition(); - if (!latest_position) { - ROS_ERROR_STREAM("Could not extract position from empty pose graph!"); - return; - } - - input->current_position = *latest_position; - } else { - if (ros_config_.use_posegraph_pos) { - ROS_WARN_STREAM("Falling back to using tf for latest pos"); - } - - const auto latest_position = getLatestPositionTf(places->header.stamp); - if (!latest_position) { - ROS_ERROR_STREAM("Could not lookup latest position from tf!"); - return; - } - - input->current_position = *latest_position; - } - - //input->places = places; - //input->mesh = mesh; - input->timestamp_ns = places->header.stamp.toNSec(); - - // send all cached messages to frontend - input->pose_graphs = pose_graph_queue_; - pose_graph_queue_.clear(); - - queue_->push(input); -} - -void RosFrontend::poseGraphCallback(const PoseGraph::ConstPtr& pose_graph) { - pose_graph_queue_.push_back(pose_graph); -} - -void RosFrontend::publishActiveVertices(const MeshVertexCloud& vertices, - const MeshSegmenter::IndicesVector& indices, - const LabelIndices&) const { - MeshVertexCloud::Ptr active_cloud(new MeshVertexCloud()); - active_cloud->reserve(indices.size()); - for (const auto idx : indices) { - active_cloud->push_back(vertices.at(idx)); - } - - active_cloud->header.frame_id = "world"; - pcl_conversions::toPCL(ros::Time::now(), active_cloud->header.stamp); - active_vertices_pub_.publish(active_cloud); -} - -void RosFrontend::publishObjectClouds(const MeshVertexCloud& vertices, - const MeshSegmenter::IndicesVector&, - const LabelIndices& label_indices) const { - for (const auto& label_index_pair : label_indices) { - MeshVertexCloud label_cloud; - label_cloud.reserve(label_index_pair.second->size()); - for (const auto idx : *label_index_pair.second) { - label_cloud.push_back(vertices.at(idx)); - } - - label_cloud.header.frame_id = "world"; - pcl_conversions::toPCL(ros::Time::now(), label_cloud.header.stamp); - segmented_vertices_pub_->publish(label_index_pair.first, label_cloud); - } -} - -std::optional RosFrontend::getLatestPosition() const { - const auto& msg = pose_graph_queue_.back(); - if (msg->nodes.empty()) { - return std::nullopt; - } - - const auto& pose = msg->nodes.back().pose; - return Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); -} - -std::optional RosFrontend::getLatestPositionTf( - const ros::Time& time_to_use) const { - geometry_msgs::TransformStamped transform; - try { - transform = - buffer_.lookupTransform(ros_config_.world_frame, - ros_config_.sensor_frame, - ros_config_.use_latest_tf ? ros::Time(0) : time_to_use); - } catch (const tf2::TransformException& ex) { - ROS_WARN_STREAM(ex.what()); - return std::nullopt; - } - - return Eigen::Vector3d(transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z); -} - -} // namespace hydra diff --git a/hydra_ros/src/pipeline/ros_lcd_registration.cpp b/hydra_ros/src/pipeline/ros_lcd_registration.cpp deleted file mode 100644 index de5bb44b..00000000 --- a/hydra_ros/src/pipeline/ros_lcd_registration.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/pipeline/ros_lcd_registration.h" - -#include -#include -#include -#include -#include - -#include - -namespace hydra { - -using hydra::timing::ScopedTimer; -using lcd::DsgRegistrationInput; -using lcd::DsgRegistrationSolution; - -inline size_t getRobotIdFromNode(const DynamicSceneGraph& graph, NodeId node_id) { - const auto& attrs = - graph.getNode(node_id).value().get().attributes(); - // TODO(yun) cleaner way to track robot prefix to id? - return kimera_pgmo::robot_prefix_to_id.at(NodeSymbol(attrs.external_key).category()); -} - -inline size_t getFrameIdFromNode(const DynamicSceneGraph& graph, NodeId node_id) { - const auto& attrs = - graph.getNode(node_id).value().get().attributes(); - return NodeSymbol(attrs.external_key).categoryId(); -} - -std::string getPoseRepr(const Eigen::Quaterniond& q, const Eigen::Vector3d& v) { - const Eigen::IOFormat format(3, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); - - std::stringstream ss; - ss << std::fixed << std::setprecision(3) << "R={w: " << q.w() << ", x: " << q.x() - << ", y: " << q.y() << ", z: " << q.z() << "}, t=" << v.format(format); - return ss.str(); -} - -DsgRegistrationSolution DsgAgentSolver::solve(const DynamicSceneGraph& dsg, - const DsgRegistrationInput& match, - NodeId) const { - if (match.query_nodes.empty() || match.match_nodes.empty()) { - return {}; - } - - if (!ros::service::exists("frame_registration", true)) { - LOG(ERROR) << "[Hydra LCD] Frame registration service missing!"; - return {}; - } - - // at the agent level, match sets are one node each - const NodeId query_id = *match.query_nodes.begin(); - const NodeId match_id = *match.match_nodes.begin(); - - if (!dsg.hasNode(query_id) || !dsg.hasNode(match_id)) { - LOG(ERROR) << "Query or match node does not exist in graph!"; - return {}; - } - - uint64_t timestamp; - pose_graph_tools::LcdFrameRegistration msg; - msg.request.query_robot = getRobotIdFromNode(dsg, query_id); - msg.request.match_robot = getRobotIdFromNode(dsg, match_id); - msg.request.query = getFrameIdFromNode(dsg, query_id); - msg.request.match = getFrameIdFromNode(dsg, match_id); - timestamp = dsg.getDynamicNode(query_id).value().get().timestamp.count(); - - ScopedTimer timer("lcd/register_agent", timestamp, true, 2, false); - - if (!ros::service::call("frame_registration", msg)) { - LOG(ERROR) << "Frame registration service failed!"; - return {}; - } - - VLOG(3) << "Visual registration request: query={robot: " << msg.request.query_robot - << ", frame: " << msg.request.query - << "}, match={robot: " << msg.request.match_robot - << ", frame: " << msg.request.match; - - if (!msg.response.valid) { - VLOG(1) << "Visual registration failed: " << NodeSymbol(query_id).getLabel() - << " -> " << NodeSymbol(match_id).getLabel(); - return {}; - } - - Eigen::Quaterniond match_q_query; - Eigen::Vector3d match_t_query; - tf2::convert(msg.response.match_T_query.orientation, match_q_query); - tf2::convert(msg.response.match_T_query.position, match_t_query); - const Eigen::IOFormat format(3, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); - VLOG(3) << "Visual registration succeded: " - << getPoseRepr(match_q_query, match_t_query); - return {true, - query_id, - match_id, - gtsam::Pose3(gtsam::Rot3(match_q_query), match_t_query), - -1}; -} - -} // namespace hydra diff --git a/hydra_ros/src/pipeline/ros_reconstruction.cpp b/hydra_ros/src/pipeline/ros_reconstruction.cpp deleted file mode 100644 index d9a73331..00000000 --- a/hydra_ros/src/pipeline/ros_reconstruction.cpp +++ /dev/null @@ -1,304 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/pipeline/ros_reconstruction.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hydra_ros/config/ros_utilities.h" - -namespace hydra { - -DECLARE_STRUCT_NAME(RosReconstructionConfig); -DECLARE_STRUCT_NAME(ReconstructionConfig); - -using pose_graph_tools::PoseGraph; -using pose_graph_tools::PoseGraphEdge; -using pose_graph_tools::PoseGraphNode; -using RosPointcloud = RosReconstruction::Pointcloud; -using places::CompressionGraphExtractor; - -inline geometry_msgs::Pose tfToPose(const geometry_msgs::Transform& transform) { - geometry_msgs::Pose pose; - pose.position.x = transform.translation.x; - pose.position.y = transform.translation.y; - pose.position.z = transform.translation.z; - pose.orientation = transform.rotation; - return pose; -} - -std::string showQuaternion(const Eigen::Quaterniond& q) { - std::stringstream ss; - ss << "{w: " << q.w() << ", x: " << q.x() << ", y: " << q.y() << ", z: " << q.z() - << "}"; - return ss.str(); -} - -bool RosReconstruction::handleFreespaceSrv(hydra_msgs::QueryFreespace::Request& req, - hydra_msgs::QueryFreespace::Response& res) { - if (req.x.size() != req.y.size() || req.x.size() != req.z.size()) { - return false; - } - - if (req.x.empty()) { - return true; - } - - ReconstructionModule::PositionMatrix points(3, req.x.size()); - for (size_t i = 0; i < req.x.size(); ++i) { - points(0, i) = req.x[i]; - points(1, i) = req.y[i]; - points(2, i) = req.z[i]; - } - - const auto result = inFreespace(points, req.freespace_distance_m); - for (const auto flag : result) { - res.in_freespace.push_back(flag ? 1 : 0); - } - return true; -} - -RosReconstruction::RosReconstruction(const ros::NodeHandle& nh, - const RobotPrefixConfig& prefix, - const OutputQueue::Ptr& output_queue) - : ReconstructionModule( - prefix, - load_config(nh), - output_queue ? output_queue : std::make_shared()), - nh_(nh) { - ros_config_ = load_config(nh); - - buffer_.reset(new tf2_ros::Buffer(ros::Duration(ros_config_.tf_buffer_size_s))); - - if (!ros_config_.kimera_extrinsics_file.empty()) { - const auto node = YAML::LoadFile(ros_config_.kimera_extrinsics_file); - const auto elements = node["T_BS"]["data"].as>(); - CHECK_EQ(elements.size(), 16u) << "Invalid transform!"; - Eigen::Matrix4d body_T_camera; - for (size_t r = 0; r < 4; r++) { - for (size_t c = 0; c < 4; c++) { - body_T_camera(r, c) = elements.at(4 * r + c); - } - } - config_.body_R_camera = Eigen::Quaterniond(body_T_camera.block<3, 3>(0, 0)); - config_.body_t_camera = body_T_camera.block<3, 1>(0, 3); - LOG(WARNING) << "Loaded extrinsics from Kimera: R=" - << showQuaternion(config_.body_R_camera) - << ", t=" << config_.body_t_camera.transpose(); - } - - LOG(WARNING) << "Using pointcloud and TF as input!"; - pcl_sub_ = nh_.subscribe( - "pointcloud", 10, &RosReconstruction::handlePointcloud, this); - pose_graph_sub_ = - nh_.subscribe("pose_graph", 1000, &RosReconstruction::handlePoseGraph, this); - tf_listener_.reset(new tf2_ros::TransformListener(*buffer_)); - pointcloud_thread_.reset(new std::thread(&RosReconstruction::pointcloudSpin, this)); - - if (!ros_config_.enable_output_queue && !output_queue) { - // reset output queue so we don't waste memory with queued packets - output_queue_.reset(); - } - - if (ros_config_.visualize_reconstruction) { - visualizer_.reset(new TopologyServerVisualizer(ros_config_.topology_visualizer_ns)); - } - - if (ros_config_.publish_mesh) { - mesh_pub_ = nh_.advertise("mesh", 10); - } - - freespace_server_ = nh_.advertiseService( - "query_freespace", &RosReconstruction::handleFreespaceSrv, this); - - output_callbacks_.push_back( - [this](const ReconstructionModule&, const ReconstructionOutput& output) { - this->visualize(output); - }); -} - -RosReconstruction::~RosReconstruction() { - stop(); - - if (pointcloud_thread_) { - VLOG(2) << "[Hydra Reconstruction] stopping pointcloud input thread"; - pointcloud_thread_->join(); - pointcloud_thread_.reset(); - VLOG(2) << "[Hydra Reconstruction] stopped pointcloud input thread"; - } - - VLOG(2) << "[Hydra Reconstruction] pointcloud queue: " << pointcloud_queue_.size(); -} - -void RosReconstruction::handlePointcloud(const RosPointcloud::ConstPtr& cloud) { - // pcl timestamps are in microseconds - ros::Time curr_time; - curr_time.fromNSec(cloud->header.stamp * 1000); - - VLOG(1) << "[Hydra Reconstruction] Got raw pointcloud input @ " << curr_time.toNSec() - << " [ns]"; - if (num_poses_received_ > 0) { - if (last_time_received_ && ((curr_time - *last_time_received_).toSec() < - ros_config_.pointcloud_separation_s)) { - return; - } - - last_time_received_.reset(new ros::Time(curr_time)); - } - - VLOG(1) << "[Hydra Reconstruction] Got ROS input @ " << curr_time.toNSec() << " [ns]"; - - // TODO(nathan) consider setting prev_time_ here? - pointcloud_queue_.push(cloud); -} - -void RosReconstruction::handlePoseGraph(const PoseGraph::ConstPtr& pose_graph) { - if (pose_graph->nodes.empty()) { - ROS_ERROR("Received empty pose graph!"); - return; - } - - std::unique_lock lock(pose_graph_mutex_); - pose_graphs_.push_back(pose_graph); -} - -void RosReconstruction::pointcloudSpin() { - while (!should_shutdown_) { - bool has_data = pointcloud_queue_.poll(); - if (!has_data) { - continue; - } - - const auto cloud = pointcloud_queue_.pop(); - - ros::Time curr_time; - curr_time.fromNSec(cloud->header.stamp * 1000); - - VLOG(1) << "[Hydra Reconstruction] popped pointcloud input @ " << curr_time.toNSec() - << " [ns]"; - - ros::WallRate tf_wait_rate(1.0 / ros_config_.tf_wait_duration_s); - - // note that this is okay in a separate thread from the callback queue because tf2 - // is threadsafe - bool have_transform = false; - std::string err_str; - for (size_t i = 0; i < 5; ++i) { - if (buffer_->canTransform(config_.world_frame, - config_.robot_frame, - curr_time, - ros::Duration(0), - &err_str)) { - have_transform = true; - break; - } - - if (should_shutdown_) { - return; - } - - tf_wait_rate.sleep(); - } - - if (!have_transform) { - ROS_WARN_STREAM("Failed to get tf from " - << config_.robot_frame << " to " << config_.world_frame << " @ " - << curr_time.toNSec() << " [ns]. Reason: " << err_str); - continue; - } - - geometry_msgs::TransformStamped transform; - try { - transform = - buffer_->lookupTransform(config_.world_frame, config_.robot_frame, curr_time); - } catch (const tf2::TransformException& ex) { - LOG(ERROR) << "Failed to look up: " << config_.world_frame << " to " - << config_.robot_frame; - ROS_WARN_STREAM(ex.what()); - return; - } - - ReconstructionInput::Ptr input(new ReconstructionInput()); - input->timestamp_ns = curr_time.toNSec(); - - input->pointcloud.reset(new voxblox::Pointcloud()); - input->pointcloud_colors.reset(new voxblox::Colors()); - voxblox::convertPointcloud( - *cloud, nullptr, input->pointcloud.get(), input->pointcloud_colors.get()); - - geometry_msgs::Pose curr_pose = tfToPose(transform.transform); - tf2::convert(curr_pose.position, input->world_t_body); - tf2::convert(curr_pose.orientation, input->world_R_body); - - { // start pose graph critical section - std::unique_lock lock(pose_graph_mutex_); - input->pose_graphs = pose_graphs_; - pose_graphs_.clear(); - } // end pose graph critical section - - queue_->push(input); - } -} - -void RosReconstruction::visualize(const ReconstructionOutput& output) { - if (ros_config_.publish_mesh && output.mesh) { - hydra_msgs::ActiveMesh::ConstPtr msg(new hydra_msgs::ActiveMesh()); - auto mesh = const_cast(*msg); - mesh_pub_.publish(msg); - } - - if (visualizer_) { - visualizer_->visualize(gvd_integrator_->getGraph(), - gvd_integrator_->getGvdGraph(), - *gvd_, - *tsdf_, - output.timestamp_ns, - mesh_.get()); - if (config_.gvd.graph_extractor.use_compression_extractor) { - visualizer_->visualizeExtractor(output.timestamp_ns, - dynamic_cast( - gvd_integrator_->getGraphExtractor())); - } - } -} - -} // namespace hydra diff --git a/hydra_ros/src/utils/dsg_streaming_interface.cpp b/hydra_ros/src/utils/dsg_streaming_interface.cpp deleted file mode 100644 index ecbee564..00000000 --- a/hydra_ros/src/utils/dsg_streaming_interface.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/utils/dsg_streaming_interface.h" - -#include -#include -#include -#include -#include - -namespace hydra { - -DsgSender::DsgSender(const ros::NodeHandle& nh, - const std::string& timer_name, - bool publish_mesh, - double min_mesh_separation_s) - : nh_(nh), - timer_name_(timer_name), - publish_mesh_(publish_mesh), - min_mesh_separation_s_(min_mesh_separation_s) { - pub_ = nh_.advertise("dsg", 1); - if (publish_mesh_) { - mesh_pub_ = nh_.advertise("dsg_mesh", 1, false); - } -} - -void DsgSender::sendGraph(const DynamicSceneGraph& graph, - const ros::Time& stamp) const { - const uint64_t timestamp_ns = stamp.toNSec(); - timing::ScopedTimer timer(timer_name_, timestamp_ns); - - if (pub_.getNumSubscribers()) { - hydra_msgs::DsgUpdate msg; - msg.header.stamp = stamp; - spark_dsg::writeGraph(graph, msg.layer_contents); - msg.full_update = true; - pub_.publish(msg); - } - - if (!publish_mesh_ || !mesh_pub_.getNumSubscribers()) { - return; - } - - if (graph.isMeshEmpty()) { - return; - } - - if (last_mesh_time_ns_) { - std::chrono::nanoseconds diff_ns(timestamp_ns - *last_mesh_time_ns_); - std::chrono::duration diff_s = diff_ns; - if (diff_s.count() < min_mesh_separation_s_) { - return; - } - } - - last_mesh_time_ns_ = timestamp_ns; - - mesh_msgs::TriangleMeshStamped msg; - msg.header.stamp.fromNSec(timestamp_ns); - msg.header.frame_id = "world"; - msg.mesh = kimera_pgmo::PolygonMeshToTriangleMeshMsg(*graph.getMeshVertices(), - *graph.getMeshFaces()); - mesh_pub_.publish(msg); -} - -DsgReceiver::DsgReceiver(const ros::NodeHandle& nh) - : nh_(nh), has_update_(false), graph_(nullptr) { - sub_ = nh_.subscribe("dsg", 1, &DsgReceiver::handleUpdate, this); - mesh_sub_ = nh_.subscribe("dsg_mesh_updates", 1, &DsgReceiver::handleMesh, this); -} - -DsgReceiver::DsgReceiver(const ros::NodeHandle& nh, const LogCallback& log_cb) - : DsgReceiver(nh) { - log_callback_.reset(new LogCallback(log_cb)); -} - -void DsgReceiver::handleUpdate(const hydra_msgs::DsgUpdate::ConstPtr& msg) { - timing::ScopedTimer timer("receive_dsg", msg->header.stamp.toNSec()); - if (!msg->full_update) { - throw std::runtime_error("not implemented"); - } - - if (log_callback_) { - (*log_callback_)(msg->header.stamp, msg->layer_contents.size()); - } - - const auto size_bytes = getHumanReadableMemoryString(msg->layer_contents.size()); - ROS_INFO_STREAM("Received dsg update message of " << size_bytes); - try { - if (!graph_) { - graph_ = spark_dsg::readGraph(msg->layer_contents); - } else { - spark_dsg::updateGraph(*graph_, msg->layer_contents, true); - } - has_update_ = true; - } catch (const std::exception& e) { - ROS_FATAL_STREAM("Received invalid message: " << e.what()); - ros::shutdown(); - return; - } - - if (mesh_) { - graph_->setMeshDirectly(*mesh_); - } -} - -void DsgReceiver::handleMesh(const mesh_msgs::TriangleMeshStamped::ConstPtr& msg) { - timing::ScopedTimer timer("receive_mesh", msg->header.stamp.toNSec()); - if (!mesh_) { - mesh_.reset(new pcl::PolygonMesh()); - } - - *mesh_ = kimera_pgmo::TriangleMeshMsgToPolygonMesh(msg->mesh); - - if (graph_) { - graph_->setMeshDirectly(*mesh_); - } - - has_update_ = true; -} - -} // namespace hydra diff --git a/hydra_ros/src/utils/mask_nodelet.cpp b/hydra_ros/src/utils/mask_nodelet.cpp deleted file mode 100644 index 54b2ed9b..00000000 --- a/hydra_ros/src/utils/mask_nodelet.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include - -#include -#include - -namespace hydra { - -struct MaskNodelet : public nodelet::Nodelet { - void onInit() { - auto& pnh = getPrivateNodeHandle(); - - std::string mask_path = ""; - if (!pnh.getParam("mask_path", mask_path)) { - ROS_FATAL("mask path is required!"); - throw std::runtime_error("mask path not specified"); - } - - ROS_INFO_STREAM("Reading mask from " << mask_path); - mask_ = cv::imread(mask_path, cv::IMREAD_GRAYSCALE); - if (mask_.empty()) { - ROS_FATAL("invalid mask; mat is empty"); - throw std::runtime_error("invalid mask!"); - } - - auto& nh = getNodeHandle(); - transport_.reset(new image_transport::ImageTransport(nh)); - image_sub_ = - transport_->subscribe("depth/image_raw", 1, &MaskNodelet::callback, this); - image_pub_ = transport_->advertise("depth_masked/image_raw", 1); - } - - void callback(const sensor_msgs::ImageConstPtr& msg) { - cv_bridge::CvImageConstPtr img_ptr; - try { - img_ptr = cv_bridge::toCvShare(msg); - } catch (const cv_bridge::Exception& e) { - ROS_ERROR_STREAM("cv_bridge exception: " << e.what()); - return; - } - - if (!result_image_) { - result_image_.reset(new cv_bridge::CvImage()); - result_image_->encoding = img_ptr->encoding; - result_image_->image = - cv::Mat(img_ptr->image.rows, img_ptr->image.cols, img_ptr->image.type()); - } - - result_image_->image.setTo(0); - result_image_->header = img_ptr->header; - cv::bitwise_or(img_ptr->image, img_ptr->image, result_image_->image, mask_); - - image_pub_.publish(result_image_->toImageMsg()); - } - - std::unique_ptr transport_; - image_transport::Subscriber image_sub_; - image_transport::Publisher image_pub_; - - cv_bridge::CvImagePtr result_image_; - cv::Mat mask_; -}; - -} // namespace hydra - -PLUGINLIB_EXPORT_CLASS(hydra::MaskNodelet, nodelet::Nodelet) diff --git a/hydra_ros/src/utils/node_utilities.cpp b/hydra_ros/src/utils/node_utilities.cpp deleted file mode 100644 index 90596b04..00000000 --- a/hydra_ros/src/utils/node_utilities.cpp +++ /dev/null @@ -1,141 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/utils/node_utilities.h" - -#include -#include - -namespace hydra { - -using timing::ElapsedTimeRecorder; - -ExitMode getExitMode(const ros::NodeHandle& nh) { - std::string exit_mode_str = "NORMAL"; - nh.getParam("exit_mode", exit_mode_str); - - if (exit_mode_str == "CLOCK") { - return ExitMode::CLOCK; - } else if (exit_mode_str == "SERVICE") { - return ExitMode::SERVICE; - } else if (exit_mode_str == "NORMAL") { - return ExitMode::NORMAL; - } else { - ROS_WARN_STREAM("Unrecognized option: " << exit_mode_str - << ". Defaulting to NORMAL"); - return ExitMode::NORMAL; - } -} - -void spinWhileClockPresent() { - ros::WallRate r(50); - ROS_INFO("Waiting for bag to start"); - while (ros::ok() && !haveClock()) { - ros::spinOnce(); - r.sleep(); - } - - ROS_INFO("Running..."); - while (ros::ok() && haveClock()) { - ros::spinOnce(); - r.sleep(); - } - - ros::spinOnce(); // make sure all the callbacks are processed - ROS_WARN("Exiting!"); -} - -void spinUntilExitRequested() { - ServiceFunctor functor; - - ros::NodeHandle nh("~"); - ros::ServiceServer service = - nh.advertiseService("shutdown", &ServiceFunctor::callback, &functor); - - ros::WallRate r(50); - ROS_INFO("Running..."); - while (ros::ok() && !functor.should_exit) { - ros::spinOnce(); - r.sleep(); - } - - ros::spinOnce(); // make sure all the callbacks are processed - ROS_WARN("Exiting!"); -} - -std::string configureTimers(const ros::NodeHandle& nh) { - std::string dsg_output_path = ""; - nh.getParam("log_path", dsg_output_path); - - nh.getParam("timing_disabled", ElapsedTimeRecorder::instance().timing_disabled); - nh.getParam("disable_timer_output", ElapsedTimeRecorder::instance().disable_output); - - bool log_timing_incrementally = false; - nh.getParam("log_timing_incrementally", log_timing_incrementally); - if (log_timing_incrementally && dsg_output_path != "") { - ElapsedTimeRecorder::instance().setupIncrementalLogging(dsg_output_path); - } - - return dsg_output_path; -} - -void spinAndWait(const ros::NodeHandle& nh) { - const auto exit_mode = getExitMode(nh); - switch (exit_mode) { - case ExitMode::CLOCK: - spinWhileClockPresent(); - break; - case ExitMode::SERVICE: - spinUntilExitRequested(); - break; - case ExitMode::NORMAL: - default: - ros::spin(); - break; - } -} - -void saveTimingInformation(const std::string& dsg_output_path) { - if (dsg_output_path.empty()) { - return; - } - - LOG(INFO) << "[DSG Node] saving timing information to " << dsg_output_path; - const ElapsedTimeRecorder& timer = ElapsedTimeRecorder::instance(); - timer.logAllElapsed(dsg_output_path); - timer.logStats(dsg_output_path); - LOG(INFO) << "[DSG Node] Saved timing information to " << dsg_output_path; -} - -} // namespace hydra diff --git a/hydra_ros/src/visualizer/colormap_utilities.cpp b/hydra_ros/src/visualizer/colormap_utilities.cpp deleted file mode 100644 index 96344fb4..00000000 --- a/hydra_ros/src/visualizer/colormap_utilities.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/colormap_utilities.h" - -#include -#include - -namespace hydra { -namespace dsg_utils { - -inline double lerp(double min, double max, double ratio) { - return (max - min) * ratio + min; -} - -std_msgs::ColorRGBA makeColorMsg(const NodeColor& color, double alpha) { - std_msgs::ColorRGBA msg; - msg.r = static_cast(color(0)) / 255.0; - msg.g = static_cast(color(1)) / 255.0; - msg.b = static_cast(color(2)) / 255.0; - msg.a = alpha; - return msg; -} - -NodeColor getRgbFromHls(double hue, double luminance, double saturation) { - // make sure we clip the inputs to the expected range - hue = std::clamp(hue, 0.0, 1.0); - luminance = std::clamp(luminance, 0.0, 1.0); - saturation = std::clamp(saturation, 0.0, 1.0); - - cv::Mat hls_value(1, 1, CV_32FC3); - // hue is in degrees, not [0, 1] - hls_value.at(0) = hue * 360.0; - hls_value.at(1) = luminance; - hls_value.at(2) = saturation; - - cv::Mat bgr; - cv::cvtColor(hls_value, bgr, cv::COLOR_HLS2BGR); - - NodeColor color; - color(0, 0) = static_cast(255 * bgr.at(2)); - color(1, 0) = static_cast(255 * bgr.at(1)); - color(2, 0) = static_cast(255 * bgr.at(0)); - return color; -} - -NodeColor interpolateColorMap(const ColormapConfig& config, double ratio) { - // override ratio input to be in [0, 1] - ratio = std::clamp(ratio, 0.0, 1.0); - - cv::Mat hls_value(1, 1, CV_32FC3); - // hue is in degrees, not [0, 1] - hls_value.at(0) = lerp(config.min_hue, config.max_hue, ratio) * 360.0; - hls_value.at(1) = lerp(config.min_luminance, config.max_luminance, ratio); - hls_value.at(2) = lerp(config.min_saturation, config.max_saturation, ratio); - - cv::Mat bgr; - cv::cvtColor(hls_value, bgr, cv::COLOR_HLS2BGR); - - NodeColor color; - color(0, 0) = static_cast(255 * bgr.at(2)); - color(1, 0) = static_cast(255 * bgr.at(1)); - color(2, 0) = static_cast(255 * bgr.at(0)); - return color; -} - -} // namespace dsg_utils -} // namespace hydra diff --git a/hydra_ros/src/visualizer/dsg_mesh_plugins.cpp b/hydra_ros/src/visualizer/dsg_mesh_plugins.cpp deleted file mode 100644 index b3e5ec64..00000000 --- a/hydra_ros/src/visualizer/dsg_mesh_plugins.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/dsg_mesh_plugins.h" - -#include -#include -#include -#include -#include -#include - -namespace hydra { - -using visualization_msgs::Marker; - -struct BestIndex { - bool valid; - voxblox::BlockIndex index; -}; - -inline voxblox::Point getPoint(const pcl::PointXYZRGBA& point) { - voxblox::Point vox_point; - vox_point << point.x, point.y, point.z; - return vox_point; -} - -inline voxblox::BlockIndex getBlockIndex(const voxblox::Point& point) { - voxblox::BlockIndex index; - index << std::floor(point.x()), std::floor(point.y()), std::floor(point.z()); - return index; -} - -inline bool vectorHasNegativeElement(const voxblox::Point& point) { - return (point.array() < 0.0).any(); -} - -BestIndex getBestBlockIndex(const pcl::PointCloud& cloud, - const std::vector& indices) { - const voxblox::Point point1 = getPoint(cloud.at(indices.at(0))); - const voxblox::Point point2 = getPoint(cloud.at(indices.at(1))); - const voxblox::Point point3 = getPoint(cloud.at(indices.at(2))); - - const voxblox::BlockIndex index1 = getBlockIndex(point1); - const voxblox::BlockIndex index2 = getBlockIndex(point2); - const voxblox::BlockIndex index3 = getBlockIndex(point3); - - // TODO(nathan) this is ugly, fix - voxblox::BlockIndex best_index; - best_index << std::min(index1.x(), std::min(index2.x(), index3.x())), - std::min(index1.y(), std::min(index2.y(), index3.y())), - std::min(index1.z(), std::min(index2.z(), index3.z())); - - const voxblox::Point norm1 = (point1 - best_index.cast()) / 2.0; - if (norm1.squaredNorm() > 1.0 || (norm1.array() < 0).any()) { - LOG(ERROR) << "best index: " << best_index.transpose() << " is invalid"; - return {false, voxblox::BlockIndex::Zero()}; - } - - const voxblox::Point norm2 = (point2 - best_index.cast()) / 2.0; - if (norm2.squaredNorm() > 1.0 || (norm2.array() < 0).any()) { - LOG(ERROR) << "best index: " << best_index.transpose() << " is invalid"; - return {false, voxblox::BlockIndex::Zero()}; - } - - const voxblox::Point norm3 = (point3 - best_index.cast()) / 2.0; - if (norm3.squaredNorm() > 1.0 || (norm3.array() < 0).any()) { - LOG(ERROR) << "best index: " << best_index.transpose() << " is invalid"; - return {false, voxblox::BlockIndex::Zero()}; - } - - return {true, best_index}; -} - -inline geometry_msgs::Point pointFromPcl(const pcl::PointXYZRGBA& point) { - geometry_msgs::Point msg; - msg.x = point.x; - msg.y = point.y; - msg.z = point.z; - return msg; -} - -inline std_msgs::ColorRGBA colorFromPcl(const pcl::PointXYZRGBA& point) { - std_msgs::ColorRGBA msg; - msg.r = static_cast(point.r) / 255.0f; - msg.g = static_cast(point.g) / 255.0f; - msg.b = static_cast(point.b) / 255.0f; - msg.a = 1.0; - return msg; -} - -RvizMeshPlugin::RvizMeshPlugin(const ros::NodeHandle& nh, const std::string& name) - : DsgVisualizerPlugin(nh, name), name_(name), published_mesh_(false) { - ros::NodeHandle temp(nh); - mesh_pub_ = temp.advertise("mesh", 1, true); -} - -void RvizMeshPlugin::draw(const std_msgs::Header& header, - const DynamicSceneGraph& graph) { - if (!graph.hasMesh()) { - ROS_WARN("Attempting to visualize unitialized mesh"); - return; - } - - Marker msg; - msg.header = header; - msg.ns = name_; - msg.id = 0; - msg.type = Marker::TRIANGLE_LIST; - msg.action = Marker::ADD; - - Eigen::Vector3d origin = Eigen::Vector3d::Zero(); - tf2::convert(origin, msg.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), msg.pose.orientation); - - msg.scale.x = 1.0; - msg.scale.y = 1.0; - msg.scale.z = 1.0; - - const auto& vertices = *graph.getMeshVertices(); - const auto& faces = *graph.getMeshFaces(); - - for (const auto& face : faces) { - for (const auto& vertex_idx : face.vertices) { - const auto& point = vertices.at(vertex_idx); - msg.points.push_back(pointFromPcl(point)); - msg.colors.push_back(colorFromPcl(point)); - } - } - - mesh_pub_.publish(msg); - published_mesh_ = true; -} - -void RvizMeshPlugin::reset(const std_msgs::Header& header, const DynamicSceneGraph&) { - Marker msg; - msg.header = header; - msg.ns = name_; - msg.id = 0; - msg.action = Marker::DELETE; - - mesh_pub_.publish(msg); -} - -PgmoMeshPlugin::PgmoMeshPlugin(const ros::NodeHandle& nh, const std::string& name) - : DsgVisualizerPlugin(nh, name) { - // namespacing gives us a reasonable topic - mesh_pub_ = nh_.advertise("", 1, true); -} - -void PgmoMeshPlugin::draw(const std_msgs::Header& header, - const DynamicSceneGraph& graph) { - if (!graph.hasMesh()) { - return; - } - - if (!graph.getMeshVertices()->size()) { - return; - } - - mesh_msgs::TriangleMeshStamped msg; - msg.header = header; - - // vertices and meshes are guaranteed to not be null (from hasMesh) - msg.mesh = kimera_pgmo::PolygonMeshToTriangleMeshMsg(*graph.getMeshVertices(), - *graph.getMeshFaces()); - mesh_pub_.publish(msg); -} - -void PgmoMeshPlugin::reset(const std_msgs::Header& header, const DynamicSceneGraph&) { - mesh_msgs::TriangleMeshStamped msg; - msg.header = header; - mesh_pub_.publish(msg); -} - -VoxbloxMeshPlugin::VoxbloxMeshPlugin(const ros::NodeHandle& nh, const std::string& name) - : DsgVisualizerPlugin(nh, name) { - // namespacing gives us a reasonable topic - mesh_pub_ = nh_.advertise("", 1, true); -} - -void VoxbloxMeshPlugin::draw(const std_msgs::Header& header, - const DynamicSceneGraph& graph) { - if (!graph.hasMesh()) { - return; - } - - voxblox_msgs::Mesh msg; - msg.header = header; - msg.block_edge_length = 1.0; - - voxblox::AnyIndexHashMapType::type block_to_index; - - const auto& vertices = *graph.getMeshVertices(); - const auto& faces = *graph.getMeshFaces(); - - curr_blocks_.clear(); - - for (const auto& face : faces) { - const auto block_idx_result = getBestBlockIndex(vertices, face.vertices); - if (!block_idx_result.valid) { - continue; - } - - const auto block_idx = block_idx_result.index; - if (!block_to_index.count(block_idx)) { - block_to_index[block_idx] = msg.mesh_blocks.size(); - voxblox_msgs::MeshBlock block_msg; - block_msg.index[0] = block_idx.x(); - block_msg.index[1] = block_idx.y(); - block_msg.index[2] = block_idx.z(); - msg.mesh_blocks.push_back(block_msg); - curr_blocks_.push_back(block_idx); - } - - auto& block = msg.mesh_blocks.at(block_to_index.at(block_idx)); - const voxblox::Point block_pos = block_idx.cast(); - - for (const auto& vertex_idx : face.vertices) { - const auto& point = vertices.at(vertex_idx); - voxblox::Point vox_point; - vox_point << point.x, point.y, point.z; - const voxblox::Point normalized_point = (vox_point - block_pos) / 2.0f; - - const uint16_t max_value = std::numeric_limits::max(); - block.x.push_back(max_value * normalized_point.x()); - block.y.push_back(max_value * normalized_point.y()); - block.z.push_back(max_value * normalized_point.z()); - block.r.push_back(point.r); - block.g.push_back(point.g); - block.b.push_back(point.b); - } - } - - mesh_pub_.publish(msg); -} - -void VoxbloxMeshPlugin::reset(const std_msgs::Header& header, - const DynamicSceneGraph&) { - if (curr_blocks_.empty()) { - return; - } - - voxblox_msgs::Mesh msg; - msg.header = header; - msg.block_edge_length = 1.0; - - for (const auto& block_idx : curr_blocks_) { - voxblox_msgs::MeshBlock block_msg; - block_msg.index[0] = block_idx.x(); - block_msg.index[1] = block_idx.y(); - block_msg.index[2] = block_idx.z(); - msg.mesh_blocks.push_back(block_msg); - } - - curr_blocks_.clear(); - mesh_pub_.publish(msg); -} - -} // namespace hydra diff --git a/hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp b/hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp deleted file mode 100644 index b88a8cd9..00000000 --- a/hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp +++ /dev/null @@ -1,632 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/dynamic_scene_graph_visualizer.h" - -#include - -#include "hydra_ros/visualizer/colormap_utilities.h" -#include "hydra_ros/visualizer/visualizer_utilities.h" - -namespace hydra { - -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; -using Node = SceneGraphNode; - -void clearPrevMarkers(const std_msgs::Header& header, - const std::set& curr_nodes, - const std::string& ns, - std::set& prev_nodes, - MarkerArray& msg) { - for (const auto& node : prev_nodes) { - if (curr_nodes.count(node)) { - continue; - } - - Marker marker = makeDeleteMarker(header, node, ns); - msg.markers.push_back(marker); - } - - prev_nodes = curr_nodes; -} - -DynamicSceneGraphVisualizer::DynamicSceneGraphVisualizer( - const ros::NodeHandle& nh, const DynamicSceneGraph::LayerIds& layer_ids) - : nh_(nh), - need_redraw_(false), - periodic_redraw_(false), - world_frame_("world"), - visualizer_ns_(nh.resolveName("config")), - visualizer_layer_ns_(nh.resolveName("config/layer")) { - nh_.param("world_frame", world_frame_, world_frame_); - nh_.param("visualizer_ns", visualizer_ns_, visualizer_ns_); - nh_.param("visualizer_layer_ns", visualizer_layer_ns_, visualizer_layer_ns_); - - dsg_pub_ = nh_.advertise("dsg_markers", 1, true); - - setupConfigs(layer_ids); - - for (const auto& id : layer_ids) { - prev_labels_[id] = {}; - curr_labels_[id] = {}; - } - - dynamic_layers_viz_pub_ = nh_.advertise("dynamic_layers_viz", 1, true); -} - -void DynamicSceneGraphVisualizer::start(bool periodic_redraw) { - periodic_redraw_ = periodic_redraw; - double visualizer_loop_period = 1.0e-1; - nh_.param("visualizer_loop_period", visualizer_loop_period, visualizer_loop_period); - visualizer_loop_timer_ = - nh_.createWallTimer(ros::WallDuration(visualizer_loop_period), - &DynamicSceneGraphVisualizer::displayLoop, - this); -} - -void DynamicSceneGraphVisualizer::reset() { - if (scene_graph_) { - std_msgs::Header header; - header.stamp = ros::Time::now(); - header.frame_id = world_frame_; - - MarkerArray msg; - resetImpl(header, msg); - - if (!msg.markers.empty()) { - dsg_pub_.publish(msg); - } - } - - scene_graph_.reset(); -} - -bool DynamicSceneGraphVisualizer::redraw() { - if (!scene_graph_) { - return false; - } - - need_redraw_ |= hasConfigChanged(); - - if (!need_redraw_) { - return false; - } - need_redraw_ = false; - - std_msgs::Header header; - header.stamp = ros::Time::now(); - header.frame_id = world_frame_; - - MarkerArray msg; - redrawImpl(header, msg); - - if (!msg.markers.empty()) { - dsg_pub_.publish(msg); - } - - clearConfigChangeFlags(); - return true; -} - -void DynamicSceneGraphVisualizer::setGraph(const DynamicSceneGraph::Ptr& scene_graph, - bool need_reset) { - if (scene_graph == nullptr) { - ROS_ERROR("Request to visualize invalid scene graph! Ignoring"); - return; - } - - if (need_reset) { - reset(); - } - - scene_graph_ = scene_graph; - need_redraw_ = true; -} - -const DynamicLayerConfig& DynamicSceneGraphVisualizer::getConfig(LayerId layer) { - if (!dynamic_configs_.count(layer)) { - const std::string ns = visualizer_ns_ + "/dynamic_layer/" + std::to_string(layer); - - ros::NodeHandle nh(""); - dynamic_configs_[layer] = std::make_shared(nh, ns); - } - - return dynamic_configs_.at(layer)->get(); -} - -inline double getDynamicHue(const DynamicLayerConfig& config, char prefix) { - // distance is measured from first relatively readable character prefix - int color_num = (std::abs((prefix - '0')) + config.color_offset) % config.num_colors; - return static_cast(color_num) / static_cast(config.num_colors); -} - -NodeColor getNodeColor(const DynamicLayerConfig& config, char prefix) { - const double hue = getDynamicHue(config, prefix); - return dsg_utils::getRgbFromHls(hue, config.luminance, config.saturation); -} - -NodeColor getEdgeColor(const DynamicLayerConfig& config, char prefix) { - const double hue = getDynamicHue(config, prefix); - const double saturation = config.saturation * config.edge_sl_ratio; - const double luminance = config.luminance * config.edge_sl_ratio; - return dsg_utils::getRgbFromHls(hue, saturation, luminance); -} - -void DynamicSceneGraphVisualizer::drawDynamicLayer(const std_msgs::Header& header, - const DynamicSceneGraphLayer& layer, - const DynamicLayerConfig& config, - const VisualizerConfig& viz_config, - size_t viz_idx, - MarkerArray& msg) { - const std::string node_ns = getDynamicNodeNamespace(layer.prefix); - Marker nodes = makeDynamicCentroidMarkers(header, - config, - layer, - viz_config, - getNodeColor(config, layer.prefix), - node_ns, - viz_idx); - addMultiMarkerIfValid(nodes, msg); - - const std::string edge_ns = getDynamicEdgeNamespace(layer.prefix); - Marker edges = makeDynamicEdgeMarkers(header, - config, - layer, - viz_config, - getEdgeColor(config, layer.prefix), - edge_ns, - viz_idx); - addMultiMarkerIfValid(edges, msg); - - if (layer.numNodes() == 0) { - deleteLabel(header, layer.prefix, msg); - return; - } - - const std::string label_ns = getDynamicLabelNamespace(layer.prefix); - Marker label = - makeDynamicLabelMarker(header, config, layer, viz_config, label_ns, viz_idx); - msg.markers.push_back(label); - published_dynamic_labels_.insert(label_ns); -} - -void DynamicSceneGraphVisualizer::deleteLabel(const std_msgs::Header& header, - char prefix, - MarkerArray& msg) { - const std::string label_ns = getDynamicLabelNamespace(prefix); - - if (published_dynamic_labels_.count(label_ns)) { - Marker marker = makeDeleteMarker(header, 0, label_ns); - msg.markers.push_back(marker); - } - published_dynamic_labels_.erase(label_ns); -} - -void DynamicSceneGraphVisualizer::deleteDynamicLayer(const std_msgs::Header& header, - char prefix, - MarkerArray& msg) { - const std::string node_ns = getDynamicNodeNamespace(prefix); - deleteMultiMarker(header, node_ns, msg); - - const std::string edge_ns = getDynamicEdgeNamespace(prefix); - deleteMultiMarker(header, edge_ns, msg); - - deleteLabel(header, prefix, msg); -} - -void DynamicSceneGraphVisualizer::drawDynamicLayers(const std_msgs::Header& header, - MarkerArray& msg) { - const VisualizerConfig& viz_config = visualizer_config_->get(); - - for (const auto& id_layer_map_pair : scene_graph_->dynamicLayers()) { - const LayerId layer_id = id_layer_map_pair.first; - if (!layer_configs_.count(layer_id)) { - continue; - } - - const DynamicLayerConfig& config = getConfig(layer_id); - - size_t viz_layer_idx = 0; - for (const auto& prefix_layer_pair : id_layer_map_pair.second) { - if (!config.visualize) { - deleteDynamicLayer(header, prefix_layer_pair.first, msg); - continue; - } - - const DynamicSceneGraphLayer& layer = *prefix_layer_pair.second; - drawDynamicLayer(header, layer, config, viz_config, viz_layer_idx, msg); - viz_layer_idx++; - } - } -} - -void DynamicSceneGraphVisualizer::resetImpl(const std_msgs::Header& header, - MarkerArray& msg) { - auto to_delete = published_multimarkers_; - for (const auto& ns : to_delete) { - deleteMultiMarker(header, ns, msg); - } - - for (const auto& id_layer_pair : scene_graph_->layers()) { - const LayerId layer_id = id_layer_pair.first; - const std::string label_ns = getLayerLabelNamespace(layer_id); - clearPrevMarkers(header, {}, label_ns, prev_labels_.at(layer_id), msg); - } - - for (auto& label_set : prev_labels_) { - label_set.second.clear(); - } - for (auto& label_set : curr_labels_) { - label_set.second.clear(); - } - - // vanilla scene graph also makes delete markers for dynamic layers, so we duplicate - // them here (rviz checks for topic / namespace coherence) - MarkerArray dynamic_msg = msg; - auto to_delete_dynamic = published_dynamic_labels_; - for (const auto& ns : to_delete_dynamic) { - Marker marker = makeDeleteMarker(header, 0, ns); - dynamic_msg.markers.push_back(marker); - } - - if (!dynamic_msg.markers.empty()) { - dynamic_layers_viz_pub_.publish(dynamic_msg); - } - - published_dynamic_labels_.clear(); - - for (const auto& plugin : plugins_) { - plugin->reset(header, *scene_graph_); - } -} - -void DynamicSceneGraphVisualizer::redrawImpl(const std_msgs::Header& header, - MarkerArray& msg) { - for (const auto& id_layer_pair : scene_graph_->layers()) { - if (!layer_configs_.count(id_layer_pair.first)) { - continue; - } - - LayerConfig config = layer_configs_.at(id_layer_pair.first)->get(); - const SceneGraphLayer& layer = *(id_layer_pair.second); - - if (!config.visualize) { - deleteLayer(header, layer, msg); - } else { - drawLayer(header, layer, config, msg); - } - } - - if (visualizer_config_->get().draw_mesh_edges) { - drawLayerMeshEdges(header, mesh_edge_source_layer_, mesh_edge_ns_, msg); - } - - std::map all_configs; - for (const auto& id_manager_pair : layer_configs_) { - all_configs[id_manager_pair.first] = id_manager_pair.second->get(); - } - - MarkerArray interlayer_edge_markers = - makeGraphEdgeMarkers(header, - *scene_graph_, - all_configs, - visualizer_config_->get(), - interlayer_edge_ns_prefix_); - - std::set seen_edge_labels; - for (const auto& marker : interlayer_edge_markers.markers) { - addMultiMarkerIfValid(marker, msg); - seen_edge_labels.insert(marker.ns); - } - - for (const auto& source_pair : all_configs) { - for (const auto& target_pair : all_configs) { - if (source_pair.first == target_pair.first) { - continue; - } - - const std::string curr_ns = interlayer_edge_ns_prefix_ + - std::to_string(source_pair.first) + "_" + - std::to_string(target_pair.first); - if (seen_edge_labels.count(curr_ns)) { - continue; - } - - deleteMultiMarker(header, curr_ns, msg); - } - } - - MarkerArray dynamic_markers; - drawDynamicLayers(header, dynamic_markers); - - std::map all_dynamic_configs; - for (const auto& id_manager_pair : dynamic_configs_) { - all_dynamic_configs[id_manager_pair.first] = id_manager_pair.second->get(); - } - - const std::string dynamic_interlayer_edge_prefix = "dynamic_interlayer_edges_"; - MarkerArray dynamic_interlayer_edge_markers = - makeDynamicGraphEdgeMarkers(header, - *scene_graph_, - all_configs, - all_dynamic_configs, - visualizer_config_->get(), - dynamic_interlayer_edge_prefix); - - std::set seen_dyn_edge_labels; - for (const auto& marker : dynamic_interlayer_edge_markers.markers) { - addMultiMarkerIfValid(marker, msg); - seen_dyn_edge_labels.insert(marker.ns); - } - - for (const auto& source_pair : all_configs) { - for (const auto& target_pair : all_dynamic_configs) { - std::string source_to_target_ns = dynamic_interlayer_edge_prefix + - std::to_string(source_pair.first) + "_" + - std::to_string(target_pair.first); - if (!seen_dyn_edge_labels.count(source_to_target_ns)) { - deleteMultiMarker(header, source_to_target_ns, msg); - } - - std::string target_to_source_ns = dynamic_interlayer_edge_prefix + - std::to_string(target_pair.first) + "_" + - std::to_string(source_pair.first); - if (!seen_dyn_edge_labels.count(target_to_source_ns)) { - deleteMultiMarker(header, target_to_source_ns, msg); - } - } - } - - if (!dynamic_markers.markers.empty()) { - dynamic_layers_viz_pub_.publish(dynamic_markers); - } - - // TODO(nathan) move to scene graph probably - for (const auto& plugin : plugins_) { - plugin->draw(header, *scene_graph_); - } -} - -bool DynamicSceneGraphVisualizer::hasConfigChanged() const { - bool has_changed = false; - - has_changed |= visualizer_config_->hasChange(); - has_changed |= places_colormap_->hasChange(); - for (const auto& id_manager_pair : layer_configs_) { - has_changed |= id_manager_pair.second->hasChange(); - } - - for (const auto& id_manager_pair : dynamic_configs_) { - has_changed |= id_manager_pair.second->hasChange(); - } - - return has_changed; -} - -void DynamicSceneGraphVisualizer::clearConfigChangeFlags() { - visualizer_config_->clearChangeFlag(); - places_colormap_->clearChangeFlag(); - for (auto& id_manager_pair : layer_configs_) { - id_manager_pair.second->clearChangeFlag(); - } - - for (auto& id_manager_pair : dynamic_configs_) { - id_manager_pair.second->clearChangeFlag(); - } -} - -void DynamicSceneGraphVisualizer::deleteMultiMarker(const std_msgs::Header& header, - const std::string& ns, - MarkerArray& msg) { - if (!published_multimarkers_.count(ns)) { - return; - } - - Marker delete_marker = makeDeleteMarker(header, 0, ns); - msg.markers.push_back(delete_marker); - - published_multimarkers_.erase(ns); -} - -void DynamicSceneGraphVisualizer::addMultiMarkerIfValid(const Marker& marker, - MarkerArray& msg) { - if (!marker.points.empty()) { - msg.markers.push_back(marker); - published_multimarkers_.insert(marker.ns); - return; - } - - deleteMultiMarker(marker.header, marker.ns, msg); -} - -void DynamicSceneGraphVisualizer::setupConfigs( - const DynamicSceneGraph::LayerIds& layer_ids) { - ros::NodeHandle nh(""); - visualizer_config_.reset(new VisualizerConfigManager(nh, visualizer_ns_)); - - const std::string colormap_ns = visualizer_ns_ + "/places_colormap"; - places_colormap_.reset(new ColormapConfigManager(nh, colormap_ns)); - - for (const auto& layer : layer_ids) { - const std::string layer_ns = visualizer_layer_ns_ + std::to_string(layer); - layer_configs_[layer] = std::make_shared(nh, layer_ns); - } -} - -void DynamicSceneGraphVisualizer::displayLoop(const ros::WallTimerEvent&) { - if (periodic_redraw_) { - need_redraw_ = true; - } - redraw(); -} - -void DynamicSceneGraphVisualizer::deleteLayer(const std_msgs::Header& header, - const SceneGraphLayer& layer, - MarkerArray& msg) { - deleteMultiMarker(header, getLayerNodeNamespace(layer.id), msg); - deleteMultiMarker(header, getLayerEdgeNamespace(layer.id), msg); - deleteMultiMarker(header, getLayerBboxNamespace(layer.id), msg); - - const std::string label_ns = getLayerLabelNamespace(layer.id); - for (const auto& node : prev_labels_.at(layer.id)) { - Marker marker = makeDeleteMarker(header, node, label_ns); - msg.markers.push_back(marker); - } - prev_labels_.at(layer.id).clear(); -} - -void DynamicSceneGraphVisualizer::drawLayer(const std_msgs::Header& header, - const SceneGraphLayer& layer, - const LayerConfig& config, - MarkerArray& msg) { - const auto& viz_config = visualizer_config_->get(); - const std::string node_ns = getLayerNodeNamespace(layer.id); - - const bool color_by_distance = - layer.id == DsgLayers::PLACES && viz_config.color_places_by_distance; - - Marker nodes; - if (viz_config.color_nodes_by_active_flag) { - nodes = makeCentroidMarkers( - header, - config, - layer, - viz_config, - node_ns, - [&](const SceneGraphNode& node) -> NodeColor { - return node.attributes().is_active ? NodeColor(0, 255, 0) : NodeColor::Zero(); - }); - } else if (color_by_distance) { - nodes = makeCentroidMarkers( - header, config, layer, viz_config, node_ns, places_colormap_->get()); - } else if (layer.id == DsgLayers::PLACES) { - nodes = makeCentroidMarkers(header, - config, - layer, - viz_config, - node_ns, - [&](const SceneGraphNode& node) -> NodeColor { - auto parent = node.getParent(); - if (!parent) { - return NodeColor::Zero(); - } - - return scene_graph_->getNode(*parent) - .value() - .get() - .attributes() - .color; - }); - } else { - nodes = makeCentroidMarkers(header, config, layer, viz_config, node_ns); - } - addMultiMarkerIfValid(nodes, msg); - - const std::string edge_ns = getLayerEdgeNamespace(layer.id); - Marker edges = makeLayerEdgeMarkers( - header, config, layer, viz_config, NodeColor::Zero(), edge_ns); - addMultiMarkerIfValid(edges, msg); - - const std::string label_ns = getLayerLabelNamespace(layer.id); - - curr_labels_.at(layer.id).clear(); - for (const auto& id_node_pair : layer.nodes()) { - const Node& node = *id_node_pair.second; - - if (config.use_label) { - Marker label = makeTextMarker(header, config, node, viz_config, label_ns); - msg.markers.push_back(label); - curr_labels_.at(layer.id).insert(node.id); - } - } - - if (config.use_bounding_box) { - const std::string bbox_ns = getLayerBboxNamespace(layer.id); - const std::string bbox_edge_ns = getLayerBboxEdgeNamespace(layer.id); - try { - Marker bbox = - makeLayerWireframeBoundingBoxes(header, config, layer, viz_config, bbox_ns); - addMultiMarkerIfValid(bbox, msg); - - if (config.collapse_bounding_box) { - Marker bbox_edges = - makeEdgesToBoundingBoxes(header, config, layer, viz_config, bbox_edge_ns); - addMultiMarkerIfValid(bbox_edges, msg); - } - } catch (const std::bad_cast&) { - // TODO(nathan) consider warning - return; - } - for (const auto& id_node_pair : layer.nodes()) { - const Node& node = *id_node_pair.second; - - Marker label = makeTextMarkerNoHeight(header, config, node, viz_config, label_ns); - msg.markers.push_back(label); - curr_labels_.at(layer.id).insert(node.id); - } - } - - clearPrevMarkers( - header, curr_labels_.at(layer.id), label_ns, prev_labels_.at(layer.id), msg); -} - -void DynamicSceneGraphVisualizer::drawLayerMeshEdges(const std_msgs::Header& header, - LayerId layer_id, - const std::string& ns, - MarkerArray& msg) { - if (!scene_graph_->hasLayer(layer_id)) { - return; - } - - if (!layer_configs_.count(layer_id)) { - return; - } - - LayerConfig config = layer_configs_.at(layer_id)->get(); - if (!config.visualize) { - deleteMultiMarker(header, ns, msg); - return; - } - - Marker mesh_edges = makeMeshEdgesMarker(header, - config, - visualizer_config_->get(), - *scene_graph_, - scene_graph_->getLayer(layer_id), - ns); - addMultiMarkerIfValid(mesh_edges, msg); -} - -} // namespace hydra diff --git a/hydra_ros/src/visualizer/gvd_visualization_utilities.cpp b/hydra_ros/src/visualizer/gvd_visualization_utilities.cpp deleted file mode 100644 index b1265497..00000000 --- a/hydra_ros/src/visualizer/gvd_visualization_utilities.cpp +++ /dev/null @@ -1,735 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/gvd_visualization_utilities.h" - -#include - -#include - -#include "hydra_ros/visualizer/colormap_utilities.h" - -namespace hydra { - -using places::GvdGraph; -using places::GvdVoxel; -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; -using voxblox::BlockIndexList; -using voxblox::FloatingPoint; -using voxblox::Layer; -using voxblox::MeshLayer; -using voxblox::TsdfVoxel; - -MarkerGroupPub::MarkerGroupPub(const ros::NodeHandle& nh) : nh_(nh) {} - -void MarkerGroupPub::publish(const std::string& name, - const MarkerCallback& func) const { - publish(name, [&func](MarkerArray& msg) { - Marker marker; - msg.markers.push_back(marker); - return func(msg.markers.back()); - }); -} - -void MarkerGroupPub::publish(const std::string& name, const ArrayCallback& func) const { - auto iter = pubs_.find(name); - if (iter == pubs_.end()) { - iter = pubs_.emplace(name, nh_.advertise(name, 1, true)).first; - } - - if (!iter->second.getNumSubscribers()) { - return; // avoid doing computation if we don't need to publish - } - - MarkerArray msg; - if (func(msg)) { - iter->second.publish(msg); - } -} - -double computeRatio(double min, double max, double value) { - double ratio = (value - min) / (max - min); - ratio = !std::isfinite(ratio) ? 0.0 : ratio; - return ratio; -} - -double getRatioFromDistance(const GvdVisualizerConfig& config, const GvdVoxel& voxel) { - return computeRatio(config.gvd_min_distance, config.gvd_max_distance, voxel.distance); -} - -double getRatioFromBasisPoints(const GvdVisualizerConfig& config, - const GvdVoxel& voxel) { - return computeRatio(static_cast(config.min_num_basis), - static_cast(config.max_num_basis), - static_cast(voxel.num_extra_basis)); -} - -double getRatio(const GvdVisualizerConfig& config, const GvdVoxel& voxel) { - switch (static_cast(config.gvd_mode)) { - case GvdVisualizationMode::BASIS_POINTS: - return getRatioFromBasisPoints(config, voxel); - case GvdVisualizationMode::DISTANCE: - case GvdVisualizationMode::DEFAULT: - default: - return getRatioFromDistance(config, voxel); - } - return 0.0; -} - -Marker makeGvdMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const Layer& layer) { - BlockIndexList blocks; - layer.getAllAllocatedBlocks(&blocks); - - Marker marker; - marker.type = Marker::CUBE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = "gvd_markers"; - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - marker.scale.x = layer.voxel_size(); - marker.scale.y = layer.voxel_size(); - marker.scale.z = layer.voxel_size(); - - for (const auto& idx : blocks) { - const auto& block = layer.getBlockByIndex(idx); - for (size_t i = 0; i < block.num_voxels(); ++i) { - const auto& voxel = block.getVoxelByLinearIndex(i); - if (!voxel.observed || voxel.num_extra_basis < config.basis_threshold) { - continue; - } - - Eigen::Vector3d voxel_pos = - block.computeCoordinatesFromLinearIndex(i).cast(); - geometry_msgs::Point marker_pos; - tf2::convert(voxel_pos, marker_pos); - marker.points.push_back(marker_pos); - - double ratio = getRatio(config, voxel); - NodeColor color = dsg_utils::interpolateColorMap(colors, ratio); - - std_msgs::ColorRGBA color_msg = dsg_utils::makeColorMsg(color, config.gvd_alpha); - marker.colors.push_back(color_msg); - } - } - - return marker; -} - -Marker makeErrorMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const Layer& lhs, - const Layer& rhs, - double threshold) { - Marker marker; - marker.type = Marker::CUBE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = "error_locations"; - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - marker.scale.x = lhs.voxel_size(); - marker.scale.y = lhs.voxel_size(); - marker.scale.z = lhs.voxel_size(); - - BlockIndexList blocks; - lhs.getAllAllocatedBlocks(&blocks); - - for (const auto& idx : blocks) { - if (!rhs.hasBlock(idx)) { - continue; - } - - const auto& lhs_block = lhs.getBlockByIndex(idx); - const auto& rhs_block = rhs.getBlockByIndex(idx); - - for (size_t i = 0; i < lhs_block.num_voxels(); ++i) { - const auto& lvoxel = lhs_block.getVoxelByLinearIndex(i); - const auto& rvoxel = rhs_block.getVoxelByLinearIndex(i); - - if (!lvoxel.observed || !rvoxel.observed) { - continue; - } - - const double error = std::abs(lvoxel.distance - rvoxel.distance); - if (error <= threshold) { - continue; - } - - double ratio = computeRatio(0, 10, error); - NodeColor color = dsg_utils::interpolateColorMap(colors, ratio); - - Eigen::Vector3d voxel_pos = - lhs_block.computeCoordinatesFromLinearIndex(i).cast(); - geometry_msgs::Point marker_pos; - tf2::convert(voxel_pos, marker_pos); - marker.points.push_back(marker_pos); - - std_msgs::ColorRGBA color_msg = dsg_utils::makeColorMsg(color, config.gvd_alpha); - marker.colors.push_back(color_msg); - } - } - - return marker; -} - -Marker makeSurfaceVoxelMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const Layer& layer) { - BlockIndexList blocks; - layer.getAllAllocatedBlocks(&blocks); - - Marker marker; - marker.type = Marker::CUBE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = "surface_markers"; - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - marker.scale.x = layer.voxel_size(); - marker.scale.y = layer.voxel_size(); - marker.scale.z = layer.voxel_size(); - - for (const auto& idx : blocks) { - const auto& block = layer.getBlockByIndex(idx); - for (size_t i = 0; i < block.num_voxels(); ++i) { - const auto& voxel = block.getVoxelByLinearIndex(i); - if (!voxel.on_surface) { - continue; - } - - Eigen::Vector3d voxel_pos = - block.computeCoordinatesFromLinearIndex(i).cast(); - geometry_msgs::Point marker_pos; - tf2::convert(voxel_pos, marker_pos); - marker.points.push_back(marker_pos); - - double ratio = computeRatio(-0.4, 0.4, voxel.distance); - NodeColor color = dsg_utils::interpolateColorMap(colors, ratio); - - std_msgs::ColorRGBA color_msg = dsg_utils::makeColorMsg(color, config.gvd_alpha); - marker.colors.push_back(color_msg); - } - } - - return marker; -} - -Marker makeEsdfMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const Layer& layer) { - BlockIndexList blocks; - layer.getAllAllocatedBlocks(&blocks); - - Marker marker; - marker.type = Marker::CUBE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = "esdf_slice_markers"; - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - marker.scale.x = layer.voxel_size(); - marker.scale.y = layer.voxel_size(); - marker.scale.z = layer.voxel_size(); - - const FloatingPoint voxel_size = layer.voxel_size(); - const FloatingPoint half_voxel_size = voxel_size / 2.0; - // rounds down and points the slice at the middle of the nearest voxel boundary - const FloatingPoint slice_height = - std::floor(config.slice_height / voxel_size) * voxel_size + half_voxel_size; - - for (const auto& idx : blocks) { - const auto& block = layer.getBlockByIndex(idx); - for (size_t i = 0; i < block.num_voxels(); ++i) { - const auto& voxel = block.getVoxelByLinearIndex(i); - if (!voxel.observed) { - continue; - } - - Eigen::Vector3d voxel_pos = - block.computeCoordinatesFromLinearIndex(i).cast(); - if (voxel_pos(2) < slice_height - half_voxel_size || - voxel_pos(2) > slice_height + half_voxel_size) { - continue; - } - - geometry_msgs::Point marker_pos; - tf2::convert(voxel_pos, marker_pos); - marker.points.push_back(marker_pos); - - double ratio = computeRatio( - config.esdf_min_distance, config.esdf_max_distance, voxel.distance); - NodeColor color = dsg_utils::interpolateColorMap(colors, ratio); - - std_msgs::ColorRGBA color_msg = dsg_utils::makeColorMsg(color, config.esdf_alpha); - marker.colors.push_back(color_msg); - } - } - - return marker; -} - -inline Eigen::Vector3d getOffset(double side_length, - bool x_high, - bool y_high, - bool z_high) { - Eigen::Vector3d offset; - offset(0) = x_high ? side_length : 0.0; - offset(1) = y_high ? side_length : 0.0; - offset(2) = z_high ? side_length : 0.0; - return offset; -} - -geometry_msgs::Point getPointFromMatrix(const Eigen::MatrixXd& matrix, int col) { - geometry_msgs::Point point; - tf2::convert(matrix.block<3, 1>(0, col).eval(), point); - return point; -} - -void fillMarkerFromBlock(Marker& marker, - const Eigen::Vector3d& position, - double side_length) { - Eigen::MatrixXd corners(3, 8); - for (int c = 0; c < corners.cols(); ++c) { - // x: lsb, y: second lsb, z: third lsb - corners.block<3, 1>(0, c) = - position + - getOffset(side_length, ((c & 0x01) != 0), ((c & 0x02) != 0), ((c & 0x04) != 0)); - } - - for (int c = 0; c < corners.cols(); ++c) { - // edges are 1-bit pertubations - int x_neighbor = c | 0x01; - int y_neighbor = c | 0x02; - int z_neighbor = c | 0x04; - if (c != x_neighbor) { - marker.points.push_back(getPointFromMatrix(corners, c)); - marker.points.push_back(getPointFromMatrix(corners, x_neighbor)); - } - if (c != y_neighbor) { - marker.points.push_back(getPointFromMatrix(corners, c)); - marker.points.push_back(getPointFromMatrix(corners, y_neighbor)); - } - if (c != z_neighbor) { - marker.points.push_back(getPointFromMatrix(corners, c)); - marker.points.push_back(getPointFromMatrix(corners, z_neighbor)); - } - } -} - -template -Marker makeBlocksMarkerImpl(const LayerType& layer, double scale) { - Marker marker; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.color.r = 0.662; - marker.color.g = 0.0313; - marker.color.b = 0.7607; - marker.color.a = 0.8; - marker.scale.x = scale; - marker.scale.y = scale; - marker.scale.z = scale; - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - BlockIndexList blocks; - layer.getAllAllocatedBlocks(&blocks); - - for (const auto& idx : blocks) { - const auto& block = layer.getBlockByIndex(idx); - Eigen::Vector3f block_pos = block.origin(); - fillMarkerFromBlock(marker, block_pos.cast(), block.block_size()); - } - - return marker; -} - -Marker makeMeshBlocksMarker(const MeshLayer& layer, double scale) { - Marker marker; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.scale.x = scale; - marker.scale.y = scale; - marker.scale.z = scale; - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - BlockIndexList blocks; - layer.getAllAllocatedMeshes(&blocks); - - std_msgs::ColorRGBA good; - good.r = 0.2; - good.g = 1.0; - good.b = 0.2; - good.a = 0.8; - - std_msgs::ColorRGBA bad; - bad.r = 1.0; - bad.g = 0.2; - bad.b = 0.2; - bad.a = 0.8; - - for (const auto& idx : blocks) { - const auto block = layer.getMeshPtrByIndex(idx); - Eigen::Vector3f block_pos = block->origin; - fillMarkerFromBlock(marker, block_pos.cast(), block->block_size); - - while (marker.colors.size() < marker.points.size()) { - marker.colors.push_back(block->vertices.size() != 0 ? good : bad); - } - } - - return marker; -} - -Marker makeBlocksMarker(const Layer& layer, double scale) { - return makeBlocksMarkerImpl(layer, scale); -} - -Marker makeBlocksMarker(const Layer& layer, double scale) { - return makeBlocksMarkerImpl(layer, scale); -} - -std_msgs::ColorRGBA makeGvdColor(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - double distance, - uint8_t num_basis_points) { - double ratio; - double alpha; - double alpha_diff = config.gvd_alpha - config.gvd_min_alpha; - switch (static_cast(config.gvd_mode)) { - case GvdVisualizationMode::BASIS_POINTS: - ratio = computeRatio(static_cast(config.min_num_basis), - static_cast(config.max_num_basis), - static_cast(num_basis_points)); - alpha = config.gvd_min_alpha + ratio * alpha_diff; - break; - case GvdVisualizationMode::DISTANCE: - case GvdVisualizationMode::DEFAULT: - default: - ratio = computeRatio(config.gvd_min_distance, config.gvd_max_distance, distance); - alpha = config.gvd_alpha; - break; - } - - NodeColor color = dsg_utils::interpolateColorMap(colors, ratio); - return dsg_utils::makeColorMsg(color, alpha); -} - -using EdgeMap = std::unordered_map>; - -std::unordered_set& getNodeSet(EdgeMap& edge_map, uint64_t node) { - auto iter = edge_map.find(node); - if (iter == edge_map.end()) { - iter = edge_map.emplace(node, std::unordered_set()).first; - } - return iter->second; -} - -MarkerArray makeGvdGraphMarkers(const GvdGraph& graph, - const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const std::string& ns, - size_t marker_id) { - MarkerArray marker; - if (graph.empty()) { - return marker; - } - - const Eigen::Vector3d p_identity = Eigen::Vector3d::Zero(); - const Eigen::Quaterniond q_identity = Eigen::Quaterniond::Identity(); - { // scope to make handling stuff a little easier - Marker nodes; - nodes.type = Marker::SPHERE_LIST; - nodes.id = marker_id; - nodes.ns = ns + "_nodes"; - nodes.action = Marker::ADD; - nodes.scale.x = config.gvd_graph_scale; - nodes.scale.y = config.gvd_graph_scale; - nodes.scale.z = config.gvd_graph_scale; - tf2::convert(p_identity, nodes.pose.position); - tf2::convert(q_identity, nodes.pose.orientation); - marker.markers.push_back(nodes); - } - - { // scope to make handling stuff a little easier - Marker edges; - edges.type = Marker::LINE_LIST; - edges.id = marker_id; - edges.ns = ns + "_edges"; - edges.action = Marker::ADD; - edges.scale.x = config.gvd_graph_scale; - tf2::convert(p_identity, edges.pose.position); - tf2::convert(q_identity, edges.pose.orientation); - marker.markers.push_back(edges); - } - - auto& nodes = marker.markers[0]; - auto& edges = marker.markers[1]; - - EdgeMap seen_edges; - for (const auto& id_node_pair : graph.nodes()) { - geometry_msgs::Point node_centroid; - tf2::convert(id_node_pair.second.position, node_centroid); - nodes.points.push_back(node_centroid); - nodes.colors.push_back(makeGvdColor(config, - colors, - id_node_pair.second.distance, - id_node_pair.second.num_basis_points)); - - auto& curr_seen = getNodeSet(seen_edges, id_node_pair.first); - for (const auto sibling : id_node_pair.second.siblings) { - if (curr_seen.count(sibling)) { - continue; - } - - curr_seen.insert(sibling); - getNodeSet(seen_edges, sibling).insert(id_node_pair.first); - - edges.points.push_back(nodes.points.back()); - edges.colors.push_back(nodes.colors.back()); - - const auto& other = *graph.getNode(sibling); - geometry_msgs::Point neighbor_centroid; - tf2::convert(other.position, neighbor_centroid); - edges.points.push_back(neighbor_centroid); - edges.colors.push_back( - makeGvdColor(config, colors, other.distance, other.num_basis_points)); - } - } - - return marker; -} - -size_t fillColors(const CompressedNodeMap& clusters, - std::map& colors) { - for (const auto& id_node_pair : clusters) { - size_t max_color = 0; - std::set seen_colors; - for (const auto sibling : id_node_pair.second.siblings) { - const auto iter = colors.find(sibling); - if (iter == colors.end()) { - continue; - } - - seen_colors.insert(iter->second); - if (iter->second > max_color) { - max_color = iter->second; - } - } - - if (seen_colors.empty()) { - colors[id_node_pair.first] = 0; - continue; - } - - bool found_color = false; - for (size_t i = 0; i < max_color; ++i) { - if (!seen_colors.count(i)) { - colors[id_node_pair.first] = i; - found_color = true; - break; - } - } - - if (found_color) { - continue; - } - - colors[id_node_pair.first] = max_color + 1; - } - - size_t num_colors = 0; - for (const auto& id_color_pair : colors) { - if (id_color_pair.second > num_colors) { - num_colors = id_color_pair.second; - } - } - - return num_colors + 1; -} - -MarkerArray showGvdClusters(const GvdGraph& graph, - const CompressedNodeMap& clusters, - const std::unordered_map& remapping, - const GvdVisualizerConfig& config, - const ColormapConfig& colormap, - const std::string& ns, - size_t marker_id) { - MarkerArray marker; - if (graph.empty()) { - return marker; - } - - const Eigen::Vector3d p_identity = Eigen::Vector3d::Zero(); - const Eigen::Quaterniond q_identity = Eigen::Quaterniond::Identity(); - { // scope to make handling stuff a little easier - Marker nodes; - nodes.type = Marker::SPHERE_LIST; - nodes.id = marker_id; - nodes.ns = ns + "_nodes"; - nodes.action = Marker::ADD; - nodes.scale.x = config.gvd_graph_scale; - nodes.scale.y = config.gvd_graph_scale; - nodes.scale.z = config.gvd_graph_scale; - tf2::convert(p_identity, nodes.pose.position); - tf2::convert(q_identity, nodes.pose.orientation); - marker.markers.push_back(nodes); - } - - { // scope to make handling stuff a little easier - Marker edges; - edges.type = Marker::LINE_LIST; - edges.id = marker_id; - edges.ns = ns + "_edges"; - edges.action = Marker::ADD; - edges.scale.x = config.gvd_graph_scale; - tf2::convert(p_identity, edges.pose.position); - tf2::convert(q_identity, edges.pose.orientation); - marker.markers.push_back(edges); - } - - auto& nodes = marker.markers[0]; - auto& edges = marker.markers[1]; - - std::map color_mapping; - const size_t num_colors = fillColors(clusters, color_mapping); - std::vector colors; - for (size_t i = 0; i < num_colors; ++i) { - const double ratio = static_cast(i) / static_cast(num_colors); - const auto color = dsg_utils::interpolateColorMap(colormap, ratio); - colors.push_back(dsg_utils::makeColorMsg(color, config.gvd_alpha)); - } - - EdgeMap seen_edges; - for (const auto& id_node_pair : graph.nodes()) { - geometry_msgs::Point node_centroid; - tf2::convert(id_node_pair.second.position, node_centroid); - nodes.points.push_back(node_centroid); - if (remapping.count(id_node_pair.first)) { - const auto cluster_id = remapping.at(id_node_pair.first); - const auto& cluster_color = colors.at(color_mapping.at(cluster_id)); - nodes.colors.push_back(cluster_color); - } else { - nodes.colors.push_back( - dsg_utils::makeColorMsg(NodeColor(0, 0, 0), config.gvd_alpha)); - } - - auto& curr_seen = getNodeSet(seen_edges, id_node_pair.first); - for (const auto sibling : id_node_pair.second.siblings) { - if (curr_seen.count(sibling)) { - continue; - } - - curr_seen.insert(sibling); - getNodeSet(seen_edges, sibling).insert(id_node_pair.first); - - edges.points.push_back(nodes.points.back()); - edges.colors.push_back(nodes.colors.back()); - - const auto& other = *graph.getNode(sibling); - geometry_msgs::Point neighbor_centroid; - tf2::convert(other.position, neighbor_centroid); - edges.points.push_back(neighbor_centroid); - - if (remapping.count(sibling)) { - const auto& neighbor_cluster = remapping.at(sibling); - const auto& neighbor_color = colors.at(color_mapping.at(neighbor_cluster)); - edges.colors.push_back(neighbor_color); - } else { - edges.colors.push_back( - dsg_utils::makeColorMsg(NodeColor(0, 0, 0), config.gvd_alpha)); - } - } - } - - return marker; -} - -MarkerArray makePlaceSpheres(const std_msgs::Header& header, - const SceneGraphLayer& layer, - const std::string& ns, - double alpha) { - MarkerArray spheres; - size_t id = 0; - for (const auto& id_node_pair : layer.nodes()) { - const auto& attrs = id_node_pair.second->attributes(); - - Marker marker; - marker.header = header; - marker.type = Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - marker.id = id; - marker.ns = ns; - - marker.scale.x = 2 * attrs.distance; - marker.scale.y = 2 * attrs.distance; - marker.scale.z = 2 * attrs.distance; - marker.pose.orientation.w = 1.0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - tf2::convert(id_node_pair.second->attributes().position, marker.pose.position); - - NodeColor desired_color(255, 0, 0); - marker.color = dsg_utils::makeColorMsg(desired_color, alpha); - spheres.markers.push_back(marker); - ++id; - } - - return spheres; -} - -} // namespace hydra diff --git a/hydra_ros/src/visualizer/topology_server_visualizer.cpp b/hydra_ros/src/visualizer/topology_server_visualizer.cpp deleted file mode 100644 index 68e932c5..00000000 --- a/hydra_ros/src/visualizer/topology_server_visualizer.cpp +++ /dev/null @@ -1,387 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/topology_server_visualizer.h" - -#include - -#include "hydra_ros/config/ros_utilities.h" - -namespace hydra { - -using places::CompressionGraphExtractor; -using places::GvdGraph; -using places::GvdVoxel; -using timing::ScopedTimer; -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; -using voxblox::Layer; -using voxblox::MeshLayer; -using voxblox::TsdfVoxel; - -TopologyServerVisualizer::TopologyServerVisualizer(const std::string& ns) - : nh_(ns), previous_spheres_(0), published_gvd_graph_(false) { - pubs_.reset(new MarkerGroupPub(nh_)); - - config_ = config_parser::load_from_ros_nh(nh_); - config_.graph.layer_z_step = 0; - config_.graph.color_places_by_distance = true; - - setupConfigServers(); -} - -void TopologyServerVisualizer::visualize(const SceneGraphLayer& graph, - const GvdGraph& gvd_graph, - const Layer& gvd, - const Layer& tsdf, - uint64_t timestamp_ns, - const MeshLayer* mesh) { - ScopedTimer timer("topology/topology_visualizer", timestamp_ns); - - std_msgs::Header header; - header.frame_id = config_.world_frame; - header.stamp.fromNSec(timestamp_ns); - - visualizeGraph(header, graph); - visualizeGvdGraph(header, gvd_graph); - visualizeGvd(header, gvd); - if (config_.show_block_outlines) { - visualizeBlocks(header, gvd, tsdf, mesh); - } -} - -void TopologyServerVisualizer::visualizeExtractor( - uint64_t timestamp_ns, const CompressionGraphExtractor& extractor) { - std_msgs::Header header; - header.frame_id = config_.world_frame; - header.stamp.fromNSec(timestamp_ns); - - MarkerArray markers; - pubs_->publish("gvd_cluster_viz", [&](MarkerArray& markers) { - const std::string ns = "gvd_cluster_graph"; - if (extractor.getGvdGraph().empty() && published_gvd_clusters_) { - published_gvd_graph_ = false; - markers.markers.push_back(makeDeleteMarker(header, 0, ns + "_nodes")); - markers.markers.push_back(makeDeleteMarker(header, 0, ns + "_edges")); - return true; - } - - markers = showGvdClusters(extractor.getGvdGraph(), - extractor.getCompressedNodeInfo(), - extractor.getCompressedRemapping(), - config_.gvd, - config_.colormap, - ns); - - if (markers.markers.empty()) { - return false; - } - - markers.markers.at(0).header = header; - markers.markers.at(1).header = header; - published_gvd_clusters_ = true; - return true; - }); -} - -void TopologyServerVisualizer::visualizeError(const Layer& lhs, - const Layer& rhs, - double threshold, - uint64_t timestamp_ns) { - pubs_->publish("error_viz", [&](Marker& msg) { - msg = makeErrorMarker(config_.gvd, config_.colormap, lhs, rhs, threshold); - msg.header.frame_id = config_.world_frame; - msg.header.stamp.fromNSec(timestamp_ns); - - if (msg.points.size()) { - return true; - } else { - LOG(INFO) << "no voxels with error above threshold"; - return false; - } - }); -} - -void TopologyServerVisualizer::visualizeGraph(const std_msgs::Header& header, - const SceneGraphLayer& graph) { - if (graph.nodes().empty()) { - LOG(INFO) << "visualizing empty graph!"; - return; - } - - pubs_->publish("graph_viz", [&](MarkerArray& markers) { - const std::string node_ns = config_.topology_marker_ns + "_nodes"; - Marker node_marker = makeCentroidMarkers( - header, config_.graph_layer, graph, config_.graph, node_ns, config_.colormap); - markers.markers.push_back(node_marker); - - if (!graph.edges().empty()) { - Marker edge_marker = makeLayerEdgeMarkers(header, - config_.graph_layer, - graph, - config_.graph, - NodeColor::Zero(), - config_.topology_marker_ns + "_edges"); - markers.markers.push_back(edge_marker); - } - - return true; - }); - - publishFreespace(header, graph); - publishGraphLabels(header, graph); -} - -void TopologyServerVisualizer::visualizeGvdGraph(const std_msgs::Header& header, - const GvdGraph& graph) const { - pubs_->publish("gvd_graph_viz", [&](MarkerArray& markers) { - const std::string ns = config_.topology_marker_ns + "_gvd_graph"; - if (graph.empty() && published_gvd_graph_) { - published_gvd_graph_ = false; - markers.markers.push_back(makeDeleteMarker(header, 0, ns + "_nodes")); - markers.markers.push_back(makeDeleteMarker(header, 0, ns + "_edges")); - return true; - } - - markers = makeGvdGraphMarkers(graph, config_.gvd, config_.colormap, ns); - if (markers.markers.empty()) { - return false; - } - - markers.markers.at(0).header = header; - markers.markers.at(1).header = header; - published_gvd_graph_ = true; - return true; - }); -} - -void TopologyServerVisualizer::visualizeGvd(const std_msgs::Header& header, - const Layer& gvd) const { - pubs_->publish("esdf_viz", [&](Marker& msg) { - msg = makeEsdfMarker(config_.gvd, config_.colormap, gvd); - msg.header = header; - msg.ns = "gvd_visualizer"; - - if (msg.points.size()) { - return true; - } else { - LOG(INFO) << "visualizing empty ESDF slice"; - return false; - } - }); - - pubs_->publish("gvd_viz", [&](Marker& msg) { - msg = makeGvdMarker(config_.gvd, config_.colormap, gvd); - msg.header = header; - msg.ns = "gvd_visualizer"; - - if (msg.points.size()) { - return true; - } else { - LOG(INFO) << "visualizing empty GVD slice"; - return false; - } - }); - - pubs_->publish("surface_viz", [&](Marker& msg) { - msg = makeSurfaceVoxelMarker(config_.gvd, config_.colormap, gvd); - msg.header = header; - msg.ns = "gvd_visualizer"; - - if (msg.points.size()) { - return true; - } else { - LOG(INFO) << "visualizing empty surface slice"; - return false; - } - }); -} - -void TopologyServerVisualizer::visualizeBlocks(const std_msgs::Header& header, - const Layer& gvd, - const Layer& tsdf, - const MeshLayer* mesh) const { - pubs_->publish("voxel_block_viz", [&](Marker& msg) { - if (config_.use_gvd_block_outlines) { - msg = makeBlocksMarker(gvd, config_.outline_scale); - } else { - msg = makeBlocksMarker(tsdf, config_.outline_scale); - } - - msg.header = header; - msg.ns = "topology_server_blocks"; - return true; - }); - - if (mesh) { - pubs_->publish("mesh_block_viz", [&](Marker& msg) { - msg = makeMeshBlocksMarker(*mesh, config_.outline_scale); - msg.header = header; - msg.ns = "topology_server_mesh_blocks"; - return true; - }); - } -} - -void TopologyServerVisualizer::publishFreespace(const std_msgs::Header& header, - const SceneGraphLayer& graph) { - const std::string label_ns = config_.topology_marker_ns + "_freespace"; - - MarkerArray spheres = makePlaceSpheres(header, graph, label_ns, 0.15); - - MarkerArray delete_markers; - for (size_t id = 0; id < previous_spheres_; ++id) { - Marker delete_label; - delete_label.action = Marker::DELETE; - delete_label.id = id; - delete_label.ns = label_ns; - delete_markers.markers.push_back(delete_label); - } - previous_spheres_ = spheres.markers.size(); - - // there's not really a clean way to delay computation on either of these markers, so - // we just assign the messages in the callbacks - pubs_->publish("freespace_viz", [&](MarkerArray& msg) { - msg = delete_markers; - return true; - }); - pubs_->publish("freespace_viz", [&](MarkerArray& msg) { - msg = spheres; - return true; - }); - - pubs_->publish("freespace_graph_viz", [&](MarkerArray& markers) { - const std::string node_ns = config_.topology_marker_ns + "_freespace_nodes"; - auto freespace_conf = config_.graph_layer; - freespace_conf.use_sphere_marker = false; - freespace_conf.marker_scale = 0.08; - freespace_conf.marker_alpha = 0.5; - Marker node_marker = makeCentroidMarkers( - header, freespace_conf, graph, config_.graph, node_ns, [](const auto&) { - return NodeColor::Zero(); - }); - markers.markers.push_back(node_marker); - - if (!graph.edges().empty()) { - Marker edge_marker = - makeLayerEdgeMarkers(header, - config_.graph_layer, - graph, - config_.graph, - NodeColor::Zero(), - config_.topology_marker_ns + "_freespace_edges"); - markers.markers.push_back(edge_marker); - } - - return true; - }); -} - -void TopologyServerVisualizer::publishGraphLabels(const std_msgs::Header& header, - const SceneGraphLayer& graph) { - if (!config_.graph_layer.use_label) { - return; - } - - const std::string label_ns = config_.topology_marker_ns + "_labels"; - - MarkerArray labels; - for (const auto& id_node_pair : graph.nodes()) { - const SceneGraphNode& node = *id_node_pair.second; - Marker label = - makeTextMarker(header, config_.graph_layer, node, config_.graph, label_ns); - labels.markers.push_back(label); - } - - std::set current_ids; - for (const auto& label : labels.markers) { - current_ids.insert(label.id); - } - - std::set ids_to_delete; - for (auto previous : previous_labels_) { - if (!current_ids.count(previous)) { - ids_to_delete.insert(previous); - } - } - previous_labels_ = current_ids; - - MarkerArray delete_markers; - for (auto to_delete : ids_to_delete) { - Marker delete_label; - delete_label.action = Marker::DELETE; - delete_label.id = to_delete; - delete_label.ns = label_ns; - delete_markers.markers.push_back(delete_label); - } - - // there's not really a clean way to delay computation on either of these markers, so - // we just assign the messages in the callbacks - pubs_->publish("graph_label_viz", [&](MarkerArray& msg) { - msg = delete_markers; - return true; - }); - pubs_->publish("graph_label_viz", [&](MarkerArray& msg) { - msg = labels; - return true; - }); -} - -void TopologyServerVisualizer::graphConfigCb(LayerConfig& config, uint32_t) { - config_.graph_layer = config; -} - -void TopologyServerVisualizer::colormapCb(ColormapConfig& config, uint32_t) { - config_.colormap = config; -} - -void TopologyServerVisualizer::gvdConfigCb(GvdVisualizerConfig& config, uint32_t) { - config_.gvd = config; - config_.graph.places_colormap_min_distance = config.gvd_min_distance; - config_.graph.places_colormap_max_distance = config.gvd_max_distance; -} - -void TopologyServerVisualizer::setupConfigServers() { - startRqtServer( - "gvd_visualizer", gvd_config_server_, &TopologyServerVisualizer::gvdConfigCb); - - startRqtServer("graph_visualizer", - graph_config_server_, - &TopologyServerVisualizer::graphConfigCb); - - startRqtServer( - "visualizer_colormap", colormap_server_, &TopologyServerVisualizer::colormapCb); -} - -} // namespace hydra diff --git a/hydra_ros/src/visualizer/visualizer_plugins.cpp b/hydra_ros/src/visualizer/visualizer_plugins.cpp deleted file mode 100644 index 754a2047..00000000 --- a/hydra_ros/src/visualizer/visualizer_plugins.cpp +++ /dev/null @@ -1,433 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/visualizer_plugins.h" - -#include -#include -#include -#include -#include - -#include "hydra_ros/visualizer/colormap_utilities.h" -#include "hydra_ros/visualizer/visualizer_utilities.h" - -namespace hydra { - -using dsg_utils::makeColorMsg; -using kimera_pgmo::DeformationGraph; -using kimera_pgmo::DeformationGraphPtr; -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; - -#define READ_PARAM(nh, variable) nh.getParam(#variable, variable) - -PMGraphPluginConfig::PMGraphPluginConfig(const ros::NodeHandle& nh) { - READ_PARAM(nh, mesh_edge_scale); - READ_PARAM(nh, mesh_edge_alpha); - READ_PARAM(nh, mesh_marker_scale); - READ_PARAM(nh, mesh_marker_alpha); - - // purple - std::vector leaf_color_float{0.662, 0.0313, 0.7607}; - READ_PARAM(nh, leaf_color_float); - if (leaf_color_float.size() != 3) { - throw std::runtime_error("color size must be 3!"); - } - leaf_color << 255 * leaf_color_float[0], 255 * leaf_color_float[1], - 255 * leaf_color_float[2]; - - invalid_color << 64, 235, 52; - - // grey - std::vector interior_color_float{0.3333, 0.3764, 0.4509}; - READ_PARAM(nh, interior_color_float); - if (interior_color_float.size() != 3) { - throw std::runtime_error("color size must be 3!"); - } - interior_color << 255 * interior_color_float[0], 255 * interior_color_float[1], - 255 * interior_color_float[2]; - - // TODO(nathan) fix the config for this -} - -#undef READ_PARAM - -inline void fillPoseWithIdentity(Marker& marker) { - Eigen::Vector3d origin = Eigen::Vector3d::Zero(); - tf2::convert(origin, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); -} - -MarkerArray makeLeafEdges(const PMGraphPluginConfig& config, - char vertex_prefix, - const MinimumSpanningTreeInfo& mst_info, - const DeformationGraph& dgraph, - const SceneGraphLayer& layer) { - MarkerArray markers; - if (!dgraph.hasVertexKey(vertex_prefix)) { - return markers; - } - - Marker edges; - edges.type = Marker::LINE_LIST; - edges.action = Marker::ADD; - edges.id = 0; - edges.ns = "places_mesh_graph_leaf_edges"; - - Marker vertices; - vertices.type = Marker::CUBE_LIST; - vertices.action = Marker::ADD; - vertices.id = 0; - vertices.ns = "places_mesh_graph_vertices"; - - edges.scale.x = config.mesh_edge_scale; - edges.color = makeColorMsg(config.leaf_color, config.mesh_edge_alpha); - vertices.scale.x = config.mesh_marker_scale; - vertices.scale.y = config.mesh_marker_scale; - vertices.scale.z = config.mesh_marker_scale; - vertices.color = makeColorMsg(config.leaf_color, config.mesh_marker_alpha); - - fillPoseWithIdentity(edges); - fillPoseWithIdentity(vertices); - - std::vector vertex_positions = - dgraph.getInitialPositionsVertices(vertex_prefix); - - std::set seen; - for (const auto& id_node_pair : layer.nodes()) { - if (!mst_info.leaves.count(id_node_pair.first)) { - continue; - } - - const PlaceNodeAttributes& attrs = - id_node_pair.second->attributes(); - - geometry_msgs::Point start; - tf2::convert(attrs.position, start); - - for (const auto vertex : attrs.pcl_mesh_connections) { - if (vertex >= vertex_positions.size()) { - continue; - } - - const gtsam::Point3& pos = vertex_positions.at(vertex); - geometry_msgs::Point end; - end.x = pos.x(); - end.y = pos.y(); - end.z = pos.z(); - edges.points.push_back(start); - edges.points.push_back(end); - - if (seen.count(vertex)) { - continue; - } - - seen.insert(vertex); - vertices.points.push_back(end); - } - } - - markers.markers.push_back(edges); - markers.markers.push_back(vertices); - return markers; -} - -Marker makeMstEdges(const PMGraphPluginConfig& config, - const MinimumSpanningTreeInfo& mst_info, - const SceneGraphLayer& layer) { - Marker edges; - edges.type = Marker::LINE_LIST; - edges.action = Marker::ADD; - edges.id = 0; - edges.ns = "places_mesh_graph_mst_edges"; - - edges.scale.x = config.mesh_edge_scale; - edges.color = - makeColorMsg(NodeColor::Zero(), config.layer_config.intralayer_edge_alpha); - - fillPoseWithIdentity(edges); - - for (const auto& edge : mst_info.edges) { - Eigen::Vector3d start_pos = layer.getPosition(edge.source); - geometry_msgs::Point source; - tf2::convert(start_pos, source); - - Eigen::Vector3d end_pos = layer.getPosition(edge.target); - geometry_msgs::Point target; - tf2::convert(end_pos, target); - - edges.points.push_back(source); - edges.points.push_back(target); - } - - return edges; -} - -MeshPlaceConnectionsPlugin::MeshPlaceConnectionsPlugin(const ros::NodeHandle& nh, - const std::string& name) - : DsgVisualizerPlugin(nh, name), - config_(nh), - published_nodes_(false), - published_edges_(false) { - marker_pub_ = nh_.advertise("places_mesh_connection_viz", 10); -} - -void MeshPlaceConnectionsPlugin::draw(const std_msgs::Header& header, - const DynamicSceneGraph& graph) { - if (!graph.hasLayer(DsgLayers::PLACES)) { - return; - } - - if (!graph.hasMesh()) { - return; - } - - const auto& layer = graph.getLayer(DsgLayers::PLACES); - - MinimumSpanningTreeInfo mst_info = getMinimumSpanningEdges(layer); - - VisualizerConfig viz_config; - viz_config.layer_z_step = 0.0; - - MarkerArray msg; - - Marker node_marker = - makeCentroidMarkers(header, - config_.layer_config, - layer, - viz_config, - "place_mesh_graph_nodes", - [&](const SceneGraphNode& node) { - if (mst_info.counts.at(node.id) == 0) { - return config_.invalid_color; - } else if (mst_info.leaves.count(node.id)) { - return config_.leaf_color; - } else { - return config_.interior_color; - } - }); - if (!node_marker.points.empty()) { - msg.markers.push_back(node_marker); - published_nodes_ = true; - } - - Marker mst_edge_marker = makeMstEdges(config_, mst_info, layer); - if (!mst_edge_marker.points.empty()) { - mst_edge_marker.header = header; - msg.markers.push_back(mst_edge_marker); - published_edges_ = true; - } - - if (!msg.markers.empty()) { - marker_pub_.publish(msg); - } -} - -void MeshPlaceConnectionsPlugin::reset(const std_msgs::Header& header, - const DynamicSceneGraph&) { - MarkerArray msg; - if (published_nodes_) { - msg.markers.push_back(makeDeleteMarker(header, 0, "place_mesh_graph_nodes")); - published_nodes_ = false; - } - - if (published_edges_) { - msg.markers.push_back(makeDeleteMarker(header, 0, "places_mesh_graph_mst_edges")); - published_edges_ = false; - } - - if (!msg.markers.empty()) { - marker_pub_.publish(msg); - } -} - -PlacesFactorGraphViz::PlacesFactorGraphViz(const ros::NodeHandle& nh) - : nh_(nh), config_(nh) { - marker_pub_ = nh_.advertise("places_factor_graph", 10); -} - -void PlacesFactorGraphViz::draw(char vertex_prefix, - const SceneGraphLayer& places, - const MinimumSpanningTreeInfo& mst_info, - const DeformationGraph& deformations) { - VisualizerConfig viz_config; - viz_config.layer_z_step = 0.0; - - std_msgs::Header header; - header.frame_id = "world"; - header.stamp = ros::Time::now(); - - MarkerArray msg = - makeLeafEdges(config_, vertex_prefix, mst_info, deformations, places); - for (auto& marker : msg.markers) { - marker.header = header; - } - - Marker node_marker = makeCentroidMarkers( - header, - config_.layer_config, - places, - viz_config, - "place_factor_graph_nodes", - [&](const SceneGraphNode& node) { - if (!node.hasSiblings()) { - VLOG(3) << "Invalid node: " << NodeSymbol(node.id).getLabel(); - return config_.invalid_color; - } else if (mst_info.leaves.count(node.id)) { - return config_.leaf_color; - } else { - return config_.interior_color; - } - }); - if (!node_marker.points.empty()) { - msg.markers.push_back(node_marker); - } - - Marker mst_edge_marker = makeMstEdges(config_, mst_info, places); - if (!mst_edge_marker.points.empty()) { - mst_edge_marker.header = header; - msg.markers.push_back(mst_edge_marker); - } - - if (!msg.markers.empty()) { - marker_pub_.publish(msg); - } -} - -PlaceParentsPlugin::PlaceParentsPlugin(const ros::NodeHandle& nh, - const std::string& name) - : DsgVisualizerPlugin(nh, name), - config_(nh), - published_nodes_(false), - published_edges_(false) { - marker_pub_ = nh_.advertise("places_parents", 10); -} - -void PlaceParentsPlugin::draw(const std_msgs::Header& header, - const DynamicSceneGraph& graph) { - if (!graph.hasLayer(DsgLayers::PLACES)) { - return; - } - - const auto& layer = graph.getLayer(DsgLayers::PLACES); - - VisualizerConfig viz_config; - viz_config.layer_z_step = 0.0; - - MarkerArray msg; - - Marker nodes = makeCentroidMarkers( - header, - config_.layer_config, - layer, - viz_config, - "places_parent_graph_nodes", - [&](const SceneGraphNode&) { return config_.interior_color; }); - - if (!nodes.points.empty()) { - msg.markers.push_back(nodes); - published_nodes_ = true; - } - - Marker edges; - edges.type = Marker::LINE_LIST; - edges.action = Marker::ADD; - edges.id = 0; - edges.ns = "places_parent_edges"; - edges.scale.x = config_.mesh_edge_scale; - edges.color = - makeColorMsg(config_.interior_color, config_.layer_config.intralayer_edge_alpha); - - Marker parents; - parents.type = Marker::CUBE_LIST; - parents.action = Marker::ADD; - parents.id = 0; - parents.ns = "places_parents"; - parents.scale.x = config_.mesh_marker_scale; - parents.scale.y = config_.mesh_marker_scale; - parents.scale.z = config_.mesh_marker_scale; - parents.color = makeColorMsg(config_.leaf_color, config_.mesh_marker_alpha); - - fillPoseWithIdentity(edges); - fillPoseWithIdentity(parents); - - for (const auto& id_node_pair : layer.nodes()) { - auto& attrs = id_node_pair.second->attributes(); - geometry_msgs::Point start; - tf2::convert(attrs.position, start); - - for (const auto& info : attrs.voxblox_mesh_connections) { - geometry_msgs::Point end; - Eigen::Vector3d pos = Eigen::Map(info.voxel_pos); - tf2::convert(pos, end); - edges.points.push_back(start); - edges.points.push_back(end); - parents.points.push_back(end); - } - } - - if (!edges.points.empty()) { - edges.header = header; - parents.header = header; - msg.markers.push_back(edges); - msg.markers.push_back(parents); - published_edges_ = true; - } - - if (!msg.markers.empty()) { - marker_pub_.publish(msg); - } -} - -void PlaceParentsPlugin::reset(const std_msgs::Header& header, - const DynamicSceneGraph&) { - MarkerArray msg; - if (published_nodes_) { - msg.markers.push_back(makeDeleteMarker(header, 0, "places_parent_graph_nodes")); - published_nodes_ = false; - } - - if (published_edges_) { - msg.markers.push_back(makeDeleteMarker(header, 0, "places_parent_edges")); - msg.markers.push_back(makeDeleteMarker(header, 0, "places_parents")); - published_edges_ = false; - } - - if (!msg.markers.empty()) { - marker_pub_.publish(msg); - } -} - -} // namespace hydra diff --git a/hydra_ros/src/visualizer/visualizer_utilities.cpp b/hydra_ros/src/visualizer/visualizer_utilities.cpp deleted file mode 100644 index 96b99bb9..00000000 --- a/hydra_ros/src/visualizer/visualizer_utilities.cpp +++ /dev/null @@ -1,933 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_ros/visualizer/visualizer_utilities.h" - -#include - -#include "hydra_ros/visualizer/colormap_utilities.h" - -namespace hydra { - -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; -using Node = SceneGraphLayer::Node; -using dsg_utils::makeColorMsg; - -namespace { - -inline double getRatio(double min, double max, double value) { - double ratio = (value - min) / (max - min); - ratio = !std::isfinite(ratio) ? 0.0 : ratio; - ratio = ratio > 1.0 ? 1.0 : ratio; - ratio = ratio < 0.0 ? 0.0 : ratio; - return ratio; -} - -inline NodeColor getDistanceColor(const VisualizerConfig& config, - const ColormapConfig& colors, - double distance) { - if (config.places_colormap_max_distance <= config.places_colormap_min_distance) { - // TODO(nathan) consider warning - return NodeColor::Zero(); - } - - double ratio = getRatio(config.places_colormap_min_distance, - config.places_colormap_max_distance, - distance); - - return dsg_utils::interpolateColorMap(colors, ratio); -} - -inline void fillPoseWithIdentity(geometry_msgs::Pose& pose) { - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), pose.orientation); -} - -} // namespace - -Marker makeDeleteMarker(const std_msgs::Header& header, - size_t id, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.action = Marker::DELETE; - marker.id = id; - marker.ns = ns; - return marker; -} - -geometry_msgs::Point getPointFromMatrix(const Eigen::MatrixXf& matrix, int col) { - geometry_msgs::Point point; - point.x = matrix(0, col); - point.y = matrix(1, col); - point.z = matrix(2, col); - return point; -} - -void fillCornersFromBbox(const BoundingBox& bbox, Eigen::MatrixXf& corners) { - const Eigen::Vector3f dims = bbox.max - bbox.min; - - corners.block<3, 1>(0, 0) = bbox.min; - for (int c = 1; c < corners.cols(); ++c) { - // x: lsb, y: second lsb, z: third lsb - Eigen::Vector3f offset; - offset(0) = ((c & 0x01) != 0) ? dims(0) : 0.0f; - offset(1) = ((c & 0x02) != 0) ? dims(1) : 0.0f; - offset(2) = ((c & 0x04) != 0) ? dims(2) : 0.0f; - corners.block<3, 1>(0, c) = bbox.min + offset; - } - - if (bbox.type != BoundingBox::Type::AABB) { - corners = (bbox.world_R_center * corners).eval(); - for (int c = 0; c < corners.cols(); ++c) { - corners.block<3, 1>(0, c) += bbox.world_P_center; - } - } -} - -void addWireframeToMarker(const Eigen::MatrixXf& corners, - const std_msgs::ColorRGBA& color, - Marker& marker) { - for (int c = 0; c < corners.cols(); ++c) { - // edges are 1-bit pertubations - int x_neighbor = c | 0x01; - int y_neighbor = c | 0x02; - int z_neighbor = c | 0x04; - if (c != x_neighbor) { - marker.points.push_back(getPointFromMatrix(corners, c)); - marker.colors.push_back(color); - marker.points.push_back(getPointFromMatrix(corners, x_neighbor)); - marker.colors.push_back(color); - } - if (c != y_neighbor) { - marker.points.push_back(getPointFromMatrix(corners, c)); - marker.colors.push_back(color); - marker.points.push_back(getPointFromMatrix(corners, y_neighbor)); - marker.colors.push_back(color); - } - if (c != z_neighbor) { - marker.points.push_back(getPointFromMatrix(corners, c)); - marker.colors.push_back(color); - marker.points.push_back(getPointFromMatrix(corners, z_neighbor)); - marker.colors.push_back(color); - } - } -} - -void addEdgesToCorners(const Eigen::MatrixXf& corners, - const geometry_msgs::Point& node_centroid, - const std_msgs::ColorRGBA& color, - Marker& marker) { - for (size_t i = 0; i < 8; ++i) { - marker.colors.push_back(color); - } - - // top box corners are 4, 5, 6, 7 - marker.points.push_back(node_centroid); - marker.points.push_back(getPointFromMatrix(corners, 4)); - marker.points.push_back(node_centroid); - marker.points.push_back(getPointFromMatrix(corners, 5)); - marker.points.push_back(node_centroid); - marker.points.push_back(getPointFromMatrix(corners, 6)); - marker.points.push_back(node_centroid); - marker.points.push_back(getPointFromMatrix(corners, 7)); -} - -Marker makeEdgesToBoundingBoxes(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.id = 0; - marker.ns = ns; - marker.scale.x = config.bbox_wireframe_edge_scale; - - fillPoseWithIdentity(marker.pose); - - marker.points.reserve(8 * layer.numNodes()); - marker.colors.reserve(8 * layer.numNodes()); - - Eigen::MatrixXf corners(3, 8); - for (const auto& id_node_pair : layer.nodes()) { - const auto& attrs = id_node_pair.second->attributes(); - const auto color = makeColorMsg(attrs.color, config.bounding_box_alpha); - fillCornersFromBbox(attrs.bounding_box, corners); - - geometry_msgs::Point node_centroid; - tf2::convert(attrs.position, node_centroid); - node_centroid.z += getZOffset(config, visualizer_config); - - geometry_msgs::Point center_point; - tf2::convert(attrs.position, center_point); - center_point.z += - visualizer_config.mesh_edge_break_ratio * getZOffset(config, visualizer_config); - - marker.points.push_back(node_centroid); - marker.colors.push_back(color); - marker.points.push_back(center_point); - marker.colors.push_back(color); - - addEdgesToCorners(corners, center_point, color, marker); - } - - return marker; -} - -Marker makeLayerWireframeBoundingBoxes(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.id = 0; - marker.ns = ns; - marker.scale.x = config.bbox_wireframe_scale; - - fillPoseWithIdentity(marker.pose); - marker.pose.position.z += - config.collapse_bounding_box ? 0.0 : getZOffset(config, visualizer_config); - - marker.points.reserve(12 * layer.numNodes()); - marker.colors.reserve(12 * layer.numNodes()); - - Eigen::MatrixXf corners(3, 8); - for (const auto& id_node_pair : layer.nodes()) { - const auto& attrs = id_node_pair.second->attributes(); - const auto color = makeColorMsg(attrs.color, config.bounding_box_alpha); - fillCornersFromBbox(attrs.bounding_box, corners); - addWireframeToMarker(corners, color, marker); - } - - return marker; -} - -Marker makeBoundingBoxMarker(const std_msgs::Header& header, - const LayerConfig& config, - const Node& node, - const VisualizerConfig& visualizer_config, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.type = Marker::CUBE; - marker.action = Marker::ADD; - marker.id = node.id; - marker.ns = ns; - marker.color = makeColorMsg(node.attributes().color, - config.bounding_box_alpha); - - BoundingBox bounding_box = node.attributes().bounding_box; - - Eigen::Quaternionf world_q_center = - bounding_box.type == BoundingBox::Type::AABB - ? Eigen::Quaternionf::Identity() - : Eigen::Quaternionf(bounding_box.world_R_center); - - switch (bounding_box.type) { - case BoundingBox::Type::OBB: - marker.pose.position = - tf2::toMsg(bounding_box.world_P_center.cast().eval()); - tf2::convert(world_q_center.cast(), marker.pose.orientation); - marker.pose.position.z += - config.collapse_bounding_box ? 0.0 : getZOffset(config, visualizer_config); - break; - case BoundingBox::Type::AABB: - case BoundingBox::Type::RAABB: - marker.pose.position = - tf2::toMsg(bounding_box.world_P_center.cast().eval()); - tf2::convert(world_q_center.cast(), marker.pose.orientation); - marker.pose.position.z += - config.collapse_bounding_box ? 0.0 : getZOffset(config, visualizer_config); - break; - default: - ROS_ERROR("Invalid bounding box encountered!"); - break; - } - - tf2::toMsg((bounding_box.max - bounding_box.min).cast().eval(), marker.scale); - - return marker; -} - -Marker makeTextMarker(const std_msgs::Header& header, - const LayerConfig& config, - const Node& node, - const VisualizerConfig& visualizer_config, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.ns = ns; - marker.id = node.id; - marker.type = Marker::TEXT_VIEW_FACING; - marker.action = Marker::ADD; - marker.lifetime = ros::Duration(0); - marker.text = node.attributes().name; - marker.scale.z = config.label_scale; - marker.color = makeColorMsg(NodeColor::Zero()); - - fillPoseWithIdentity(marker.pose); - tf2::convert(node.attributes().position, marker.pose.position); - marker.pose.position.z += getZOffset(config, visualizer_config) + config.label_height; - - return marker; -} - -Marker makeTextMarkerNoHeight(const std_msgs::Header& header, - const LayerConfig& config, - const Node& node, - const VisualizerConfig&, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.ns = ns; - marker.id = node.id; - marker.type = Marker::TEXT_VIEW_FACING; - marker.action = Marker::ADD; - marker.lifetime = ros::Duration(0); - marker.text = node.attributes().name; - marker.scale.z = config.label_scale; - marker.color = makeColorMsg(NodeColor::Zero()); - - fillPoseWithIdentity(marker.pose); - tf2::convert(node.attributes().position, marker.pose.position); - marker.pose.position.z += config.label_height; - - return marker; -} - -Marker makeCentroidMarkers(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns) { - return makeCentroidMarkers(header, - config, - layer, - visualizer_config, - ns, - [&](const SceneGraphNode& node) -> NodeColor { - try { - return node.attributes().color; - } catch (const std::bad_cast&) { - return NodeColor::Zero(); - } - }); -} - -Marker makeCentroidMarkers(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const ColormapConfig& colors) { - return makeCentroidMarkers( - header, config, layer, visualizer_config, ns, [&](const SceneGraphNode& node) { - return getDistanceColor( - visualizer_config, colors, node.attributes().distance); - }); -} - -Marker makeCentroidMarkers(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const ColorFunction& color_func) { - Marker marker; - marker.header = header; - marker.type = config.use_sphere_marker ? Marker::SPHERE_LIST : Marker::CUBE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.id = 0; - marker.ns = ns; - - marker.scale.x = config.marker_scale; - marker.scale.y = config.marker_scale; - marker.scale.z = config.marker_scale; - - fillPoseWithIdentity(marker.pose); - - marker.points.reserve(layer.numNodes()); - marker.colors.reserve(layer.numNodes()); - for (const auto& id_node_pair : layer.nodes()) { - geometry_msgs::Point node_centroid; - tf2::convert(id_node_pair.second->attributes().position, node_centroid); - node_centroid.z += getZOffset(config, visualizer_config); - marker.points.push_back(node_centroid); - - NodeColor desired_color = color_func(*id_node_pair.second); - marker.colors.push_back(makeColorMsg(desired_color, config.marker_alpha)); - } - - return marker; -} - -namespace { - -inline Marker makeNewEdgeList(const std_msgs::Header& header, - const LayerConfig& config, - const std::string& ns_prefix, - LayerId source, - LayerId target) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = ns_prefix + std::to_string(source) + "_" + std::to_string(target); - marker.scale.x = config.interlayer_edge_scale; - fillPoseWithIdentity(marker.pose); - return marker; -} - -} // namespace - -bool shouldVisualize(const DynamicSceneGraph& graph, - const SceneGraphNode& node, - const std::map& configs, - const std::map& dynamic_configs) { - if (graph.isDynamic(node.id)) { - return dynamic_configs.count(node.layer) && - dynamic_configs.at(node.layer).visualize && - dynamic_configs.at(node.layer).visualize_interlayer_edges; - } - - return configs.count(node.layer) && configs.at(node.layer).visualize; -} - -LayerId getConfigLayer(const DynamicSceneGraph& graph, - const SceneGraphNode& source, - const SceneGraphNode& target) { - if (graph.isDynamic(source.id)) { - return source.layer; - } else { - return target.layer; - } -} - -MarkerArray makeDynamicGraphEdgeMarkers( - const std_msgs::Header& header, - const DynamicSceneGraph& graph, - const std::map& configs, - const std::map& dynamic_configs, - const VisualizerConfig& visualizer_config, - const std::string& ns_prefix) { - MarkerArray layer_edges; - std::map layer_markers; - std::map num_since_last_insertion; - - for (const auto& edge : graph.dynamic_interlayer_edges()) { - const Node& source = graph.getNode(edge.second.source).value(); - const Node& target = graph.getNode(edge.second.target).value(); - - if (!shouldVisualize(graph, source, configs, dynamic_configs)) { - continue; - } - - if (!shouldVisualize(graph, target, configs, dynamic_configs)) { - continue; - } - - DynamicLayerConfig config = - dynamic_configs.at(getConfigLayer(graph, source, target)); - - size_t num_between_insertions = config.interlayer_edge_insertion_skip; - - if (layer_markers.count(source.layer) == 0) { - layer_markers[source.layer] = makeNewEdgeList( - header, configs.at(source.layer), ns_prefix, source.layer, target.layer); - layer_markers[source.layer].color = - makeColorMsg(NodeColor::Zero(), config.edge_alpha); - // make sure we always draw at least one edge - num_since_last_insertion[source.layer] = num_between_insertions; - } - - if (num_since_last_insertion[source.layer] >= num_between_insertions) { - num_since_last_insertion[source.layer] = 0; - } else { - num_since_last_insertion[source.layer]++; - continue; - } - - Marker& marker = layer_markers.at(source.layer); - geometry_msgs::Point source_point; - tf2::convert(source.attributes().position, source_point); - source_point.z += getZOffset(configs.at(source.layer), visualizer_config); - marker.points.push_back(source_point); - - geometry_msgs::Point target_point; - tf2::convert(target.attributes().position, target_point); - target_point.z += getZOffset(configs.at(target.layer), visualizer_config); - marker.points.push_back(target_point); - } - - for (const auto& id_marker_pair : layer_markers) { - layer_edges.markers.push_back(id_marker_pair.second); - } - return layer_edges; -} - -// TODO(nathan) consider making this shorter -MarkerArray makeGraphEdgeMarkers(const std_msgs::Header& header, - const DynamicSceneGraph& graph, - const std::map& configs, - const VisualizerConfig& visualizer_config, - const std::string& ns_prefix) { - MarkerArray layer_edges; - std::map layer_markers; - std::map num_since_last_insertion; - - for (const auto& edge : graph.interlayer_edges()) { - const Node& source = *(graph.getNode(edge.second.source)); - const Node& target = *(graph.getNode(edge.second.target)); - - if (!configs.count(source.layer) || !configs.count(target.layer)) { - continue; - } - - if (!configs.at(source.layer).visualize) { - continue; - } - - if (!configs.at(target.layer).visualize) { - continue; - } - - size_t num_between_insertions = - configs.at(source.layer).interlayer_edge_insertion_skip; - - // parent is always source - // TODO(nathan) make the above statement an invariant - if (layer_markers.count(source.layer) == 0) { - layer_markers[source.layer] = makeNewEdgeList( - header, configs.at(source.layer), ns_prefix, source.layer, target.layer); - // make sure we always draw at least one edge - num_since_last_insertion[source.layer] = num_between_insertions; - } - - if (num_since_last_insertion[source.layer] >= num_between_insertions) { - num_since_last_insertion[source.layer] = 0; - } else { - num_since_last_insertion[source.layer]++; - continue; - } - - Marker& marker = layer_markers.at(source.layer); - geometry_msgs::Point source_point; - tf2::convert(source.attributes().position, source_point); - source_point.z += getZOffset(configs.at(source.layer), visualizer_config); - marker.points.push_back(source_point); - - geometry_msgs::Point target_point; - tf2::convert(target.attributes().position, target_point); - target_point.z += getZOffset(configs.at(target.layer), visualizer_config); - marker.points.push_back(target_point); - - NodeColor edge_color; - if (configs.at(source.layer).interlayer_edge_use_color) { - if (configs.at(source.layer).use_edge_source) { - // TODO(nathan) this might not be a safe cast in general - edge_color = source.attributes().color; - } else { - // TODO(nathan) this might not be a safe cast in general - edge_color = target.attributes().color; - } - } else { - edge_color = NodeColor::Zero(); - } - - marker.colors.push_back( - makeColorMsg(edge_color, configs.at(source.layer).intralayer_edge_alpha)); - marker.colors.push_back( - makeColorMsg(edge_color, configs.at(source.layer).intralayer_edge_alpha)); - } - - for (const auto& id_marker_pair : layer_markers) { - layer_edges.markers.push_back(id_marker_pair.second); - } - return layer_edges; -} - -Marker makeMeshEdgesMarker(const std_msgs::Header& header, - const LayerConfig& config, - const VisualizerConfig& visualizer_config, - const DynamicSceneGraph& graph, - const SceneGraphLayer& layer, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = ns; - - marker.scale.x = config.interlayer_edge_scale; - fillPoseWithIdentity(marker.pose); - - for (const auto& id_node_pair : layer.nodes()) { - const Node& node = *id_node_pair.second; - const auto& attrs = node.attributes(); - const auto& mesh_edge_indices = attrs.mesh_connections; - if (mesh_edge_indices.empty()) { - continue; - } - - geometry_msgs::Point center_point; - tf2::convert(attrs.position, center_point); - center_point.z += - visualizer_config.mesh_edge_break_ratio * getZOffset(config, visualizer_config); - - geometry_msgs::Point centroid_location; - tf2::convert(attrs.position, centroid_location); - centroid_location.z += getZOffset(config, visualizer_config); - - // make first edge - marker.points.push_back(centroid_location); - marker.points.push_back(center_point); - if (config.interlayer_edge_use_color) { - marker.colors.push_back(makeColorMsg(attrs.color, config.interlayer_edge_alpha)); - marker.colors.push_back(makeColorMsg(attrs.color, config.interlayer_edge_alpha)); - } else { - marker.colors.push_back( - makeColorMsg(NodeColor::Zero(), config.interlayer_edge_alpha)); - marker.colors.push_back( - makeColorMsg(NodeColor::Zero(), config.interlayer_edge_alpha)); - } - - size_t i = 0; - for (const auto idx : mesh_edge_indices) { - ++i; - if ((i - 1) % (config.interlayer_edge_insertion_skip + 1) != 0) { - continue; - } - - std::optional vertex_pos = graph.getMeshPosition(idx); - if (!vertex_pos) { - continue; - } - - geometry_msgs::Point vertex; - tf2::convert(*vertex_pos, vertex); - if (!visualizer_config.collapse_layers) { - vertex.z += visualizer_config.mesh_layer_offset; - } - - marker.points.push_back(center_point); - marker.points.push_back(vertex); - - if (config.interlayer_edge_use_color) { - marker.colors.push_back( - makeColorMsg(attrs.color, config.interlayer_edge_alpha)); - marker.colors.push_back( - makeColorMsg(attrs.color, config.interlayer_edge_alpha)); - } else { - marker.colors.push_back( - makeColorMsg(NodeColor::Zero(), config.interlayer_edge_alpha)); - marker.colors.push_back( - makeColorMsg(NodeColor::Zero(), config.interlayer_edge_alpha)); - } - } - } - - return marker; -} - -MarkerArray makeGvdWireframe(const std_msgs::Header& header, - const LayerConfig& config, - const VisualizerConfig& visualizer_config, - const SceneGraphLayer& layer, - const std::string& ns, - const ColormapConfig& colors, - size_t marker_id) { - return makeGvdWireframe( - header, - config, - layer, - ns, - [&](const SceneGraphNode& node) { - return getDistanceColor( - visualizer_config, colors, node.attributes().distance); - }, - marker_id); -} - -MarkerArray makeGvdWireframe(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const std::string& ns, - const ColorFunction& color_func, - size_t marker_id) { - MarkerArray marker; - { // scope to make handling stuff a little easier - Marker edges; - edges.header = header; - edges.type = Marker::LINE_LIST; - edges.id = marker_id; - edges.ns = ns + "_edges"; - edges.action = Marker::ADD; - edges.scale.x = config.intralayer_edge_scale; - fillPoseWithIdentity(edges.pose); - - Marker nodes; - nodes.header = header; - nodes.type = Marker::SPHERE_LIST; - nodes.id = marker_id; - nodes.ns = ns + "_nodes"; - nodes.action = Marker::ADD; - nodes.scale.x = config.intralayer_edge_scale; - nodes.scale.y = config.intralayer_edge_scale; - nodes.scale.z = config.intralayer_edge_scale; - fillPoseWithIdentity(nodes.pose); - - marker.markers.push_back(nodes); - marker.markers.push_back(edges); - } - auto& nodes = marker.markers[0]; - auto& edges = marker.markers[1]; - - if (layer.nodes().empty()) { - marker.markers.clear(); - return marker; - } - - for (const auto& id_node_pair : layer.nodes()) { - geometry_msgs::Point node_centroid; - tf2::convert(id_node_pair.second->attributes().position, node_centroid); - nodes.points.push_back(node_centroid); - - NodeColor desired_color = color_func(*id_node_pair.second); - nodes.colors.push_back(makeColorMsg(desired_color, config.marker_alpha)); - } - - if (layer.edges().empty()) { - marker.markers.resize(1); - return marker; - } - - for (const auto& id_edge_pair : layer.edges()) { - // TODO(nathan) filter by node symbol category - const auto& edge = id_edge_pair.second; - const SceneGraphNode& source_node = layer.getNode(edge.source).value(); - const SceneGraphNode& target_node = layer.getNode(edge.target).value(); - - geometry_msgs::Point source; - tf2::convert(source_node.attributes().position, source); - edges.points.push_back(source); - - geometry_msgs::Point target; - tf2::convert(target_node.attributes().position, target); - edges.points.push_back(target); - - edges.colors.push_back(makeColorMsg(color_func(source_node), config.marker_alpha)); - edges.colors.push_back(makeColorMsg(color_func(target_node), config.marker_alpha)); - } - - return marker; -} - -Marker makeLayerEdgeMarkers(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const NodeColor& color, - const std::string& ns) { - return makeLayerEdgeMarkers( - header, - config, - layer, - visualizer_config, - ns, - [&](const SceneGraphNode&, const SceneGraphNode&, bool) { return color; }); -} - -Marker makeLayerEdgeMarkers(const std_msgs::Header& header, - const LayerConfig& config, - const SceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const EdgeColorFunction& color_func) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.id = 0; - marker.ns = ns; - - marker.action = Marker::ADD; - marker.scale.x = config.intralayer_edge_scale; - fillPoseWithIdentity(marker.pose); - - auto edge_iter = layer.edges().begin(); - while (edge_iter != layer.edges().end()) { - const SceneGraphNode& source_node = layer.getNode(edge_iter->second.source).value(); - const SceneGraphNode& target_node = layer.getNode(edge_iter->second.target).value(); - - geometry_msgs::Point source; - tf2::convert(source_node.attributes().position, source); - source.z += getZOffset(config, visualizer_config); - marker.points.push_back(source); - - geometry_msgs::Point target; - tf2::convert(target_node.attributes().position, target); - target.z += getZOffset(config, visualizer_config); - marker.points.push_back(target); - - marker.colors.push_back(makeColorMsg(color_func(source_node, target_node, true), - config.intralayer_edge_alpha)); - marker.colors.push_back(makeColorMsg(color_func(source_node, target_node, false), - config.intralayer_edge_alpha)); - - std::advance(edge_iter, config.intralayer_edge_insertion_skip + 1); - } - - return marker; -} - -Marker makeDynamicCentroidMarkers(const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const NodeColor& color, - const std::string& ns, - size_t marker_id) { - return makeDynamicCentroidMarkers( - header, - config, - layer, - config.z_offset_scale, - visualizer_config, - ns, - [&](const auto&) -> NodeColor { return color; }, - marker_id); -} - -Marker makeDynamicCentroidMarkers(const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - double layer_offset_scale, - const VisualizerConfig& visualizer_config, - const std::string& ns, - const ColorFunction& color_func, - size_t marker_id) { - Marker marker; - marker.header = header; - marker.type = config.node_use_sphere ? Marker::SPHERE_LIST : Marker::CUBE_LIST; - marker.action = visualization_msgs::Marker::ADD; - marker.ns = ns; - marker.id = marker_id; - - marker.scale.x = config.node_scale; - marker.scale.y = config.node_scale; - marker.scale.z = config.node_scale; - - fillPoseWithIdentity(marker.pose); - - marker.points.reserve(layer.numNodes()); - for (const auto& node : layer.nodes()) { - geometry_msgs::Point node_centroid; - tf2::convert(node->attributes().position, node_centroid); - node_centroid.z += getZOffset(layer_offset_scale, visualizer_config); - marker.points.push_back(node_centroid); - marker.colors.push_back(makeColorMsg(color_func(*node), config.node_alpha)); - } - - return marker; -} - -Marker makeDynamicEdgeMarkers(const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const NodeColor& color, - const std::string& ns, - size_t marker_id) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.ns = ns; - marker.id = marker_id; - - marker.action = Marker::ADD; - marker.scale.x = config.edge_scale; - marker.color = makeColorMsg(color, config.edge_alpha); - fillPoseWithIdentity(marker.pose); - - for (const auto& id_edge_pair : layer.edges()) { - geometry_msgs::Point source; - tf2::convert(layer.getPosition(id_edge_pair.second.source), source); - source.z += getZOffset(config.z_offset_scale, visualizer_config); - marker.points.push_back(source); - - geometry_msgs::Point target; - tf2::convert(layer.getPosition(id_edge_pair.second.target), target); - target.z += getZOffset(config.z_offset_scale, visualizer_config); - marker.points.push_back(target); - } - - return marker; -} - -Marker makeDynamicLabelMarker(const std_msgs::Header& header, - const DynamicLayerConfig& config, - const DynamicSceneGraphLayer& layer, - const VisualizerConfig& visualizer_config, - const std::string& ns, - size_t marker_id) { - Marker marker; - marker.header = header; - marker.type = Marker::TEXT_VIEW_FACING; - marker.ns = ns; - marker.id = marker_id; - marker.action = Marker::ADD; - marker.lifetime = ros::Duration(0); - marker.text = "Agent"; // std::to_string(layer.id) + ":" + layer.prefix.str(); - marker.scale.z = config.label_scale; - marker.color = makeColorMsg(NodeColor::Zero()); - - Eigen::Vector3d latest_position = layer.getPositionByIndex(layer.numNodes() - 1); - fillPoseWithIdentity(marker.pose); - tf2::convert(latest_position, marker.pose.position); - marker.pose.position.z += - getZOffset(config.z_offset_scale, visualizer_config) + config.label_height; - - return marker; -} - -} // namespace hydra diff --git a/hydra_ros/tests/CMakeLists.txt b/hydra_ros/tests/CMakeLists.txt deleted file mode 100644 index 9d9ad41e..00000000 --- a/hydra_ros/tests/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -find_package(rostest REQUIRED) -add_rostest_gtest(test_${PROJECT_NAME} hydra_ros.test main.cpp test_ros_config.cpp) -target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/hydra_ros/tests/hydra_ros.test b/hydra_ros/tests/hydra_ros.test deleted file mode 100644 index a0ed194d..00000000 --- a/hydra_ros/tests/hydra_ros.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - diff --git a/hydra_ros/tests/main.cpp b/hydra_ros/tests/main.cpp deleted file mode 100644 index 4e31b036..00000000 --- a/hydra_ros/tests/main.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include - -auto main(int argc, char** argv) -> int { - ::testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_hydra_ros"); - - return RUN_ALL_TESTS(); -} diff --git a/hydra_ros/tests/resources/map_config.yaml b/hydra_ros/tests/resources/map_config.yaml deleted file mode 100644 index b30e9ced..00000000 --- a/hydra_ros/tests/resources/map_config.yaml +++ /dev/null @@ -1,10 +0,0 @@ ---- -configs: - config_a: - a: -3 - b: 5.2 - c: hello - config_b: - a: 5 - b: -1.2 - c: other diff --git a/hydra_ros/tests/resources/nested_test_config.yaml b/hydra_ros/tests/resources/nested_test_config.yaml deleted file mode 100644 index 823c0cd9..00000000 --- a/hydra_ros/tests/resources/nested_test_config.yaml +++ /dev/null @@ -1,9 +0,0 @@ ---- -fake_config: - foo: 10.0 - bar: 5.0 - a: -3 - b: 1 - c: 2 -msg: again -foo: 5.0 diff --git a/hydra_ros/tests/resources/test_config.yaml b/hydra_ros/tests/resources/test_config.yaml deleted file mode 100644 index 6ca663ef..00000000 --- a/hydra_ros/tests/resources/test_config.yaml +++ /dev/null @@ -1,17 +0,0 @@ ---- -foo: 10.0 -bar: 5.0 -a: -3 -b: 1 -c: 2 -msg: world -type: GREEN -values: [4, 5, 6] -value_map: - '3': 4 - '5': 6 -vec: [7, 8, 9] -enable_map: - GREEN: true - BLUE: false -unique_values: [4, 5, 4, 4, 5, 5] diff --git a/hydra_ros/tests/test_ros_config.cpp b/hydra_ros/tests/test_ros_config.cpp deleted file mode 100644 index 72152a5f..00000000 --- a/hydra_ros/tests/test_ros_config.cpp +++ /dev/null @@ -1,282 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include - -namespace hydra { - -enum class TestEnum { RED, GREEN, BLUE }; - -struct FakeConfig { - float foo = 5.0f; - double bar = 10.0; - int a = 1; - uint8_t b = 2; - int64_t c = -3; - std::string msg = "hello"; - std::vector values{1, 2, 3}; - std::map value_map{{"1", 2}, {"3", 4}}; - TestEnum type = TestEnum::RED; - Eigen::Matrix vec = Eigen::Matrix::Zero(); - std::map enable_map{{TestEnum::RED, true}, {TestEnum::GREEN, false}}; - std::set unique_values{1, 2, 3}; -}; - -struct FakeConfig2 { - FakeConfig fake_config; - std::string msg = "world"; -}; - -struct BarConfig { - int a = 1; - float b = 2.0f; - std::string c = "test"; -}; - -struct BarMapConfig { - std::map configs; -}; - -struct MapConverter { - MapConverter() = default; - - std::map to(const std::map& other) const; - - std::map from(const std::map& other) const; -}; - -template -void visit_config(const Visitor& v, FakeConfig& config) { - v.visit("foo", config.foo); - v.visit("bar", config.bar); - v.visit("a", config.a); - v.visit("b", config.b); - v.visit("c", config.c); - v.visit("msg", config.msg); - v.visit("values", config.values); - v.visit("value_map", config.value_map); - v.visit("type", config.type); - v.visit("vec", config.vec); - v.visit("enable_map", config.enable_map, MapConverter()); - v.visit("unique_values", config.unique_values); -} - -template -void visit_config(const Visitor& v, FakeConfig2& config) { - v.visit("fake_config", config.fake_config); - v.visit("msg", config.msg); -} - -template -void visit_config(const Visitor& v, BarConfig& config) { - v.visit("a", config.a); - v.visit("b", config.b); - v.visit("c", config.c); -} - -template -void visit_config(const Visitor& v, BarMapConfig& config) { - v.visit("configs", config.configs); -} - -struct TestLogger : config_parser::Logger { - TestLogger() = default; - - ~TestLogger() = default; - - inline void log_missing(const std::string& message) const override { - ss << message << std::endl; - } - - mutable std::stringstream ss; -}; - -} // namespace hydra - -namespace config_parser { - -template <> -struct ConfigVisitor> { - using MapType = std::map; - - template ::value, bool>::type = true> - static auto visit_config(const V& v, MapType& value) { - for (const auto& child : v.children()) { - value[child] = hydra::BarConfig(); - v.visit(child, value[child]); - } - } - - template ::value, bool>::type = true> - static auto visit_config(const V& v, MapType& value) { - v.pre_visit(); - v.post_visit(); - for (auto& kv_pair : value) { - v.visit(kv_pair.first, kv_pair.second); - } - } -}; - -} // namespace config_parser - -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, FakeConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, FakeConfig2) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, BarConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, BarMapConfig) - -DECLARE_CONFIG_ENUM(hydra, - TestEnum, - {TestEnum::RED, "RED"}, - {TestEnum::GREEN, "GREEN"}, - {TestEnum::BLUE, "BLUE"}) - -namespace hydra { - -std::map MapConverter::to( - const std::map& other) const { - std::map to_return; - for (const auto& kv_pair : other) { - // defined by enum macro - TestEnum new_enum; - readConfigEnumFromString(kv_pair.first, new_enum); - to_return[new_enum] = kv_pair.second; - } - return to_return; -} - -std::map MapConverter::from( - const std::map& other) const { - std::map to_return; - for (const auto& kv_pair : other) { - std::stringstream ss; - ss << configEnumToString(kv_pair.first); - to_return[ss.str()] = kv_pair.second; - } - - return to_return; -} - -TEST(ConfigParser, RosName) { - config_parser::RosParserImpl parser(ros::NodeHandle("/plain_test_config")); - EXPECT_EQ(parser.name(), "/plain_test_config"); - - auto new_parser = parser.child("foo"); - EXPECT_EQ(new_parser.name(), "/plain_test_config/foo"); -} - -TEST(ConfigParsing, RosChildren) { - config_parser::RosParserImpl parser(ros::NodeHandle("/plain_test_config")); - auto children = parser.children(); - EXPECT_EQ(children.size(), 12u); - - auto value_parser = parser.child("value_map"); - auto value_children = value_parser.children(); - EXPECT_EQ(value_children.size(), 2u); - - auto enable_parser = parser.child("enable_map"); - auto enable_children = enable_parser.children(); - EXPECT_EQ(enable_children.size(), 2u); -} - -TEST(ConfigParsing, ParseSingleStructRos) { - auto config = config_parser::load_from_ros("/plain_test_config"); - - EXPECT_EQ(config.foo, 10.0f); - EXPECT_EQ(config.bar, 5.0); - EXPECT_EQ(config.a, -3); - EXPECT_EQ(static_cast(config.b), 1); - EXPECT_EQ(config.c, 2); - EXPECT_EQ(config.msg, "world"); - - std::vector expected_values{4, 5, 6}; - EXPECT_EQ(config.values, expected_values); - - std::map expected_value_map{{"3", 4}, {"5", 6}}; - EXPECT_EQ(config.value_map, expected_value_map); - - EXPECT_EQ(config.type, TestEnum::GREEN); - - Eigen::Matrix expected_vec; - expected_vec << 7, 8, 9; - EXPECT_EQ(config.vec, expected_vec); - - std::map expected_enable_map{{TestEnum::BLUE, 0}, - {TestEnum::GREEN, 1}}; - EXPECT_EQ(config.enable_map, expected_enable_map); - - std::set expected_unique_values{4, 5}; - EXPECT_EQ(config.unique_values, expected_unique_values); -} - -TEST(ConfigParsing, ParseNestedStructRos) { - auto config = config_parser::load_from_ros("/nested_test_config"); - - EXPECT_EQ(config.fake_config.foo, 10.0f); - EXPECT_EQ(config.fake_config.bar, 5.0); - EXPECT_EQ(config.fake_config.a, -3); - EXPECT_EQ(static_cast(config.fake_config.b), 1); - EXPECT_EQ(config.fake_config.c, 2); - EXPECT_EQ(config.fake_config.msg, "hello"); - - std::vector expected_values{1, 2, 3}; - EXPECT_EQ(config.fake_config.values, expected_values); - EXPECT_EQ(config.msg, "again"); - - // make sure conversion respects the default - std::map expected_enable_map{{TestEnum::RED, 1}, - {TestEnum::GREEN, 0}}; - EXPECT_EQ(config.fake_config.enable_map, expected_enable_map); -} - -TEST(ConfigParsing, ParseMapStructRos) { - auto config = config_parser::load_from_ros("/map_test_config"); - - EXPECT_EQ(config.configs.size(), 2u); - ASSERT_TRUE(config.configs.count("config_a")); - ASSERT_TRUE(config.configs.count("config_b")); - - BarConfig config_a = config.configs["config_a"]; - EXPECT_EQ(config_a.a, -3); - EXPECT_NEAR(config_a.b, 5.2f, 1.0e-6f); - EXPECT_EQ(config_a.c, "hello"); - - BarConfig config_b = config.configs["config_b"]; - EXPECT_EQ(config_b.a, 5); - EXPECT_NEAR(config_b.b, -1.2f, 1.0e-6f); - EXPECT_EQ(config_b.c, "other"); -} - -} // namespace hydra diff --git a/hydra/include/hydra/backend/backend_config.h b/include/hydra/backend/backend_config.h similarity index 100% rename from hydra/include/hydra/backend/backend_config.h rename to include/hydra/backend/backend_config.h diff --git a/hydra/include/hydra/backend/backend_module.h b/include/hydra/backend/backend_module.h similarity index 100% rename from hydra/include/hydra/backend/backend_module.h rename to include/hydra/backend/backend_module.h diff --git a/hydra/include/hydra/backend/merge_handler.h b/include/hydra/backend/merge_handler.h similarity index 100% rename from hydra/include/hydra/backend/merge_handler.h rename to include/hydra/backend/merge_handler.h diff --git a/hydra/include/hydra/backend/update_functions.h b/include/hydra/backend/update_functions.h similarity index 100% rename from hydra/include/hydra/backend/update_functions.h rename to include/hydra/backend/update_functions.h diff --git a/hydra/include/hydra/common/common.h b/include/hydra/common/common.h similarity index 100% rename from hydra/include/hydra/common/common.h rename to include/hydra/common/common.h diff --git a/hydra/include/hydra/common/dsg_types.h b/include/hydra/common/dsg_types.h similarity index 100% rename from hydra/include/hydra/common/dsg_types.h rename to include/hydra/common/dsg_types.h diff --git a/hydra/include/hydra/common/hydra_config.h b/include/hydra/common/hydra_config.h similarity index 100% rename from hydra/include/hydra/common/hydra_config.h rename to include/hydra/common/hydra_config.h diff --git a/hydra/include/hydra/common/input_queue.h b/include/hydra/common/input_queue.h similarity index 100% rename from hydra/include/hydra/common/input_queue.h rename to include/hydra/common/input_queue.h diff --git a/hydra/include/hydra/common/robot_prefix_config.h b/include/hydra/common/robot_prefix_config.h similarity index 100% rename from hydra/include/hydra/common/robot_prefix_config.h rename to include/hydra/common/robot_prefix_config.h diff --git a/hydra/include/hydra/common/shared_module_state.h b/include/hydra/common/shared_module_state.h similarity index 100% rename from hydra/include/hydra/common/shared_module_state.h rename to include/hydra/common/shared_module_state.h diff --git a/hydra/include/hydra/config/config.h b/include/hydra/config/config.h similarity index 100% rename from hydra/include/hydra/config/config.h rename to include/hydra/config/config.h diff --git a/hydra/include/hydra/config/config_formatter.h b/include/hydra/config/config_formatter.h similarity index 100% rename from hydra/include/hydra/config/config_formatter.h rename to include/hydra/config/config_formatter.h diff --git a/hydra/include/hydra/config/config_parser.h b/include/hydra/config/config_parser.h similarity index 100% rename from hydra/include/hydra/config/config_parser.h rename to include/hydra/config/config_parser.h diff --git a/hydra/include/hydra/config/config_traits.h b/include/hydra/config/config_traits.h similarity index 100% rename from hydra/include/hydra/config/config_traits.h rename to include/hydra/config/config_traits.h diff --git a/hydra/include/hydra/config/config_visitor.h b/include/hydra/config/config_visitor.h similarity index 100% rename from hydra/include/hydra/config/config_visitor.h rename to include/hydra/config/config_visitor.h diff --git a/hydra/include/hydra/config/eigen_config_types.h b/include/hydra/config/eigen_config_types.h similarity index 100% rename from hydra/include/hydra/config/eigen_config_types.h rename to include/hydra/config/eigen_config_types.h diff --git a/hydra/include/hydra/config/ostream_formatter.h b/include/hydra/config/ostream_formatter.h similarity index 100% rename from hydra/include/hydra/config/ostream_formatter.h rename to include/hydra/config/ostream_formatter.h diff --git a/hydra/include/hydra/config/yaml_parser.h b/include/hydra/config/yaml_parser.h similarity index 100% rename from hydra/include/hydra/config/yaml_parser.h rename to include/hydra/config/yaml_parser.h diff --git a/hydra/include/hydra/frontend/frontend_config.h b/include/hydra/frontend/frontend_config.h similarity index 100% rename from hydra/include/hydra/frontend/frontend_config.h rename to include/hydra/frontend/frontend_config.h diff --git a/hydra/include/hydra/frontend/frontend_module.h b/include/hydra/frontend/frontend_module.h similarity index 100% rename from hydra/include/hydra/frontend/frontend_module.h rename to include/hydra/frontend/frontend_module.h diff --git a/hydra/include/hydra/frontend/mesh_segmenter.h b/include/hydra/frontend/mesh_segmenter.h similarity index 100% rename from hydra/include/hydra/frontend/mesh_segmenter.h rename to include/hydra/frontend/mesh_segmenter.h diff --git a/hydra/include/hydra/gnn/gnn_interface.h b/include/hydra/gnn/gnn_interface.h similarity index 100% rename from hydra/include/hydra/gnn/gnn_interface.h rename to include/hydra/gnn/gnn_interface.h diff --git a/hydra/include/hydra/gnn/ort_utilities.h b/include/hydra/gnn/ort_utilities.h similarity index 100% rename from hydra/include/hydra/gnn/ort_utilities.h rename to include/hydra/gnn/ort_utilities.h diff --git a/hydra/include/hydra/gnn/tensor.h b/include/hydra/gnn/tensor.h similarity index 100% rename from hydra/include/hydra/gnn/tensor.h rename to include/hydra/gnn/tensor.h diff --git a/hydra/include/hydra/loop_closure/descriptor_matching.h b/include/hydra/loop_closure/descriptor_matching.h similarity index 100% rename from hydra/include/hydra/loop_closure/descriptor_matching.h rename to include/hydra/loop_closure/descriptor_matching.h diff --git a/hydra/include/hydra/loop_closure/detector.h b/include/hydra/loop_closure/detector.h similarity index 100% rename from hydra/include/hydra/loop_closure/detector.h rename to include/hydra/loop_closure/detector.h diff --git a/hydra/include/hydra/loop_closure/gnn_descriptors.h b/include/hydra/loop_closure/gnn_descriptors.h similarity index 100% rename from hydra/include/hydra/loop_closure/gnn_descriptors.h rename to include/hydra/loop_closure/gnn_descriptors.h diff --git a/hydra/include/hydra/loop_closure/loop_closure_config.h b/include/hydra/loop_closure/loop_closure_config.h similarity index 100% rename from hydra/include/hydra/loop_closure/loop_closure_config.h rename to include/hydra/loop_closure/loop_closure_config.h diff --git a/hydra/include/hydra/loop_closure/loop_closure_module.h b/include/hydra/loop_closure/loop_closure_module.h similarity index 100% rename from hydra/include/hydra/loop_closure/loop_closure_module.h rename to include/hydra/loop_closure/loop_closure_module.h diff --git a/hydra/include/hydra/loop_closure/registration.h b/include/hydra/loop_closure/registration.h similarity index 100% rename from hydra/include/hydra/loop_closure/registration.h rename to include/hydra/loop_closure/registration.h diff --git a/hydra/include/hydra/loop_closure/scene_graph_descriptors.h b/include/hydra/loop_closure/scene_graph_descriptors.h similarity index 100% rename from hydra/include/hydra/loop_closure/scene_graph_descriptors.h rename to include/hydra/loop_closure/scene_graph_descriptors.h diff --git a/hydra/include/hydra/loop_closure/subgraph_extraction.h b/include/hydra/loop_closure/subgraph_extraction.h similarity index 100% rename from hydra/include/hydra/loop_closure/subgraph_extraction.h rename to include/hydra/loop_closure/subgraph_extraction.h diff --git a/hydra/include/hydra/places/compression_graph_extractor.h b/include/hydra/places/compression_graph_extractor.h similarity index 100% rename from hydra/include/hydra/places/compression_graph_extractor.h rename to include/hydra/places/compression_graph_extractor.h diff --git a/hydra/include/hydra/places/floodfill_graph_extractor.h b/include/hydra/places/floodfill_graph_extractor.h similarity index 100% rename from hydra/include/hydra/places/floodfill_graph_extractor.h rename to include/hydra/places/floodfill_graph_extractor.h diff --git a/hydra/include/hydra/places/graph_extractor_config.h b/include/hydra/places/graph_extractor_config.h similarity index 100% rename from hydra/include/hydra/places/graph_extractor_config.h rename to include/hydra/places/graph_extractor_config.h diff --git a/hydra/include/hydra/places/graph_extractor_interface.h b/include/hydra/places/graph_extractor_interface.h similarity index 100% rename from hydra/include/hydra/places/graph_extractor_interface.h rename to include/hydra/places/graph_extractor_interface.h diff --git a/hydra/include/hydra/places/graph_extractor_types.h b/include/hydra/places/graph_extractor_types.h similarity index 100% rename from hydra/include/hydra/places/graph_extractor_types.h rename to include/hydra/places/graph_extractor_types.h diff --git a/hydra/include/hydra/places/graph_extractor_utilities.h b/include/hydra/places/graph_extractor_utilities.h similarity index 100% rename from hydra/include/hydra/places/graph_extractor_utilities.h rename to include/hydra/places/graph_extractor_utilities.h diff --git a/hydra/include/hydra/places/gvd_graph.h b/include/hydra/places/gvd_graph.h similarity index 100% rename from hydra/include/hydra/places/gvd_graph.h rename to include/hydra/places/gvd_graph.h diff --git a/hydra/include/hydra/places/gvd_integrator.h b/include/hydra/places/gvd_integrator.h similarity index 100% rename from hydra/include/hydra/places/gvd_integrator.h rename to include/hydra/places/gvd_integrator.h diff --git a/hydra/include/hydra/places/gvd_integrator_config.h b/include/hydra/places/gvd_integrator_config.h similarity index 100% rename from hydra/include/hydra/places/gvd_integrator_config.h rename to include/hydra/places/gvd_integrator_config.h diff --git a/hydra/include/hydra/places/gvd_parent_tracker.h b/include/hydra/places/gvd_parent_tracker.h similarity index 100% rename from hydra/include/hydra/places/gvd_parent_tracker.h rename to include/hydra/places/gvd_parent_tracker.h diff --git a/hydra/include/hydra/places/gvd_thinning.h b/include/hydra/places/gvd_thinning.h similarity index 100% rename from hydra/include/hydra/places/gvd_thinning.h rename to include/hydra/places/gvd_thinning.h diff --git a/hydra/include/hydra/places/gvd_utilities.h b/include/hydra/places/gvd_utilities.h similarity index 100% rename from hydra/include/hydra/places/gvd_utilities.h rename to include/hydra/places/gvd_utilities.h diff --git a/hydra/include/hydra/places/gvd_voxel.h b/include/hydra/places/gvd_voxel.h similarity index 100% rename from hydra/include/hydra/places/gvd_voxel.h rename to include/hydra/places/gvd_voxel.h diff --git a/hydra/include/hydra/places/nearest_voxel_utilities.h b/include/hydra/places/nearest_voxel_utilities.h similarity index 100% rename from hydra/include/hydra/places/nearest_voxel_utilities.h rename to include/hydra/places/nearest_voxel_utilities.h diff --git a/hydra/include/hydra/places/update_statistics.h b/include/hydra/places/update_statistics.h similarity index 100% rename from hydra/include/hydra/places/update_statistics.h rename to include/hydra/places/update_statistics.h diff --git a/hydra/include/hydra/places/vertex_voxel.h b/include/hydra/places/vertex_voxel.h similarity index 100% rename from hydra/include/hydra/places/vertex_voxel.h rename to include/hydra/places/vertex_voxel.h diff --git a/hydra/include/hydra/places/voxblox_types.h b/include/hydra/places/voxblox_types.h similarity index 100% rename from hydra/include/hydra/places/voxblox_types.h rename to include/hydra/places/voxblox_types.h diff --git a/hydra/include/hydra/reconstruction/combo_integrator.h b/include/hydra/reconstruction/combo_integrator.h similarity index 100% rename from hydra/include/hydra/reconstruction/combo_integrator.h rename to include/hydra/reconstruction/combo_integrator.h diff --git a/hydra/include/hydra/reconstruction/configs.h b/include/hydra/reconstruction/configs.h similarity index 100% rename from hydra/include/hydra/reconstruction/configs.h rename to include/hydra/reconstruction/configs.h diff --git a/hydra/include/hydra/reconstruction/reconstruction_config.h b/include/hydra/reconstruction/reconstruction_config.h similarity index 100% rename from hydra/include/hydra/reconstruction/reconstruction_config.h rename to include/hydra/reconstruction/reconstruction_config.h diff --git a/hydra/include/hydra/reconstruction/reconstruction_module.h b/include/hydra/reconstruction/reconstruction_module.h similarity index 100% rename from hydra/include/hydra/reconstruction/reconstruction_module.h rename to include/hydra/reconstruction/reconstruction_module.h diff --git a/hydra/include/hydra/reconstruction/reconstruction_output.h b/include/hydra/reconstruction/reconstruction_output.h similarity index 100% rename from hydra/include/hydra/reconstruction/reconstruction_output.h rename to include/hydra/reconstruction/reconstruction_output.h diff --git a/hydra/include/hydra/reconstruction/voxblox_utilities.h b/include/hydra/reconstruction/voxblox_utilities.h similarity index 100% rename from hydra/include/hydra/reconstruction/voxblox_utilities.h rename to include/hydra/reconstruction/voxblox_utilities.h diff --git a/hydra/include/hydra/reconstruction/voxel_aware_marching_cubes.h b/include/hydra/reconstruction/voxel_aware_marching_cubes.h similarity index 100% rename from hydra/include/hydra/reconstruction/voxel_aware_marching_cubes.h rename to include/hydra/reconstruction/voxel_aware_marching_cubes.h diff --git a/hydra/include/hydra/reconstruction/voxel_aware_mesh_integrator.h b/include/hydra/reconstruction/voxel_aware_mesh_integrator.h similarity index 100% rename from hydra/include/hydra/reconstruction/voxel_aware_mesh_integrator.h rename to include/hydra/reconstruction/voxel_aware_mesh_integrator.h diff --git a/hydra/include/hydra/rooms/graph_clustering.h b/include/hydra/rooms/graph_clustering.h similarity index 100% rename from hydra/include/hydra/rooms/graph_clustering.h rename to include/hydra/rooms/graph_clustering.h diff --git a/hydra/include/hydra/rooms/graph_filtration.h b/include/hydra/rooms/graph_filtration.h similarity index 100% rename from hydra/include/hydra/rooms/graph_filtration.h rename to include/hydra/rooms/graph_filtration.h diff --git a/hydra/include/hydra/rooms/room_finder.h b/include/hydra/rooms/room_finder.h similarity index 100% rename from hydra/include/hydra/rooms/room_finder.h rename to include/hydra/rooms/room_finder.h diff --git a/hydra/include/hydra/rooms/room_finder_config.h b/include/hydra/rooms/room_finder_config.h similarity index 100% rename from hydra/include/hydra/rooms/room_finder_config.h rename to include/hydra/rooms/room_finder_config.h diff --git a/hydra/include/hydra/rooms/room_utilities.h b/include/hydra/rooms/room_utilities.h similarity index 100% rename from hydra/include/hydra/rooms/room_utilities.h rename to include/hydra/rooms/room_utilities.h diff --git a/hydra/include/hydra/utils/disjoint_set.h b/include/hydra/utils/disjoint_set.h similarity index 100% rename from hydra/include/hydra/utils/disjoint_set.h rename to include/hydra/utils/disjoint_set.h diff --git a/hydra/include/hydra/utils/display_utilities.h b/include/hydra/utils/display_utilities.h similarity index 100% rename from hydra/include/hydra/utils/display_utilities.h rename to include/hydra/utils/display_utilities.h diff --git a/hydra/include/hydra/utils/log_utilities.h b/include/hydra/utils/log_utilities.h similarity index 100% rename from hydra/include/hydra/utils/log_utilities.h rename to include/hydra/utils/log_utilities.h diff --git a/hydra/include/hydra/utils/minimum_spanning_tree.h b/include/hydra/utils/minimum_spanning_tree.h similarity index 100% rename from hydra/include/hydra/utils/minimum_spanning_tree.h rename to include/hydra/utils/minimum_spanning_tree.h diff --git a/hydra/include/hydra/utils/nearest_neighbor_utilities.h b/include/hydra/utils/nearest_neighbor_utilities.h similarity index 100% rename from hydra/include/hydra/utils/nearest_neighbor_utilities.h rename to include/hydra/utils/nearest_neighbor_utilities.h diff --git a/hydra/include/hydra/utils/timing_utilities.h b/include/hydra/utils/timing_utilities.h similarity index 100% rename from hydra/include/hydra/utils/timing_utilities.h rename to include/hydra/utils/timing_utilities.h diff --git a/install/hydra.rosinstall b/install/hydra.rosinstall index 788852c0..ae64b9b0 100644 --- a/install/hydra.rosinstall +++ b/install/hydra.rosinstall @@ -17,6 +17,9 @@ local-name: gtsam uri: git@github.com:borglab/gtsam.git version: 4.2a9 +- git: + local-name: hydra_ros + uri: git@github.com:MIT-SPARK/Hydra-ROS.git - git: local-name: kimera_pgmo uri: git@github.com:MIT-SPARK/Kimera-PGMO.git @@ -31,7 +34,7 @@ - git: local-name: kimera_semantics uri: git@github.com:MIT-SPARK/Kimera-Semantics.git - version: feature/noetic_fixes + version: develop - git: local-name: mesh_tools uri: git@github.com:yunzc/mesh_tools.git diff --git a/install/hydra_mit.rosinstall b/install/hydra_mit.rosinstall index 8efbf2db..68fa589b 100644 --- a/install/hydra_mit.rosinstall +++ b/install/hydra_mit.rosinstall @@ -17,6 +17,10 @@ local-name: gtsam uri: git@github.com:borglab/gtsam.git version: 4.2a9 +- git: + local-name: hydra_ros + uri: git@github.mit.edu:SPARK/Hydra-ROS.git + version: main - git: local-name: kimera_pgmo uri: git@github.mit.edu:SPARK/Kimera-PGMO.git diff --git a/hydra/models/.gitignore b/models/.gitignore similarity index 100% rename from hydra/models/.gitignore rename to models/.gitignore diff --git a/hydra/models/lcd/.gitignore b/models/lcd/.gitignore similarity index 100% rename from hydra/models/lcd/.gitignore rename to models/lcd/.gitignore diff --git a/hydra/models/lcd/object_gnn.onnx b/models/lcd/object_gnn.onnx similarity index 100% rename from hydra/models/lcd/object_gnn.onnx rename to models/lcd/object_gnn.onnx diff --git a/hydra/models/lcd/place_gnn.onnx b/models/lcd/place_gnn.onnx similarity index 100% rename from hydra/models/lcd/place_gnn.onnx rename to models/lcd/place_gnn.onnx diff --git a/hydra/output/.gitignore b/output/.gitignore similarity index 100% rename from hydra/output/.gitignore rename to output/.gitignore diff --git a/hydra/output/default/.gitignore b/output/default/.gitignore similarity index 100% rename from hydra/output/default/.gitignore rename to output/default/.gitignore diff --git a/hydra/output/default/backend/.gitignore b/output/default/backend/.gitignore similarity index 100% rename from hydra/output/default/backend/.gitignore rename to output/default/backend/.gitignore diff --git a/hydra/output/default/frontend/.gitignore b/output/default/frontend/.gitignore similarity index 100% rename from hydra/output/default/frontend/.gitignore rename to output/default/frontend/.gitignore diff --git a/hydra/output/default/lcd/.gitignore b/output/default/lcd/.gitignore similarity index 100% rename from hydra/output/default/lcd/.gitignore rename to output/default/lcd/.gitignore diff --git a/hydra/output/default/pgmo/.gitignore b/output/default/pgmo/.gitignore similarity index 100% rename from hydra/output/default/pgmo/.gitignore rename to output/default/pgmo/.gitignore diff --git a/hydra/output/default/topology/.gitignore b/output/default/topology/.gitignore similarity index 100% rename from hydra/output/default/topology/.gitignore rename to output/default/topology/.gitignore diff --git a/hydra/output/sidpac/.gitignore b/output/sidpac/.gitignore similarity index 100% rename from hydra/output/sidpac/.gitignore rename to output/sidpac/.gitignore diff --git a/hydra/output/sidpac/backend/.gitignore b/output/sidpac/backend/.gitignore similarity index 100% rename from hydra/output/sidpac/backend/.gitignore rename to output/sidpac/backend/.gitignore diff --git a/hydra/output/sidpac/frontend/.gitignore b/output/sidpac/frontend/.gitignore similarity index 100% rename from hydra/output/sidpac/frontend/.gitignore rename to output/sidpac/frontend/.gitignore diff --git a/hydra/output/sidpac/lcd/.gitignore b/output/sidpac/lcd/.gitignore similarity index 100% rename from hydra/output/sidpac/lcd/.gitignore rename to output/sidpac/lcd/.gitignore diff --git a/hydra/output/sidpac/pgmo/.gitignore b/output/sidpac/pgmo/.gitignore similarity index 100% rename from hydra/output/sidpac/pgmo/.gitignore rename to output/sidpac/pgmo/.gitignore diff --git a/hydra/output/sidpac/topology/.gitignore b/output/sidpac/topology/.gitignore similarity index 100% rename from hydra/output/sidpac/topology/.gitignore rename to output/sidpac/topology/.gitignore diff --git a/hydra/output/uhumans2/.gitignore b/output/uhumans2/.gitignore similarity index 100% rename from hydra/output/uhumans2/.gitignore rename to output/uhumans2/.gitignore diff --git a/hydra/output/uhumans2/backend/.gitignore b/output/uhumans2/backend/.gitignore similarity index 100% rename from hydra/output/uhumans2/backend/.gitignore rename to output/uhumans2/backend/.gitignore diff --git a/hydra/output/uhumans2/frontend/.gitignore b/output/uhumans2/frontend/.gitignore similarity index 100% rename from hydra/output/uhumans2/frontend/.gitignore rename to output/uhumans2/frontend/.gitignore diff --git a/hydra/output/uhumans2/lcd/.gitignore b/output/uhumans2/lcd/.gitignore similarity index 100% rename from hydra/output/uhumans2/lcd/.gitignore rename to output/uhumans2/lcd/.gitignore diff --git a/hydra/output/uhumans2/pgmo/.gitignore b/output/uhumans2/pgmo/.gitignore similarity index 100% rename from hydra/output/uhumans2/pgmo/.gitignore rename to output/uhumans2/pgmo/.gitignore diff --git a/hydra/output/uhumans2/topology/.gitignore b/output/uhumans2/topology/.gitignore similarity index 100% rename from hydra/output/uhumans2/topology/.gitignore rename to output/uhumans2/topology/.gitignore diff --git a/hydra/package.xml b/package.xml similarity index 100% rename from hydra/package.xml rename to package.xml diff --git a/hydra/scripts/export_label_embeddings.py b/scripts/export_label_embeddings.py similarity index 100% rename from hydra/scripts/export_label_embeddings.py rename to scripts/export_label_embeddings.py diff --git a/hydra/scripts/make_colormap.py b/scripts/make_colormap.py similarity index 100% rename from hydra/scripts/make_colormap.py rename to scripts/make_colormap.py diff --git a/hydra/scripts/modify_dgraph.py b/scripts/modify_dgraph.py similarity index 100% rename from hydra/scripts/modify_dgraph.py rename to scripts/modify_dgraph.py diff --git a/hydra/src/CMakeLists.txt b/src/CMakeLists.txt similarity index 100% rename from hydra/src/CMakeLists.txt rename to src/CMakeLists.txt diff --git a/hydra/src/backend/backend_module.cpp b/src/backend/backend_module.cpp similarity index 100% rename from hydra/src/backend/backend_module.cpp rename to src/backend/backend_module.cpp diff --git a/hydra/src/backend/merge_handler.cpp b/src/backend/merge_handler.cpp similarity index 100% rename from hydra/src/backend/merge_handler.cpp rename to src/backend/merge_handler.cpp diff --git a/hydra/src/backend/update_functions.cpp b/src/backend/update_functions.cpp similarity index 100% rename from hydra/src/backend/update_functions.cpp rename to src/backend/update_functions.cpp diff --git a/hydra/src/common/hydra_config.cpp b/src/common/hydra_config.cpp similarity index 100% rename from hydra/src/common/hydra_config.cpp rename to src/common/hydra_config.cpp diff --git a/hydra/src/common/robot_prefix_config.cpp b/src/common/robot_prefix_config.cpp similarity index 100% rename from hydra/src/common/robot_prefix_config.cpp rename to src/common/robot_prefix_config.cpp diff --git a/hydra/src/common/shared_module_state.cpp b/src/common/shared_module_state.cpp similarity index 100% rename from hydra/src/common/shared_module_state.cpp rename to src/common/shared_module_state.cpp diff --git a/hydra/src/config/yaml_parser.cpp b/src/config/yaml_parser.cpp similarity index 100% rename from hydra/src/config/yaml_parser.cpp rename to src/config/yaml_parser.cpp diff --git a/hydra/src/frontend/frontend_module.cpp b/src/frontend/frontend_module.cpp similarity index 100% rename from hydra/src/frontend/frontend_module.cpp rename to src/frontend/frontend_module.cpp diff --git a/hydra/src/frontend/mesh_segmenter.cpp b/src/frontend/mesh_segmenter.cpp similarity index 100% rename from hydra/src/frontend/mesh_segmenter.cpp rename to src/frontend/mesh_segmenter.cpp diff --git a/hydra/src/gnn/CMakeLists.txt b/src/gnn/CMakeLists.txt similarity index 100% rename from hydra/src/gnn/CMakeLists.txt rename to src/gnn/CMakeLists.txt diff --git a/hydra/src/gnn/cmake/ort.CMakeLists.txt.in b/src/gnn/cmake/ort.CMakeLists.txt.in similarity index 100% rename from hydra/src/gnn/cmake/ort.CMakeLists.txt.in rename to src/gnn/cmake/ort.CMakeLists.txt.in diff --git a/hydra/src/gnn/gnn_interface.cpp b/src/gnn/gnn_interface.cpp similarity index 100% rename from hydra/src/gnn/gnn_interface.cpp rename to src/gnn/gnn_interface.cpp diff --git a/hydra/src/gnn/gnn_model_reader.cpp b/src/gnn/gnn_model_reader.cpp similarity index 100% rename from hydra/src/gnn/gnn_model_reader.cpp rename to src/gnn/gnn_model_reader.cpp diff --git a/hydra/src/gnn/ort_utilities.cpp b/src/gnn/ort_utilities.cpp similarity index 100% rename from hydra/src/gnn/ort_utilities.cpp rename to src/gnn/ort_utilities.cpp diff --git a/hydra/src/gnn/tensor.cpp b/src/gnn/tensor.cpp similarity index 100% rename from hydra/src/gnn/tensor.cpp rename to src/gnn/tensor.cpp diff --git a/hydra/src/gnn/tests/resources/make_simple_model.py b/src/gnn/tests/resources/make_simple_model.py similarity index 100% rename from hydra/src/gnn/tests/resources/make_simple_model.py rename to src/gnn/tests/resources/make_simple_model.py diff --git a/hydra/src/gnn/tests/resources/simple_model.onnx b/src/gnn/tests/resources/simple_model.onnx similarity index 100% rename from hydra/src/gnn/tests/resources/simple_model.onnx rename to src/gnn/tests/resources/simple_model.onnx diff --git a/hydra/src/gnn/tests/utest_gnn_interface.cpp b/src/gnn/tests/utest_gnn_interface.cpp similarity index 100% rename from hydra/src/gnn/tests/utest_gnn_interface.cpp rename to src/gnn/tests/utest_gnn_interface.cpp diff --git a/hydra/src/gnn/tests/utest_main.cpp b/src/gnn/tests/utest_main.cpp similarity index 100% rename from hydra/src/gnn/tests/utest_main.cpp rename to src/gnn/tests/utest_main.cpp diff --git a/hydra/src/gnn/tests/utest_tensor.cpp b/src/gnn/tests/utest_tensor.cpp similarity index 100% rename from hydra/src/gnn/tests/utest_tensor.cpp rename to src/gnn/tests/utest_tensor.cpp diff --git a/hydra/src/loop_closure/descriptor_matching.cpp b/src/loop_closure/descriptor_matching.cpp similarity index 100% rename from hydra/src/loop_closure/descriptor_matching.cpp rename to src/loop_closure/descriptor_matching.cpp diff --git a/hydra/src/loop_closure/detector.cpp b/src/loop_closure/detector.cpp similarity index 100% rename from hydra/src/loop_closure/detector.cpp rename to src/loop_closure/detector.cpp diff --git a/hydra/src/loop_closure/gnn_descriptors.cpp b/src/loop_closure/gnn_descriptors.cpp similarity index 100% rename from hydra/src/loop_closure/gnn_descriptors.cpp rename to src/loop_closure/gnn_descriptors.cpp diff --git a/hydra/src/loop_closure/loop_closure_module.cpp b/src/loop_closure/loop_closure_module.cpp similarity index 100% rename from hydra/src/loop_closure/loop_closure_module.cpp rename to src/loop_closure/loop_closure_module.cpp diff --git a/hydra/src/loop_closure/registration.cpp b/src/loop_closure/registration.cpp similarity index 100% rename from hydra/src/loop_closure/registration.cpp rename to src/loop_closure/registration.cpp diff --git a/hydra/src/loop_closure/scene_graph_descriptors.cpp b/src/loop_closure/scene_graph_descriptors.cpp similarity index 100% rename from hydra/src/loop_closure/scene_graph_descriptors.cpp rename to src/loop_closure/scene_graph_descriptors.cpp diff --git a/hydra/src/loop_closure/subgraph_extraction.cpp b/src/loop_closure/subgraph_extraction.cpp similarity index 100% rename from hydra/src/loop_closure/subgraph_extraction.cpp rename to src/loop_closure/subgraph_extraction.cpp diff --git a/hydra/src/places/compression_graph_extractor.cpp b/src/places/compression_graph_extractor.cpp similarity index 100% rename from hydra/src/places/compression_graph_extractor.cpp rename to src/places/compression_graph_extractor.cpp diff --git a/hydra/src/places/floodfill_graph_extractor.cpp b/src/places/floodfill_graph_extractor.cpp similarity index 100% rename from hydra/src/places/floodfill_graph_extractor.cpp rename to src/places/floodfill_graph_extractor.cpp diff --git a/hydra/src/places/graph_extractor_interface.cpp b/src/places/graph_extractor_interface.cpp similarity index 100% rename from hydra/src/places/graph_extractor_interface.cpp rename to src/places/graph_extractor_interface.cpp diff --git a/hydra/src/places/graph_extractor_types.cpp b/src/places/graph_extractor_types.cpp similarity index 100% rename from hydra/src/places/graph_extractor_types.cpp rename to src/places/graph_extractor_types.cpp diff --git a/hydra/src/places/graph_extractor_utilities.cpp b/src/places/graph_extractor_utilities.cpp similarity index 100% rename from hydra/src/places/graph_extractor_utilities.cpp rename to src/places/graph_extractor_utilities.cpp diff --git a/hydra/src/places/gvd_graph.cpp b/src/places/gvd_graph.cpp similarity index 100% rename from hydra/src/places/gvd_graph.cpp rename to src/places/gvd_graph.cpp diff --git a/hydra/src/places/gvd_integrator.cpp b/src/places/gvd_integrator.cpp similarity index 100% rename from hydra/src/places/gvd_integrator.cpp rename to src/places/gvd_integrator.cpp diff --git a/hydra/src/places/gvd_parent_tracker.cpp b/src/places/gvd_parent_tracker.cpp similarity index 100% rename from hydra/src/places/gvd_parent_tracker.cpp rename to src/places/gvd_parent_tracker.cpp diff --git a/hydra/src/places/gvd_thinning.cpp b/src/places/gvd_thinning.cpp similarity index 100% rename from hydra/src/places/gvd_thinning.cpp rename to src/places/gvd_thinning.cpp diff --git a/hydra/src/places/gvd_utilities.cpp b/src/places/gvd_utilities.cpp similarity index 100% rename from hydra/src/places/gvd_utilities.cpp rename to src/places/gvd_utilities.cpp diff --git a/hydra/src/places/gvd_voxel.cpp b/src/places/gvd_voxel.cpp similarity index 100% rename from hydra/src/places/gvd_voxel.cpp rename to src/places/gvd_voxel.cpp diff --git a/hydra/src/places/nearest_voxel_utilities.cpp b/src/places/nearest_voxel_utilities.cpp similarity index 100% rename from hydra/src/places/nearest_voxel_utilities.cpp rename to src/places/nearest_voxel_utilities.cpp diff --git a/hydra/src/places/update_statistics.cpp b/src/places/update_statistics.cpp similarity index 100% rename from hydra/src/places/update_statistics.cpp rename to src/places/update_statistics.cpp diff --git a/hydra/src/reconstruction/reconstruction_config.cpp b/src/reconstruction/reconstruction_config.cpp similarity index 100% rename from hydra/src/reconstruction/reconstruction_config.cpp rename to src/reconstruction/reconstruction_config.cpp diff --git a/hydra/src/reconstruction/reconstruction_module.cpp b/src/reconstruction/reconstruction_module.cpp similarity index 100% rename from hydra/src/reconstruction/reconstruction_module.cpp rename to src/reconstruction/reconstruction_module.cpp diff --git a/hydra/src/reconstruction/voxel_aware_marching_cubes.cpp b/src/reconstruction/voxel_aware_marching_cubes.cpp similarity index 100% rename from hydra/src/reconstruction/voxel_aware_marching_cubes.cpp rename to src/reconstruction/voxel_aware_marching_cubes.cpp diff --git a/hydra/src/reconstruction/voxel_aware_mesh_integrator.cpp b/src/reconstruction/voxel_aware_mesh_integrator.cpp similarity index 100% rename from hydra/src/reconstruction/voxel_aware_mesh_integrator.cpp rename to src/reconstruction/voxel_aware_mesh_integrator.cpp diff --git a/hydra/src/rooms/graph_clustering.cpp b/src/rooms/graph_clustering.cpp similarity index 100% rename from hydra/src/rooms/graph_clustering.cpp rename to src/rooms/graph_clustering.cpp diff --git a/hydra/src/rooms/graph_filtration.cpp b/src/rooms/graph_filtration.cpp similarity index 100% rename from hydra/src/rooms/graph_filtration.cpp rename to src/rooms/graph_filtration.cpp diff --git a/hydra/src/rooms/room_finder.cpp b/src/rooms/room_finder.cpp similarity index 100% rename from hydra/src/rooms/room_finder.cpp rename to src/rooms/room_finder.cpp diff --git a/hydra/src/rooms/room_utilities.cpp b/src/rooms/room_utilities.cpp similarity index 100% rename from hydra/src/rooms/room_utilities.cpp rename to src/rooms/room_utilities.cpp diff --git a/hydra/src/utils/disjoint_set.cpp b/src/utils/disjoint_set.cpp similarity index 100% rename from hydra/src/utils/disjoint_set.cpp rename to src/utils/disjoint_set.cpp diff --git a/hydra/src/utils/display_utilities.cpp b/src/utils/display_utilities.cpp similarity index 100% rename from hydra/src/utils/display_utilities.cpp rename to src/utils/display_utilities.cpp diff --git a/hydra/src/utils/log_utilities.cpp b/src/utils/log_utilities.cpp similarity index 100% rename from hydra/src/utils/log_utilities.cpp rename to src/utils/log_utilities.cpp diff --git a/hydra/src/utils/minimum_spanning_tree.cpp b/src/utils/minimum_spanning_tree.cpp similarity index 100% rename from hydra/src/utils/minimum_spanning_tree.cpp rename to src/utils/minimum_spanning_tree.cpp diff --git a/hydra/src/utils/nearest_neighbor_utilities.cpp b/src/utils/nearest_neighbor_utilities.cpp similarity index 100% rename from hydra/src/utils/nearest_neighbor_utilities.cpp rename to src/utils/nearest_neighbor_utilities.cpp diff --git a/hydra/src/utils/timing_utilities.cpp b/src/utils/timing_utilities.cpp similarity index 100% rename from hydra/src/utils/timing_utilities.cpp rename to src/utils/timing_utilities.cpp diff --git a/hydra/tests/CMakeLists.txt b/tests/CMakeLists.txt similarity index 100% rename from hydra/tests/CMakeLists.txt rename to tests/CMakeLists.txt diff --git a/hydra/tests/backend/test_merge_handler.cpp b/tests/backend/test_merge_handler.cpp similarity index 100% rename from hydra/tests/backend/test_merge_handler.cpp rename to tests/backend/test_merge_handler.cpp diff --git a/hydra/tests/backend/test_update_functions.cpp b/tests/backend/test_update_functions.cpp similarity index 100% rename from hydra/tests/backend/test_update_functions.cpp rename to tests/backend/test_update_functions.cpp diff --git a/hydra/tests/config/test_config.cpp b/tests/config/test_config.cpp similarity index 100% rename from hydra/tests/config/test_config.cpp rename to tests/config/test_config.cpp diff --git a/hydra/tests/frontend/test_frontend.cpp b/tests/frontend/test_frontend.cpp similarity index 100% rename from hydra/tests/frontend/test_frontend.cpp rename to tests/frontend/test_frontend.cpp diff --git a/hydra/tests/include/hydra_test/gvd_layer_utils.h b/tests/include/hydra_test/gvd_layer_utils.h similarity index 100% rename from hydra/tests/include/hydra_test/gvd_layer_utils.h rename to tests/include/hydra_test/gvd_layer_utils.h diff --git a/hydra/tests/include/hydra_test/place_fixtures.h b/tests/include/hydra_test/place_fixtures.h similarity index 100% rename from hydra/tests/include/hydra_test/place_fixtures.h rename to tests/include/hydra_test/place_fixtures.h diff --git a/hydra/tests/include/hydra_test/resources.h b/tests/include/hydra_test/resources.h similarity index 100% rename from hydra/tests/include/hydra_test/resources.h rename to tests/include/hydra_test/resources.h diff --git a/hydra/tests/loop_closure/test_descriptor_matching.cpp b/tests/loop_closure/test_descriptor_matching.cpp similarity index 100% rename from hydra/tests/loop_closure/test_descriptor_matching.cpp rename to tests/loop_closure/test_descriptor_matching.cpp diff --git a/hydra/tests/loop_closure/test_detector.cpp b/tests/loop_closure/test_detector.cpp similarity index 100% rename from hydra/tests/loop_closure/test_detector.cpp rename to tests/loop_closure/test_detector.cpp diff --git a/hydra/tests/loop_closure/test_gnn_descriptors.cpp b/tests/loop_closure/test_gnn_descriptors.cpp similarity index 100% rename from hydra/tests/loop_closure/test_gnn_descriptors.cpp rename to tests/loop_closure/test_gnn_descriptors.cpp diff --git a/hydra/tests/loop_closure/test_registration.cpp b/tests/loop_closure/test_registration.cpp similarity index 100% rename from hydra/tests/loop_closure/test_registration.cpp rename to tests/loop_closure/test_registration.cpp diff --git a/hydra/tests/loop_closure/test_scene_graph_descriptors.cpp b/tests/loop_closure/test_scene_graph_descriptors.cpp similarity index 100% rename from hydra/tests/loop_closure/test_scene_graph_descriptors.cpp rename to tests/loop_closure/test_scene_graph_descriptors.cpp diff --git a/hydra/tests/loop_closure/test_subgraph_extraction.cpp b/tests/loop_closure/test_subgraph_extraction.cpp similarity index 100% rename from hydra/tests/loop_closure/test_subgraph_extraction.cpp rename to tests/loop_closure/test_subgraph_extraction.cpp diff --git a/hydra/tests/main.cpp b/tests/main.cpp similarity index 100% rename from hydra/tests/main.cpp rename to tests/main.cpp diff --git a/hydra/tests/places/test_compression_graph_extractor.cpp b/tests/places/test_compression_graph_extractor.cpp similarity index 100% rename from hydra/tests/places/test_compression_graph_extractor.cpp rename to tests/places/test_compression_graph_extractor.cpp diff --git a/hydra/tests/places/test_esdf_comparison.cpp b/tests/places/test_esdf_comparison.cpp similarity index 100% rename from hydra/tests/places/test_esdf_comparison.cpp rename to tests/places/test_esdf_comparison.cpp diff --git a/hydra/tests/places/test_floodfill_graph_extractor.cpp b/tests/places/test_floodfill_graph_extractor.cpp similarity index 100% rename from hydra/tests/places/test_floodfill_graph_extractor.cpp rename to tests/places/test_floodfill_graph_extractor.cpp diff --git a/hydra/tests/places/test_graph_extractor_utilities.cpp b/tests/places/test_graph_extractor_utilities.cpp similarity index 100% rename from hydra/tests/places/test_graph_extractor_utilities.cpp rename to tests/places/test_graph_extractor_utilities.cpp diff --git a/hydra/tests/places/test_gvd_incremental.cpp b/tests/places/test_gvd_incremental.cpp similarity index 100% rename from hydra/tests/places/test_gvd_incremental.cpp rename to tests/places/test_gvd_incremental.cpp diff --git a/hydra/tests/places/test_gvd_integrator.cpp b/tests/places/test_gvd_integrator.cpp similarity index 100% rename from hydra/tests/places/test_gvd_integrator.cpp rename to tests/places/test_gvd_integrator.cpp diff --git a/hydra/tests/places/test_gvd_thinning.cpp b/tests/places/test_gvd_thinning.cpp similarity index 100% rename from hydra/tests/places/test_gvd_thinning.cpp rename to tests/places/test_gvd_thinning.cpp diff --git a/hydra/tests/places/test_gvd_utilities.cpp b/tests/places/test_gvd_utilities.cpp similarity index 100% rename from hydra/tests/places/test_gvd_utilities.cpp rename to tests/places/test_gvd_utilities.cpp diff --git a/hydra/tests/reconstruction/test_marching_cubes.cpp b/tests/reconstruction/test_marching_cubes.cpp similarity index 100% rename from hydra/tests/reconstruction/test_marching_cubes.cpp rename to tests/reconstruction/test_marching_cubes.cpp diff --git a/hydra/tests/reconstruction/test_reconstruction_module.cpp b/tests/reconstruction/test_reconstruction_module.cpp similarity index 100% rename from hydra/tests/reconstruction/test_reconstruction_module.cpp rename to tests/reconstruction/test_reconstruction_module.cpp diff --git a/hydra/tests/resources/config/map_config.yaml b/tests/resources/config/map_config.yaml similarity index 100% rename from hydra/tests/resources/config/map_config.yaml rename to tests/resources/config/map_config.yaml diff --git a/hydra/tests/resources/config/missing_config.yaml b/tests/resources/config/missing_config.yaml similarity index 100% rename from hydra/tests/resources/config/missing_config.yaml rename to tests/resources/config/missing_config.yaml diff --git a/hydra/tests/resources/config/nested_test_config.yaml b/tests/resources/config/nested_test_config.yaml similarity index 100% rename from hydra/tests/resources/config/nested_test_config.yaml rename to tests/resources/config/nested_test_config.yaml diff --git a/hydra/tests/resources/config/test_config.yaml b/tests/resources/config/test_config.yaml similarity index 100% rename from hydra/tests/resources/config/test_config.yaml rename to tests/resources/config/test_config.yaml diff --git a/hydra/tests/resources/loop_closure/make_lcd_models.py b/tests/resources/loop_closure/make_lcd_models.py similarity index 100% rename from hydra/tests/resources/loop_closure/make_lcd_models.py rename to tests/resources/loop_closure/make_lcd_models.py diff --git a/hydra/tests/resources/loop_closure/objects.onnx b/tests/resources/loop_closure/objects.onnx similarity index 100% rename from hydra/tests/resources/loop_closure/objects.onnx rename to tests/resources/loop_closure/objects.onnx diff --git a/hydra/tests/resources/loop_closure/objects_pos.onnx b/tests/resources/loop_closure/objects_pos.onnx similarity index 100% rename from hydra/tests/resources/loop_closure/objects_pos.onnx rename to tests/resources/loop_closure/objects_pos.onnx diff --git a/hydra/tests/resources/loop_closure/places.onnx b/tests/resources/loop_closure/places.onnx similarity index 100% rename from hydra/tests/resources/loop_closure/places.onnx rename to tests/resources/loop_closure/places.onnx diff --git a/hydra/tests/resources/loop_closure/places_pos.onnx b/tests/resources/loop_closure/places_pos.onnx similarity index 100% rename from hydra/tests/resources/loop_closure/places_pos.onnx rename to tests/resources/loop_closure/places_pos.onnx diff --git a/hydra/tests/resources/loop_closure/test_embeddings.yaml b/tests/resources/loop_closure/test_embeddings.yaml similarity index 100% rename from hydra/tests/resources/loop_closure/test_embeddings.yaml rename to tests/resources/loop_closure/test_embeddings.yaml diff --git a/hydra/tests/resources/test_semantic_map.csv b/tests/resources/test_semantic_map.csv similarity index 100% rename from hydra/tests/resources/test_semantic_map.csv rename to tests/resources/test_semantic_map.csv diff --git a/hydra/tests/rooms/test_graph_clustering.cpp b/tests/rooms/test_graph_clustering.cpp similarity index 100% rename from hydra/tests/rooms/test_graph_clustering.cpp rename to tests/rooms/test_graph_clustering.cpp diff --git a/hydra/tests/rooms/test_graph_filtration.cpp b/tests/rooms/test_graph_filtration.cpp similarity index 100% rename from hydra/tests/rooms/test_graph_filtration.cpp rename to tests/rooms/test_graph_filtration.cpp diff --git a/hydra/tests/rooms/test_room_finder.cpp b/tests/rooms/test_room_finder.cpp similarity index 100% rename from hydra/tests/rooms/test_room_finder.cpp rename to tests/rooms/test_room_finder.cpp diff --git a/hydra/tests/rooms/test_room_finder_config.cpp b/tests/rooms/test_room_finder_config.cpp similarity index 100% rename from hydra/tests/rooms/test_room_finder_config.cpp rename to tests/rooms/test_room_finder_config.cpp diff --git a/hydra/tests/rooms/test_room_utilities.cpp b/tests/rooms/test_room_utilities.cpp similarity index 100% rename from hydra/tests/rooms/test_room_utilities.cpp rename to tests/rooms/test_room_utilities.cpp diff --git a/hydra/tests/src/place_fixtures.cpp b/tests/src/place_fixtures.cpp similarity index 100% rename from hydra/tests/src/place_fixtures.cpp rename to tests/src/place_fixtures.cpp diff --git a/hydra/tests/src/resources.cpp b/tests/src/resources.cpp similarity index 100% rename from hydra/tests/src/resources.cpp rename to tests/src/resources.cpp diff --git a/hydra/tests/utils/test_minimum_spanning_tree.cpp b/tests/utils/test_minimum_spanning_tree.cpp similarity index 100% rename from hydra/tests/utils/test_minimum_spanning_tree.cpp rename to tests/utils/test_minimum_spanning_tree.cpp diff --git a/hydra/tests/utils/test_nearest_neighbor_utilities.cpp b/tests/utils/test_nearest_neighbor_utilities.cpp similarity index 100% rename from hydra/tests/utils/test_nearest_neighbor_utilities.cpp rename to tests/utils/test_nearest_neighbor_utilities.cpp diff --git a/hydra/tests/utils/test_timing_utilities.cpp b/tests/utils/test_timing_utilities.cpp similarity index 100% rename from hydra/tests/utils/test_timing_utilities.cpp rename to tests/utils/test_timing_utilities.cpp