├── roman_ros2 ├── resource │ └── roman_ros2 ├── roman_ros2 │ ├── __init__.py │ ├── roman_align_node.py │ ├── lc_viz_node.py │ ├── fastsam_node.py │ ├── utils.py │ ├── roman_map_node.py │ └── roman_loop_closure_node.py ├── cfg │ ├── zed2i │ │ ├── fastsam_node.yaml │ │ ├── roman_map_node.yaml │ │ ├── mapper.yaml │ │ ├── roman_loop_closure.yaml │ │ ├── roman_loop_closure_node.yaml │ │ └── fastsam.yaml │ ├── d455 │ │ ├── fastsam_node.yaml │ │ ├── roman_map_node.yaml │ │ ├── mapper.yaml │ │ ├── roman_loop_closure.yaml │ │ ├── fastsam.yaml │ │ └── roman_loop_closure_node.yaml │ └── d455_with_prior_map │ │ ├── roman_loop_closure.yaml │ │ └── roman_loop_closure_node.yaml ├── setup.cfg ├── package.xml ├── test │ ├── test_copyright.py │ ├── test_pep257.py │ └── test_flake8.py ├── setup.py └── launch │ ├── roman_node.launch.yaml │ └── roman.launch.yaml ├── media └── opposite_view_loop_closure.jpg ├── roman_msgs ├── msg │ ├── FrameDescriptor.msg │ ├── ObservationArray.msg │ ├── Observation.msg │ ├── Segment.msg │ └── LoopClosure.msg ├── package.xml └── CMakeLists.txt ├── examples ├── d455_two_session │ ├── params │ │ ├── submap_align.yaml │ │ └── data.yaml │ └── run.bash ├── zed2i_two_session │ ├── params │ │ ├── submap_align.yaml │ │ └── data.yaml │ └── run.bash ├── zed2i_single_session │ ├── run.bash │ └── zed2i_online.yaml ├── d455_single_session │ ├── run.bash │ ├── d455_online.yaml │ └── d455_online.rviz ├── d455_bag │ ├── run_single_session.sh │ ├── run_with_prior_map.sh │ └── d455_bag.yaml └── README.md ├── install ├── packages.yaml └── install_roman.bash ├── scripts └── configure_cpu_or_gpu.py ├── .gitignore └── README.md /roman_ros2/resource/roman_ros2: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /media/opposite_view_loop_closure.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mit-acl/roman_ros/HEAD/media/opposite_view_loop_closure.jpg -------------------------------------------------------------------------------- /roman_msgs/msg/FrameDescriptor.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | geometry_msgs/Pose pose 3 | std_msgs/Float64MultiArray descriptor -------------------------------------------------------------------------------- /roman_ros2/cfg/zed2i/fastsam_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | odom_frame_id: odom 4 | base_flu_frame_id: zed_camera_link 5 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /roman_ros2/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/roman_ros2 3 | [install] 4 | install_scripts=$base/lib/roman_ros2 5 | [build_scripts] 6 | executable = /usr/bin/env python3 -------------------------------------------------------------------------------- /roman_msgs/msg/ObservationArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Pose pose_flu 4 | std_msgs/Float64MultiArray frame_descriptor 5 | Observation[] observations -------------------------------------------------------------------------------- /roman_ros2/cfg/d455/fastsam_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | odom_frame_id: $(var robot_name)/odom 4 | base_flu_frame_id: $(var camera)_link 5 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /examples/d455_two_session/params/submap_align.yaml: -------------------------------------------------------------------------------- 1 | method: roman 2 | submap_radius: 20.0 3 | submap_center_dist: 10.0 4 | submap_center_time: 1000.0 5 | submap_max_size: 50 6 | cosine_max: 0.95 7 | sigma: 0.3 8 | epsilon: 0.5 -------------------------------------------------------------------------------- /examples/zed2i_two_session/params/submap_align.yaml: -------------------------------------------------------------------------------- 1 | method: roman 2 | submap_radius: 20.0 3 | submap_center_dist: 10.0 4 | submap_center_time: 1000.0 5 | submap_max_size: 50 6 | cosine_max: 0.95 7 | sigma: 0.3 8 | epsilon: 0.5 -------------------------------------------------------------------------------- /roman_msgs/msg/Observation.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time stamp 2 | geometry_msgs/Pose pose 3 | float64[2] pixel 4 | int32 mask_width 5 | int32 mask_height 6 | int32 img_width 7 | int32 img_height 8 | int8[] mask 9 | float64[] point_cloud 10 | float64[] descriptor -------------------------------------------------------------------------------- /roman_msgs/msg/Segment.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | int32 robot_id 3 | int32 segment_id 4 | geometry_msgs/Point position # Position in odom frame 5 | float64 volume 6 | float64[4] shape_attributes # volume, linearity, planarity, scattering 7 | float64[] semantic_descriptor # semantic embedding -------------------------------------------------------------------------------- /examples/zed2i_single_session/run.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 3 | 4 | export ROMAN_ROS_WS=$(realpath $SCRIPT_DIR/../../../../) 5 | export OUTPUT_DIR=~/.roman_ros2 6 | export RECORD=false 7 | 8 | tmuxp load $SCRIPT_DIR/zed2i_online.yaml -------------------------------------------------------------------------------- /roman_ros2/cfg/zed2i/roman_map_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | odom_frame_id: odom 4 | base_flu_frame_id: zed_camera_link 5 | use_multiple_cams: false 6 | robot_id: 0 7 | visualize: true 8 | viz_min_viz_dt: 0.25 9 | output_roman_map: ~/.roman_ros2/roman_map.pkl 10 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /roman_ros2/cfg/d455/roman_map_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | odom_frame_id: $(var robot_name)/odom 4 | base_flu_frame_id: $(var camera)_link 5 | use_multiple_cams: false 6 | robot_id: 0 7 | visualize: true 8 | viz_min_viz_dt: 0.25 9 | output_roman_map: ~/.roman_ros2/roman_map.pkl 10 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /roman_msgs/msg/LoopClosure.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | int32 robot1_id 4 | int32 robot2_id 5 | 6 | int32 submap1_id 7 | int32 submap2_id 8 | 9 | builtin_interfaces/Time robot1_time 10 | builtin_interfaces/Time robot2_time 11 | 12 | int32 num_associations 13 | int32[] robot1_associated_segment_ids 14 | int32[] robot2_associated_segment_ids 15 | 16 | geometry_msgs/Pose transform_robot1_robot2 17 | float64[36] covariance # rotation first -------------------------------------------------------------------------------- /examples/d455_single_session/run.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 3 | 4 | export ROS_IP=127.0.0.1 5 | export ROS_MASTER_URI=http://$ROS_IP:11311 6 | export ROMAN_ROS_WS=$(realpath $SCRIPT_DIR/../../../../) 7 | export KIMERA_VIO_ROS2_REALSENSE_DIR=$ROMAN_ROS_WS/src/kimera-vio-ros2-realsense 8 | export OUTPUT_DIR=~/.roman_ros2 9 | export RECORD=false 10 | 11 | tmuxp load $SCRIPT_DIR/d455_online.yaml -------------------------------------------------------------------------------- /roman_ros2/cfg/d455/mapper.yaml: -------------------------------------------------------------------------------- 1 | # ROMAN (non-ROS) MapperParams 2 | geometric_association_method: 'iou' 3 | semantic_association_method: 'none' 4 | geometric_score_range: (0.25, 1.0) 5 | semantic_score_range: (0.8, 1.0) 6 | min_2d_iou: 0.8 7 | min_sightings: 2 8 | max_t_no_sightings: 0.4 9 | mask_downsample_factor: 8 10 | min_max_extent: 0.25 11 | plane_prune_params: [3.0, 3.0, 0.5] 12 | segment_graveyard_time: 15.0 13 | segment_graveyard_dist: 10.0 14 | iou_voxel_size: 0.2 15 | segment_voxel_size: 0.05 -------------------------------------------------------------------------------- /roman_ros2/cfg/zed2i/mapper.yaml: -------------------------------------------------------------------------------- 1 | # ROMAN (non-ROS) MapperParams 2 | geometric_association_method: 'iou' 3 | semantic_association_method: 'none' 4 | geometric_score_range: (0.25, 1.0) 5 | semantic_score_range: (0.8, 1.0) 6 | min_2d_iou: 0.8 7 | min_sightings: 2 8 | max_t_no_sightings: 0.4 9 | mask_downsample_factor: 8 10 | min_max_extent: 0.25 11 | plane_prune_params: [3.0, 3.0, 0.5] 12 | segment_graveyard_time: 15.0 13 | segment_graveyard_dist: 10.0 14 | iou_voxel_size: 0.2 15 | segment_voxel_size: 0.05 -------------------------------------------------------------------------------- /install/packages.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros2_numpy: 3 | type: git 4 | url: https://github.com/Box-Robotics/ros2_numpy.git 5 | version: jazzy 6 | ros_system_monitor: 7 | type: git 8 | url: https://github.com/MIT-SPARK/ROS-System-Monitor.git 9 | version: main 10 | pose_graph_tools: 11 | type: git 12 | url: https://github.com/MIT-SPARK/pose_graph_tools.git 13 | version: ros2 14 | roman: 15 | type: git 16 | url: https://github.com/mit-acl/roman.git 17 | version: main -------------------------------------------------------------------------------- /roman_ros2/cfg/d455/roman_loop_closure.yaml: -------------------------------------------------------------------------------- 1 | dim: 3 2 | method: roman 3 | fusion_method: geometric_mean 4 | submap_radius: 15.0 5 | submap_center_dist: 10.0 6 | submap_center_time: 50.0 7 | submap_max_size: 40 8 | sigma: 0.3 9 | epsilon: 0.5 10 | mindist: 0.4 11 | cosine_min: 0.5 12 | cosine_max: 0.7 13 | force_rm_lc_roll_pitch: true 14 | force_rm_upside_down: true 15 | use_object_bottom_middle: false 16 | submap_descriptor: 'stacked_frame_descriptors' 17 | frame_descriptor_dist: 10.0 18 | submap_descriptor_thresh: 0.7 -------------------------------------------------------------------------------- /roman_ros2/cfg/zed2i/roman_loop_closure.yaml: -------------------------------------------------------------------------------- 1 | dim: 3 2 | method: roman 3 | fusion_method: geometric_mean 4 | submap_radius: 15.0 5 | submap_center_dist: 10.0 6 | submap_center_time: 50.0 7 | submap_max_size: 40 8 | sigma: 0.3 9 | epsilon: 0.5 10 | mindist: 0.4 11 | cosine_min: 0.5 12 | cosine_max: 0.7 13 | force_rm_lc_roll_pitch: true 14 | force_rm_upside_down: true 15 | use_object_bottom_middle: false 16 | submap_descriptor: 'stacked_frame_descriptors' 17 | frame_descriptor_dist: 10.0 18 | submap_descriptor_thresh: 0.7 -------------------------------------------------------------------------------- /roman_ros2/cfg/d455_with_prior_map/roman_loop_closure.yaml: -------------------------------------------------------------------------------- 1 | dim: 3 2 | method: roman 3 | fusion_method: geometric_mean 4 | submap_radius: 15.0 5 | submap_center_dist: 10.0 6 | submap_center_time: 50.0 7 | submap_max_size: 40 8 | sigma: 0.3 9 | epsilon: 0.5 10 | mindist: 0.4 11 | cosine_min: 0.5 12 | cosine_max: 0.7 13 | force_rm_lc_roll_pitch: true 14 | force_rm_upside_down: true 15 | use_object_bottom_middle: false 16 | submap_descriptor: 'stacked_frame_descriptors' 17 | frame_descriptor_dist: 10.0 18 | submap_descriptor_thresh: 0.7 -------------------------------------------------------------------------------- /roman_ros2/cfg/zed2i/roman_loop_closure_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | submap_num_segments: 40 4 | submap_overlapping_segments: 20 5 | lc_required_associations: 6 6 | ego_id: 0 7 | ego_name: $(var robot_name) 8 | ego_flu_ref_frame: zed_camera_link 9 | ego_odom_frame: odom 10 | team_ids: [0] 11 | team_names: [''] 12 | team_flu_ref_frames: [''] 13 | team_odom_frames: [''] 14 | prior_session_maps: [''] 15 | prior_session_ids: [0] 16 | submap_knn: 5 17 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /roman_ros2/cfg/d455/fastsam.yaml: -------------------------------------------------------------------------------- 1 | weights_path: "$ROMAN_WEIGHTS/FastSAM-x.pt" 2 | yolo_weights_path: "$ROMAN_WEIGHTS/yolov7.pt" 3 | imgsz: [256, 256] 4 | device: 'cuda' 5 | mask_downsample_factor: 8 6 | min_mask_len_div: 30 7 | max_mask_len_div: 3 8 | ignore_people: False 9 | erosion_size: 3 10 | voxel_size: 0.05 11 | ignore_labels: ['person'] 12 | use_keep_labels: False 13 | keep_labels: [] 14 | plane_filter_params: [3.0, 1.0, 0.2] 15 | semantics: 'dino' 16 | yolo_imgsz: [256, 256] 17 | depth_scale: 1000.0 18 | max_depth: 6.0 19 | frame_descriptor: 'dino-gem' -------------------------------------------------------------------------------- /roman_ros2/cfg/zed2i/fastsam.yaml: -------------------------------------------------------------------------------- 1 | weights_path: "$ROMAN_WEIGHTS/FastSAM-x.pt" 2 | yolo_weights_path: "$ROMAN_WEIGHTS/yolov7.pt" 3 | imgsz: [256, 256] 4 | device: 'cuda' 5 | mask_downsample_factor: 8 6 | min_mask_len_div: 30 7 | max_mask_len_div: 3 8 | ignore_people: False 9 | erosion_size: 3 10 | voxel_size: 0.05 11 | ignore_labels: ['person'] 12 | use_keep_labels: False 13 | keep_labels: [] 14 | plane_filter_params: [3.0, 1.0, 0.2] 15 | semantics: 'dino' 16 | yolo_imgsz: [256, 256] 17 | depth_scale: 1.0 18 | max_depth: 6.0 19 | frame_descriptor: 'dino-gem' -------------------------------------------------------------------------------- /roman_ros2/cfg/d455/roman_loop_closure_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | submap_num_segments: 40 4 | submap_overlapping_segments: 20 5 | lc_required_associations: 6 6 | ego_id: 0 7 | ego_name: $(var robot_name) 8 | ego_flu_ref_frame: $(var camera)_link 9 | ego_odom_frame: $(var robot_name)/odom 10 | team_ids: [0] 11 | team_names: [''] 12 | team_flu_ref_frames: [''] 13 | team_odom_frames: [''] 14 | prior_session_maps: [''] 15 | prior_session_ids: [0] 16 | submap_knn: 5 17 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /roman_ros2/cfg/d455_with_prior_map/roman_loop_closure_node.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | submap_num_segments: 40 4 | submap_overlapping_segments: 20 5 | lc_required_associations: 6 6 | ego_id: 0 7 | ego_name: $(var robot_name) 8 | ego_flu_ref_frame: $(var camera)_link 9 | ego_odom_frame: $(var robot_name)/odom 10 | team_ids: [0] 11 | team_names: [''] 12 | team_flu_ref_frames: [''] 13 | team_odom_frames: [''] 14 | prior_session_maps: ['$(env ROMAN_PRIOR_MAP)'] 15 | prior_session_ids: [1] 16 | submap_knn: 5 17 | use_sim_time: $(var sim_time) -------------------------------------------------------------------------------- /examples/d455_bag/run_single_session.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Usage: ./run_single_session.sh 3 | export ROMAN_BAG=$1 4 | export ROMAN_OUTPUT_DIR=${2:-"~/.roman_ros2"} 5 | 6 | if [ ! -d "$ROMAN_BAG" ]; then 7 | echo "ERROR: Missing positional command line argument 'ROMAN_BAG' or ROMAN_BAG does not exist" 8 | exit 9 | fi 10 | 11 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 12 | 13 | export ROMAN_ROS_WS=$(realpath $SCRIPT_DIR/../../../../) 14 | export ROMAN_PRIOR_MAP="" 15 | export ROMAN_LC_CONFIG_DIR=$(realpath $SCRIPT_DIR/../../roman_ros2/cfg/d455) 16 | 17 | tmuxp load $SCRIPT_DIR/d455_bag.yaml -------------------------------------------------------------------------------- /roman_ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | roman_ros2 5 | 0.0.0 6 | TODO: Package description 7 | masonbp 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /examples/d455_two_session/params/data.yaml: -------------------------------------------------------------------------------- 1 | dt: 0.166666666666666667 2 | runs: [run1, run2] 3 | run_env: "RUN" 4 | img_data: 5 | path: ${OUTPUT_ROOT}/${RUN}/bag 6 | topic: /robot/d455/color/image_raw/compressed 7 | camera_info_topic: /robot/d455/color/camera_info 8 | compressed: True 9 | color_space: 'bgr8' 10 | depth_data: 11 | path: ${OUTPUT_ROOT}/${RUN}/bag 12 | topic: /robot/d455/aligned_depth_to_color/image_raw 13 | camera_info_topic: /robot/d455/color/camera_info 14 | compressed: False 15 | pose_data: 16 | type: bag 17 | path: ${OUTPUT_ROOT}/${RUN}/bag 18 | topic: /robot/kimera_vio_ros/odometry 19 | time_tol: 10.0 20 | T_camera_flu: 21 | input_type: "string" 22 | string: "T_RDFFLU" -------------------------------------------------------------------------------- /examples/zed2i_two_session/params/data.yaml: -------------------------------------------------------------------------------- 1 | dt: 0.166666666666666667 2 | runs: [run1, run2] 3 | run_env: "RUN" 4 | img_data: 5 | path: ${OUTPUT_ROOT}/${RUN}/bag 6 | topic: /robot/zed/rgb/image_rect_color/compressed 7 | camera_info_topic: /robot/zed/rgb/camera_info 8 | compressed: True 9 | color_space: 'bgr8' 10 | depth_data: 11 | path: ${OUTPUT_ROOT}/${RUN}/bag 12 | topic: /robot/zed/depth/depth_registered 13 | camera_info_topic: /robot/zed/depth/camera_info 14 | compressed: False 15 | pose_data: 16 | type: bag 17 | path: ${OUTPUT_ROOT}/${RUN}/bag 18 | topic: /robot/zed/odom 19 | time_tol: 10.0 20 | T_camera_flu: 21 | input_type: "string" 22 | string: "T_RDFFLU" 23 | T_odombase_camera: 24 | input_type: "tf" 25 | parent: "zed_camera_link" 26 | child: "zed_left_camera_optical_frame" 27 | -------------------------------------------------------------------------------- /examples/d455_bag/run_with_prior_map.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Usage: ./run_with_prior_map.sh 3 | export ROMAN_BAG=$1 4 | export ROMAN_PRIOR_MAP=$2 5 | export ROMAN_OUTPUT_DIR=${3:-"~/.roman_ros2"} 6 | 7 | if [ ! -d "$ROMAN_BAG" ]; then 8 | echo "ERROR: Missing positional command line argument 'ROMAN_BAG' or ROMAN_BAG does not exist" 9 | exit 10 | fi 11 | 12 | if [ ! -f "$ROMAN_PRIOR_MAP" ]; then 13 | echo "ERROR: Missing positional command line argument 'ROMAN_PRIOR_MAP' or ROMAN_PRIOR_MAP does not exist" 14 | exit 15 | fi 16 | 17 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 18 | 19 | export ROMAN_ROS_WS=$(realpath $SCRIPT_DIR/../../../../) 20 | export ROMAN_LC_CONFIG_DIR=$(realpath $SCRIPT_DIR/../../roman_ros2/cfg/d455_with_prior_map) 21 | 22 | tmuxp load $SCRIPT_DIR/d455_bag.yaml -------------------------------------------------------------------------------- /roman_ros2/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /roman_ros2/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /install/install_roman.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ROMAN_ROS_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )/.." 3 | ROMAN_DIR="$ROMAN_ROS_DIR/../roman" 4 | cd $ROMAN_DIR 5 | 6 | # Install CLIPPER 7 | git submodule update --init --recursive 8 | mkdir dependencies/clipper/build 9 | cd dependencies/clipper/build 10 | cmake .. && make && make pip-install 11 | 12 | # pip install 13 | cd $ROMAN_DIR 14 | pip install . 15 | # TODO figure out how to get ros setup.py to include these 16 | # (or just include them in roman setup.py) 17 | pip install transforms3d gdown 18 | 19 | # download weights 20 | mkdir -p $ROMAN_ROS_DIR/weights 21 | cd $ROMAN_ROS_DIR/weights 22 | wget https://github.com/WongKinYiu/yolov7/releases/download/v0.1/yolov7.pt 23 | gdown 'https://drive.google.com/uc?id=1m1sjY4ihXBU1fZXdQ-Xdj-mDltW-2Rqv' 24 | 25 | # maybe handle this somewhere else 26 | sudo apt install ros-${ROS_DISTRO}-tf-transformations 27 | sudo apt install ros-${ROS_DISTRO}-image-transport -------------------------------------------------------------------------------- /roman_ros2/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /roman_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | roman_msgs 5 | 0.0.0 6 | TODO: Package description 7 | masonbp 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | 13 | rosidl_default_generators 14 | builtin_interfaces 15 | std_msgs 16 | geometry_msgs 17 | 18 | 19 | rosidl_default_runtime 20 | builtin_interfaces 21 | std_msgs 22 | geometry_msgs 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | rosidl_default_runtime 32 | 33 | rosidl_interface_packages 34 | 35 | -------------------------------------------------------------------------------- /roman_ros2/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from glob import glob 3 | 4 | package_name = 'roman_ros2' 5 | 6 | setup( 7 | name=package_name, 8 | version='0.0.0', 9 | packages=[package_name], 10 | data_files=[ 11 | ('share/ament_index/resource_index/packages', 12 | ['resource/' + package_name]), 13 | ('share/' + package_name, ['package.xml']), 14 | ('share/' + package_name + '/launch', glob('launch/*.launch.*')), 15 | ('share/' + package_name + '/cfg/d455', glob('cfg/d455/*.yaml')), 16 | ('share/' + package_name + '/cfg/zed2i', glob('cfg/zed2i/*.yaml')), 17 | ], 18 | install_requires=[ 19 | 'setuptools', 20 | 'transforms3d' 21 | # TODO: do these actually get installed by being here? 22 | ], 23 | zip_safe=True, 24 | maintainer='masonbp', 25 | maintainer_email='mbpeterson70@gmail.com', 26 | description='TODO: Package description', 27 | license='TODO: License declaration', 28 | tests_require=['pytest'], 29 | entry_points={ 30 | 'console_scripts': [ 31 | 'fastsam_node.py = roman_ros2.fastsam_node:main', 32 | 'roman_map_node.py = roman_ros2.roman_map_node:main', 33 | 'roman_align_node.py = roman_ros2.roman_align_node:main', 34 | 'roman_loop_closure_node.py = roman_ros2.roman_loop_closure_node:main', 35 | 'lc_viz_node.py = roman_ros2.lc_viz_node:main' 36 | ], 37 | }, 38 | ) 39 | -------------------------------------------------------------------------------- /roman_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(roman_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | if(BUILD_TESTING) 15 | find_package(ament_lint_auto REQUIRED) 16 | # the following line skips the linter which checks for copyrights 17 | # comment the line when a copyright and license is added to all source files 18 | set(ament_cmake_copyright_FOUND TRUE) 19 | # the following line skips cpplint (only works in a git repo) 20 | # comment the line when this package is in a git repo and when 21 | # a copyright and license is added to all source files 22 | set(ament_cmake_cpplint_FOUND TRUE) 23 | ament_lint_auto_find_test_dependencies() 24 | endif() 25 | 26 | find_package(geometry_msgs REQUIRED) 27 | find_package(rosidl_default_generators REQUIRED) 28 | find_package(std_msgs REQUIRED) 29 | find_package(geometry_msgs REQUIRED) 30 | find_package(builtin_interfaces REQUIRED) 31 | 32 | rosidl_generate_interfaces(${PROJECT_NAME} 33 | "msg/FrameDescriptor.msg" 34 | "msg/LoopClosure.msg" 35 | "msg/Observation.msg" 36 | "msg/ObservationArray.msg" 37 | "msg/Segment.msg" 38 | DEPENDENCIES std_msgs geometry_msgs builtin_interfaces 39 | ) 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /examples/d455_bag/d455_bag.yaml: -------------------------------------------------------------------------------- 1 | session_name: roman_ros2 2 | 3 | environment: 4 | ROMAN_BAG: ${ROMAN_BAG} 5 | ROMAN_OUTPUT_DIR: ${ROMAN_OUTPUT_DIR} 6 | ROMAN_PRIOR_MAP: ${ROMAN_PRIOR_MAP} 7 | ROMAN_ROS_WS: ${ROMAN_ROS_WS} 8 | ROMAN_ENV_ACTIVATE: ${ROMAN_ENV_ACTIVATE} 9 | ROMAN_CAMERA: d455 10 | ROMAN_ROBOT_NAME: robot 11 | ROMAN_LC_CONFIG_DIR: ${ROMAN_LC_CONFIG_DIR} 12 | 13 | options: 14 | default-command: /bin/zsh 15 | 16 | windows: 17 | - window_name: roman 18 | layout: tiled 19 | shell_command_before: 20 | - source $ROMAN_ROS_WS/install/setup.zsh 21 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 22 | - $ROMAN_ENV_ACTIVATE 23 | panes: 24 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROMAN_ROBOT_NAME camera:=$ROMAN_CAMERA 25 | launch_fastsam:=true sim_time:=true 26 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROMAN_ROBOT_NAME camera:=$ROMAN_CAMERA 27 | launch_mapping:=true output_dir:=$ROMAN_OUTPUT_DIR sim_time:=true 28 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROMAN_ROBOT_NAME camera:=$ROMAN_CAMERA 29 | launch_loop_closure:=true launch_loop_closure_viz:=true config_dir:=$ROMAN_LC_CONFIG_DIR 30 | output_dir:=$ROMAN_OUTPUT_DIR sim_time:=true 31 | - rviz2 -d $ROMAN_ROS_WS/src/roman_ros2/examples/d455_single_session/d455_online.rviz 32 | - echo "PRESS ENTER TO KILL"; 33 | tmux send-keys -t roman_ros2:roman.1 C-c && sleep 3 && tmux kill-session \ 34 | 35 | - window_name: bag 36 | layout: tiled 37 | shell_command_before: 38 | - source $ROMAN_ROS_WS/install/setup.zsh 39 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 40 | panes: 41 | - ros2 run rmw_zenoh_cpp rmw_zenohd 42 | - ros2 bag play $ROMAN_BAG --clock 43 | - ros2 run image_transport republish 44 | --ros-args -p in_transport:=compressed -p out_transport:=raw 45 | -r in/compressed:=$ROMAN_ROBOT_NAME/$ROMAN_CAMERA/color/image_raw/compressed 46 | -r out:=$ROMAN_ROBOT_NAME/$ROMAN_CAMERA/color/image_raw 47 | 48 | 49 | -------------------------------------------------------------------------------- /roman_ros2/launch/roman_node.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | launch: 3 | - arg: {name: robot_name} 4 | - arg: {name: config_dir} 5 | - arg: {name: sim_time, default: "false"} 6 | - arg: {name: output_dir, default: "~/.roman_ros2"} 7 | - arg: {name: launch_fastsam, default: "false"} 8 | - arg: {name: launch_mapping, default: "false"} 9 | - arg: {name: launch_loop_closure, default: "false"} 10 | - arg: {name: launch_loop_closure_viz, default: "false"} 11 | 12 | - node: 13 | if: $(var launch_fastsam) 14 | pkg: roman_ros2 15 | exec: fastsam_node.py 16 | name: fastsam_node 17 | namespace: $(var robot_name) 18 | param: 19 | - from: $(var config_dir)/fastsam_node.yaml 20 | allow_substs: true 21 | - {name: config_path, value: $(var config_dir)/fastsam.yaml} 22 | 23 | - node: 24 | if: $(var launch_mapping) 25 | pkg: roman_ros2 26 | exec: roman_map_node.py 27 | name: roman_map_node 28 | namespace: $(var robot_name) 29 | param: 30 | - from: $(var config_dir)/roman_map_node.yaml 31 | allow_substs: true 32 | - {name: output_roman_map, value: $(var output_dir)/roman_map.pkl} 33 | 34 | - node: 35 | if: $(var launch_loop_closure) 36 | pkg: roman_ros2 37 | exec: roman_loop_closure_node.py 38 | name: roman_lc_node 39 | namespace: $(var robot_name) 40 | param: 41 | - from: $(var config_dir)/roman_loop_closure_node.yaml 42 | allow_substs: true 43 | - {name: config_path, value: $(var config_dir)/roman_loop_closure.yaml} 44 | 45 | - node: 46 | if: $(var launch_loop_closure_viz) 47 | pkg: roman_ros2 48 | exec: lc_viz_node.py 49 | name: lc_viz_node 50 | namespace: $(var robot_name) 51 | param: 52 | - from: $(var config_dir)/roman_loop_closure_node.yaml 53 | allow_substs: true 54 | - {name: config_path, value: $(var config_dir)/roman_loop_closure.yaml} 55 | - {name: output_dir, value: $(var output_dir)} 56 | -------------------------------------------------------------------------------- /examples/zed2i_two_session/run.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Usage: ./run.bash <12> 4 | # output_directory and 12 are optional 5 | # 12, 1, 2, or "none" can be used to skip mapping sessions. 6 | 7 | # Command line args 8 | export OUTPUT_ROOT=${1:-''} 9 | export RUNS=${2:-"12"} 10 | 11 | # Internal vars 12 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"\ 13 | 14 | export ROMAN_ROS_WS=$(realpath $SCRIPT_DIR/../../../../) 15 | export RECORD=true 16 | 17 | 18 | if [ -z "$OUTPUT_ROOT" ]; then 19 | read -p "Desired output directory: " OUTPUT_ROOT 20 | export OUTPUT_ROOT=$(eval echo "$OUTPUT_ROOT") 21 | fi 22 | mkdir -p $OUTPUT_ROOT 23 | 24 | # Run first session of ROMAN mapping 25 | if [[ "$RUNS" == *"1"* ]]; then 26 | read -p "Press enter to start first session. " 27 | export OUTPUT_DIR=$OUTPUT_ROOT/run1 28 | if [ -d "$OUTPUT_DIR" ]; then 29 | rm -r $OUTPUT_DIR 30 | fi 31 | mkdir -p $OUTPUT_DIR 32 | tmuxp load $SCRIPT_DIR/../zed2i_single_session/zed2i_online.yaml 33 | else 34 | echo "Skipping session 1..." 35 | fi 36 | 37 | # Run second session of ROMAN mapping 38 | if [[ "$RUNS" == *"2"* ]]; then 39 | read -p "Press enter to start second session. " 40 | export OUTPUT_DIR=$OUTPUT_ROOT/run2 41 | if [ -d "$OUTPUT_DIR" ]; then 42 | rm -r $OUTPUT_DIR 43 | fi 44 | mkdir -p $OUTPUT_DIR 45 | tmuxp load $SCRIPT_DIR/../zed2i_single_session/zed2i_online.yaml 46 | else 47 | echo "Skipping session 2..." 48 | fi 49 | 50 | mkdir -p $OUTPUT_ROOT/roman/map 51 | cp $OUTPUT_ROOT/run1/roman_map.pkl $OUTPUT_ROOT/roman/map/run1.pkl 52 | cp $OUTPUT_ROOT/run2/roman_map.pkl $OUTPUT_ROOT/roman/map/run2.pkl 53 | 54 | $(eval echo "$ROMAN_ENV_ACTIVATE") 55 | python3 $ROMAN_ROS_WS/src/roman/demo/demo.py -p $SCRIPT_DIR/params -o $OUTPUT_ROOT/roman --skip-map --skip-rpgo 56 | 57 | python3 $ROMAN_ROS_WS/src/roman/demo/association_vid.py $OUTPUT_ROOT/roman $OUTPUT_ROOT/association_vid.mp4 -r run1 run2 -m 58 | xdg-open $OUTPUT_ROOT/association_vid.mp4 -------------------------------------------------------------------------------- /examples/d455_two_session/run.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Usage: ./run.bash <12> 4 | # output_directory and 12 are optional 5 | # 12, 1, 2, or "none" can be used to skip mapping sessions. 6 | 7 | # Command line args 8 | export OUTPUT_ROOT=${1:-''} 9 | export RUNS=${2:-"12"} 10 | 11 | # Internal vars 12 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"\ 13 | 14 | export ROS_IP=127.0.0.1 15 | export ROS_MASTER_URI=http://$ROS_IP:11311 16 | export ROMAN_ROS_WS=$(realpath $SCRIPT_DIR/../../../../) 17 | export KIMERA_VIO_ROS2_REALSENSE_DIR=$ROMAN_ROS_WS/src/kimera-vio-ros2-realsense 18 | export RECORD=true 19 | 20 | 21 | if [ -z "$OUTPUT_ROOT" ]; then 22 | read -p "Desired output directory: " OUTPUT_ROOT 23 | export OUTPUT_ROOT=$(eval echo "$OUTPUT_ROOT") 24 | fi 25 | mkdir -p $OUTPUT_ROOT 26 | 27 | # Run first session of ROMAN mapping 28 | if [[ "$RUNS" == *"1"* ]]; then 29 | read -p "Press enter to start first session. " 30 | export OUTPUT_DIR=$OUTPUT_ROOT/run1 31 | if [ -d "$OUTPUT_DIR" ]; then 32 | rm -r $OUTPUT_DIR 33 | fi 34 | mkdir -p $OUTPUT_DIR 35 | tmuxp load $SCRIPT_DIR/../d455_single_session/d455_online.yaml 36 | else 37 | echo "Skipping session 1..." 38 | fi 39 | 40 | # Run second session of ROMAN mapping 41 | if [[ "$RUNS" == *"2"* ]]; then 42 | read -p "Press enter to start second session. " 43 | export OUTPUT_DIR=$OUTPUT_ROOT/run2 44 | if [ -d "$OUTPUT_DIR" ]; then 45 | rm -r $OUTPUT_DIR 46 | fi 47 | mkdir -p $OUTPUT_DIR 48 | tmuxp load $SCRIPT_DIR/../d455_single_session/d455_online.yaml 49 | else 50 | echo "Skipping session 2..." 51 | fi 52 | 53 | mkdir -p $OUTPUT_ROOT/roman/map 54 | cp $OUTPUT_ROOT/run1/roman_map.pkl $OUTPUT_ROOT/roman/map/run1.pkl 55 | cp $OUTPUT_ROOT/run2/roman_map.pkl $OUTPUT_ROOT/roman/map/run2.pkl 56 | 57 | $(eval echo "$ROMAN_ENV_ACTIVATE") 58 | python3 $ROMAN_ROS_WS/src/roman/demo/demo.py -p $SCRIPT_DIR/params -o $OUTPUT_ROOT/roman --skip-map --skip-rpgo 59 | 60 | python3 $ROMAN_ROS_WS/src/roman/demo/association_vid.py $OUTPUT_ROOT/roman $OUTPUT_ROOT/association_vid.mp4 -r run1 run2 -m 61 | xdg-open $OUTPUT_ROOT/association_vid.mp4 -------------------------------------------------------------------------------- /examples/zed2i_single_session/zed2i_online.yaml: -------------------------------------------------------------------------------- 1 | session_name: roman_ros2 2 | 3 | environment: 4 | CAMERA: zed2i 5 | ROBOT_NAME: robot 6 | ROMAN_ROS_WS: ${ROMAN_ROS_WS} 7 | ROMAN_ENV_ACTIVATE: ${ROMAN_ENV_ACTIVATE} 8 | OUTPUT_DIR: ${OUTPUT_DIR} 9 | RECORD: ${RECORD} 10 | 11 | options: 12 | default-command: /bin/zsh 13 | 14 | windows: 15 | - window_name: control 16 | layout: even-vertical 17 | shell_command_before: 18 | - source $ROMAN_ROS_WS/install/setup.zsh 19 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 20 | panes: 21 | - echo "\n\n\n"; 22 | echo "INSTRUCTIONS:"; 23 | echo "Move around and create a map with your camera."; 24 | echo "Once you have finished your run, push enter with the focus in the bottom panel (the bottom panel will be in focus by default)."; 25 | echo "This will kill the session cleanly." 26 | - echo "PRESS ENTER TO KILL"; 27 | tmux send-keys -t roman_ros2:roman.1 C-c && 28 | tmux send-keys -t roman_ros2:record.0 C-c && 29 | sleep 3 && 30 | tmux kill-session \ 31 | 32 | 33 | - window_name: roman 34 | layout: tiled 35 | shell_command_before: 36 | - source $ROMAN_ROS_WS/install/setup.zsh 37 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 38 | - $ROMAN_ENV_ACTIVATE 39 | panes: 40 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROBOT_NAME camera:=$CAMERA 41 | launch_fastsam:=true 42 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROBOT_NAME camera:=$CAMERA 43 | launch_mapping:=true output_dir:=$OUTPUT_DIR 44 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROBOT_NAME camera:=$CAMERA 45 | launch_loop_closure:=true launch_loop_closure_viz:=true output_dir:=$OUTPUT_DIR 46 | - rviz2 -d $ROMAN_ROS_WS/src/roman_ros2/examples/d455_single_session/d455_online.rviz 47 | 48 | - window_name: camera 49 | layout: tiled 50 | shell_command_before: 51 | - source $ROMAN_ROS_WS/install/setup.zsh 52 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 53 | panes: 54 | - ros2 run rmw_zenoh_cpp rmw_zenohd 55 | - ros2 launch zed_wrapper zed_camera.launch.py namespace:=$ROBOT_NAME 56 | camera_model:=$CAMERA 57 | 58 | - window_name: record 59 | layout: tiled 60 | shell_command_before: 61 | - source $ROMAN_ROS_WS/install/setup.zsh 62 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 63 | panes: 64 | - if [ "$RECORD" = true ]; 65 | then 66 | ros2 bag record -o $OUTPUT_DIR/bag 67 | /$ROBOT_NAME/zed/rgb/camera_info 68 | /$ROBOT_NAME/zed/rgb/image_rect_color/compressed 69 | /$ROBOT_NAME/zed/depth/camera_info 70 | /$ROBOT_NAME/zed/depth/depth_registered 71 | /$ROBOT_NAME/zed/odom 72 | /tf 73 | /tf_static; 74 | fi -------------------------------------------------------------------------------- /roman_ros2/launch/roman.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | launch: 3 | - arg: {name: robot_name} 4 | - arg: {name: camera} 5 | - arg: {name: sim_time, default: "false"} 6 | - arg: {name: config_dir, default: "$(find-pkg-share roman_ros2)/cfg/$(var camera)"} 7 | - arg: {name: output_dir, default: "~/.roman_ros2"} 8 | - arg: {name: launch_fastsam, default: "false"} 9 | - arg: {name: launch_mapping, default: "false"} 10 | - arg: {name: launch_loop_closure, default: "false"} 11 | - arg: {name: launch_loop_closure_viz, default: "false"} 12 | 13 | ######################################### 14 | # d455 # 15 | ######################################### 16 | - group: 17 | - set_remap: {from: color/camera_info, to: $(var camera)/color/camera_info} 18 | - set_remap: {from: color/image_raw, to: $(var camera)/color/image_raw} 19 | - set_remap: {from: depth/camera_info, to: $(var camera)/aligned_depth_to_color/camera_info} 20 | - set_remap: {from: depth/image_raw, to: $(var camera)/aligned_depth_to_color/image_raw} 21 | - set_remap: {from: tf, to: /tf} 22 | - set_remap: {from: tf_static, to: /tf_static} 23 | 24 | - include: 25 | if: $(eval '"$(var camera)" == "d455"') 26 | file: $(find-pkg-share roman_ros2)/launch/roman_node.launch.yaml 27 | arg: 28 | - {name: robot_name, value: $(var robot_name)} 29 | - {name: config_dir, value: $(var config_dir)} 30 | - {name: sim_time, value: $(var sim_time)} 31 | - {name: output_dir, value: $(var output_dir)} 32 | - {name: launch_fastsam, value: $(var launch_fastsam)} 33 | - {name: launch_mapping, value: $(var launch_mapping)} 34 | - {name: launch_loop_closure, value: $(var launch_loop_closure)} 35 | - {name: launch_loop_closure_viz, value: $(var launch_loop_closure_viz)} 36 | 37 | ######################################### 38 | # zed2i # 39 | ######################################### 40 | - group: 41 | - set_remap: {from: color/camera_info, to: zed/rgb/camera_info} 42 | - set_remap: {from: color/image_raw, to: zed/rgb/image_rect_color} 43 | - set_remap: {from: depth/camera_info, to: zed/depth/camera_info} 44 | - set_remap: {from: depth/image_raw, to: zed/depth/depth_registered} 45 | - set_remap: {from: tf, to: /tf} 46 | - set_remap: {from: tf_static, to: /tf_static} 47 | 48 | - include: 49 | if: $(eval '"$(var camera)" == "zed2i"') 50 | file: $(find-pkg-share roman_ros2)/launch/roman_node.launch.yaml 51 | arg: 52 | - {name: robot_name, value: $(var robot_name)} 53 | - {name: config_dir, value: $(var config_dir)} 54 | - {name: sim_time, value: $(var sim_time)} 55 | - {name: output_dir, value: $(var output_dir)} 56 | - {name: launch_fastsam, value: $(var launch_fastsam)} 57 | - {name: launch_mapping, value: $(var launch_mapping)} 58 | - {name: launch_loop_closure, value: $(var launch_loop_closure)} 59 | - {name: launch_loop_closure_viz, value: $(var launch_loop_closure_viz)} 60 | -------------------------------------------------------------------------------- /scripts/configure_cpu_or_gpu.py: -------------------------------------------------------------------------------- 1 | 2 | import argparse 3 | import os 4 | from pathlib import Path 5 | import glob 6 | 7 | def change_yaml_tag(file_pattern, tag, value): 8 | """ 9 | Change the specified tag in the YAML file to the given value. 10 | """ 11 | matching_files = glob.glob(file_pattern, recursive=True) 12 | for file in matching_files: 13 | with open(file, 'r') as f: 14 | lines = f.readlines() 15 | 16 | # replace the tag value 17 | for i, line in enumerate(lines): 18 | if line.strip().startswith(f"{tag}:"): 19 | lines[i] = f"{tag}: {value}\n" 20 | 21 | # print(lines) 22 | with open(file, 'w') as f: 23 | f.writelines(lines) 24 | 25 | 26 | if __name__ == '__main__': 27 | info_str = """ 28 | This script is used to configure the ROMAN repo for using either GPU or only CPU. 29 | Normally, ROMAN uses GPU to extract segments from an image and to create 30 | semantic embeddings for the segments. ROMAN can still function on CPU, but a 31 | few things need to be configured. This script handles reconfiguring the repo for 32 | use with CPU. When running with CPU, ROMAN will likely run much slower, and performance 33 | will be reduced. 34 | """ 35 | parser = argparse.ArgumentParser(description=info_str) 36 | parser.add_argument('-d', '--device', type=str, default='cpu', choices=['cpu', 'gpu'], 37 | help='Specify the device to use: "cpu" for CPU or "gpu" for GPU. Default is "cpu".') 38 | args = parser.parse_args() 39 | 40 | script_path = os.path.realpath(__file__) 41 | script_dir = os.path.dirname(script_path) 42 | roman_ros_repo_dir = os.path.dirname(script_dir) 43 | 44 | if args.device == 'cpu': 45 | change_yaml_tag(f"{roman_ros_repo_dir}/**/fastsam.yaml", "device", "cpu") 46 | change_yaml_tag(f"{roman_ros_repo_dir}/**/fastsam.yaml", "semantics", "none") 47 | change_yaml_tag(f"{roman_ros_repo_dir}/**/submap_align.yaml", "method", "roman_no_semantics") 48 | change_yaml_tag(f"{roman_ros_repo_dir}/**/roman_loop_closure.yaml", "method", "roman_no_semantics") 49 | 50 | print("Repo now configured for CPU usage. ROMAN will run slower, and performance will be reduced.") 51 | print("To apply changes, run colcon build in teh ROS2 workspace.") 52 | print() 53 | print("To revert to GPU usage, run this script again with the --device gpu flag.") 54 | 55 | elif args.device == 'gpu': 56 | change_yaml_tag(f"{roman_ros_repo_dir}/**/fastsam.yaml", "device", "'cuda'") 57 | change_yaml_tag(f"{roman_ros_repo_dir}/**/fastsam.yaml", "semantics", "'dino'") 58 | change_yaml_tag(f"{roman_ros_repo_dir}/**/submap_align.yaml", "method", "roman") 59 | change_yaml_tag(f"{roman_ros_repo_dir}/**/roman_loop_closure.yaml", "method", "roman") 60 | 61 | print("Repo now configured for GPU usage.") 62 | print("To apply changes, run colcon build in the ROS2 workspace.") 63 | print() 64 | print("To configure for running with CPU, run this script again with the --device cpu flag.") -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | weights 2 | 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | share/python-wheels/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | MANIFEST 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .nox/ 45 | .coverage 46 | .coverage.* 47 | .cache 48 | nosetests.xml 49 | coverage.xml 50 | *.cover 51 | *.py,cover 52 | .hypothesis/ 53 | .pytest_cache/ 54 | cover/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | .pybuilder/ 78 | target/ 79 | 80 | # Jupyter Notebook 81 | .ipynb_checkpoints 82 | 83 | # IPython 84 | profile_default/ 85 | ipython_config.py 86 | 87 | # pyenv 88 | # For a library or package, you might want to ignore these files since the code is 89 | # intended to run in multiple environments; otherwise, check them in: 90 | # .python-version 91 | 92 | # pipenv 93 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 94 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 95 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 96 | # install all needed dependencies. 97 | #Pipfile.lock 98 | 99 | # poetry 100 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 101 | # This is especially recommended for binary packages to ensure reproducibility, and is more 102 | # commonly ignored for libraries. 103 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 104 | #poetry.lock 105 | 106 | # pdm 107 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 108 | #pdm.lock 109 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 110 | # in version control. 111 | # https://pdm.fming.dev/#use-with-ide 112 | .pdm.toml 113 | 114 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 115 | __pypackages__/ 116 | 117 | # Celery stuff 118 | celerybeat-schedule 119 | celerybeat.pid 120 | 121 | # SageMath parsed files 122 | *.sage.py 123 | 124 | # Environments 125 | .env 126 | .venv 127 | env/ 128 | venv/ 129 | ENV/ 130 | env.bak/ 131 | venv.bak/ 132 | 133 | # Spyder project settings 134 | .spyderproject 135 | .spyproject 136 | 137 | # Rope project settings 138 | .ropeproject 139 | 140 | # mkdocs documentation 141 | /site 142 | 143 | # mypy 144 | .mypy_cache/ 145 | .dmypy.json 146 | dmypy.json 147 | 148 | # Pyre type checker 149 | .pyre/ 150 | 151 | # pytype static type analyzer 152 | .pytype/ 153 | 154 | # Cython debug symbols 155 | cython_debug/ 156 | 157 | # PyCharm 158 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 159 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 160 | # and can be added to the global gitignore or merged into this file. For a more nuclear 161 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 162 | #.idea/ 163 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | 3 | Examples are provided for running ROMAN online with a D455 or Zed2i, or for running ROMAN on a bag. 4 | 5 | The online examples are set up so that the user: 6 | 1. Creates an initial map in a first session 7 | 2. Ends the first session and sets up for a second run (perhaps starting from a new location) 8 | 3. Creates a second map and uses ROMAN to relocalize in the previous map 9 | 10 | After running the two sessions, a video is created showing the best set of associations found by ROMAN. 11 | Example output can be visualized in [this YouTube video](https://www.youtube.com/watch?v=y51NDoPpBy8&t=5s). 12 | 13 | The following sections give further instructions for setting up and running the examples. 14 | 15 | ## `roman_ros2` with D455 RealSense stereo camera and Kimera-VIO 16 | 17 | #### Demo setup 18 | 19 | Some additional installation steps must be taken to set up the D455 and Kimera-VIO. 20 | As prerequisites, follow the [RealSense instructions](https://github.com/IntelRealSense/realsense-ros?tab=readme-ov-file#installation-on-ubuntu) for installing the RealSense SDK and RealSense ROS2 Wrapper. 21 | Additionally, install `docker`, and the following packages: 22 | 23 | ``` 24 | sudo apt install tmux tmuxp ros-${ROS_DISTRO}-rmw-zenoh-cpp zsh 25 | ``` 26 | 27 | Next, clone and install the `kimera-vio-ros2-realsense` repo: 28 | 29 | ``` 30 | git clone https://github.com/mbpeterson70/kimera-vio-ros2-realsense.git src/kimera-vio-ros2-realsense 31 | ./src/kimera-vio-ros2-realsense/build.bash 32 | ``` 33 | 34 | This will create a docker image for running Kimera-VIO in ROS1, meanwhile bridging camera data from your local machine in ROS2 to the docker in ROS1. 35 | 36 | #### Running the demo 37 | 38 | The goal of this demo is to give ROMAN to video sequences and let ROMAN align maps created from the two videos to perform global localization across the two sessions. 39 | 40 | After following the installation instructions above, start the demo with 41 | 42 | ``` 43 | ./src/roman_ros2/examples/d455_two_session/run.bash 44 | ``` 45 | 46 | You will be prompted to enter a directory to store the output of the demo. 47 | You will then be prompted to press enter when you are ready to start the first video. 48 | Record a video (duration isn't super important, but ROMAN performs better with more complete maps) and press Enter when finished. 49 | Move the camera to the start of the second session and press enter to beging recording the second session. 50 | Press enter again to finish the second session. 51 | Finally, a video will be generated and opened automatically showing the object matches found by ROMAN. 52 | 53 | Optionally, the `run.bash` script can be run with command line arguments: 54 | 55 | ``` 56 | ./src/roman_ros2/examples/d455_two_session/run.bash 57 | ``` 58 | 59 | where `` can be `1`, `2`, `12`, or `none`. This defaults to `12` to create maps for sessions 1 and 2. 60 | This functionality enables re-running if a single sessions needs to be re-run but you would like to use one of the previously recorded sessions. 61 | 62 | ## `roman_ros2` on bagged data 63 | 64 | An example tmux file is included for running ROMAN mapping using D455 RGB-D images. This example allows you to first create an initial map (and find loop closures while performing initial mapping). If you have a second bag in the same area, you can perform loop closures to the prior map as well. 65 | 66 | To run the session, run [this file](./d455_bag/run_single_session.sh): 67 | 68 | `./run_single_session.sh ` 69 | 70 | Rviz will pop up for visualizing the mapping. Visualizations of the loop closures will he saved to the output directory. Once the bag has finished, press enter in the bottom tmux pane and that will save the ROMAN map into the output directory and close the tmux. 71 | 72 | To run a second session with loop closures to a prior map, run [this file](./d455_bag/run_with_prior_map.sh): 73 | 74 | `./run_with_prior_map.sh `. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROMAN ROS2 2 | 3 | Opposite view loop closure 4 | 5 | Welcome to roman_ros2, a ROS2 wrapper for [ROMAN](https://github.com/mit-acl/ROMAN) (Robust Object Map Alignment Anywhere). 6 | ROMAN is a view-invariant global localization method that maps open-set objects and uses the geometry, shape, and semantics of objects to find the transformation between a current pose and previously created object map. 7 | This enables loop closure between robots even when a scene is observed from *opposite views.* 8 | 9 | Demo videos, the paper, and more can be found at the [ROMAN project website](https://acl.mit.edu/roman/). 10 | Checkout the main branch for the ROS1 wrapper. 11 | 12 | ## Citation 13 | 14 | If you find ROMAN useful in your work, please cite our [paper](https://www.roboticsproceedings.org/rss21/p029.pdf): 15 | 16 | M.B. Peterson, Y.X. Jia, Y. Tian, A. Thomas, and J.P. How, "ROMAN: Open-Set Object Map Alignment for Robust View-Invariant Global Localization," 17 | *Robotics: Science and Systems*, 2025. 18 | 19 | ``` 20 | @inproceedings{peterson2025roman, 21 | title={{ROMAN: Open-Set Object Map Alignment for Robust View-Invariant Global Localization}}, 22 | author={Peterson, Mason B and Jia, Yi Xuan and Tian, Yulun and Thomas, Annika and How, Jonathan P}, 23 | booktitle={Robotics: Science and Systems (RSS)}, 24 | pdf={https://www.roboticsproceedings.org/rss21/p029.pdf}, 25 | year={2025} 26 | } 27 | ``` 28 | 29 | # Install 30 | 31 | #### Step 1: Set up and build ROS workspace 32 | 33 | In the **root directory of your ROS workspace** (for example, `mkdir ~/roman_ws && cd ~/roman_ws`) run: 34 | 35 | ``` 36 | git clone https://github.com/mit-acl/roman_ros.git src/roman_ros2 37 | vcs import src < src/roman_ros2/install/packages.yaml 38 | colcon build 39 | ``` 40 | 41 | #### Step 2: Install ROMAN Python package 42 | 43 | First, **activate the python environment** you would like to use with ROMAN. For example, build and source a python virtual environment as follows: 44 | 45 | ``` 46 | python3 -m venv ./venv 47 | touch ./venv/COLCON_IGNORE # so that colcon does not try to build packages in the environment 48 | source ./venv/bin/activate 49 | ``` 50 | 51 | Once your environment has been activated, run 52 | 53 | ``` 54 | ./src/roman_ros2/install/install_roman.bash 55 | ``` 56 | 57 | to install the ROMAN python package and download required model weights. 58 | 59 | #### Step 3: Set up environment variables 60 | 61 | For running `roman_ros2`, you will need to set the `ROMAN_WEIGHTS` environment variable. 62 | You may want to run the following to add this environment variable to your `.zshrc` file: 63 | 64 | ``` 65 | echo export ROMAN_WEIGHTS=$(realpath ./src/roman_ros2/weights) >> ~/.zshrc 66 | ``` 67 | 68 | or `.bashrc` file: 69 | 70 | ``` 71 | echo export ROMAN_WEIGHTS=$(realpath ./src/roman_ros2/weights) >> ~/.bashrc 72 | ``` 73 | 74 | Finally, the examples will source your python environment by calling `$ROMAN_ENV_ACTIVATE`. 75 | Before running the examples run (or put the following in your `~/.zshrc` or `~/.bashrc`): 76 | 77 | ``` 78 | export ROMAN_ENV_ACTIVATE= 79 | ``` 80 | 81 | If you used a Python virtual environment as described above, this would look something like: `export ROMAN_ENV_ACTIVATE="source ~/roman_ws/venv/bin/activate"` 82 | 83 | #### Step 4: Build the ROS2 workspace 84 | 85 | Note that `roman_ros2` currently requires that `--simlink-install` option is not used when building. In `~/roman_ws` or the root of your workspace location, run: 86 | 87 | ``` 88 | colcon build 89 | ``` 90 | 91 | # Examples 92 | 93 | A few example demos can be run following the [examples instructions](./examples/README.md). 94 | The provided examples include running ROMAN on a pre-recorded bag, using ROMAN for single robot loop closures, and the main `roman_ros2` demo that creates ROMAN maps across two camera sessions and then aligns the maps from the camera sessions without any initial pose information. 95 | 96 | # Device Requirements 97 | 98 | To run ROMAN with full performance, a GPU is required. 99 | However, ROMAN can still be run on CPU only although semantics cannot be computed and ROMAN will run much slower, which both cause a degradation in performance. 100 | 101 | If you would like to configure the repo to use CPU, cd into this repo and run 102 | 103 | ``` 104 | python3 ./scripts/configure_cpu_or_gpu.py --device cpu 105 | ``` 106 | 107 | --- 108 | 109 | This research is supported by Ford Motor Company, DSTA, ONR, and 110 | ARL DCIST under Cooperative Agreement Number W911NF-17-2-0181. 111 | -------------------------------------------------------------------------------- /examples/d455_single_session/d455_online.yaml: -------------------------------------------------------------------------------- 1 | session_name: roman_ros2 2 | 3 | environment: 4 | ROS_MASTER_URI: ${ROS_MASTER_URI} 5 | ROS_IP: ${ROS_IP} 6 | CAMERA: d455 7 | KIMERA_VIO_CAMERA: d455 8 | ROBOT_NAME: robot 9 | ROMAN_ROS_WS: ${ROMAN_ROS_WS} 10 | ROMAN_ENV_ACTIVATE: ${ROMAN_ENV_ACTIVATE} 11 | OUTPUT_DIR: ${OUTPUT_DIR} 12 | RECORD: "${RECORD}" 13 | 14 | options: 15 | default-command: /bin/zsh 16 | 17 | windows: 18 | - window_name: control 19 | layout: even-vertical 20 | shell_command_before: 21 | - source $ROMAN_ROS_WS/install/setup.zsh 22 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 23 | panes: 24 | - echo "\n\n\n"; 25 | echo "INSTRUCTIONS:"; 26 | echo "Do not move the camera during startup."; 27 | echo "Once you see odometry messages published below, go ahead and start moving."; 28 | echo "Once you have finished your run, push enter with the focus in the bottom panel (the bottom panel will be in focus by default)."; 29 | echo "This will kill the session cleanly." 30 | - for i in {1..10}; 31 | do 32 | ros2 topic echo /robot/kimera_vio_ros/odometry --field pose.pose.position; 33 | sleep 1; 34 | done || echo "Kimera-VIO seems to have failed starting. Please kill and try again." 35 | - echo "PRESS ENTER TO KILL"; 36 | tmux send-keys -t roman_ros2:kimera_vio_ros1_docker C-d '"' && 37 | tmux send-keys -t roman_ros2:kimera_vio_ros1_docker 'tmux kill-session -t kimera_vio_ros1_docker' Enter && 38 | tmux send-keys -t roman_ros2:roman.1 C-c && 39 | tmux send-keys -t roman_ros2:record.0 C-c && 40 | sleep 3 && 41 | tmux kill-session \ 42 | 43 | 44 | - window_name: roman 45 | layout: tiled 46 | shell_command_before: 47 | - source $ROMAN_ROS_WS/install/setup.zsh 48 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 49 | - $ROMAN_ENV_ACTIVATE 50 | panes: 51 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROBOT_NAME camera:=$CAMERA 52 | launch_fastsam:=true 53 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROBOT_NAME camera:=$CAMERA 54 | launch_mapping:=true output_dir:=$OUTPUT_DIR 55 | - ros2 launch roman_ros2 roman.launch.yaml robot_name:=$ROBOT_NAME camera:=$CAMERA 56 | launch_loop_closure:=true launch_loop_closure_viz:=true output_dir:=$OUTPUT_DIR 57 | - rviz2 -d $ROMAN_ROS_WS/src/roman_ros2/examples/d455_single_session/d455_online.rviz 58 | 59 | - window_name: camera 60 | layout: tiled 61 | shell_command_before: 62 | - source $ROMAN_ROS_WS/install/setup.zsh 63 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 64 | panes: 65 | - ros2 run rmw_zenoh_cpp rmw_zenohd 66 | - ros2 launch realsense2_camera rs_launch.py camera_namespace:=$ROBOT_NAME 67 | camera_name:=$CAMERA unite_imu_method:=2 enable_accel:=true enable_gyro:=true 68 | enable_infra1:=true enable_infra2:=true align_depth.enable:=true 69 | - sleep 5 && ros2 param set /$ROBOT_NAME/$CAMERA depth_module.emitter_enabled 0 70 | - ros2 run tf2_ros static_transform_publisher 0 0 0 0.5 0.5 0.5 0.5 ${CAMERA}_link ${ROBOT_NAME}/base 71 | - source ${ROMAN_ROS_WS}/src/kimera-vio-ros2-realsense/dependencies/ros-${ROS_DISTRO}-ros1-bridge/install/local_setup.zsh 72 | && ros2 run ros1_bridge dynamic_bridge 73 | 74 | - window_name: kimera_vio_ros1_docker 75 | layout: tiled 76 | shell_command_before: 77 | - source $ROMAN_ROS_WS/install/setup.zsh 78 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 79 | panes: 80 | - $ROMAN_ROS_WS/src/kimera-vio-ros2-realsense/docker/enter.sh 81 | 82 | - window_name: record 83 | layout: tiled 84 | shell_command_before: 85 | - source $ROMAN_ROS_WS/install/setup.zsh 86 | - export RMW_IMPLEMENTATION=rmw_zenoh_cpp 87 | panes: 88 | # for kimera vio ros to show up in the ros2 topic list, we must subscribe to it 89 | - if [ "$RECORD" = true ]; 90 | then 91 | ros2 bag record -o $OUTPUT_DIR/bag 92 | /$ROBOT_NAME/$CAMERA/color/camera_info 93 | /$ROBOT_NAME/$CAMERA/color/image_raw/compressed 94 | /$ROBOT_NAME/$CAMERA/aligned_depth_to_color/camera_info 95 | /$ROBOT_NAME/$CAMERA/aligned_depth_to_color/image_raw 96 | /$ROBOT_NAME/kimera_vio_ros/odometry 97 | /tf 98 | /tf_static; 99 | fi 100 | - | 101 | if [ "$RECORD" = true ]; 102 | then 103 | python3 -c "import rclpy 104 | from rclpy.node import Node 105 | from nav_msgs.msg import Odometry 106 | 107 | class KimeraVIOOdomSub(Node): 108 | def __init__(self): 109 | super().__init__('kimera_vio_odom_sub') 110 | self.sub = self.create_subscription(Odometry, '/$ROBOT_NAME/kimera_vio_ros/odometry', self.cb, 10) 111 | 112 | def cb(self, msg): 113 | pass # No-op 114 | 115 | rclpy.init() 116 | rclpy.spin(KimeraVIOOdomSub()) 117 | rclpy.shutdown()" 118 | fi -------------------------------------------------------------------------------- /examples/d455_single_session/d455_online.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 284 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Class: rviz_default_plugins/Image 51 | Enabled: true 52 | Max Value: 1 53 | Median window: 5 54 | Min Value: 0 55 | Name: Image 56 | Normalize Range: true 57 | Topic: 58 | Depth: 5 59 | Durability Policy: Volatile 60 | History Policy: Keep Last 61 | Reliability Policy: Reliable 62 | Value: /robot/roman/annotated_img 63 | Value: true 64 | Enabled: true 65 | Global Options: 66 | Background Color: 48; 48; 48 67 | Fixed Frame: map 68 | Frame Rate: 30 69 | Name: root 70 | Tools: 71 | - Class: rviz_default_plugins/Interact 72 | Hide Inactive Objects: true 73 | - Class: rviz_default_plugins/MoveCamera 74 | - Class: rviz_default_plugins/Select 75 | - Class: rviz_default_plugins/FocusCamera 76 | - Class: rviz_default_plugins/Measure 77 | Line color: 128; 128; 0 78 | - Class: rviz_default_plugins/SetInitialPose 79 | Covariance x: 0.25 80 | Covariance y: 0.25 81 | Covariance yaw: 0.06853891909122467 82 | Topic: 83 | Depth: 5 84 | Durability Policy: Volatile 85 | History Policy: Keep Last 86 | Reliability Policy: Reliable 87 | Value: /initialpose 88 | - Class: rviz_default_plugins/SetGoal 89 | Topic: 90 | Depth: 5 91 | Durability Policy: Volatile 92 | History Policy: Keep Last 93 | Reliability Policy: Reliable 94 | Value: /goal_pose 95 | - Class: rviz_default_plugins/PublishPoint 96 | Single click: true 97 | Topic: 98 | Depth: 5 99 | Durability Policy: Volatile 100 | History Policy: Keep Last 101 | Reliability Policy: Reliable 102 | Value: /clicked_point 103 | Transformation: 104 | Current: 105 | Class: rviz_default_plugins/TF 106 | Value: true 107 | Views: 108 | Current: 109 | Class: rviz_default_plugins/Orbit 110 | Distance: 10 111 | Enable Stereo Rendering: 112 | Stereo Eye Separation: 0.05999999865889549 113 | Stereo Focal Distance: 1 114 | Swap Stereo Eyes: false 115 | Value: false 116 | Focal Point: 117 | X: 0 118 | Y: 0 119 | Z: 0 120 | Focal Shape Fixed Size: true 121 | Focal Shape Size: 0.05000000074505806 122 | Invert Z Axis: false 123 | Name: Current View 124 | Near Clip Distance: 0.009999999776482582 125 | Pitch: 0.785398006439209 126 | Target Frame: 127 | Value: Orbit (rviz) 128 | Yaw: 0.785398006439209 129 | Saved: ~ 130 | Window Geometry: 131 | Displays: 132 | collapsed: false 133 | Height: 1531 134 | Hide Left Dock: false 135 | Hide Right Dock: true 136 | Image: 137 | collapsed: false 138 | QMainWindow State: 000000ff00000000fd0000000400000000000004b900000559fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000001aa000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001ef000003a90000001700ffffff000000010000010f00000559fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f00000559000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004df0000003efc0100000002fb0000000800540069006d00650100000000000004df0000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000000200000055900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 139 | Selection: 140 | collapsed: false 141 | Time: 142 | collapsed: false 143 | Tool Properties: 144 | collapsed: false 145 | Views: 146 | collapsed: true 147 | Width: 1247 148 | X: 66 149 | Y: 32 150 | -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/roman_align_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | from dataclasses import dataclass 5 | 6 | # ROS imports 7 | import rclpy 8 | from rclpy.node import Node 9 | import ros2_numpy as rnp 10 | 11 | # ROS msgs 12 | import roman_msgs.msg as roman_msgs 13 | import geometry_msgs.msg as geometry_msgs 14 | import visualization_msgs.msg as visualization_msgs 15 | 16 | # Custom modules 17 | from robotdatapy.transform import transform_to_xyzrpy 18 | 19 | from roman.object.segment import Segment, SegmentMinimalData 20 | from roman.align.roman_registration import ROMANRegistration, ROMANParams 21 | from roman.utils import transform_rm_roll_pitch 22 | 23 | # Local imports 24 | from roman_ros2.utils import msg_to_segment, MapColors, default_marker 25 | 26 | class RecentROMANMap(): 27 | 28 | def __init__(self, time_pass_thresh=30.0): 29 | self.time = -np.inf 30 | self.segments = dict() 31 | self.time_pass_thresh = time_pass_thresh 32 | 33 | def update(self, segment: Segment): 34 | self.time = segment.last_seen 35 | 36 | if segment.id in self.segments: 37 | segment.first_seen = self.segments[segment.id].first_seen 38 | else: 39 | segment.first_seen = segment.last_seen 40 | 41 | # update segment 42 | self.segments[segment.id] = segment 43 | 44 | # remove segments that have not been seen for a while 45 | to_delete = [] 46 | for seg_id, seg in self.segments.items(): 47 | if self.time - seg.last_seen > self.time_pass_thresh: 48 | to_delete.append(seg_id) 49 | 50 | for seg_id in to_delete: 51 | del self.segments[seg_id] 52 | 53 | @property 54 | def segment_list(self): 55 | return list(self.segments.values()) 56 | 57 | 58 | class ROMANAlignNode(Node): 59 | 60 | def __init__(self): 61 | 62 | super().__init__('roman_align_node') 63 | 64 | # required ros parameters 65 | self.declare_parameter("robot1", rclpy.parameter.Parameter.Type.STRING) 66 | self.declare_parameter("robot2", rclpy.parameter.Parameter.Type.STRING) 67 | self.robot1 = self.get_parameter("robot1").value 68 | self.robot2 = self.get_parameter("robot2").value 69 | assert self.robot1 is not None and self.robot2 is not None, \ 70 | "robot1 and robot2 params must be set." 71 | 72 | # ros params 73 | self.declare_parameters( 74 | namespace='', 75 | parameters=[ 76 | ("time_pass_thresh", 60.0), 77 | ("align_dt", 0.5), 78 | ("clipper/sigma", 0.4), 79 | ("clipper/epsilon", 0.6), 80 | ("clipper/mindist", 0.1), 81 | ("clipper/volume_epsilon", 0.0), 82 | ("clipper/min_associations", 4), 83 | ("estimate_roll_pitch", True), 84 | ] 85 | ) 86 | 87 | 88 | self.declare_parameter("frame1", f"{self.robot1}") 89 | self.declare_parameter("frame2", f"{self.robot2}") 90 | 91 | self.frame1 = self.get_parameter("frame1").value 92 | self.frame2 = self.get_parameter("frame2").value 93 | time_pass_thresh = self.get_parameter("time_pass_thresh").value 94 | self.align_dt = self.get_parameter("align_dt").value 95 | clipper_sigma = self.get_parameter("clipper/sigma").value 96 | clipper_epsilon = self.get_parameter("clipper/epsilon").value 97 | clipper_mindist = self.get_parameter("clipper/mindist").value 98 | clipper_volume_epsilon = self.get_parameter("clipper/volume_epsilon").value 99 | self.clipper_min_associations = self.get_parameter("clipper/min_associations").value 100 | self.estimate_roll_pitch = self.get_parameter("estimate_roll_pitch").value 101 | 102 | # internal variables 103 | self.seg_maps = {robot: RecentROMANMap(time_pass_thresh=time_pass_thresh) for robot in [self.robot1, self.robot2]} 104 | roman_registration_params = ROMANParams( 105 | sigma=clipper_sigma, 106 | epsilon=clipper_epsilon, 107 | mindist=clipper_mindist, 108 | gravity=True, 109 | volume=True, 110 | pca=True, 111 | epsilon_shape=clipper_volume_epsilon, 112 | ) 113 | self.roman_registration = ROMANRegistration(roman_registration_params) 114 | 115 | self.setup_ros() 116 | 117 | def setup_ros(self): 118 | 119 | # ros subscribers 120 | self.segment_subs = [ 121 | self.create_subscription(roman_msgs.Segment, f"/{self.robot1}/roman/segment_updates", 122 | lambda msg: self.seg_cb(msg, self.robot1), 20), 123 | self.create_subscription(roman_msgs.Segment, f"/{self.robot2}/roman/segment_updates", 124 | lambda msg: self.seg_cb(msg, self.robot2), 20), 125 | ] 126 | 127 | self.timer = self.create_timer(self.align_dt, self.timer_cb) 128 | 129 | # ros publishers 130 | self.transform_pub = self.create_publisher(geometry_msgs.TransformStamped, 131 | f"/{self.robot1}/roman/frame_align/{self.robot2}", 10) 132 | self.map_association_pub = self.create_publisher(visualization_msgs.MarkerArray, 133 | f"/{self.robot1}/roman/frame_align/{self.robot2}/markers", 10) 134 | 135 | def seg_cb(self, seg_msg: roman_msgs.Segment, robot: str): 136 | """ 137 | Callback function for segment messages. 138 | """ 139 | segment = msg_to_segment(seg_msg) 140 | self.seg_maps[robot].update(segment) 141 | 142 | def timer_cb(self): 143 | """ 144 | Performs object map registration when timer is triggered. 145 | 146 | Args: 147 | timer (_type_): _description_ 148 | """ 149 | # self.pub_map(self.robot1) 150 | # self.pub_map(self.robot2) 151 | map1 = self.seg_maps[self.robot1].segment_list 152 | map2 = self.seg_maps[self.robot2].segment_list 153 | 154 | print(f"Map1: {len(map1)} segments, Map2: {len(map2)} segments.") 155 | if len(map1) < self.clipper_min_associations or len(map2) < self.clipper_min_associations: 156 | print("Not enough segments to align.") 157 | # print(f"Map1: {len(map1)} segments, Map2: {len(map2)} segments.") 158 | return 159 | 160 | # compute object-to-object associations 161 | associations = self.roman_registration.register(map1, map2) 162 | print(f"Found {len(associations)} associations.") 163 | 164 | if len(associations) < self.clipper_min_associations: 165 | self.publish_association_viz(map1, map2, associations) 166 | return 167 | 168 | # compute frame alignment (the pose of frame2 with respect to frame1) 169 | T_frame1_frame2 = self.roman_registration.T_align(map1, map2, associations) 170 | if not self.estimate_roll_pitch: 171 | T_frame1_frame2 = transform_rm_roll_pitch(T_frame1_frame2) 172 | xyzrpy = transform_to_xyzrpy(T_frame1_frame2, degrees=True) 173 | print(f"T^{self.frame1}_{self.frame2} = ") 174 | print(f"x: {xyzrpy[0]:.2f}, y: {xyzrpy[1]:.2f}, z: {xyzrpy[2]:.2f}, ") 175 | print(f"roll: {xyzrpy[3]:.2f}, pitch: {xyzrpy[4]:.2f}, yaw: {xyzrpy[5]:.2f}") 176 | 177 | # publish transform 178 | tf_msg = geometry_msgs.TransformStamped() 179 | tf_msg.header.stamp = self.get_clock().now().to_msg() 180 | tf_msg.header.frame_id = self.frame1 181 | tf_msg.child_frame_id = self.frame2 182 | tf_msg.transform = rnp.msgify(geometry_msgs.Transform, T_frame1_frame2) 183 | self.transform_pub.publish(tf_msg) 184 | self.publish_association_viz(map1, map2, associations) 185 | return 186 | 187 | def publish_association_viz(self, map1, map2, associations): 188 | if len(associations) > self.clipper_min_associations: 189 | ego_color = MapColors.ego_map 190 | other_color = MapColors.other_map 191 | association_color = MapColors.correspondences 192 | else: 193 | ego_color = MapColors.dimmed(MapColors.ego_map) 194 | other_color = MapColors.dimmed(MapColors.other_map) 195 | association_color = (0.0, 0.0, 0.0) 196 | 197 | markers = [] 198 | for i, obj in enumerate(map1): 199 | color = association_color if i in associations[:,0] else ego_color 200 | markers.append(default_marker(obj.center, color, id=obj.id)) 201 | 202 | for i, obj in enumerate(map2): 203 | color = association_color if i in associations[:,1] else other_color 204 | markers.append(default_marker(obj.center, color, id=int(obj.id + 1e6))) 205 | 206 | self.map_association_pub.publish(visualization_msgs.MarkerArray(markers=markers)) 207 | 208 | def main(): 209 | 210 | rclpy.init() 211 | node = ROMANAlignNode() 212 | rclpy.spin(node) 213 | 214 | node.destroy_node() 215 | rclpy.shutdown() 216 | 217 | if __name__ == "__main__": 218 | main() 219 | 220 | -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/lc_viz_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | from numpy.linalg import inv 5 | from scipy.spatial.transform import Rotation as Rot 6 | from dataclasses import dataclass 7 | from typing import List 8 | from copy import deepcopy 9 | from pathlib import Path 10 | import cv2 as cv 11 | 12 | # ROS imports 13 | import rclpy 14 | from rclpy.node import Node 15 | import ros2_numpy as rnp 16 | from rclpy.executors import MultiThreadedExecutor 17 | import tf2_ros 18 | from rclpy.qos import QoSProfile 19 | import cv_bridge 20 | 21 | # ROS msgs 22 | import roman_msgs.msg as roman_msgs 23 | import geometry_msgs.msg as geometry_msgs 24 | import visualization_msgs.msg as visualization_msgs 25 | import sensor_msgs.msg as sensor_msgs 26 | import std_msgs.msg as std_msgs 27 | from ros_system_monitor_msgs.msg import NodeInfoMsg 28 | from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge 29 | 30 | # Custom modules 31 | from robotdatapy.transform import transform_to_xyzrpy 32 | from robotdatapy.data import PoseData 33 | 34 | from roman.object.segment import Segment, SegmentMinimalData 35 | from roman.align.roman_registration import ROMANRegistration, ROMANParams 36 | from roman.align.object_registration import InsufficientAssociationsException 37 | from roman.utils import expandvars_recursive 38 | 39 | from roman.params.submap_align_params import SubmapAlignParams 40 | from roman.utils import transform_rm_roll_pitch 41 | from roman.map.map import Submap, ROMANMap, submaps_from_roman_map, SubmapParams 42 | 43 | # Local imports 44 | from roman_ros2.roman_loop_closure_node import ROMANLoopClosureNodeBaseClass 45 | from roman_ros2.utils import msg_to_segment, float_to_ros_time, lc_to_pose_graph_msg, \ 46 | lc_to_msg, time_stamp_to_float 47 | 48 | 49 | class LCVizNode(ROMANLoopClosureNodeBaseClass): 50 | 51 | def __init__(self): 52 | 53 | super().__init__('lc_viz_node') 54 | 55 | # additional parameters 56 | self.declare_parameters( 57 | namespace='', 58 | parameters=[ 59 | ("aligned_traj_viz", True), 60 | ("output_dir", "~/.roman_ros2/loop_closures"), 61 | ("img_dim", 300), 62 | ("img_border_frac", .1), 63 | ("pose_update_dt", 0.25), 64 | ("traj_num_pts", 200) 65 | ] 66 | ) 67 | 68 | self.aligned_traj_viz = self.get_parameter("aligned_traj_viz").value 69 | self.output_dir = Path(expandvars_recursive(self.get_parameter("output_dir").value)) 70 | self.img_dim = self.get_parameter("img_dim").value 71 | self.img_border_frac = self.get_parameter("img_border_frac").value 72 | self.pose_update_dt = self.get_parameter("pose_update_dt").value 73 | self.traj_num_pts = self.get_parameter("traj_num_pts").value 74 | 75 | assert self.aligned_traj_viz, "Only aligned trajectory visualizer currently supported." 76 | self.get_logger().info(f"Saving loop closure visualizations to {str(self.output_dir )}") 77 | 78 | # internal variables 79 | self.trajectories = {id_i: None for id_i in self.all_ids} 80 | self.output_dir.mkdir(parents=True, exist_ok=True) 81 | self.bridge = cv_bridge.CvBridge() 82 | 83 | # load prior session maps 84 | self.get_logger().info("Loading prior maps.") 85 | for prior_id, map_path in zip(self.prior_session_ids, self.prior_session_maps): 86 | prior_map = ROMANMap.from_pickle(map_path) 87 | self.trajectories[prior_id] = PoseData.from_times_and_poses(prior_map.times, prior_map.trajectory, time_tol=np.inf) 88 | self.get_logger().info(f"Loaded map for robot {prior_id} from {map_path}.") 89 | 90 | self.setup_ros() 91 | self.get_logger().info("ROMAN Loop Closure Visualizer Setup complete.",) 92 | 93 | def setup_ros(self): 94 | 95 | # ros publishers 96 | if self.aligned_traj_viz: 97 | self.traj_img_pub = self.create_publisher(sensor_msgs.Image, 98 | "roman/roman_lc/aligned_traj", qos_profile=QoSProfile(depth=10)) 99 | 100 | # tf buffer 101 | self.tf_buffer = tf2_ros.Buffer() 102 | self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) 103 | 104 | # ros subscribers 105 | self.create_subscription(roman_msgs.LoopClosure, 106 | "roman/roman_lc/loop_closure", self.lc_cb, qos_profile=QoSProfile(depth=10)) 107 | self.timer = self.create_timer(self.pose_update_dt, self.timer_cb) 108 | 109 | 110 | def lc_cb(self, lc_msg: roman_msgs.LoopClosure): 111 | 112 | # align the trajectories 113 | traj1 = deepcopy(self.trajectories[lc_msg.robot1_id]) 114 | traj2 = deepcopy(self.trajectories[lc_msg.robot2_id]) 115 | t1 = time_stamp_to_float(lc_msg.robot1_time) 116 | t2 = time_stamp_to_float(lc_msg.robot2_time) 117 | T_robot1t1_robot2t2 = rnp.numpify(lc_msg.transform_robot1_robot2).astype(np.float64) 118 | traj1.T_premulitply = None 119 | traj2.T_premulitply = None 120 | 121 | T_odom1_robot1t1 = traj1.pose(t1) 122 | T_odom2_robot2t2 = traj2.pose(t2) 123 | T_odom1_odom2 = T_odom1_robot1t1 @ T_robot1t1_robot2t2 @ np.linalg.inv(T_odom2_robot2t2) 124 | traj2.T_premultiply = T_odom1_odom2 125 | 126 | # collect the trajectoriesalong 127 | points = [] 128 | for i, traj in enumerate([traj1, traj2]): 129 | times = np.linspace(traj.t0, traj.tf, self.traj_num_pts) 130 | points.append(np.array([traj.position(t)[:2] for t in times])) 131 | points[-1][:,1] *= -1 # flip along y axis since this will be in image coordinates 132 | # loop closure points 133 | points.append(np.array([traj1.position(t1)[:2], traj2.position(t2)[:2]])) 134 | points[-1][:,1] *= -1 135 | 136 | # scale trajectories 137 | minx = np.min(np.concatenate([points[0][:,0], points[1][:,0]])) 138 | maxx = np.max(np.concatenate([points[0][:,0], points[1][:,0]])) 139 | miny = np.min(np.concatenate([points[0][:,1], points[1][:,1]])) 140 | maxy = np.max(np.concatenate([points[0][:,1], points[1][:,1]])) 141 | dx = maxx - minx 142 | dy = maxy - miny 143 | maxx += 0.5*dx*self.img_border_frac 144 | minx -= 0.5*dx*self.img_border_frac 145 | maxy += 0.5*dy*self.img_border_frac 146 | miny -= 0.5*dy*self.img_border_frac 147 | max_dim = np.max([maxx - minx, maxy - miny]) 148 | 149 | for i in range(3): 150 | points[i][:,0] -= minx 151 | points[i][:,1] -= miny 152 | points[i][:,0] *= self.img_dim / max_dim 153 | points[i][:,1] *= self.img_dim / max_dim 154 | 155 | # draw trajectories 156 | img = np.ones((self.img_dim, self.img_dim, 3)).astype(np.uint8)*255 157 | colors = [(255, 0, 0), (0, 0, 255), (0, 255, 0)] 158 | for i in range(3): 159 | points_i = points[i].astype(np.int32).reshape((-1,1,2)) 160 | img = cv.polylines(img, [points_i], isClosed=False, color=colors[i], thickness=2) 161 | 162 | img = cv.putText(img, f"loop closure between {lc_msg.robot1_id} and {lc_msg.robot2_id}", (10, self.img_dim - 2*15), 163 | cv.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,0), 1, cv.LINE_AA) 164 | img = cv.putText(img, f"{lc_msg.num_associations} matched objects", (10, self.img_dim - 15), 165 | cv.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,0), 1, cv.LINE_AA) 166 | f_name = f"robots_{lc_msg.robot1_id}_{lc_msg.robot2_id}_submaps_{lc_msg.submap1_id}_{lc_msg.submap2_id}.png" 167 | cv.imwrite(str(self.output_dir / f_name), img) 168 | img_msg = self.bridge.cv2_to_imgmsg(img, encoding="bgr8") 169 | img_msg.header.stamp = self.get_clock().now().to_msg() 170 | img_msg.header.frame_id = "overhead" 171 | self.traj_img_pub.publish(img_msg) 172 | 173 | def timer_cb(self): 174 | # record times and poses 175 | curr_time = self.get_clock().now() 176 | curr_time_float = time_stamp_to_float(curr_time.to_msg()) 177 | 178 | for robot_id in self.live_ids: 179 | 180 | try: 181 | T_odom_flu_msg = self.tf_buffer.lookup_transform(self.odom_frames[robot_id], 182 | self.flu_ref_frames[robot_id], curr_time, rclpy.duration.Duration(seconds=0.5)) 183 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: 184 | # self.get_logger().warning("tf lookup failed") 185 | # print(ex) 186 | return 187 | T_odom_flu = rnp.numpify(T_odom_flu_msg.transform).astype(np.float64) 188 | if self.trajectories[robot_id] is None: 189 | self.trajectories[robot_id] = PoseData.from_times_and_poses(np.array([curr_time_float]), np.array([T_odom_flu]), time_tol=np.inf) 190 | else: 191 | self.trajectories[robot_id].times = np.concatenate([self.trajectories[robot_id].times, [curr_time_float]]) 192 | self.trajectories[robot_id].positions = np.concatenate([self.trajectories[robot_id].positions, [T_odom_flu[:3,3]]]) 193 | self.trajectories[robot_id].orientations = np.concatenate([self.trajectories[robot_id].orientations, [Rot.as_quat(Rot.from_matrix(T_odom_flu[:3,:3]))]]) 194 | 195 | 196 | def main(): 197 | 198 | rclpy.init() 199 | node = LCVizNode() 200 | executor = MultiThreadedExecutor() 201 | executor.add_node(node) 202 | try: 203 | executor.spin() 204 | finally: 205 | node.destroy_node() 206 | rclpy.shutdown() 207 | 208 | if __name__ == "__main__": 209 | main() 210 | 211 | -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/fastsam_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import numpy as np 3 | import os 4 | from scipy.spatial.transform import Rotation as Rot 5 | import struct 6 | import open3d as o3d 7 | import time 8 | 9 | # ROS imports 10 | import rclpy 11 | from rclpy.node import Node 12 | import cv_bridge 13 | import message_filters 14 | import ros2_numpy as rnp 15 | from rcl_interfaces.msg import ParameterDescriptor 16 | from rclpy.qos import QoSProfile 17 | import tf2_ros 18 | from rclpy.executors import MultiThreadedExecutor 19 | 20 | # ROS msgs 21 | import geometry_msgs.msg as geometry_msgs 22 | import nav_msgs.msg as nav_msgs 23 | import sensor_msgs.msg as sensor_msgs 24 | import roman_msgs.msg as roman_msgs 25 | from ros_system_monitor_msgs.msg import NodeInfoMsg 26 | 27 | # robot_utils 28 | from robotdatapy.camera import CameraParams 29 | 30 | # ROMAN 31 | from roman.map.fastsam_wrapper import FastSAMWrapper 32 | from roman.params.fastsam_params import FastSAMParams 33 | 34 | # relative 35 | from roman_ros2.utils import observation_to_msg, descriptor_to_array_msg, frame_descriptor_to_msg, TimingFifo 36 | 37 | class FastSAMNode(Node): 38 | 39 | def __init__(self): 40 | super().__init__('fastsam_node') 41 | 42 | # internal variables 43 | self.bridge = cv_bridge.CvBridge() 44 | 45 | # ros params 46 | self.declare_parameters( 47 | namespace='', 48 | parameters=[ 49 | ("odom_frame_id", "odom"), # odometry frame id to map objects in 50 | ("base_flu_frame_id", "base_link"), # robot coordinate frame with xyz = forward left up 51 | ("config_path", ""), # ROMAN fastsam.yaml path 52 | ("min_dt", 0.1), # will not attempt to process images faster than this 53 | ("nickname", "fastsam"), # nickname for this node 54 | ("timing_window", 10), # timing window for processing node processing speed 55 | ] 56 | ) 57 | 58 | self.cam_frame_id = None 59 | self.odom_frame_id = self.get_parameter("odom_frame_id").value 60 | self.base_flu_frame_id = self.get_parameter("base_flu_frame_id").value 61 | self.min_dt = self.get_parameter("min_dt").value 62 | self.nickname = self.get_parameter("nickname").value 63 | timing_window = self.get_parameter("timing_window").value 64 | self.timing_fifo = TimingFifo(timing_window) 65 | config_path = self.get_parameter("config_path").value 66 | 67 | # self.visualize = self.get_parameter("fastsam_viz").value 68 | self.visualize = False # TODO: is supporting this helpful? 69 | self.last_t = -np.inf 70 | 71 | # FastSAM set up after camera info can be retrieved 72 | self.status_pub = self.create_publisher(NodeInfoMsg, "roman/fastsam/status", qos_profile=QoSProfile(depth=10)) 73 | self.log_and_send_status("Waiting for camera info messages...", status=NodeInfoMsg.STARTUP) 74 | depth_info_msg = self._wait_for_message("depth/camera_info", sensor_msgs.CameraInfo) 75 | self.log_and_send_status("Received for depth camera info messages...", status=NodeInfoMsg.STARTUP) 76 | self.log_and_send_status("Waiting for color camera info messages...", status=NodeInfoMsg.STARTUP) 77 | color_info_msg = self._wait_for_message("color/camera_info", sensor_msgs.CameraInfo) 78 | self.log_and_send_status("Received for color camera info messages...", status=NodeInfoMsg.STARTUP) 79 | self.depth_params = CameraParams.from_msg(depth_info_msg) 80 | color_params = CameraParams.from_msg(color_info_msg) 81 | 82 | # fastsam wrapper 83 | if config_path != "": 84 | fastsam_params = FastSAMParams.from_yaml(config_path) 85 | else: 86 | fastsam_params = FastSAMParams() 87 | 88 | self.publish_descriptor = fastsam_params.frame_descriptor is not None 89 | 90 | self.fastsam = FastSAMWrapper.from_params(fastsam_params, self.depth_params) 91 | 92 | self.setup_ros() 93 | 94 | def _wait_for_message(self, topic, msg_type): 95 | """ 96 | Wait for a message on topic of type msg_type 97 | """ 98 | subscription = self.create_subscription(msg_type, topic, self._wait_for_message_cb, 1) 99 | 100 | self._wait_for_message_msg = None 101 | while self._wait_for_message_msg is None: 102 | rclpy.spin_once(self) 103 | msg = self._wait_for_message_msg 104 | 105 | return msg 106 | 107 | def _wait_for_message_cb(self, msg): 108 | self._wait_for_message_msg = msg 109 | return 110 | 111 | def setup_ros(self): 112 | 113 | # ros publishers 114 | self.obs_pub = self.create_publisher(roman_msgs.ObservationArray, "roman/observations", qos_profile=QoSProfile(depth=10)) 115 | if self.publish_descriptor: 116 | self.descriptor_pub = self.create_publisher(roman_msgs.FrameDescriptor, "roman/frame_descriptor", qos_profile=QoSProfile(depth=10)) 117 | 118 | if self.visualize: 119 | self.ptcld_pub = self.create_publisher(sensor_msgs.PointCloud, "roman/observations/ptcld") 120 | 121 | self.tf_buffer = tf2_ros.Buffer() 122 | self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) 123 | 124 | # ros subscribers 125 | subs = [ 126 | message_filters.Subscriber(self, sensor_msgs.Image, "color/image_raw"), 127 | message_filters.Subscriber(self, sensor_msgs.Image, "depth/image_raw"), 128 | ] 129 | self.ts = message_filters.ApproximateTimeSynchronizer(subs, queue_size=10, slop=0.1) 130 | self.ts.registerCallback(self.cb) # registers incoming messages to callback 131 | 132 | self.log_and_send_status("FastSAM node setup complete.") 133 | 134 | def cb(self, *msgs): 135 | """ 136 | This function gets called every time synchronized odometry, image message, and 137 | depth image message are received. 138 | """ 139 | 140 | start_t = time.time() 141 | img_msg, depth_msg = msgs 142 | if self.cam_frame_id is None: 143 | self.cam_frame_id = img_msg.header.frame_id 144 | 145 | # check that enough time has passed since last observation (to not overwhelm GPU) 146 | t = rclpy.time.Time.from_msg(img_msg.header.stamp).nanoseconds * 1e-9 147 | if t - self.last_t < self.min_dt: 148 | return 149 | else: 150 | self.last_t = t 151 | 152 | try: 153 | # self.tf_buffer.waitForTransform(self.odom_frame_id, self.cam_frame_id, img_msg.header.stamp, rospy.Duration(0.5)) 154 | transform_stamped_msg = self.tf_buffer.lookup_transform(self.odom_frame_id, self.cam_frame_id, img_msg.header.stamp, rclpy.duration.Duration(seconds=1.0)) 155 | flu_transformed_stamped_msg = self.tf_buffer.lookup_transform(self.odom_frame_id, self.base_flu_frame_id, img_msg.header.stamp, rclpy.duration.Duration(seconds=0.1)) 156 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: 157 | self.log_and_send_status("tf lookup failed", status=NodeInfoMsg.WARNING) 158 | self.get_logger().warning(str(ex)) 159 | return 160 | 161 | pose = rnp.numpify(transform_stamped_msg.transform).astype(np.float64) 162 | pose_flu = rnp.numpify(flu_transformed_stamped_msg.transform).astype(np.float64) 163 | 164 | 165 | 166 | # conversion from ros msg to cv img 167 | img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') 168 | depth = self.bridge.imgmsg_to_cv2(depth_msg) 169 | 170 | observations, frame_descriptor = self.fastsam.run(t, pose, img, depth_data=depth) 171 | 172 | observation_msgs = [observation_to_msg(obs) for obs in observations] 173 | frame_descriptor_arr_msg = descriptor_to_array_msg(frame_descriptor) 174 | 175 | observation_array = roman_msgs.ObservationArray( 176 | header=img_msg.header, 177 | pose=rnp.msgify(geometry_msgs.Pose, pose), 178 | pose_flu=rnp.msgify(geometry_msgs.Pose, pose_flu), 179 | frame_descriptor=frame_descriptor_arr_msg, 180 | observations=observation_msgs 181 | ) 182 | self.obs_pub.publish(observation_array) 183 | 184 | if self.publish_descriptor: 185 | self.descriptor_pub.publish(frame_descriptor_to_msg(frame_descriptor_arr_msg, img_msg.header.stamp, pose)) 186 | 187 | self.timing_fifo.update(time.time() - start_t) 188 | avg_t = self.timing_fifo.mean() 189 | self.log_and_send_status( 190 | f"Windowed image processing rate: {np.round(1 / avg_t, 3)} Hz", 191 | status=NodeInfoMsg.NOMINAL) 192 | 193 | return 194 | 195 | def log_and_send_status(self, note, status=NodeInfoMsg.NOMINAL): 196 | """ 197 | Log a message and send it to the status topic. 198 | """ 199 | self.get_logger().info(note) 200 | status_msg = NodeInfoMsg() 201 | status_msg.nickname = self.nickname 202 | status_msg.node_name = self.get_fully_qualified_name() 203 | status_msg.status = status 204 | status_msg.notes = note 205 | self.status_pub.publish(status_msg) 206 | 207 | def main(): 208 | 209 | rclpy.init() 210 | node = FastSAMNode() 211 | executor = MultiThreadedExecutor() 212 | executor.add_node(node) 213 | try: 214 | executor.spin() 215 | finally: 216 | node.destroy_node() 217 | rclpy.shutdown() 218 | 219 | 220 | if __name__ == "__main__": 221 | main() 222 | 223 | -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as Rot 3 | from dataclasses import dataclass 4 | from typing import Tuple, Union 5 | 6 | import rclpy 7 | from builtin_interfaces.msg import Time 8 | import roman_msgs.msg as roman_msgs 9 | import geometry_msgs.msg as geometry_msgs 10 | import std_msgs.msg as std_msgs 11 | import visualization_msgs.msg as visualization_msgs 12 | from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge 13 | 14 | import ros2_numpy as rnp 15 | 16 | from roman.map.observation import Observation 17 | from roman.map.map import Submap 18 | from roman.object.segment import Segment, SegmentMinimalData 19 | 20 | class MapColors: 21 | 22 | ego_map: Tuple[float, float, float] = (1.0, 0.0, 0.0) 23 | other_map: Tuple[float, float, float] = (0.0, 0.0, 1.0) 24 | correspondences: Tuple[float, float, float] = (0.0, 1.0, 0.0) 25 | 26 | @classmethod 27 | def dimmed(cls, color): 28 | return tuple([c * 0.5 for c in color]) 29 | 30 | 31 | # Function to convert a float timestamp to ROS 2 Time 32 | def float_to_ros_time(float_time): 33 | ros_time = Time() 34 | ros_time.sec = int(float_time) 35 | ros_time.nanosec = int((float_time % 1.0) * 1e9) 36 | return ros_time 37 | 38 | def time_stamp_to_float(stamp): 39 | return rclpy.time.Time.from_msg(stamp).nanoseconds * 1e-9 40 | 41 | def numpy_to_float64_multiarray(array: np.ndarray) -> std_msgs.Float64MultiArray: 42 | """ 43 | Convert a NumPy array to a ROS Float64MultiArray with proper layout (sizes and strides). 44 | """ 45 | msg = std_msgs.Float64MultiArray() 46 | msg.data = array.astype(np.float64).flatten().tolist() # store flattened data 47 | msg.layout.data_offset = 0 48 | 49 | dims = [] 50 | shape = array.shape 51 | for i, size in enumerate(shape): 52 | dim = std_msgs.MultiArrayDimension() 53 | dim.label = f"dim{i}" 54 | dim.size = size 55 | dim.stride = int(np.prod(shape[i:])) 56 | dims.append(dim) 57 | msg.layout.dim = dims 58 | return msg 59 | 60 | def float64_multiarray_to_numpy(msg: std_msgs.Float64MultiArray) -> np.ndarray: 61 | """ 62 | Convert a ROS Float64MultiArray to a NumPy array using the layout information. 63 | """ 64 | shape = tuple(dim.size for dim in msg.layout.dim) 65 | array = np.array(msg.data, dtype=np.float64).reshape(shape) 66 | return array 67 | 68 | def observation_from_msg(observation_msg: roman_msgs.Observation): 69 | """ 70 | Convert observation message to observation data class 71 | 72 | Args: 73 | observation_msg (roman_msgs.Observation): observation message 74 | 75 | Returns: 76 | Observation: observation data class 77 | """ 78 | observation = Observation( 79 | time=time_stamp_to_float(observation_msg.stamp), 80 | pose=rnp.numpify(observation_msg.pose), 81 | mask=np.array(observation_msg.mask).reshape( 82 | (observation_msg.img_height, observation_msg.img_width) 83 | ) if observation_msg.mask else None, 84 | mask_downsampled=np.array(observation_msg.mask).reshape( 85 | (observation_msg.img_height, observation_msg.img_width) 86 | ) if observation_msg.mask else None, 87 | point_cloud=(np.array(observation_msg.point_cloud).reshape((-1, 3)) 88 | if observation_msg.point_cloud else None), 89 | semantic_descriptor=(np.array(observation_msg.descriptor).reshape((-1,)) 90 | if observation_msg.descriptor else None), 91 | ) 92 | return observation 93 | 94 | def observation_to_msg(observation: Observation): 95 | # def observation_to_msg(observation: Observation, img_width: int, img_height: int): 96 | """ 97 | Convert observation data class to observation message 98 | 99 | Args: 100 | observation (Observation): observation data class 101 | 102 | Returns: 103 | roman_msgs.Observation: observation message 104 | """ 105 | observation_msg = roman_msgs.Observation( 106 | stamp=float_to_ros_time(observation.time), 107 | pose=geometry_msgs.Pose( 108 | position=rnp.msgify(geometry_msgs.Point, observation.pose[:3,3]), 109 | orientation=rnp.msgify(geometry_msgs.Quaternion, Rot.from_matrix(observation.pose[:3,:3]).as_quat()) 110 | ), 111 | img_width=int(observation.mask_downsampled.shape[1]), 112 | img_height=int(observation.mask_downsampled.shape[0]), 113 | mask=observation.mask_downsampled.flatten().astype(np.int8).tolist() if observation.mask is not None else None, 114 | point_cloud=(observation.point_cloud.flatten().tolist() 115 | if observation.point_cloud is not None else None), 116 | descriptor=observation.semantic_descriptor.flatten().tolist() if observation.semantic_descriptor is not None else [], 117 | ) 118 | return observation_msg 119 | 120 | def descriptor_to_array_msg(descriptor: Union[np.ndarray, None]) -> std_msgs.Float64MultiArray: 121 | return numpy_to_float64_multiarray(descriptor) if descriptor is not None else std_msgs.Float64MultiArray() 122 | 123 | def descriptor_from_array_msg(descriptor_array_msg: std_msgs.Float64MultiArray) -> Union[np.ndarray, None]: 124 | return float64_multiarray_to_numpy(descriptor_array_msg) if descriptor_array_msg.data else None 125 | 126 | def frame_descriptor_to_msg(descriptor_array_msg: std_msgs.Float64MultiArray, stamp, pose: np.ndarray) -> roman_msgs.FrameDescriptor: 127 | descriptor_msg = roman_msgs.FrameDescriptor() 128 | descriptor_msg.stamp = stamp 129 | descriptor_msg.pose = rnp.msgify(geometry_msgs.Pose, pose) 130 | descriptor_msg.descriptor = descriptor_array_msg 131 | return descriptor_msg 132 | 133 | def frame_descriptor_from_msg(descriptor_msg: roman_msgs.FrameDescriptor) -> Tuple[float, np.ndarray, np.ndarray]: 134 | descriptor = descriptor_from_array_msg(descriptor_msg.descriptor) 135 | pose = rnp.numpify(descriptor_msg.pose) 136 | time_stamp = time_stamp_to_float(descriptor_msg.stamp) 137 | return time_stamp, pose, descriptor 138 | 139 | """ 140 | segment.msg 141 | 142 | std_msgs/Header header 143 | int32 robot_id 144 | int32 segment_id 145 | geometry_msgs/Point position # Position in odom frame 146 | float64 volume 147 | """ 148 | 149 | def segment_to_msg(robot_id: int, segment: Segment): 150 | """ 151 | Convert segment data class to segment message 152 | 153 | Args: 154 | segment (Segment): segment data class 155 | 156 | Returns: 157 | roman_msgs.Segment: segment message 158 | """ 159 | segment_msg = roman_msgs.Segment( 160 | header=std_msgs.Header(stamp=float_to_ros_time(segment.last_seen)), 161 | robot_id=robot_id, 162 | segment_id=segment.id, 163 | position=rnp.msgify(geometry_msgs.Point, centroid_from_segment(segment)), 164 | # volume=estimate_volume(segment.points) if segment.points is not None else 0.0, 165 | volume=segment.volume, 166 | shape_attributes=[segment.volume, segment.linearity, segment.planarity, segment.scattering], 167 | semantic_descriptor=segment.semantic_descriptor.flatten().tolist() if segment.semantic_descriptor is not None else [], 168 | ) 169 | return segment_msg 170 | 171 | def msg_to_segment(segment_msg: roman_msgs.Segment) -> SegmentMinimalData: 172 | """ 173 | Convert segment message to segment data class 174 | 175 | Args: 176 | segment_msg (roman_msgs.Segment): segment message 177 | 178 | Returns: 179 | Segment: segment data class 180 | """ 181 | segment = SegmentMinimalData( 182 | id=segment_msg.segment_id, 183 | center=np.array([segment_msg.position.x, segment_msg.position.y, segment_msg.position.z]), 184 | volume=segment_msg.shape_attributes[0], 185 | linearity=segment_msg.shape_attributes[1], 186 | planarity=segment_msg.shape_attributes[2], 187 | scattering=segment_msg.shape_attributes[3], 188 | semantic_descriptor=np.array(segment_msg.semantic_descriptor) if len(segment_msg.semantic_descriptor) != 0 else None, 189 | extent=None, 190 | first_seen=None, 191 | last_seen=time_stamp_to_float(segment_msg.header.stamp), 192 | ) 193 | return segment 194 | 195 | def centroid_from_segment(segment: Segment): 196 | """ 197 | Method to get a single point representing a segment. 198 | 199 | Args: 200 | segment (Segment): segment object 201 | 202 | Returns: 203 | np.array, shape=(3,): representative point 204 | """ 205 | if segment.points is not None: 206 | pt = np.mean(segment.points, axis=0) 207 | return pt 208 | else: 209 | return None 210 | 211 | def estimate_volume(points, axis_discretization=10): 212 | """Estimate the volume by voxelizing the bounding box and checking whether sampled points 213 | are inside each voxel""" 214 | min_bounds = np.min(points, axis=0) 215 | max_bounds = np.max(points, axis=0) 216 | x_seg_size = (max_bounds[0] - min_bounds[0])/ axis_discretization 217 | y_seg_size = (max_bounds[1] - min_bounds[1])/ axis_discretization 218 | z_seg_size = (max_bounds[2] - min_bounds[2])/ axis_discretization 219 | volume = 0.0 220 | for i in range(axis_discretization): 221 | x = min_bounds[0] + x_seg_size * i 222 | for j in range(axis_discretization): 223 | y = min_bounds[1] + y_seg_size * j 224 | for k in range(axis_discretization): 225 | z = min_bounds[2] + z_seg_size * k 226 | if np.any(np.bitwise_and(points < np.array([x + x_seg_size, y + y_seg_size, z + z_seg_size]), 227 | points > np.array([x, y, z]))): 228 | volume += x_seg_size * y_seg_size * z_seg_size 229 | return volume 230 | 231 | def default_marker(position: Tuple[float, float, float], color: Tuple[float, float, float], id=0) -> visualization_msgs.Marker: 232 | """ 233 | Create a default marker for visualization 234 | 235 | Args: 236 | position (Tuple[float, float, float]): position of the marker 237 | 238 | Returns: 239 | visualization_msgs.Marker: marker message 240 | """ 241 | marker = visualization_msgs.Marker() 242 | marker.header.frame_id = "map" # TODO: not sure what frame we want to do this visualization 243 | marker.header.stamp = rclpy.time.Time().to_msg() 244 | marker.ns = "default" 245 | marker.id = id 246 | marker.type = visualization_msgs.Marker.SPHERE 247 | marker.action = visualization_msgs.Marker.ADD 248 | marker.pose.position = rnp.msgify(geometry_msgs.Point, np.array(position).reshape(-1)) 249 | marker.pose.orientation = rnp.msgify(geometry_msgs.Quaternion, Rot.from_euler('xyz', [0, 0, 0]).as_quat()) 250 | marker.scale.x = 0.25 251 | marker.scale.y = 0.25 252 | marker.scale.z = 0.25 253 | marker.color.a = 1.0 254 | marker.color.r = color[0] 255 | marker.color.g = color[1] 256 | marker.color.b = color[2] 257 | marker.lifetime = rclpy.duration.Duration(seconds=1.0).to_msg() 258 | return marker 259 | 260 | def lc_to_pose_graph_msg(robot_id1: int, robot_id2: int, submap1: Submap, submap2: Submap, 261 | T_submap1_submap2: np.ndarray, covariance: np.ndarray, stamp): 262 | edge = PoseGraphEdge() 263 | edge.header.stamp = stamp 264 | 265 | edge.key_from = int(submap1.time*1e9) 266 | edge.key_to = int(submap2.time*1e9) 267 | edge.robot_from = robot_id1 268 | edge.robot_to = robot_id2 269 | edge.type = PoseGraphEdge.LOOPCLOSE 270 | 271 | # Pose Graph Tools assumes T_to_from or T_submap2_submap1 272 | edge.pose = rnp.msgify(geometry_msgs.Pose, np.linalg.inv(T_submap1_submap2)) 273 | edge.covariance = covariance.reshape(-1).tolist() 274 | 275 | pg = PoseGraph() 276 | pg.header.stamp = stamp 277 | pg.edges.append(edge) 278 | 279 | return pg 280 | 281 | def lc_to_msg(robot_id1: int, robot_id2: int, submap1: Submap, submap2: Submap, 282 | associations: np.ndarray, T_submap1_submap2: np.ndarray, covariance: np.ndarray, stamp): 283 | lc_msg = roman_msgs.LoopClosure() 284 | lc_msg.header.stamp = stamp 285 | 286 | lc_msg.robot1_id = robot_id1 287 | lc_msg.robot2_id = robot_id2 288 | 289 | lc_msg.submap1_id = submap1.id 290 | lc_msg.submap2_id = submap2.id 291 | 292 | lc_msg.robot1_time = float_to_ros_time(submap1.time) 293 | lc_msg.robot2_time = float_to_ros_time(submap2.time) 294 | 295 | lc_msg.num_associations = len(associations) 296 | lc_msg.robot1_associated_segment_ids = [submap1.segments[i].id for i in associations[:,0]] 297 | lc_msg.robot2_associated_segment_ids = [submap2.segments[i].id for i in associations[:,1]] 298 | 299 | lc_msg.transform_robot1_robot2 = rnp.msgify(geometry_msgs.Pose, T_submap1_submap2) 300 | lc_msg.covariance = covariance.reshape(-1).tolist() 301 | 302 | return lc_msg 303 | 304 | 305 | class TimingFifo: 306 | def __init__(self, max_size: int): 307 | self.max_size = max_size 308 | self.data = [] 309 | 310 | def update(self, value: float): 311 | self.data.append(value) 312 | self.data = self.data[-self.max_size:] 313 | 314 | def mean(self) -> float: 315 | return np.mean(self.data) if self.data else 0.0 316 | 317 | def __len__(self) -> int: 318 | return len(self.data) -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/roman_map_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import os 5 | import cv2 as cv 6 | import struct 7 | import pickle 8 | import time 9 | import signal 10 | from pathlib import Path 11 | 12 | # ROS imports 13 | import rclpy 14 | from rclpy.node import Node 15 | import cv_bridge 16 | import message_filters 17 | import ros2_numpy as rnp 18 | from rcl_interfaces.msg import ParameterDescriptor 19 | from rclpy.qos import QoSProfile 20 | import tf2_ros 21 | from rclpy.executors import MultiThreadedExecutor 22 | import time 23 | 24 | # ROS msgs 25 | import std_msgs.msg as std_msgs 26 | import geometry_msgs.msg as geometry_msgs 27 | import nav_msgs.msg as nav_msgs 28 | import sensor_msgs.msg as sensor_msgs 29 | import roman_msgs.msg as roman_msgs 30 | from ros_system_monitor_msgs.msg import NodeInfoMsg 31 | 32 | # robot_utils 33 | from robotdatapy.camera import CameraParams 34 | 35 | # ROMAN 36 | from roman.map.fastsam_wrapper import FastSAMWrapper 37 | from roman.map.mapper import Mapper, MapperParams 38 | from roman.map.map import ROMANMap 39 | from roman.object.segment import Segment 40 | from roman.viz import visualize_map_on_img 41 | 42 | # relative 43 | from roman_ros2.utils import observation_from_msg, descriptor_from_array_msg, segment_to_msg, \ 44 | time_stamp_to_float, TimingFifo 45 | 46 | class RomanMapNode(Node): 47 | 48 | def __init__(self): 49 | super().__init__('roman_map_node') 50 | self.up = True 51 | 52 | # ros params 53 | self.declare_parameters( 54 | namespace='', 55 | parameters=[ 56 | ("robot_id", 0), # robot id for this node 57 | ("config_path", ""), # non-ros ROMAN mapper.yaml param file 58 | ("visualize", False), # whether to publish annotated images 59 | ("output_roman_map", ""), # output file to save the map to (must kill node with ctrl-c) 60 | ("odom_frame_id", "odom"), # odometry frame id to map objects in 61 | ("base_flu_frame_id", "base_link"), # robot coordinate frame with xyz = forward left up 62 | ("object_ref", "center"), # bottom_middle or center, what reference point to use for objects 63 | ("publish_active_segments", False), # whether to wait till segments are inactive before publishing them 64 | ("nickname", "roman_map"), # nickname for this node 65 | ("use_multiple_cams", False), # whether mapper will get FastSAM measurements from multiple cameras 66 | ("timing_window", 10), # timing window used for report processing speed 67 | ("viz_num_objs", 20), # maximum number of objects to visualize 68 | ("viz_pts_per_obj", 250), # number of points to visualize per object 69 | ("viz_min_viz_dt", 0.25), # minimum time between visualizations 70 | ("viz_rotate_img", ""), # rotate image for visualization, options: CW, CCW, 180 71 | ("viz_pointcloud", False) # whether to publish pointcloud of objects 72 | ] 73 | ) 74 | 75 | self.robot_id = self.get_parameter("robot_id").value 76 | self.visualize = self.get_parameter("visualize").value 77 | self.output_file = self.get_parameter("output_roman_map").value 78 | self.object_ref = self.get_parameter("object_ref").value 79 | self.base_flu_frame_id = self.get_parameter("base_flu_frame_id").value 80 | self.publish_active_segments = self.get_parameter("publish_active_segments").value 81 | self.nickname = self.get_parameter("nickname").value 82 | self.use_multiple_cams = self.get_parameter("use_multiple_cams").value 83 | timing_window = self.get_parameter("timing_window").value 84 | self.timing_fifo = TimingFifo(timing_window) 85 | config_path = self.get_parameter("config_path").value 86 | 87 | assert self.base_flu_frame_id != "", "base_flu_frame_id must be set" 88 | 89 | if self.visualize: 90 | self.odom_frame_id = self.get_parameter("odom_frame_id").value 91 | self.viz_num_objs = self.get_parameter("viz_num_objs").value 92 | self.viz_pts_per_obj = self.get_parameter("viz_pts_per_obj").value 93 | self.min_viz_dt = self.get_parameter("viz_min_viz_dt").value 94 | self.viz_rotate_img = self.get_parameter("viz_rotate_img").value 95 | self.viz_pointcloud = self.get_parameter("viz_pointcloud").value 96 | if self.viz_rotate_img == "": 97 | self.viz_rotate_img = None 98 | if self.output_file != "": 99 | self.output_file = os.path.expanduser(self.output_file) 100 | output_file_parent = Path(self.output_file).parent 101 | output_file_parent.mkdir(parents=True, exist_ok=True) 102 | self.get_logger().info(f"Output file: {self.output_file}") 103 | else: 104 | self.output_file = None 105 | 106 | # mapper 107 | self.status_pub = self.create_publisher(NodeInfoMsg, "roman/roman_map/status", qos_profile=QoSProfile(depth=10)) 108 | self.log_and_send_status("RomanMapNode setting up mapping...", status=NodeInfoMsg.STARTUP) 109 | self.log_and_send_status("RomanMapNode waiting for color camera info messages...", status=NodeInfoMsg.STARTUP) 110 | color_info_msg = self._wait_for_message("color/camera_info", sensor_msgs.CameraInfo) 111 | color_params = CameraParams.from_msg(color_info_msg) 112 | self.log_and_send_status("RomanMapNode received for color camera info messages...", status=NodeInfoMsg.STARTUP) 113 | 114 | if config_path != "": 115 | mapper_params = MapperParams.from_yaml(config_path) 116 | else: 117 | mapper_params = MapperParams() 118 | self.mapper = Mapper( 119 | mapper_params, 120 | camera_params=color_params 121 | ) 122 | 123 | # handle transforming segments in ROS node to support listening to multiple cameras. 124 | # internally, the mapper assumes a single camera, we will say that the camera is pointed 125 | # xyz = forward-right-up 126 | self.mapper.set_T_camera_flu(np.eye(4)) 127 | self.T_camera_flu_set = False 128 | 129 | self.setup_ros() 130 | 131 | def setup_ros(self): 132 | 133 | # ros publishers 134 | self.segments_pub = self.create_publisher(roman_msgs.Segment, "roman/segment_updates", qos_profile=10) 135 | self.pulse_pub = self.create_publisher(std_msgs.Empty, "roman/pulse", qos_profile=10) 136 | 137 | # ros tf 138 | self.tf_buffer = tf2_ros.Buffer() 139 | self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) 140 | 141 | # ros subscribers 142 | self.create_subscription(roman_msgs.ObservationArray, "roman/observations", self.obs_cb, 10) 143 | 144 | # visualization 145 | if self.visualize: 146 | self.last_viz_t = -np.inf 147 | 148 | self.create_subscription(sensor_msgs.Image, "color/image_raw", self.viz_cb, 10) 149 | self.bridge = cv_bridge.CvBridge() 150 | self.annotated_img_pub = self.create_publisher(sensor_msgs.Image, "roman/annotated_img", qos_profile=10) 151 | self.object_points_pub = self.create_publisher(sensor_msgs.PointCloud, "roman/object_points", qos_profile=10) 152 | 153 | self.log_and_send_status("ROMAN Map Node setup complete.", status=NodeInfoMsg.STARTUP) 154 | self.log_and_send_status("Waiting for observation.", status=NodeInfoMsg.STARTUP) 155 | 156 | def set_T_camera_flu(self, obs_array_msg): 157 | """ 158 | Gets transform to set mapper T_camera_flu 159 | """ 160 | # get transform from camera to base_link 161 | transform_stamped_msg = self.tf_buffer.lookup_transform(self.base_flu_frame_id, obs_array_msg.header.frame_id, obs_array_msg.header.stamp, rclpy.duration.Duration(seconds=2.0)) 162 | T_baselink_camera = rnp.numpify(transform_stamped_msg.transform).astype(np.float64) 163 | self.mapper.set_T_camera_flu(np.linalg.inv(T_baselink_camera)) 164 | self.T_camera_flu_set = True 165 | 166 | def obs_cb(self, obs_array_msg): 167 | """ 168 | Triggered by incoming observation messages 169 | """ 170 | if not self.up: 171 | return 172 | 173 | # publish pulse 174 | map_size = len(self.mapper.segments) + \ 175 | len(self.mapper.inactive_segments) + \ 176 | len(self.mapper.segment_graveyard) 177 | self.log_and_send_status((f"Map size: {map_size}, " 178 | f"Avg time (ms): {np.round(self.timing_fifo.mean()*1000)}") 179 | if len(self.timing_fifo) > 0 else f"Map size: {map_size}",) 180 | self.pulse_pub.publish(std_msgs.Empty()) 181 | 182 | if len(obs_array_msg.observations) == 0: 183 | return 184 | 185 | start_t = time.time() 186 | observations = [] 187 | for obs_msg in obs_array_msg.observations: 188 | observations.append(observation_from_msg(obs_msg)) 189 | 190 | t = observations[0].time 191 | assert all([obs.time == t for obs in observations]) 192 | 193 | inactive_ids = [segment.id for segment in self.mapper.inactive_segments] 194 | 195 | if not self.use_multiple_cams: 196 | if not self.T_camera_flu_set: 197 | self.set_T_camera_flu(obs_array_msg) 198 | pose = rnp.numpify(obs_array_msg.pose) # camera frame (T_camera_flu applied internally) 199 | else: 200 | pose = rnp.numpify(obs_array_msg.pose_flu) # base link frame 201 | 202 | frame_descriptor = descriptor_from_array_msg(obs_array_msg.frame_descriptor) 203 | 204 | self.mapper.update(time_stamp_to_float(obs_array_msg.header.stamp), pose, observations, frame_descriptor) 205 | updated_inactive_ids = [segment.id for segment in self.mapper.inactive_segments] 206 | new_inactive_ids = [seg_id for seg_id in updated_inactive_ids if seg_id not in inactive_ids] 207 | 208 | # publish segments 209 | segment: Segment 210 | for segment in self.mapper.inactive_segments: 211 | # TODO: this does not include a way to notify of a deleted segment 212 | if segment.last_seen == t or segment.id in new_inactive_ids: 213 | self.publish_segment(segment) 214 | if self.publish_active_segments: 215 | for segment in self.mapper.segments: 216 | self.publish_segment(segment) 217 | self.timing_fifo.update(time.time() - start_t) 218 | 219 | def publish_segment(self, segment: Segment): 220 | if self.object_ref == 'bottom_middle': 221 | segment.set_center_ref('bottom_middle') 222 | self.segments_pub.publish(segment_to_msg(self.robot_id, segment)) 223 | 224 | def viz_cb(self, img_msg): 225 | """ 226 | Triggered by incoming odometry and image messages 227 | """ 228 | if not self.up: 229 | return 230 | 231 | # rospy.logwarn("Received messages") 232 | t = time_stamp_to_float(img_msg.header.stamp) 233 | if t - self.last_viz_t < self.min_viz_dt: 234 | return 235 | else: 236 | self.last_viz_t = t 237 | 238 | cam_frame_id = img_msg.header.frame_id 239 | 240 | try: 241 | transform_stamped_msg = self.tf_buffer.lookup_transform(self.odom_frame_id, cam_frame_id, img_msg.header.stamp, rclpy.duration.Duration(seconds=2.0)) 242 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: 243 | self.get_logger().warning("tf lookup failed") 244 | self.get_logger().warning(str(ex)) 245 | return 246 | 247 | pose = rnp.numpify(transform_stamped_msg.transform).astype(np.float64) 248 | 249 | # conversion from ros msg to cv img 250 | img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') 251 | img = visualize_map_on_img(t, pose, img, self.mapper) 252 | 253 | if self.viz_rotate_img is not None: 254 | if self.viz_rotate_img == "CW": 255 | img = cv.rotate(img, cv.ROTATE_90_CLOCKWISE) 256 | elif self.viz_rotate_img == "CCW": 257 | img = cv.rotate(img, cv.ROTATE_90_COUNTERCLOCKWISE) 258 | elif self.viz_rotate_img == "180": 259 | img = cv.rotate(img, cv.ROTATE_180) 260 | 261 | annotated_img_msg = self.bridge.cv2_to_imgmsg(img, encoding="bgr8") 262 | annotated_img_msg.header = img_msg.header 263 | self.annotated_img_pub.publish(annotated_img_msg) 264 | 265 | def log_and_send_status(self, note, status=NodeInfoMsg.NOMINAL): 266 | """ 267 | Log a message and send it to the status topic. 268 | """ 269 | self.get_logger().info(note) 270 | status_msg = NodeInfoMsg() 271 | status_msg.nickname = self.nickname 272 | status_msg.node_name = self.get_fully_qualified_name() 273 | status_msg.status = status 274 | status_msg.notes = note 275 | self.status_pub.publish(status_msg) 276 | 277 | # Point cloud publishing 278 | # points_msg = sensor_msgs.PointCloud() 279 | # points_msg.header = img_msg.header 280 | # points_msg.header.frame_id = self.odom_frame_id 281 | # points_msg.points = [] 282 | # # points_msg.channels = [sensor_msgs.ChannelFloat32(name=channel, values=[]) for channel in ['r', 'g', 'b']] 283 | # points_msg.channels = [sensor_msgs.ChannelFloat32(name='rgb', values=[])] 284 | 285 | 286 | # most_recently_seen_segments = sorted( 287 | # self.mapper.segments + self.mapper.inactive_segments + self.mapper.segment_graveyard, 288 | # key=lambda x: x.last_seen if len(x.points) > 10 else 0, reverse=True)[:self.viz_num_objs] 289 | 290 | # for segment in most_recently_seen_segments: 291 | # # color 292 | # np.random.seed(segment.id) 293 | # color_unpacked = np.random.rand(3)*256 294 | # color_raw = int(color_unpacked[0]*256**2 + color_unpacked[1]*256 + color_unpacked[2]) 295 | # color_packed = struct.unpack('f', struct.pack('i', color_raw))[0] 296 | 297 | # points = segment.points 298 | # sampled_points = np.random.choice(len(points), min(len(points), 1000), replace=True) 299 | # points = [points[i] for i in sampled_points] 300 | # points_msg.points += [geometry_msgs.Point32(x=p[0], y=p[1], z=p[2]) for p in points] 301 | # points_msg.channels[0].values += [color_packed for _ in points] 302 | 303 | # self.object_points_pub.publish(points_msg) 304 | 305 | return 306 | 307 | def shutdown(self): 308 | if self.output_file is None: 309 | print(f"No file to save to.") 310 | if self.output_file is not None: 311 | self.up = False 312 | print(f"Saving map to {self.output_file}...") 313 | time.sleep(1.0) 314 | self.mapper.make_pickle_compatible() 315 | pkl_file = open(self.output_file, 'wb') 316 | pickle.dump(self.mapper.get_roman_map(), pkl_file, -1) 317 | pkl_file.close() 318 | self.destroy_node() 319 | 320 | def _wait_for_message(self, topic, msg_type): 321 | """ 322 | Wait for a message on topic of type msg_type 323 | """ 324 | subscription = self.create_subscription(msg_type, topic, self._wait_for_message_cb, 1) 325 | 326 | self._wait_for_message_msg = None 327 | while self._wait_for_message_msg is None: 328 | rclpy.spin_once(self) 329 | msg = self._wait_for_message_msg 330 | # subscription.destroy() 331 | 332 | return msg 333 | 334 | def _wait_for_message_cb(self, msg): 335 | self._wait_for_message_msg = msg 336 | return 337 | 338 | def main(): 339 | 340 | rclpy.init() 341 | node = RomanMapNode() 342 | 343 | # signal.signal(signal.SIGINT, node.shutdown) 344 | executor = MultiThreadedExecutor() 345 | executor.add_node(node) 346 | try: 347 | executor.spin() 348 | finally: 349 | node.shutdown() 350 | rclpy.shutdown() 351 | 352 | if __name__ == "__main__": 353 | main() -------------------------------------------------------------------------------- /roman_ros2/roman_ros2/roman_loop_closure_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | from numpy.linalg import inv 5 | from dataclasses import dataclass 6 | from typing import List 7 | from copy import deepcopy 8 | import time 9 | import os 10 | 11 | # ROS imports 12 | import rclpy 13 | from rclpy.node import Node 14 | import ros2_numpy as rnp 15 | from rclpy.executors import MultiThreadedExecutor 16 | import tf2_ros 17 | from rclpy.qos import QoSProfile 18 | 19 | # ROS msgs 20 | import roman_msgs.msg as roman_msgs 21 | import geometry_msgs.msg as geometry_msgs 22 | import visualization_msgs.msg as visualization_msgs 23 | from ros_system_monitor_msgs.msg import NodeInfoMsg 24 | from pose_graph_tools_msgs.msg import PoseGraph, PoseGraphEdge 25 | 26 | # Custom modules 27 | from robotdatapy.transform import transform_to_xyzrpy 28 | from robotdatapy.data import PoseData 29 | 30 | from roman.object.segment import Segment, SegmentMinimalData 31 | from roman.align.roman_registration import ROMANRegistration, ROMANParams 32 | from roman.align.object_registration import InsufficientAssociationsException 33 | 34 | from roman.params.submap_align_params import SubmapAlignParams 35 | from roman.utils import transform_rm_roll_pitch 36 | from roman.map.map import Submap, ROMANMap, submaps_from_roman_map, extract_submap_descriptors, SubmapParams 37 | 38 | # Local imports 39 | from roman_ros2.utils import msg_to_segment, frame_descriptor_from_msg, float_to_ros_time, lc_to_pose_graph_msg, \ 40 | lc_to_msg 41 | 42 | @dataclass 43 | class FrameDescriptor(): 44 | descriptor: np.ndarray 45 | pose: np.ndarray 46 | time: float 47 | 48 | @dataclass 49 | class SegmentUpdateResult(): 50 | """ 51 | Class to store the result of a segment update. 52 | """ 53 | submap_created: bool 54 | submap_segments: List[SegmentMinimalData] = None 55 | 56 | class SegmentQueue(): 57 | 58 | def __init__(self, submap_num_segments: int, submap_overlapping_segments: int): 59 | self.segments = dict() 60 | self.submap_num_segments = submap_num_segments 61 | self.submap_overlapping_segments = submap_overlapping_segments 62 | 63 | def update(self, segment: Segment): 64 | # update existing segment 65 | if segment.id in self.segments: 66 | segment.first_seen = self.segments[segment.id].first_seen 67 | self.segments[segment.id] = segment 68 | return SegmentUpdateResult(False) 69 | 70 | # create new segment 71 | segment.first_seen = segment.last_seen 72 | self.segments[segment.id] = segment 73 | 74 | # check if we have enough segments to create a submap 75 | if len(self.segments) < self.submap_num_segments: 76 | return SegmentUpdateResult(False) 77 | 78 | assert len(self.segments) == self.submap_num_segments, \ 79 | "ERROR: Segment queue is incorrect size." 80 | 81 | # create submap 82 | submap = [] 83 | for seg_id, seg in self.segments.items(): 84 | submap.append(seg) 85 | 86 | segments_sorted_by_time = sorted(submap, key=lambda seg: np.mean([seg.first_seen, seg.last_seen])) 87 | rm_segment_ids = [seg.id for seg in segments_sorted_by_time[:-self.submap_overlapping_segments]] 88 | 89 | # remove segments that will not be include in subsequent submaps 90 | for seg_id in rm_segment_ids: 91 | del self.segments[seg_id] 92 | 93 | return SegmentUpdateResult(True, submap) 94 | 95 | @property 96 | def earliest_seen(self): 97 | return np.min([seg.first_seen for seg in self.segments.values()]) 98 | 99 | @dataclass 100 | class LoopClosureInfo: 101 | robot_id1: int = None 102 | robot_id2: int = None 103 | submap_id1: int = None 104 | submap_id2: int = None 105 | 106 | @property 107 | def initialized(self): 108 | return self.robot_id1 is not None and \ 109 | self.robot_id2 is not None and \ 110 | self.submap_id1 is not None and \ 111 | self.submap_id2 is not None 112 | 113 | def __str__(self): 114 | if not self.initialized: 115 | return "No loop closure found yet." 116 | else: 117 | return f"Last loop closure between robot_ids: " + \ 118 | f"({self.robot_id1}, {self.robot_id2})" + \ 119 | f" and submaps: ({self.submap_id1}, {self.submap_id2})." 120 | 121 | class ROMANLoopClosureNodeBaseClass(Node): 122 | 123 | def __init__(self, node_name): 124 | super().__init__(node_name) 125 | 126 | # ros parameters 127 | self.declare_parameters( 128 | namespace='', 129 | parameters=[ 130 | ("config_path", ""), 131 | ("submap_num_segments", 40), # number of segments to create a submap from 132 | ("submap_overlapping_segments", 20), # number of overlapping segments between submaps 133 | ("lc_required_associations", 6), # minimum number of associations to accept a loop closure 134 | ("nickname", "roman_lc"), # node nickname for status messages 135 | ("lc_std_dev_rotation_deg", 1.0), # standard deviation for rotation in degrees 136 | ("lc_std_dev_translation_m", 0.5), # standard deviation for translation in meters 137 | ('submap_knn', -1), # number of nearest neighbor submaps to consider for registration, -1 for all 138 | ("ego_id", 0), # robot id of the ego robot 139 | ("ego_name", ""), # robot name of the ego robot 140 | ("ego_flu_ref_frame", ""), # ego robot body reference frame that generally has Z point roughly up 141 | ("ego_odom_frame", ""), # ego robot odometry frame 142 | # NOTE: for the following prameters, [''] is a placeholder for [] because ROS2 does not allow empty lists as default params 143 | ("team_ids", [1]), # integer ids for online robots 144 | ("team_names", ['']), # string names for online robots 145 | ("team_flu_ref_frames", ['']), # robot body reference frames that generally have Z point roughly up 146 | ("team_odom_frames", ['']), # robot odometry frames 147 | ("prior_session_maps", ['']), # Prior session map paths or a single directory with numbered prior maps 148 | ("prior_session_ids", [-1]) # Prior session map ids. Not needed if prior session maps is a directory 149 | ] 150 | ) 151 | 152 | self.config_path = self.get_parameter("config_path").value 153 | self.submap_num_segments = self.get_parameter("submap_num_segments").value 154 | self.submap_overlapping_segments = self.get_parameter("submap_overlapping_segments").value 155 | self.lc_required_associations = self.get_parameter("lc_required_associations").value 156 | self.lc_std_dev_rotation_deg = self.get_parameter("lc_std_dev_rotation_deg").value 157 | self.lc_std_dev_translation_m = self.get_parameter("lc_std_dev_translation_m").value 158 | self.submap_knn = self.get_parameter("submap_knn").value 159 | self.nickname = self.get_parameter("nickname").value 160 | self.ego_id = self.get_parameter("ego_id").value 161 | self.ego_name = self.get_parameter("ego_name").value 162 | self.ego_flu_ref_frame = self.get_parameter("ego_flu_ref_frame").value 163 | self.ego_odom_frame = self.get_parameter("ego_odom_frame").value 164 | self.team_ids = self.get_parameter("team_ids").value 165 | self.team_names = self.get_parameter("team_names").value 166 | self.team_flu_ref_frames = self.get_parameter("team_flu_ref_frames").value 167 | self.team_odom_frames = self.get_parameter("team_odom_frames").value 168 | self.prior_session_ids = self.get_parameter("prior_session_ids").value 169 | self.prior_session_maps = self.get_parameter("prior_session_maps").value 170 | 171 | # make default lists empty 172 | if self.team_names == ['']: 173 | self.team_ids = [] 174 | self.team_names = [] 175 | self.team_flu_ref_frames = [] 176 | self.team_odom_frames = [] 177 | if self.prior_session_maps == ['']: 178 | self.prior_session_ids = [] 179 | self.prior_session_maps = [] 180 | # if prior session maps is a directory, load all .pkl files in the directory 181 | if len(self.prior_session_maps) > 0 and \ 182 | os.path.isdir(self.prior_session_maps[0]): 183 | prior_map_dir = self.prior_session_maps[0] 184 | self.prior_session_maps = [] 185 | self.prior_session_ids = [] 186 | for file_name in os.listdir(prior_map_dir): 187 | if file_name.endswith('.roman_map.pkl'): 188 | map_path = os.path.join(prior_map_dir, file_name) 189 | try: 190 | robot_id = int(file_name.split('.roman_map.pkl')[0]) 191 | except ValueError: 192 | self.get_logger().warning(f"Could not parse robot id from prior map file name: {file_name}. Skipping.") 193 | continue 194 | self.prior_session_maps.append(map_path) 195 | self.prior_session_ids.append(robot_id) 196 | 197 | # organize multi-robot metadata 198 | self.live_names = [self.ego_name] + self.team_names 199 | self.live_ids = [self.ego_id] + self.team_ids 200 | self.all_ids = self.live_ids + self.prior_session_ids 201 | 202 | self.flu_ref_frames = {robot_id: robot_flu_ref_frame 203 | for robot_id, robot_flu_ref_frame in 204 | zip(self.live_ids, [self.ego_flu_ref_frame] + self.team_flu_ref_frames)} 205 | self.odom_frames = {robot_id: robot_odom_frame 206 | for robot_id, robot_odom_frame in 207 | zip(self.live_ids, [self.ego_odom_frame] + self.team_odom_frames)} 208 | 209 | if self.submap_knn < 0: 210 | self.submap_knn = None 211 | 212 | # check params 213 | assert self.ego_name != "", "ERROR: ego_robot_name param must be set." 214 | assert self.ego_flu_ref_frame != "", "ERROR: ego_robot_flu_ref_frame param must be set." 215 | assert self.ego_odom_frame != "", "ERROR: ego_robot_odom_frame param must be set." 216 | assert len(self.team_ids) == len(self.team_names), "ERROR: team_ids and team_names must be the same length." 217 | assert len(self.team_flu_ref_frames) == len(self.team_ids), "ERROR: team_flu_ref_frames param must be set." 218 | assert len(self.team_odom_frames) == len(self.team_ids), "ERROR: team_odom_frames param must be set." 219 | assert len(self.prior_session_maps) == len(self.prior_session_ids), \ 220 | "ERROR: prior_session_maps and prior_session_ids must be the same length." 221 | assert len(set(self.all_ids)) == len(self.all_ids), \ 222 | f"ERROR: all robot ids must be unique. Current team_ids: {self.team_ids}, " + \ 223 | f"prior_session_ids: {self.prior_session_ids}." 224 | 225 | class ROMANLoopClosureNode(ROMANLoopClosureNodeBaseClass): 226 | 227 | def __init__(self): 228 | 229 | super().__init__('roman_loop_closure_node') 230 | self.status_pub = self.create_publisher(NodeInfoMsg, "roman/roman_lc/status", 231 | qos_profile=QoSProfile(depth=10)) 232 | 233 | # internal variables 234 | self.time_eps = 1.0 235 | self.covariance = np.diag([np.deg2rad(self.lc_std_dev_rotation_deg)**2]*3 + 236 | [self.lc_std_dev_translation_m**2]*3) 237 | # time taken, id1, id2, submap_id1, submap_id2 238 | self.last_lc_info = LoopClosureInfo() 239 | self.last_lc_time = None 240 | self.new_lc_flag = False 241 | 242 | self.segment_queues = { 243 | live_id: SegmentQueue(self.submap_num_segments, self.submap_overlapping_segments) 244 | for live_id in self.live_ids} 245 | self.submaps = {live_id: [] for live_id in self.live_ids} 246 | self.times = {live_id: [] for live_id in self.live_ids} 247 | self.poses = {live_id: [] for live_id in self.live_ids} 248 | self.descriptors = {live_id: [] for live_id in self.live_ids} 249 | 250 | if self.config_path == "": 251 | self.submap_align_params = SubmapAlignParams() 252 | else: 253 | self.submap_align_params = SubmapAlignParams.from_yaml(self.config_path) 254 | 255 | self.submap_params = SubmapParams.from_submap_align_params(self.submap_align_params) 256 | 257 | self.roman_reg = self.submap_align_params.get_object_registration() 258 | 259 | # load prior session maps 260 | self.log_and_send_status("Loading prior maps.", status=NodeInfoMsg.STARTUP) 261 | for prior_id, map_path in zip(self.prior_session_ids, self.prior_session_maps): 262 | prior_map = ROMANMap.from_pickle(map_path) 263 | submaps = submaps_from_roman_map(prior_map, self.submap_params) 264 | self.submaps[prior_id] = submaps 265 | self.log_and_send_status(f"Loaded {len(submaps)} submaps for robot {prior_id} from {map_path}.", 266 | status=NodeInfoMsg.STARTUP) 267 | 268 | # Check params 269 | assert self.submap_knn is None or self.submap_align_params.submap_descriptor is not None 270 | 271 | self.setup_ros() 272 | self.log_and_send_status("ROMAN Loop Closure Node setup complete.", status=NodeInfoMsg.STARTUP) 273 | 274 | def setup_ros(self): 275 | 276 | # ros publishers 277 | self.loop_closure_pub = self.create_publisher(roman_msgs.LoopClosure, 278 | "roman/roman_lc/loop_closure", qos_profile=QoSProfile(depth=10)) 279 | self.pgt_lc_pub = self.create_publisher(PoseGraph, "roman/roman_lc/pose_graph_update", 280 | qos_profile=QoSProfile(depth=10)) 281 | 282 | # ros subscribers 283 | self.segment_subs = [ 284 | self.create_subscription(roman_msgs.Segment, f"/{robot_name}/roman/segment_updates", 285 | lambda msg: self.seg_cb(msg, robot_id), 20) 286 | for robot_name, robot_id in zip(self.live_names, self.live_ids)] 287 | 288 | if self.submap_align_params.submap_descriptor is not None: 289 | self.descriptor_subs = [ 290 | self.create_subscription(roman_msgs.FrameDescriptor, f"/{robot_name}/roman/frame_descriptor", 291 | lambda msg: self.descriptor_cb(msg, robot_id), 20) 292 | for robot_name, robot_id in zip(self.live_names, self.live_ids)] 293 | 294 | # tf buffer 295 | self.tf_buffer = tf2_ros.Buffer() 296 | self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) 297 | 298 | def seg_cb(self, seg_msg: roman_msgs.Segment, robot_id: int): 299 | """ 300 | Callback function for segment messages. 301 | """ 302 | # Send pulse 303 | detection_time_info_str = f"Last detection time: {self.last_lc_time} ms" \ 304 | if self.last_lc_time is not None else "No loop closure detection run yet." 305 | self.send_status_msg(detection_time_info_str + " " + str(self.last_lc_info), 306 | status=NodeInfoMsg.NOMINAL) 307 | 308 | segment = msg_to_segment(seg_msg) 309 | seg_update_res = self.segment_queues[robot_id].update(segment) 310 | 311 | # record times and poses 312 | try: 313 | T_odom_flu_msg = self.tf_buffer.lookup_transform(self.odom_frames[robot_id], 314 | self.flu_ref_frames[robot_id], float_to_ros_time(segment.last_seen), rclpy.duration.Duration(seconds=0.25)) 315 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: 316 | # self.get_logger().warning("tf lookup failed") 317 | # print(ex) 318 | return 319 | 320 | T_odom_flu = rnp.numpify(T_odom_flu_msg.transform).astype(np.float64) 321 | 322 | if segment.last_seen not in self.times[robot_id]: 323 | self.times[robot_id].append(segment.last_seen) 324 | self.poses[robot_id].append(T_odom_flu) 325 | 326 | if not seg_update_res.submap_created: 327 | return 328 | 329 | # create submap 330 | submap_ref_time = np.mean([np.mean([seg.first_seen, seg.last_seen]) for seg in seg_update_res.submap_segments]) 331 | submap_id = len(self.submaps[robot_id]) 332 | 333 | # sort times and poses 334 | sorted_times, sorted_poses = zip(*sorted(zip(self.times[robot_id], self.poses[robot_id]))) 335 | sorted_times = list(sorted_times) 336 | sorted_poses = [pose for pose in sorted_poses] 337 | pd = PoseData.from_times_and_poses(sorted_times, sorted_poses, interp=False, time_tol=1e3) 338 | pd_idx = pd.idx(submap_ref_time, force_single=True) 339 | submap_ref_time = sorted_times[pd_idx] 340 | submap_ref_pose = sorted_poses[pd_idx] 341 | 342 | # refresh times and poses buffer 343 | pd.causal = True 344 | earliest_seen_idx = pd.idx(self.segment_queues[robot_id].earliest_seen + self.time_eps) 345 | self.times[robot_id] = sorted_times[earliest_seen_idx:] 346 | self.poses[robot_id] = sorted_poses[earliest_seen_idx:] 347 | 348 | submap = Submap( 349 | id=submap_id, 350 | time=submap_ref_time, 351 | pose_flu=submap_ref_pose, 352 | segments=[], 353 | segment_frame='submap_gravity_aligned' 354 | ) 355 | for seg_i in seg_update_res.submap_segments: 356 | segment = deepcopy(seg_i) 357 | segment.transform(np.linalg.inv(submap.pose_gravity_aligned)) 358 | submap.segments.append(segment) 359 | if self.submap_align_params.submap_descriptor == 'mean_semantic': 360 | submap.descriptor = \ 361 | np.mean([seg.semantic_descriptor.flatten() for seg in submap.segments], axis=0) 362 | submap.descriptor /= np.linalg.norm(submap.descriptor) 363 | elif self.submap_align_params.submap_descriptor is not None: 364 | # sort descriptor buffer 365 | sorted_descriptors = sorted(self.descriptors[robot_id]) 366 | descriptor_times, descriptor_poses, descriptors = zip(*sorted_descriptors) 367 | 368 | # extract submap descriptor 369 | extract_submap_descriptors( 370 | submaps=[submap], 371 | descriptor_times=descriptor_times, 372 | descriptors=descriptors, 373 | poses=descriptor_poses, 374 | submap_params=self.submap_params 375 | ) 376 | 377 | # refresh descriptor buffer 378 | pd = PoseData.from_times_and_poses(descriptor_times, descriptor_poses, interp=False, time_tol=1e3, causal=True) 379 | earliest_seen_idx = pd.idx(self.segment_queues[robot_id].earliest_seen + self.time_eps) 380 | self.descriptors[robot_id] = list(sorted_descriptors)[earliest_seen_idx:] 381 | 382 | self.submaps[robot_id].append(submap) 383 | 384 | start_t = time.time() 385 | 386 | # run submap registration 387 | if robot_id == self.ego_id: 388 | for r2 in self.all_ids: 389 | self.run_submap_registration(submap, robot_id, r2) 390 | else: 391 | # run registration with ego robot 392 | self.run_submap_registration(submap, robot_id, self.ego_robot) 393 | 394 | end_t = time.time() 395 | 396 | self.last_lc_time = np.round((end_t - start_t) * 1e3, 1) 397 | if self.new_lc_flag: 398 | self.log_and_send_status(f"Loop closure found for robot {robot_id}, submap {submap.id}. Detection time: {self.last_lc_time} ms") 399 | else: 400 | self.log_and_send_status(f"No loop closures found for robot {robot_id}, submap {submap.id}. Detection time: {self.last_lc_time} ms") 401 | self.new_lc_flag = False 402 | 403 | def descriptor_cb(self, descriptor_msg: roman_msgs.FrameDescriptor, robot_id: int): 404 | """ 405 | Callback function for frame descriptor messages. 406 | """ 407 | self.descriptors[robot_id].append(frame_descriptor_from_msg(descriptor_msg)) # (time, pose, descriptor) 408 | 409 | def run_submap_registration(self, submap: Submap, robot_id: int, other_id: int): 410 | submap2: Submap 411 | 412 | other_submaps = self.submaps[other_id] 413 | 414 | # prohibit removing a submap with itself 415 | if robot_id == other_id: 416 | other_submaps = [submap2 for submap2 in other_submaps if submap2.id != submap.id] 417 | 418 | # get k nearest neighbors in terms of submap similarity 419 | if self.submap_knn is not None and self.submap_align_params.submap_descriptor is not None: 420 | other_submaps = sorted(other_submaps, 421 | key=lambda sm: Submap.similarity(submap, sm), 422 | reverse=True 423 | )[:self.submap_knn] 424 | 425 | # reject submaps with similarity lower than threshold 426 | if self.submap_align_params.submap_descriptor is not None: 427 | other_submaps = [submap2 for submap2 in other_submaps 428 | if Submap.similarity(submap, submap2) > 429 | self.submap_align_params.submap_descriptor_thresh] 430 | 431 | self.log_and_send_status(f"Attempting to register {len(other_submaps)} " + 432 | f"submaps between robot ids: ({robot_id}, {other_id})") 433 | 434 | for submap2 in other_submaps: 435 | 436 | # run registration 437 | try: 438 | segments1 = submap.segments 439 | segments2 = submap2.segments 440 | # remove same robot same segments 441 | if robot_id == other_id: 442 | segments1_ids = [seg.id for seg in segments1] 443 | segments2_ids = [seg.id for seg in segments2] 444 | segments1 = [seg for seg in segments1 if seg.id not in segments2_ids] 445 | segments2 = [seg for seg in segments2 if seg.id not in segments1_ids] 446 | associations = self.roman_reg.register(segments1, segments2) 447 | T_submapgrav1_submapgrav2 = self.roman_reg.T_align( 448 | segments1, segments2, associations) 449 | if self.submap_align_params.force_rm_lc_roll_pitch: 450 | T_submapgrav1_submapgrav2 = transform_rm_roll_pitch(T_submapgrav1_submapgrav2) 451 | except InsufficientAssociationsException: 452 | continue 453 | 454 | if len(associations) < self.lc_required_associations: 455 | continue 456 | 457 | self.log_and_send_status(f"Found {len(associations)} associations between robot_ids: " + 458 | f"({robot_id}, {other_id}) and submaps: ({submap.id}, {submap2.id}).") 459 | 460 | T_odom1_submapgrav1 = submap.pose_gravity_aligned 461 | T_odom2_submapgrav2 = submap2.pose_gravity_aligned 462 | T_odom1_submap1 = submap.pose_flu 463 | T_odom2_submap2 = submap2.pose_flu 464 | T_submap1_submap2 = inv(T_odom1_submap1) @ T_odom1_submapgrav1 @ \ 465 | T_submapgrav1_submapgrav2 @ inv(T_odom2_submapgrav2) @ T_odom2_submap2 466 | 467 | pg_msg = lc_to_pose_graph_msg(robot_id, other_id, submap, submap2, T_submap1_submap2, 468 | self.covariance, self.get_clock().now().to_msg()) 469 | self.pgt_lc_pub.publish(pg_msg) 470 | 471 | lc_msg = lc_to_msg(robot_id, other_id, submap, submap2, associations, 472 | T_submap1_submap2, self.covariance, self.get_clock().now().to_msg()) 473 | self.loop_closure_pub.publish(lc_msg) 474 | 475 | # store info about last loop closure 476 | self.last_lc_info = LoopClosureInfo(robot_id1=robot_id, robot_id2=other_id, 477 | submap_id1=submap.id, submap_id2=submap2.id) 478 | self.new_lc_flag = True 479 | 480 | def log_and_send_status(self, note, status=NodeInfoMsg.NOMINAL): 481 | """ 482 | Log a message and send it to the status topic. 483 | """ 484 | self.get_logger().info(note) 485 | self.send_status_msg(note, status) 486 | 487 | 488 | def send_status_msg(self, note, status=NodeInfoMsg.NOMINAL): 489 | status_msg = NodeInfoMsg() 490 | status_msg.nickname = self.nickname 491 | status_msg.node_name = self.get_fully_qualified_name() 492 | status_msg.status = status 493 | status_msg.notes = note 494 | self.status_pub.publish(status_msg) 495 | 496 | 497 | def main(): 498 | 499 | rclpy.init() 500 | node = ROMANLoopClosureNode() 501 | executor = MultiThreadedExecutor() 502 | executor.add_node(node) 503 | try: 504 | executor.spin() 505 | finally: 506 | node.destroy_node() 507 | rclpy.shutdown() 508 | 509 | if __name__ == "__main__": 510 | main() 511 | 512 | --------------------------------------------------------------------------------