From b32f15e2f1b17a50084ea3be77f9a883948d24a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adri=C3=A1n=20Ric=C3=A1rdez?= <35706379+adricort@users.noreply.github.com> Date: Mon, 24 Jul 2023 20:27:00 +0200 Subject: [PATCH] Multi camera examples using D405 (#1002) Co-authored-by: Adrian Ricardez Ortigosa --- .../launch/config/slam_D405x2_config.rviz | 434 ++++++++++++++ .../launch/config/slam_D405x3_config.rviz | 538 ++++++++++++++++++ .../launch/rtabmap_D405x2.launch.py | 144 +++++ .../launch/rtabmap_D405x3.launch.py | 173 ++++++ 4 files changed, 1289 insertions(+) create mode 100644 rtabmap_examples/launch/config/slam_D405x2_config.rviz create mode 100644 rtabmap_examples/launch/config/slam_D405x3_config.rviz create mode 100644 rtabmap_examples/launch/rtabmap_D405x2.launch.py create mode 100644 rtabmap_examples/launch/rtabmap_D405x3.launch.py diff --git a/rtabmap_examples/launch/config/slam_D405x2_config.rviz b/rtabmap_examples/launch/config/slam_D405x2_config.rviz new file mode 100644 index 000000000..bc05597b9 --- /dev/null +++ b/rtabmap_examples/launch/config/slam_D405x2_config.rviz @@ -0,0 +1,434 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5340050458908081 + Tree Height: 413 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: MapCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 Vorne + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera1/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 Oben + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + camera1_color_frame: + Value: true + camera1_color_optical_frame: + Value: true + camera1_depth_frame: + Value: true + camera1_depth_optical_frame: + Value: true + camera1_link: + Value: true + camera2_color_frame: + Value: true + camera2_color_optical_frame: + Value: true + camera2_depth_frame: + Value: true + camera2_depth_optical_frame: + Value: true + camera2_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + camera1_link: + camera1_color_frame: + camera1_color_optical_frame: + {} + camera1_depth_frame: + camera1_depth_optical_frame: + {} + camera2_link: + camera2_color_frame: + camera2_color_optical_frame: + {} + camera2_depth_frame: + camera2_depth_optical_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Camera1 Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voxel_cloud1 + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Camera2 Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voxel_cloud2 + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera1/color/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/color/image_rect_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rtabmap/odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.691277027130127 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.1845460832118988 + Y: 1.409915566444397 + Z: 0.6871283054351807 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.13479618728160858 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.8135828971862793 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000018f000003d2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000228000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026b000000d10000002800fffffffb0000000a0049006d0061006700650100000342000000cd0000002800ffffff000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000048c000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1846 + X: 1994 + Y: 27 diff --git a/rtabmap_examples/launch/config/slam_D405x3_config.rviz b/rtabmap_examples/launch/config/slam_D405x3_config.rviz new file mode 100644 index 000000000..82a712626 --- /dev/null +++ b/rtabmap_examples/launch/config/slam_D405x3_config.rviz @@ -0,0 +1,538 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + Splitter Ratio: 0.5340050458908081 + Tree Height: 308 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 Vorne +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 Vorne + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 Oben + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera1/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 Unten + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + camera1_color_frame: + Value: true + camera1_color_optical_frame: + Value: true + camera1_depth_frame: + Value: true + camera1_depth_optical_frame: + Value: true + camera1_link: + Value: true + camera2_color_frame: + Value: true + camera2_color_optical_frame: + Value: true + camera2_depth_frame: + Value: true + camera2_depth_optical_frame: + Value: true + camera2_link: + Value: true + camera_color_frame: + Value: true + camera_color_optical_frame: + Value: true + camera_depth_frame: + Value: true + camera_depth_optical_frame: + Value: true + camera_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + {} + odom: + base_link: + camera1_link: + camera1_color_frame: + camera1_color_optical_frame: + {} + camera1_depth_frame: + camera1_depth_optical_frame: + {} + camera2_link: + camera2_color_frame: + camera2_color_optical_frame: + {} + camera2_depth_frame: + camera2_depth_optical_frame: + {} + camera_link: + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_frame: + camera_depth_optical_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: true + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Vorne Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voxel_cloud1 + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Oben Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voxel_cloud2 + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Unten Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /voxel_cloud3 + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Img Vorne + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/color/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Img Oben + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera1/color/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Img Unten + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera2/color/image_rect_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Odom Local Map Vorne + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rtabmap/odom_local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 14.454187393188477 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.5812196135520935 + Y: 1.577770709991455 + Z: 0.7642305493354797 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.21979624032974243 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.493584394454956 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1163 + Hide Left Dock: false + Hide Right Dock: false + Img Oben: + collapsed: false + Img Unten: + collapsed: false + Img Vorne: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000018f000003edfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bf000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120049006d006700200056006f00720065006e010000022e000000800000000000000000fb000000120049006d006700200056006f0072006e00650100000202000000930000002800fffffffb000000100049006d00670020004f00620065006e010000029b000000bb0000002800fffffffb000000120049006d006700200055006e00740065006e010000035c000000ce0000002800ffffff000000010000010f000003edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d6000003ed00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/rtabmap_examples/launch/rtabmap_D405x2.launch.py b/rtabmap_examples/launch/rtabmap_D405x2.launch.py new file mode 100644 index 000000000..9d1de478c --- /dev/null +++ b/rtabmap_examples/launch/rtabmap_D405x2.launch.py @@ -0,0 +1,144 @@ +# Program dedicated to launch 2 realsense cameras. D405 were tested! +# (Please note that this program can manage maximum 4 depth cameras) + +# Program modified by: Adrian Ricardez (https://github.com/adricort) +# Date: 07.07.2023 +# Deutsches Zentrum für Luft- und Raumfahrt + +# Requirements: + +# Be sure that you did the build on your rtabmap workspace with the -DRTABMAP_SYNC_MULTI_RGBD=ON parameter. + +# Launching the 2 realsense cameras (change your serial numbers): +# $ ros2 launch realsense2_camera rs_multi_camera_launch.py pointcloud.enable1:=true pointcloud.enable2:=true filters:=colorizer align_depth:=true serial_no1:=_128422271521 serial_no2:=_128422272518 + +# Running the static publishers depending on the position of your cameras: +# $ ros2 run tf2_ros static_transform_publisher --x 0.039 --y 0 --z 0 --yaw 0 --pitch 0 --roll 1.5708 --frame-id base_link --child-frame-id camera1_link +# $ ros2 run tf2_ros static_transform_publisher --x -0 --y 0 --z 0.02 --yaw 1.5708 --pitch -1.5708 --roll 0 --frame-id base_link --child-frame-id camera2_link + +# $ ros2 launch rtabmap_examples rtabmap_D405x2.launch.py + +# You should be able to visualize now, with the right rviz config, the camera's SLAM +# Have fun! + +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x2_config.rviz') + + rviz_node = launch_ros.actions.Node( + package='rviz2', executable='rviz2', output='screen', + arguments=[["-d"], [config_rviz]] + ) + + rgbd_sync1_node = launch_ros.actions.Node( + package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync1', output="screen", + parameters=[{ + "approx_sync": False + }], + remappings=[ + ("rgb/image", '/camera1/color/image_rect_raw'), + ("depth/image", '/camera1/depth/image_rect_raw'), + ("rgb/camera_info", '/camera1/color/camera_info'), + ("rgbd_image", 'rgbd_image')], + namespace='realsense_camera1' + ) + rgbd_sync2_node = launch_ros.actions.Node( + package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync2', output="screen", + parameters=[{ + "approx_sync": False + }], + remappings=[ + ("rgb/image", '/camera2/color/image_rect_raw'), + ("depth/image", '/camera2/depth/image_rect_raw'), + ("rgb/camera_info", '/camera2/color/camera_info'), + ("rgbd_image", 'rgbd_image')], + namespace='realsense_camera2' + ) + + # RGB-D odometry + rgbd_odometry_node = launch_ros.actions.Node( + package='rtabmap_odom', executable='rgbd_odometry', output="screen", + parameters=[{ + "frame_id": 'base_link', + "odom_frame_id": 'odom', + "publish_tf": True, + "approx_sync": True, + "subscribe_rgbd": True, + }], + remappings=[ + ("rgbd_image", '/realsense_camera1/rgbd_image'), + ("odom", 'odom')], + arguments=["--delete_db_on_start", ''], + prefix='', + namespace='rtabmap' + ) + + # SLAM + slam_node = launch_ros.actions.Node( + package='rtabmap_slam', executable='rtabmap', output="screen", + parameters=[{ + "rgbd_cameras":2, + "subscribe_depth": True, + "subscribe_rgbd": True, + "subscribe_rgb": True, + "subscribe_odom_info": True, + "frame_id": 'base_link', + "map_frame_id": 'map', + "publish_tf": True, + "database_path": '~/.ros/rtabmap.db', + "approx_sync": True, + "Mem/IncrementalMemory": "true", + "Mem/InitWMWithAllNodes": "true" + }], + remappings=[ + ("rgbd_image0", '/realsense_camera1/rgbd_image'), + ("rgbd_image1", '/realsense_camera2/rgbd_image'), + ("odom", 'odom')], + arguments=["--delete_db_on_start"], + prefix='', + namespace='rtabmap' + ) + + voxelcloud1_node = launch_ros.actions.Node( + package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb1', output='screen', + parameters=[{ + "approx_sync": True, + }], + remappings=[ + ('rgb/image', '/camera1/color/image_rect_raw'), + ('depth/image', '/camera1/depth/image_rect_raw'), + ('rgb/camera_info', '/camera1/color/camera_info'), + ('rgbd_image', 'rgbd_image'), + ('cloud', 'voxel_cloud1')] + ) + + voxelcloud2_node = launch_ros.actions.Node( + package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb2', output='screen', + parameters=[{ + "approx_sync": True, + }], + remappings=[ + ('rgb/image', '/camera2/color/image_rect_raw'), + ('depth/image', '/camera2/depth/image_rect_raw'), + ('rgb/camera_info', '/camera2/color/camera_info'), + ('rgbd_image', 'rgbd_image'), + ('cloud', 'voxel_cloud2')] + ) + + return launch.LaunchDescription( + [ + rviz_node, + rgbd_sync1_node, + rgbd_sync2_node, + rgbd_odometry_node, + slam_node, + voxelcloud1_node, + voxelcloud2_node + ] + ) \ No newline at end of file diff --git a/rtabmap_examples/launch/rtabmap_D405x3.launch.py b/rtabmap_examples/launch/rtabmap_D405x3.launch.py new file mode 100644 index 000000000..168bc40dc --- /dev/null +++ b/rtabmap_examples/launch/rtabmap_D405x3.launch.py @@ -0,0 +1,173 @@ +# Program dedicated to launch 3 realsense cameras. D405 were tested! +# (Please note that this program can manage maximum 4 depth cameras) + +# Program modified by: Adrian Ricardez (https://github.com/adricort) +# Date: 07.07.2023 +# Deutsches Zentrum für Luft- und Raumfahrt + +# Requirements: + +# Be sure that you did the build on your rtabmap workspace with the -DRTABMAP_SYNC_MULTI_RGBD=ON parameter. + +# Launching the 3 realsense cameras (change your serial numbers): +# $ ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true serial_no:=_128422271521 +# $ ros2 launch realsense2_camera rs_multi_camera_launch.py pointcloud.enable1:=true pointcloud.enable2:=true serial_no1:=_128422272518 serial_no2:=_128422272647 + +# Running the static publishers depending on the position of your cameras: +# $ ros2 run tf2_ros static_transform_publisher --x 0.039 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 --frame-id base_link --child-frame-id camera_link +# $ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0.02 --yaw 1.5708 --pitch -1.5708 --roll 0 --frame-id base_link --child-frame-id camera1_link +# $ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z -0.02 --yaw 1.5708 --pitch 1.5708 --roll 0 --frame-id base_link --child-frame-id camera2_link + +# $ ros2 launch rtabmap_examples rtabmap_D405x3.launch.py + +# You should be able to visualize now, with the right rviz config, the camera's SLAM +# Have fun! + +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + config_rviz = os.path.join( + get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x3_config.rviz') + + rviz_node = launch_ros.actions.Node( + package='rviz2', executable='rviz2', output='screen', + arguments=[["-d"], [config_rviz]] + ) + + rgbd_sync1_node = launch_ros.actions.Node( + package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync1', output="screen", + parameters=[{ + "approx_sync": False + }], + remappings=[ + ("rgb/image", '/camera/color/image_rect_raw'), + ("depth/image", '/camera/depth/image_rect_raw'), + ("rgb/camera_info", '/camera/color/camera_info'), + ("rgbd_image", 'rgbd_image')], + namespace='realsense_camera1' + ) + rgbd_sync2_node = launch_ros.actions.Node( + package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync2', output="screen", + parameters=[{ + "approx_sync": False + }], + remappings=[ + ("rgb/image", '/camera1/color/image_rect_raw'), + ("depth/image", '/camera1/depth/image_rect_raw'), + ("rgb/camera_info", '/camera1/color/camera_info'), + ("rgbd_image", 'rgbd_image')], + namespace='realsense_camera2' + ) + rgbd_sync3_node = launch_ros.actions.Node( + package='rtabmap_sync', executable='rgbd_sync', name='rgbd_sync3', output="screen", + parameters=[{ + "approx_sync": False + }], + remappings=[ + ("rgb/image", '/camera2/color/image_rect_raw'), + ("depth/image", '/camera2/depth/image_rect_raw'), + ("rgb/camera_info", '/camera2/color/camera_info'), + ("rgbd_image", 'rgbd_image')], + namespace='realsense_camera3' + ) + + # RGB-D odometry + rgbd_odometry_node = launch_ros.actions.Node( + package='rtabmap_odom', executable='rgbd_odometry', output="screen", + parameters=[{ + "frame_id": 'base_link', + "odom_frame_id": 'odom', + "publish_tf": True, + "approx_sync": True, + "subscribe_rgbd": True, + }], + remappings=[ + ("rgbd_image", '/realsense_camera1/rgbd_image'), + ("odom", 'odom')], + arguments=["--delete_db_on_start", ''], + prefix='', + namespace='rtabmap' + ) + + # SLAM + slam_node = launch_ros.actions.Node( + package='rtabmap_slam', executable='rtabmap', output="screen", + parameters=[{ + "rgbd_cameras":3, + "subscribe_depth": True, + "subscribe_rgbd": True, + "subscribe_rgb": True, + "subscribe_odom_info": True, + "frame_id": 'base_link', + "map_frame_id": 'map', + "publish_tf": True, + "database_path": '~/.ros/rtabmap.db', + "approx_sync": True, + "Mem/IncrementalMemory": "true", + "Mem/InitWMWithAllNodes": "true" + }], + remappings=[ + ("rgbd_image0", '/realsense_camera1/rgbd_image'), + ("rgbd_image1", '/realsense_camera2/rgbd_image'), + ("rgbd_image2", '/realsense_camera3/rgbd_image'), + ("odom", 'odom')], + arguments=["--delete_db_on_start"], + prefix='', + namespace='rtabmap' + ) + + voxelcloud1_node = launch_ros.actions.Node( + package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb1', output='screen', + parameters=[{ + "approx_sync": True, + }], + remappings=[ + ('rgb/image', '/camera/color/image_rect_raw'), + ('depth/image', '/camera/depth/image_rect_raw'), + ('rgb/camera_info', '/camera/color/camera_info'), + ('rgbd_image', 'rgbd_image'), + ('cloud', 'voxel_cloud1')] + ) + + voxelcloud2_node = launch_ros.actions.Node( + package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb2', output='screen', + parameters=[{ + "approx_sync": True, + }], + remappings=[ + ('rgb/image', '/camera1/color/image_rect_raw'), + ('depth/image', '/camera1/depth/image_rect_raw'), + ('rgb/camera_info', '/camera1/color/camera_info'), + ('rgbd_image', 'rgbd_image'), + ('cloud', 'voxel_cloud2')] + ) + voxelcloud3_node = launch_ros.actions.Node( + package='rtabmap_util', executable='point_cloud_xyzrgb', name='point_cloud_xyzrgb3', output='screen', + parameters=[{ + "approx_sync": True, + }], + remappings=[ + ('rgb/image', '/camera2/color/image_rect_raw'), + ('depth/image', '/camera2/depth/image_rect_raw'), + ('rgb/camera_info', '/camera2/color/camera_info'), + ('rgbd_image', 'rgbd_image'), + ('cloud', 'voxel_cloud3')] + ) + + return launch.LaunchDescription( + [ + rviz_node, + rgbd_sync1_node, + rgbd_sync2_node, + rgbd_sync3_node, + rgbd_odometry_node, + slam_node, + voxelcloud1_node, + voxelcloud2_node, + voxelcloud3_node, + ] + ) \ No newline at end of file