i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master to ros2. ros2: Uniformized qos of all subscribers and publishers.

Browse Source
This commit is contained in:
matlabbe
2021-10-04 19:29:19 -04:00
parent ccf152d877 1ecce15878
commit af1c7edeef
121 changed files with 10854 additions and 3478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/config/husky_velodyne_extra.urdf.xacro
+9
View File
@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
<xacro:VLP-16 parent="base_laser_mount" name="velodyne" topic="velodyne_points" gpu="true" organize_cloud="true">
<origin xyz="0.025 0 0.175" rpy="0 0 0" />
</xacro:VLP-16>
</robot>
launch/config/rgbd_gui.ini
+42 -34
View File
@@ -1,13 +1,10 @@
[Core]
Rtabmap\WorkingDirectory=/home/labm2414/.ros
[Gui]
AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0\0\0\a\x80)
AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\0\0\0\0\x1b\0\0\x2\xf7\0\0\x2\xfc\0\0\0\0\0\0\0\x1b\0\0\x2\xf7\0\0\x2\xfc\0\0\0\0\0\0\0\0\a\x80)
DepthCalibrationDialog\bin_depth=2
DepthCalibrationDialog\bin_height=6
DepthCalibrationDialog\bin_width=8
DepthCalibrationDialog\cone_radius=0.02
DepthCalibrationDialog\cone_stddev_thresh=0.10000000000000001
DepthCalibrationDialog\cone_stddev_thresh=0.1
DepthCalibrationDialog\decimation=1
DepthCalibrationDialog\laser_scan=false
DepthCalibrationDialog\max_depth=3.5
@@ -24,18 +21,18 @@ ExportBundlerDialog\sba_rematch_features=true
ExportBundlerDialog\sba_type=0
ExportBundlerDialog\sba_variance=1
ExportCloudsDialog\assemble=true
ExportCloudsDialog\assemble_voxel=0.0050000000000000001
ExportCloudsDialog\assemble_voxel=0.005
ExportCloudsDialog\bilateral=false
ExportCloudsDialog\bilateral_sigma_r=0.10000000000000001
ExportCloudsDialog\bilateral_sigma_r=0.1
ExportCloudsDialog\bilateral_sigma_s=10
ExportCloudsDialog\binary=true
ExportCloudsDialog\cputsdf_flattenRadius=0.0050000000000000001
ExportCloudsDialog\cputsdf_flattenRadius=0.005
ExportCloudsDialog\cputsdf_minWeight=0
ExportCloudsDialog\cputsdf_randomSplit=1
ExportCloudsDialog\cputsdf_resolution=0.01
ExportCloudsDialog\cputsdf_size=12
ExportCloudsDialog\cputsdf_truncNeg=0.029999999999999999
ExportCloudsDialog\cputsdf_truncPos=0.029999999999999999
ExportCloudsDialog\cputsdf_truncNeg=0.03
ExportCloudsDialog\cputsdf_truncPos=0.03
ExportCloudsDialog\filtering=false
ExportCloudsDialog\filtering_min_neighbors=2
ExportCloudsDialog\filtering_radius=0.02
@@ -50,7 +47,7 @@ ExportCloudsDialog\gain_rgb=true
ExportCloudsDialog\mesh=false
ExportCloudsDialog\mesh_angle_tolerance=15
ExportCloudsDialog\mesh_clean=true
ExportCloudsDialog\mesh_color_radius=0.029999999999999999
ExportCloudsDialog\mesh_color_radius=0.03
ExportCloudsDialog\mesh_decimation_factor=0
ExportCloudsDialog\mesh_dense_strategy=1
ExportCloudsDialog\mesh_k=20
@@ -58,7 +55,7 @@ ExportCloudsDialog\mesh_max_polygons=0
ExportCloudsDialog\mesh_min_cluster_size=0
ExportCloudsDialog\mesh_mu=2.5
ExportCloudsDialog\mesh_quad=false
ExportCloudsDialog\mesh_radius=0.040000000000000001
ExportCloudsDialog\mesh_radius=0.04
ExportCloudsDialog\mesh_texture=false
ExportCloudsDialog\mesh_textureBlending=true
ExportCloudsDialog\mesh_textureBlendingDecimation=0
@@ -77,6 +74,7 @@ ExportCloudsDialog\mesh_textureMaxCount=1
ExportCloudsDialog\mesh_textureMaxDepthError=0
ExportCloudsDialog\mesh_textureMaxDistance=3
ExportCloudsDialog\mesh_textureMinCluster=50
ExportCloudsDialog\mesh_textureMultiband=false
ExportCloudsDialog\mesh_textureRoiRatios=0.0 0.0 0.0 0.0
ExportCloudsDialog\mesh_textureSize=5
ExportCloudsDialog\mesh_triangle_size=2
@@ -86,22 +84,22 @@ ExportCloudsDialog\mls_dilation_voxel_size=0.01
ExportCloudsDialog\mls_output_voxel_size=0
ExportCloudsDialog\mls_point_density=0
ExportCloudsDialog\mls_polygonial_order=2
ExportCloudsDialog\mls_radius=0.040000000000000001
ExportCloudsDialog\mls_radius=0.04
ExportCloudsDialog\mls_upsampling_method=0
ExportCloudsDialog\mls_upsampling_radius=0.01
ExportCloudsDialog\mls_upsampling_step=0
ExportCloudsDialog\normals_k=10
ExportCloudsDialog\normals_radius=0
ExportCloudsDialog\openchisel_carving_dist_m=0.050000000000000003
ExportCloudsDialog\openchisel_carving_dist_m=0.05
ExportCloudsDialog\openchisel_chunk_size_x=16
ExportCloudsDialog\openchisel_chunk_size_y=16
ExportCloudsDialog\openchisel_chunk_size_z=16
ExportCloudsDialog\openchisel_far_plane_dist=1.1000000000000001
ExportCloudsDialog\openchisel_far_plane_dist=1.1
ExportCloudsDialog\openchisel_integration_weight=1
ExportCloudsDialog\openchisel_merge_vertices=true
ExportCloudsDialog\openchisel_near_plane_dist=0.050000000000000003
ExportCloudsDialog\openchisel_truncation_constant=0.0015039999999999999
ExportCloudsDialog\openchisel_truncation_linear=0.0015200000000000001
ExportCloudsDialog\openchisel_near_plane_dist=0.05
ExportCloudsDialog\openchisel_truncation_constant=0.001504
ExportCloudsDialog\openchisel_truncation_linear=0.00152
ExportCloudsDialog\openchisel_truncation_quadratic=0.0019
ExportCloudsDialog\openchisel_truncation_scale=10
ExportCloudsDialog\openchisel_use_voxel_carving=false
@@ -113,7 +111,7 @@ ExportCloudsDialog\poisson_minDepth=5
ExportCloudsDialog\poisson_outputPolygons=false
ExportCloudsDialog\poisson_pointWeight=4
ExportCloudsDialog\poisson_samples=1
ExportCloudsDialog\poisson_scale=1.1000000000000001
ExportCloudsDialog\poisson_scale=1.1
ExportCloudsDialog\poisson_solver=8
ExportCloudsDialog\regenerate=true
ExportCloudsDialog\regenerate_decimation=1
@@ -140,13 +138,13 @@ ExportScansDialog\filtering_radius=0.02
ExportScansDialog\normals_k=20
ExportScansDialog\regenerate=false
ExportScansDialog\regenerate_decimation=1
Figures\counts=3
Figures\curves=Gt/Localization_angular_error/deg Gt/Localization_linear_error/m Timing/Total/ms
Figures\counts=
Figures\curves=
General\beep=false
General\cloudCeilingHeight=0
General\cloudFiltering=false
General\cloudFilteringAngle=20
General\cloudFilteringRadius=0.20000000000000001
General\cloudFilteringRadius=0.2
General\cloudFloorHeight=0
General\cloudNoiseMinNeighbors=5
General\cloudNoiseRadius=0
@@ -162,10 +160,14 @@ General\downsamplingScan0=1
General\downsamplingScan1=1
General\figure_cache=true
General\figure_time=true
General\gravityLength0=1
General\gravityLength1=1
General\gravityShown0=false
General\gravityShown1=false
General\gridMapOpacity=0.75
General\gridMapShown=false
General\gtAlign=true
General\imageHighestHypShown=true
General\imageHighestHypShown=false
General\imageRejectedShown=true
General\imagesKept=true
General\landmarkSize=0
@@ -225,9 +227,12 @@ General\showClouds0=true
General\showClouds1=true
General\showFeatures0=false
General\showFeatures1=true
General\showFrames=false
General\showFrustums0=false
General\showFrustums1=false
General\showGraphs=true
General\showIMUAcc=false
General\showIMUGravity=false
General\showLabels=false
General\showLandmarks=true
General\showScans0=true
@@ -240,12 +245,12 @@ General\verticalLayoutUsed=true
General\voxelSizeScan0=0
General\voxelSizeScan1=0
General\wordsGraphView=false
MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\xd5\0\0\0n\0\0\x5\xd4\0\0\x3\xc6\0\0\0\xd5\0\0\0\x8a\0\0\x5\xd4\0\0\x3\xc6\0\0\0\0\0\0\0\0\a\x80)
MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\x43\0\0\0\x1b\0\0\x5\xa5\0\0\x3\xf\0\0\0\x43\0\0\0\x39\0\0\x5\xa5\0\0\x3\xf\0\0\0\0\0\0\0\0\a\x80)
MainWindow\maximized=false
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1q\0\0\x3\x15\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0(\0\0\x3\x15\0\0\x2O\0\xff\xff\xff\0\0\0\x1\0\0\x3\x89\0\0\x3\x15\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x3\x15\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\x1w\0\0\0\xf6\0\0\0{\0\xff\xff\xff\xfc\x2\0\0\0\x2\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1%\0\0\0\x19\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0(\0\0\x3\x15\0\0\0=\0\xff\xff\xff\xfc\0\0\x2s\0\0\x2\x8d\0\0\0\xe6\x1\0\0\x1d\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\x64\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\x1\x38\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0n\0\xff\xff\xff\0\0\0\0\0\0\x3\x15\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1N\0\0\x2\x9a\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0=\0\0\x2\x9a\0\0\x2)\0\xff\xff\xff\0\0\0\x1\0\0\x4\xf\0\0\x2\x9a\xfc\x2\0\0\0\x2\xfc\0\0\0=\0\0\x2\x9a\0\0\0\xdb\0\xff\xff\xff\xfc\x1\0\0\0\x3\xfc\0\0\x1T\0\0\x1\x6\0\0\0h\0\xff\xff\xff\xfc\x2\0\0\0\x2\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1%\0\0\0\x13\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0=\0\0\x2\x9a\0\0\0\x37\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\x2`\0\0\x1\xde\0\0\0\xc8\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\x4\x44\0\0\x1\x1f\0\0\0\x46\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf2\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0i\0\xff\xff\xff\0\0\0\0\0\0\x2\x9a\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
MainWindow\status_bar=false
PostProcessingDialog\cluster_angle=30
PostProcessingDialog\cluster_radius=0.29999999999999999
PostProcessingDialog\cluster_radius=0.3
PostProcessingDialog\detect_more_lc=true
PostProcessingDialog\inter_session=true
PostProcessingDialog\intra_session=true
@@ -259,7 +264,7 @@ PostProcessingDialog\sba_iterations=20
PostProcessingDialog\sba_rematch_features=true
PostProcessingDialog\sba_type=1
PostProcessingDialog\sba_variance=1
PreferencesDialog\geometry="@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\x1\xde\0\0\0\x18\0\0\x5\xe8\0\0\x3,\0\0\x1\xde\0\0\0\x18\0\0\x5\xe8\0\0\x3,\0\0\0\0\0\0\0\0\a\x80)"
PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\x1\f\0\0\0\x1b\0\0\x4\xdb\0\0\x3\x9a\0\0\x1\f\0\0\0\x1b\0\0\x4\xdb\0\0\x3\x9a\0\0\0\0\0\0\0\0\a\x80)
graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
@@ -283,14 +288,14 @@ graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n)
graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0)
graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
graphicsView_graphView\node_radius=0.0099999997764825821
graphicsView_graphView\node_radius=0.009999999776482582
graphicsView_graphView\orientation_ENU=false
graphicsView_graphView\origin_visible=true
graphicsView_graphView\referential_visible=true
graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
imageView_loopClosure\alpha=50
imageView_loopClosure\alpha=150
imageView_loopClosure\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
imageView_loopClosure\colormap=0
imageView_loopClosure\depth_shown=false
@@ -318,7 +323,7 @@ imageView_odometry\image_shown=true
imageView_odometry\lines_shown=true
imageView_odometry\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
imageView_odometry\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
imageView_source\alpha=50
imageView_source\alpha=150
imageView_source\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
imageView_source\colormap=0
imageView_source\depth_shown=false
@@ -333,12 +338,13 @@ imageView_source\lines_shown=true
imageView_source\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
imageView_source\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
widget_cloudViewer\camera_focal=@Variant(\0\0\0T?\x10\xdbz\xbe\f?\x86?\xda\x8c\xf5)
widget_cloudViewer\camera_free=true
widget_cloudViewer\camera_axis_shown=true
widget_cloudViewer\camera_focal="@Variant(\0\0\0T?>\"\xb0=!\xa5\0?>!\xe6)"
widget_cloudViewer\camera_free=false
widget_cloudViewer\camera_lockZ=true
widget_cloudViewer\camera_ortho=false
widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0M\xffz@\x95\xbe\xf1@\xb7\xae\xc3)
widget_cloudViewer\camera_target_follow=false
widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0G\xad|>\r\xa4\x10@h\xe3\x46)
widget_cloudViewer\camera_target_follow=true
widget_cloudViewer\camera_target_locked=false
widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0?\x80\0\0)
widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
@@ -347,6 +353,8 @@ widget_cloudViewer\frustum_shown=true
widget_cloudViewer\grid=false
widget_cloudViewer\grid_cell_count=50
widget_cloudViewer\grid_cell_size=1
widget_cloudViewer\intensity_max=0
widget_cloudViewer\intensity_red_colormap=false
widget_cloudViewer\normals=false
widget_cloudViewer\normals_scale=0.20000000298023224
widget_cloudViewer\normals_step=1
launch/demo/demo_hector_mapping.launch
+43 -13
View File
@@ -1,4 +1,4 @@
<!-- -->
<launch>
<!-- HECTOR MAPPING VERSION: use this with ROS bag demo_mapping_no_odom.bag generated -->
@@ -14,6 +14,23 @@
<!-- Choose hector_slam or icp_odometry for odometry -->
<arg name="hector" default="true" />
<!-- If "hector" above is false, this option feeds wheel odometry to
icp_odometry as guess ( to be more robust to corridor-like environments).
If so, use original demo_mapping.bag containing wheel odometry! -->
<arg name="odom_guess" default="false" />
<!-- Example with camera or not -->
<arg name="camera" default="true" />
<!-- Limit lidar range if > 0 (has effect only when hector:=false) -->
<arg name="max_range" default="0" />
<!-- Point to Plane ICP? (has effect only when hector:=false) -->
<arg name="p2n" default="true" />
<!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
<arg name="pm" default="true" />
<param name="use_sim_time" type="bool" value="True"/>
<node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
@@ -22,7 +39,7 @@
<!-- Odometry from laser scans -->
<!-- If argument "hector" is true, we use Hector mapping to generate odometry for us -->
<node if="$(arg hector)" pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="map_frame" value="hector_map" />
<param name="base_frame" value="base_footprint" />
@@ -51,16 +68,24 @@
<remap from="odom" to="/scanmatch_odom"/>
<remap from="odom_info" to="/rtabmap/odom_info"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param if="$(arg odom_guess)" name="odom_frame_id" type="string" value="icp_odom"/>
<param if="$(arg odom_guess)" name="guess_frame_id" type="string" value="odom"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<param name="Icp/RangeMax" type="string" value="$(arg max_range)"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="5"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0.3"/>
<param unless="$(arg odom_guess)" name="Icp/MaxTranslation" type="string" value="0"/> <!-- can be set to reject large ICP jumps -->
<param if="$(arg p2n)" name="Icp/PointToPlane" type="string" value="true"/>
<param if="$(arg p2n)" name="Icp/PointToPlaneK" type="string" value="5"/>
<param if="$(arg p2n)" name="Icp/PointToPlaneRadius" type="string" value="0.3"/>
<param unless="$(arg p2n)" name="Icp/PointToPlane" type="string" value="false"/>
<param unless="$(arg p2n)" name="Icp/PointToPlaneK" type="string" value="0"/>
<param unless="$(arg p2n)" name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
<param name="Icp/PM" type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
<param name="Icp/PMOutlierRatio" type="string" value="0.95"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
<param name="Icp/PMOutlierRatio" type="string" value="0.85"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Odom/ResetCountdown" type="string" value="0"/>
@@ -68,7 +93,7 @@
</node>
<group ns="rtabmap">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
<node if="$(arg camera)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
@@ -81,8 +106,9 @@
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg camera)"/>
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="scan" to="/jn0/base_scan"/>
@@ -98,12 +124,16 @@
<!-- RTAB-Map's parameters -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<param name="Icp/RangeMax" type="string" value="$(arg max_range)"/>
<param name="Grid/RangeMax" type="string" value="$(arg max_range)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg camera)"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
@@ -120,7 +150,7 @@
<!-- Visualisation RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgbd_image" to="/rtabmap/rgbd_image"/>
<remap from="cloud" to="voxel_cloud" />
launch/demo/demo_husky.launch
+122
View File
@@ -0,0 +1,122 @@
<launch>
<!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):
$ export HUSKY_URDF_EXTRAS=$(rospack find rtabmap_ros)/launch/config/husky_velodyne_extra.urdf.xacro
$ roslaunch husky_gazebo husky_playpen.launch realsense_enabled:=true
$ roslaunch husky_viz view_robot.launch
For ICP odometry examples, rtabmap should be built with libpointmatcher.
Examples:
1) 6DoF mapping with 3D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false
2) 6DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true
3) 6DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true
4) 3DoF mapping with 3D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true
5) 3DoF mapping with 3D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true
6) 3DoF mapping with 3D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true
7) 3DoF mapping with 2D LiDAR
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true
8) 3DoF mapping with 2D LiDAR and RGB-D camera
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true
9) 3DoF mapping with 2D LiDAR and RGB-D camera and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar2d:=true slam2d:=true camera:=true icp_odometry:=true
10) 6DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=false camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
11) 3DoF mapping with 3D LiDAR and RGB camera (depth generated by lidar projection) and ICP odometry (with wheel odometry as guess)
$ roslaunch rtabmap_ros demo_husky.launch lidar3d:=true slam2d:=true camera:=true icp_odometry:=true depth_from_lidar:=true rtabmapviz:=true
Issues:
When setting icp_odometry:=true with navigation, sending a goal to move_base could cause errors like:
"Extrapolation Error: Lookup would require extrapolation into the future. Requested
time 1340.520000000 but the latest data is at time 1340.500000000, when looking up
transform from frame [odom] to frame [map]"
To fix this change "global_frame: odom" to "global_frame: icp_odom" in "husky_navigation/config/costmap_local.yaml"
-->
<arg name="navigation" default="true"/>
<arg name="localization" default="false"/>
<arg name="icp_odometry" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg name="camera" default="false"/>
<arg name="lidar2d" default="false"/>
<arg name="lidar3d" default="false"/>
<arg name="lidar3d_ray_tracing" default="true"/>
<arg name="slam2d" default="true"/>
<arg name="depth_from_lidar" default="false"/>
<arg if="$(arg lidar3d)" name="cell_size" default="0.2"/>
<arg unless="$(arg lidar3d)" name="cell_size" default="0.05"/>
<arg if="$(arg lidar2d)" name="lidar_args" default="--Reg/Strategy 1 --RGBD/NeighborLinkRefining true --Grid/CellSize $(arg cell_size) --Icp/PointToPlaneRadius 0"/>
<arg if="$(arg lidar3d)" name="lidar_args" default="--Reg/Strategy 1 --RGBD/NeighborLinkRefining true --ICP/PM true --Icp/PMOutlierRatio 0.7 --Icp/VoxelSize $(arg cell_size) --Icp/MaxCorrespondenceDistance 1 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/Iterations 10 --Icp/Epsilon 0.001 --OdomF2M/ScanSubtractRadius $(arg cell_size) --OdomF2M/ScanMaxSize 15000 --Grid/ClusterRadius 1 --Grid/RangeMax 20 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Icp/PointToPlaneRadius 0"/>
<!--- Run rtabmap -->
<remap from="/rtabmap/grid_map" to="/map"/>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg if="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args)" />
<arg unless="$(arg localization)" name="args" value="--Reg/Force3DoF $(arg slam2d) $(arg lidar_args) -d" /> <!-- create new map -->
<arg name="localization" value="$(arg localization)" />
<arg name="visual_odometry" value="false" />
<arg name="approx_sync" value="$(eval camera or not icp_odometry)" />
<arg name="imu_topic" value="/imu/data" />
<arg unless="$(arg icp_odometry)" name="odom_topic" value="/odometry/filtered" />
<arg name="frame_id" value="base_link" />
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="gps_topic" value="/navsat/fix"/>
<!-- 2D LiDAR -->
<arg name="subscribe_scan" value="$(arg lidar2d)" />
<arg if="$(arg lidar2d)" name="scan_topic" value="/scan" />
<arg unless="$(arg lidar2d)" name="scan_topic" value="/scan_not_used" />
<!-- 3D LiDAR -->
<arg name="subscribe_scan_cloud" value="$(arg lidar3d)" />
<arg if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
<arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
<!-- If camera is used -->
<arg name="depth" value="$(eval camera and not depth_from_lidar)" />
<arg name="subscribe_rgb" value="$(eval camera)" />
<arg name="rgbd_sync" value="$(eval camera and not depth_from_lidar)" />
<arg name="rgb_topic" value="/realsense/color/image_raw" />
<arg name="camera_info_topic" value="/realsense/color/camera_info" />
<arg name="depth_topic" value="/realsense/depth/image_rect_raw" />
<arg name="approx_rgbd_sync" value="false" />
<!-- If depth generated from lidar projection (in case we have only a single RGB camera with a 3D lidar) -->
<arg name="gen_depth" value="$(arg depth_from_lidar)" />
<arg name="gen_depth_decimation" value="4" />
<arg name="gen_depth_fill_holes_size" value="3" />
<arg name="gen_depth_fill_iterations" value="1" />
<arg name="gen_depth_fill_holes_error" value="0.3" />
<!-- If icp_odometry is used -->
<arg if="$(arg icp_odometry)" name="icp_odometry" value="true" />
<arg if="$(arg icp_odometry)" name="odom_guess_frame_id" value="odom" />
<arg if="$(arg icp_odometry)" name="vo_frame_id" value="icp_odom" />
<arg unless="$(arg slam2d)" name="wait_imu_to_init" value="true" />
<arg if="$(arg lidar3d)" name="odom_args" value="--Icp/CorrespondenceRatio 0.01"/>
</include>
<!--- Run Move Base -->
<include if="$(arg navigation)" file="$(find husky_navigation)/launch/move_base.launch" />
</launch>
launch/demo/demo_robot_mapping.launch
+6 -2
View File
@@ -48,12 +48,16 @@
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Vis/MinInliers" type="string" value="12"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
<param name="RGBD/OptimizeMaxError" type="string" value="3"/> <!-- Reject any loop closure causing large errors (>3x link's covariance) in the map -->
<param name="RGBD/OptimizeMaxError" type="string" value="4"/> <!-- Reject any loop closure causing large errors (>3x link's covariance) in the map -->
<param name="Reg/Force3DoF" type="string" value="true"/> <!-- 2D SLAM -->
<param name="Grid/FromDepth" type="string" value="false"/> <!-- Create 2D occupancy grid from laser scan -->
<param name="Mem/STMSize" type="string" value="30"/> <!-- increased to 30 to avoid adding too many loop closures on just seen locations -->
<param name="RGBD/LocalRadius" type="string" value="5"/> <!-- limit length of proximity detections -->
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/> <!-- minimum scan overlap to accept loop closure -->
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/> <!-- minimum scan overlap to accept loop closure -->
<param name="Icp/PM" type="string" value="false"/>
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.15"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
launch/demo/demo_turtlebot3_navigation.launch
+30 -26
View File
@@ -31,38 +31,42 @@
</include>
<group ns="rtabmap">
<node if="$(eval model=='waffle')" pkg="rtabmap_ros" type="rgbd_sync" name="rgbd_sync" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_raw"/>
<remap from="depth/image" to="/camera/depth/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
</node>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param if="$(eval model=='waffle')" name="subscribe_rgb" type="bool" value="true"/>
<param unless="$(eval model=='waffle')" name="subscribe_rgb" type="bool" value="false"/>
<param if="$(eval model=='waffle')" name="subscribe_depth" type="bool" value="true"/>
<param unless="$(eval model=='waffle')" name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param if="$(eval model=='waffle')" name="subscribe_rgbd" type="bool" value="true"/>
<param unless="$(eval model=='waffle')" name="subscribe_rgbd" type="bool" value="false"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<!-- use actionlib to send goals to move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- use actionlib to send goals to move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="odom" to="/odom"/>
<remap from="rgb/image" to="/camera/rgb/image_raw"/>
<remap from="depth/image" to="/camera/depth/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="odom" to="/odom"/>
<remap from="rgbd_image" to="rgbd_image"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<!-- RTAB-Map's parameters -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
</node>
<!-- visualization with rtabmapviz -->
launch/jfr2018/launch_lidar
+1 -1
View File
@@ -1,5 +1,5 @@
// icp_odometry with guess from odom_combined frame
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0" rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Icp/VoxelSize 0.0 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0" rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db
// proximity space short range
scan_topic:=/base_scan_t_filtered
// proximity space longer range
launch/ros2/rtabmap.launch.py
+9 -5
View File
@@ -1,3 +1,7 @@
#
# To avoid log buffering:
# "stdbuf -o L ros2 launch rtabmap_ros rtabmap.launch.py ..."
#
from launch import LaunchDescription, Substitution, LaunchContext
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, LogInfo, OpaqueFunction
@@ -137,7 +141,7 @@ def launch_setup(context, *args, **kwargs):
"publish_tf": LaunchConfiguration('publish_tf_odom'),
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
"wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
@@ -167,7 +171,7 @@ def launch_setup(context, *args, **kwargs):
"publish_tf": LaunchConfiguration('publish_tf_odom'),
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
"wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
@@ -198,7 +202,7 @@ def launch_setup(context, *args, **kwargs):
"publish_tf": LaunchConfiguration('publish_tf_odom'),
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id'),
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id'),
"wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
@@ -235,7 +239,7 @@ def launch_setup(context, *args, **kwargs):
"odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
"odom_tf_linear_variance": LaunchConfiguration('odom_tf_linear_variance'),
"odom_sensor_sync": LaunchConfiguration('odom_sensor_sync'),
"wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"database_path": LaunchConfiguration('database_path'),
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
@@ -280,7 +284,7 @@ def launch_setup(context, *args, **kwargs):
"subscribe_odom_info": ConditionalBool(True, False, IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' == 'true' or '", LaunchConfiguration('visual_odometry'), "' == 'true'"]))._predicate_func(context)).perform(context),
"frame_id": LaunchConfiguration('frame_id'),
"odom_frame_id": LaunchConfiguration('odom_frame_id'),
"wait_for_transform_duration": LaunchConfiguration('wait_for_transform'),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"approx_sync": LaunchConfiguration('approx_sync'),
"queue_size": LaunchConfiguration('queue_size')
}],
launch/ros2/turtlebot3_rgbd.launch.py
+2
View File
@@ -2,7 +2,9 @@
# Install Turtlebot3 packages
# Install https://github.com/mlherd/ros2_turtlebot3_waffle_intel_realsense
# Example:
# $ export TURTLEBOT3_MODEL=waffle
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# $ ros2 run turtlebot3_teleop teleop_keyboard
#
# $ ros2 launch rtabmap_ros turtlebot3_rgbd.launch.py
# OR
launch/ros2/turtlebot3_rgbd_sync.launch.py
+6 -2
View File
@@ -2,11 +2,13 @@
# Install Turtlebot3 packages
# Install https://github.com/mlherd/ros2_turtlebot3_waffle_intel_realsense
# Example:
# $ export TURTLEBOT3_MODEL=waffle
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# $ ros2 run turtlebot3_teleop teleop_keyboard
#
# $ ros2 launch rtabmap_ros turtlebot3_rgbd_sync.launch.py
# OR
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true odom_topic:=/odom args:="-d" use_sim_time:=true rgbd_sync:=true rgb_topic:=/intel_realsense_r200_depth/image_raw depth_topic:=/intel_realsense_r200_depth/depth/image_raw camera_info_topic:=/intel_realsense_r200_depth/camera_info
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" use_sim_time:=true rgbd_sync:=true rgb_topic:=/intel_realsense_r200_depth/image_raw depth_topic:=/intel_realsense_r200_depth/depth/image_raw camera_info_topic:=/intel_realsense_r200_depth/camera_info
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
@@ -22,7 +24,9 @@ def generate_launch_description():
'frame_id':'base_footprint',
'use_sim_time':use_sim_time,
'subscribe_rgbd':True,
'subscribe_scan':True}]
'subscribe_scan':True,
'Reg/Strategy':'1',
'RGBD/NeighborLinkRefining':'True'}]
remappings=[
('rgb/image', '/intel_realsense_r200_depth/image_raw'),
launch/ros2/turtlebot3_scan.launch.py
+3 -1
View File
@@ -1,11 +1,13 @@
# Requirements:
# Install Turtlebot3 packages
# Example:
# $ export TURTLEBOT3_MODEL=waffle
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
# $ ros2 run turtlebot3_teleop teleop_keyboard
#
# $ ros2 launch rtabmap_ros turtlebot3_scan.launch.py
# OR
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d" use_sim_time:=true
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" use_sim_time:=true
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
launch/rtabmap.launch
+125 -35
View File
@@ -1,4 +1,4 @@
<!-- -->
<launch>
<!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
@@ -14,8 +14,11 @@
Example:
$ roslaunch rtabmap_ros bumblebee.launch -->
<!-- Choose between RGB-D and stereo -->
<arg name="stereo" default="false"/>
<!-- Choose between depth and stereo, set both to false to do only scan -->
<arg name="stereo" default="false"/>
<arg if="$(arg stereo)" name="depth" default="false"/>
<arg unless="$(arg stereo)" name="depth" default="true"/>
<arg name="subscribe_rgb" default="$(arg depth)"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="true" />
@@ -35,6 +38,7 @@
<arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="odom_frame_id_init" default=""/> <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
<arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
@@ -44,13 +48,16 @@
<arg name="wait_for_transform" default="0.2"/>
<arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="gdb" default="false"/> <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
<arg if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
<arg unless="$(arg gdb)" name="launch_prefix" default=""/>
<arg name="clear_params" default="true"/>
<arg name="output" default="screen"/> <!-- Control node output (screen or log) -->
<arg name="publish_tf_map" default="true"/>
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
<arg if="$(arg stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg stereo)" name="approx_sync" default="true"/>
<arg unless="$(arg stereo)" name="approx_sync" default="$(arg depth)"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
@@ -70,17 +77,33 @@
<arg name="approx_rgbd_sync" default="true"/> <!-- false=exact synchronization -->
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="rgbd_topic" default="rgbd_image" />
<arg name="depth_scale" default="1.0" />
<arg name="depth_scale" default="1.0" /> <!-- Deprecated, use rgbd_depth_scale instead -->
<arg name="rgbd_depth_scale" default="$(arg depth_scale)" />
<arg name="rgbd_decimation" default="1" />
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="gen_cloud" default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
<arg name="gen_cloud_decimation" default="4"/>
<arg name="gen_cloud_voxel" default="0.05"/>
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="false"/>
<arg name="subscribe_scan_cloud" default="$(arg gen_cloud)"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="scan_normal_k" default="0"/>
<arg name="subscribe_scan_descriptor" default="false"/>
<arg name="scan_descriptor_topic" default="/scan_descriptor"/>
<arg name="scan_cloud_max_points" default="0"/>
<arg name="scan_cloud_filtered" default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
<arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
<arg name="gen_depth" default="false" /> <!-- Generate depth image from scan_cloud -->
<arg name="gen_depth_decimation" default="1" />
<arg name="gen_depth_fill_holes_size" default="0" />
<arg name="gen_depth_fill_iterations" default="1" />
<arg name="gen_depth_fill_holes_error" default="0.1" />
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="false"/> <!-- Launch rtabmap icp odometry node -->
@@ -93,9 +116,22 @@
<arg name="odom_sensor_sync" default="false"/>
<arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="odom_max_rate" default="0"/>
<arg name="odom_expected_rate" default="0"/>
<arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO approaches -->
<arg name="wait_imu_to_init" default="false"/>
<arg name="use_odom_features" default="false"/>
<arg name="scan_cloud_assembling" default="false"/>
<arg name="scan_cloud_assembling_time" default="1"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_max_clouds" default="0"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_fixed_frame" default=""/>
<arg name="scan_cloud_assembling_voxel_size" default="0.05"/>
<arg name="scan_cloud_assembling_range_min" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_range_max" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_radius" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_min_neighbors" default="5"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
@@ -123,7 +159,7 @@
<group ns="$(arg namespace)">
<!-- relays -->
<group unless="$(arg stereo)">
<group if="$(arg depth)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
@@ -131,14 +167,15 @@
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="depth_scale" type="double" value="$(arg depth_scale)"/>
<param name="depth_scale" type="double" value="$(arg rgbd_depth_scale)"/>
<param name="decimation" type="double" value="$(arg rgbd_decimation)"/>
</node>
</group>
</group>
@@ -150,7 +187,7 @@
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -164,7 +201,7 @@
<group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
<remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
<remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
@@ -172,13 +209,23 @@
</node>
</group>
</group>
<node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
<remap from="cloud" to="$(arg scan_cloud_topic)" />
<param name="decimation" type="double" value="$(arg gen_cloud_decimation)"/>
<param name="voxel_size" type="double" value="$(arg gen_cloud_voxel)"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
<!-- Visual odometry -->
<group unless="$(arg icp_odometry)">
<group if="$(arg visual_odometry)">
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -200,10 +247,13 @@
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="keep_color" type="bool" value="$(arg use_odom_features)"/>
</node>
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -226,12 +276,15 @@
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="keep_color" type="bool" value="$(arg use_odom_features)"/>
</node>
</group>
</group>
<!-- ICP Odometry -->
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
@@ -249,24 +302,46 @@
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
<param name="max_clouds" type="int" value="$(arg scan_cloud_assembling_max_clouds)"/>
<param name="fixed_frame_id" type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
<param name="voxel_size" type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
<param name="range_min" type="double" value="$(arg scan_cloud_assembling_range_min)"/>
<param name="range_max" type="double" value="$(arg scan_cloud_assembling_range_max)"/>
<param name="noise_radius" type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
<param name="noise_min_neighbors" type="int" value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="odom_frame_id_init" type="string" value="$(arg odom_frame_id_init)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_map)"/>
<param name="gen_scan" type="bool" value="$(arg gen_scan)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
@@ -274,18 +349,24 @@
<param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="scan_normal_k" type="int" value="$(arg scan_normal_k)"/>
<param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<param name="gen_depth" type="bool" value="$(arg gen_depth)" />
<param name="gen_depth_decimation" type="int" value="$(arg gen_depth_decimation)" />
<param name="gen_depth_fill_holes_size" type="int" value="$(arg gen_depth_fill_holes_size)" />
<param name="gen_depth_fill_iterations" type="int" value="$(arg gen_depth_fill_iterations)" />
<param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
<remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
@@ -293,7 +374,10 @@
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
<remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/>
<remap from="user_data" to="$(arg user_data_topic)"/>
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap from="gps/fix" to="$(arg gps_topic)"/>
@@ -308,34 +392,40 @@
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param unless="$(arg icp_odometry)" name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool" value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
<param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
<remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
<remap if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
</node>
@@ -343,7 +433,7 @@
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/>
<remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/>
<remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/>
launch/tests/test_d435i_vio.launch
+20 -11
View File
@@ -1,14 +1,25 @@
<launch>
<!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i -->
<!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i.
Make sure to disable the IR emitter or put a tape on the IR emitter to
avoid VINS tracking the fixed IR points (that would cause large drifts) -->
<arg name="rtabmapviz" default="true"/>
<arg name="rviz" default="false"/>
<arg name="depth_mode" default="true"/>
<arg name="odom_strategy" default="9"/> <!-- default VINS -->
<arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" -->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="align_depth" value="true"/>
<arg name="unite_imu_method" value="linear_interpolation"/>
<arg name="align_depth" value="$(arg depth_mode)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="enable_gyro" value="true"/>
<arg name="enable_accel" value="true"/>
<arg name="enable_infra1" value="true"/>
<arg name="enable_infra2" value="true"/>
<arg name="gyro_fps" value="200"/>
<arg name="accel_fps" value="250"/>
<arg name="enable_sync" value="true"/>
</include>
<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
@@ -19,17 +30,15 @@
<remap from="/imu/data" to="/rtabmap/imu"/>
</node>
<node pkg="topic_tools" type="throttle" name="imu_throttle" args="messages /rtabmap/imu 100.0"/>
<!-- RTAB-Map: depth mode -->
<!-- We have to launch stereo_odometry externally from rtabmap.launch so that rtabmap can use RGB-D input -->
<group ns="rtabmap">
<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml --Rtabmap/ImagesAlreadyRectified false" output="screen">
<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
<remap from="left/image_rect" to="/camera/infra1/image_rect_raw"/>
<remap from="right/image_rect" to="/camera/infra2/image_rect_raw"/>
<remap from="left/camera_info" to="/camera/infra1/camera_info"/>
<remap from="right/camera_info" to="/camera/infra2/camera_info"/>
<remap from="imu" to="/rtabmap/imu_throttle"/>
<remap from="imu" to="/rtabmap/imu"/>
<param name="frame_id" value="camera_link"/>
<param name="wait_imu_to_init" value="true"/>
</node>
@@ -40,23 +49,23 @@
<arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw"/>
<arg name="camera_info_topic" value="/camera/color/camera_info"/>
<arg name="visual_odometry" value="false"/>
<arg name="approx_sync" value="false"/>
<arg name="frame_id" value="camera_link"/>
<arg name="imu_topic" value="/rtabmap/imu_throttle"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
<!-- RTAB-Map: Stereo mode -->
<include unless="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
<arg name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/>
<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
<arg name="left_image_topic" value="/camera/infra1/image_rect_raw"/>
<arg name="right_image_topic" value="/camera/infra2/image_rect_raw"/>
<arg name="left_camera_info_topic" value="/camera/infra1/camera_info"/>
<arg name="right_camera_info_topic" value="/camera/infra2/camera_info"/>
<arg name="stereo" value="true"/>
<arg name="frame_id" value="camera_link"/>
<arg name="imu_topic" value="/rtabmap/imu_throttle"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<arg name="wait_imu_to_init" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rviz" value="$(arg rviz)"/>
launch/tests/test_k4a_cloud_only.launch
+70
View File
@@ -0,0 +1,70 @@
<launch>
<arg name="rtabmapviz" default="true"/>
<include file="$(find azure_kinect_ros_driver)/launch/driver.launch">
<arg name="point_cloud" value="false"/>
<arg name="rgb_point_cloud" value="false"/>
<arg name="fps" value="15"/>
</include>
<group ns="rtabmap">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_ros/point_cloud_xyz" output="screen">
<remap from="depth/image" to="/depth_to_rgb/image_raw"/>
<remap from="depth/camera_info" to="/depth_to_rgb/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="4"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="max_depth" type="double" value="5"/>
<param name="normal_k" type="int" value="10"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="voxel_cloud"/>
<param name="frame_id" type="string" value="camera_base"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<!-- Odom parameters -->
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.05"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="5000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="camera_base"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="voxel_cloud"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="RGBD/ProximityOdomGuess" type="string" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/MaxTranslation" type="string" value="0.5"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="camera_base"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="voxel_cloud"/>
</node>
</group>
</launch>
launch/tests/test_map_assembler.launch
+14
View File
@@ -0,0 +1,14 @@
<launch>
<!-- Example to assemble 3D point clouds from depth when rtabmap is using scan for 2d occupancy grid :
$ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
$ roslaunch rtabmap_ros test_map_assembler.launch
$ rosbag play -clock demo_mapping.bag
-->
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<remap from="mapData" to="mapData"/>
<param name="regenerate_local_grids" value="true"/>
</node>
</group>
</launch>
launch/tests/test_ouster.launch
+2
View File
@@ -87,6 +87,7 @@
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<remap from="imu" to="/os1_cloud_node/imu/data"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
@@ -107,6 +108,7 @@
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0.3"/>
launch/tests/test_ouster_gen2.launch
+177
View File
@@ -0,0 +1,177 @@
<launch>
<!--
Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
RVIZ: Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
PTP mode (synchronize timestamp with host computer time)
* Install:
$ sudo apt install linuxptp httpie
$ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
* Running:
(replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
(replace "eth0" by the network interface used to communicate with ouster)
$ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
$ sudo ptp4l -i eth0 -m -f ~/os.conf -S
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
-->
<arg name="use_sim_time" default="false"/>
<!-- Required: -->
<arg unless="$(arg use_sim_time)" name="sensor_hostname"/>
<arg unless="$(arg use_sim_time)" name="udp_dest"/>
<arg name="frame_id" default="os_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<arg name="assemble" default="false"/>
<arg name="ptp" default="false"/> <!-- See comments in header to start before launching the launch -->
<arg name="distortion_correction" default="false"/> <!-- Requires this pull request: https://github.com/ouster-lidar/ouster_example/pull/245 -->
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Ouster -->
<include unless="$(arg use_sim_time)" file="$(find ouster_ros)/ouster.launch">
<arg name="sensor_hostname" value="$(arg sensor_hostname)"/>
<arg name="udp_dest" value="$(arg udp_dest)"/>
<arg name="image" value="true"/>
<arg if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
<arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
<arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/>
<arg if="$(arg distortion_correction)" name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
</include>
<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="/os_cloud_node/imu"/>
<remap from="imu/data" to="/os_cloud_node/imu/data"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="/os_cloud_node/imu/data"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param name="wait_imu_to_init" type="bool" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.1"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<!-- RTAB-Map's parameters -->
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
<param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
<param name="RGBD/LocalRadius" type="string" value="2"/>
<param name="RGBD/AngularUpdate" type="string" value="0.05"/>
<param name="RGBD/LinearUpdate" type="string" value="0.05"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Optimizer/GravitySigma" type="string" value="0.5"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
</node>
<node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<remap from="cloud" to="/os_cloud_node/points"/>
<remap from="odom" to="odom"/>
<param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="" />
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/os_cloud_node/points"/>
</node>
</group>
</launch>
launch/tests/test_pointxyzrgb.launch
+14 -1
View File
@@ -19,10 +19,23 @@
<remap from="rgb/image" to="rgb/image_out"/>
<remap from="depth/image" to="depth/image_out"/>
<remap from="rgb/camera_info" to="rgb/camera_info_out"/>
<remap from="cloud" to="/voxel_cloud" />
<remap from="cloud" to="/voxel_cloud_xyzrgb" />
<param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/>
<param name="noise_filter_radius" type="double" value="0.05"/>
<param name="normal_k" type="int" value="6"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="depth/image_out"/>
<remap from="depth/camera_info" to="rgb/camera_info_out"/>
<remap from="cloud" to="/voxel_cloud_xyz" />
<param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/>
<param name="noise_filter_radius" type="double" value="0.05"/>
<param name="normal_k" type="int" value="6"/>
</node>
</group>
launch/tests/test_use_odom_features.launch
+53
View File
@@ -0,0 +1,53 @@
<launch>
<!-- Kinect: -->
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="true" />
</include>
<arg name="frame_id" default="camera_link"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<group ns="camera">
<!-- Use RGBD synchronization -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node>
<!-- RGB-D Odometry -->
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="keep_color" type="bool" value="true"/>
</node>
<!-- RTAB-Map -->
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="rgbd_image" to="odom_rgbd_image"/>
</node>
<!-- Visualisation -->
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
</node>
</group>
</launch>
launch/tests/test_velodyne.launch
+43 -19
View File
@@ -14,14 +14,26 @@
<arg name="use_imu" default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
<arg name="imu_topic" default="/imu/data"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="organize_cloud" default="false"/>
<arg name="scan_topic" default="/velodyne_points"/>
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<arg name="frame_id" default="velodyne"/>
<arg name="queue_size" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="loop_ratio" default="0.4"/> <!-- Set to 0.2 for kitti -->
<include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/>
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (car, kitti) -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
<include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
<arg if="$(arg scan_20_hz)" name="rpm" value="1200"/>
<arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
</include>
<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
@@ -33,9 +45,11 @@
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/velodyne_points"/>
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
@@ -45,23 +59,29 @@
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
<param if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
<param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
<param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.9"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
@@ -70,8 +90,10 @@
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<remap from="imu" to="$(arg imu_topic)"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
@@ -83,28 +105,27 @@
<param name="RGBD/LinearUpdate" type="string" value="0.05"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Mem/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0.2"/>
<param name="Icp/VoxelSize" type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below -->
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Iterations" type="string" value="$(arg iterations)"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/>
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
@@ -113,15 +134,18 @@
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="scan_cloud" to="/velodyne_points"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
<remap from="odom_info" to="odom_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<remap from="cloud" to="/velodyne_points"/>
<remap from="cloud" to="$(arg scan_topic)"/>
<remap from="odom" to="odom"/>
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
<param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" />
<param name="fixed_frame_id" type="string" value="" />
<param name="voxel_size" type="double" value="$(arg resolution)" />
<param name="queue_size" type="int" value="$(arg queue_size)" />
</node>
</group>