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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-11-27 16:21:59 -08:00
parent ed248b4dbd 671c80dc3f
commit 5c3c269652
27 changed files with 1055 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_hector_mapping.launch
+1
View File
@@ -69,6 +69,7 @@
<remap from="odom_info" to="/rtabmap/odom_info"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="deskewing" type="string" value="true"/>
<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"/>
launch/demo/demo_isaac_carter_navigation.launch
+14 -1
View File
@@ -12,11 +12,18 @@
Note: carter_2dnav package can be copied to your catkin_ws from:
$ cp -r ~/.local/share/ov/pkg/isaac_sim-2021.2.0/ros_workspace/src/* ~/catkin_ws/src/.
Isaac Sim 2021:
For Lidar 3D Mode (lidar3d:=true), make sure in isaac sim that Carter robot is set with VLP16-like parameters (16 rings):
Under World -> Carter_ROS -> chassis_link -> carter_lidar -> highLod = True
-> verticalFov = 32
-> verticalResolution = 2
-> ROS_lidar -> pointCloudEnabled = True
Isaac Sim 2022 (Action Graph):
To enable all camera streams (right, depth left and depth right):
1. Stage view -> World-> Carter_ROS, right-click on ROS_Cameras -> Open graph
2. For all Branch nodes (green/red icon), select them, then check Compute Node -> Inputs -> Condition checkbox.
3D LIDAR: TODO: didn't find how to enable/publish it.
-->
<param name="use_sim_time" value="true" />
@@ -27,6 +34,7 @@
<arg name="lidar3d_grid3d" default="true" />
<arg name="camera" default="true" />
<arg name="stereo" default="false" /> <!-- RGB-D mode by default -->
<!-- Load Robot Description -->
<arg name="model" default="$(find carter_description)/urdf/carter.urdf"/>
@@ -46,7 +54,8 @@
<arg name="localization" value="$(arg localization)"/>
<arg if="$(arg lidar3d)" name="subscribe_scan_cloud" value="true"/>
<arg unless="$(arg lidar3d)" name="subscribe_scan" value="true"/>
<arg name="depth" value="$(arg camera)"/>
<arg name="stereo" value="$(eval camera and stereo)"/>
<arg name="depth" value="$(eval camera and not stereo)"/>
<arg name="subscribe_rgb" value="$(arg camera)"/>
<arg name="visual_odometry" value="false"/>
<arg name="odom_topic" value="/odom"/>
@@ -54,6 +63,10 @@
<arg name="depth_topic" value="/depth_left"/>
<arg name="rgb_topic" value="/rgb_left"/>
<arg name="camera_info_topic" value="/camera_info_left"/>
<arg name="left_image_topic" value="/rgb_left"/>
<arg name="left_camera_info_topic" value="/camera_info_left"/>
<arg name="right_image_topic" value="/rgb_right"/>
<arg name="right_camera_info_topic" value="/camera_info_right"/>
<arg name="scan_topic" value="/scan"/>
<arg name="scan_cloud_topic" value="/point_cloud"/>
<arg name="rgbd_sync" value="true"/>
launch/ros2/rtabmap.launch.py
+2
View File
@@ -262,6 +262,7 @@ def launch_setup(context, *args, **kwargs):
"map_frame_id": LaunchConfiguration('map_frame_id'),
"odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
"publish_tf": LaunchConfiguration('publish_tf_map'),
"initial_pose": LaunchConfiguration('initial_pose'),
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
"odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
@@ -404,6 +405,7 @@ def generate_launch_description():
DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'),
DeclareLaunchArgument('launch_prefix', default_value='', description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'),
DeclareLaunchArgument('output', default_value='screen', description='Control node output (screen or log).'),
DeclareLaunchArgument('initial_pose', default_value='', description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),
DeclareLaunchArgument('ground_truth_frame_id', default_value='', description='e.g., "world"'),
DeclareLaunchArgument('ground_truth_base_frame_id', default_value='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'),
launch/rtabmap.launch
+19 -4
View File
@@ -26,6 +26,7 @@
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg name="initial_pose" default=""/> <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc -->
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
@@ -96,8 +97,10 @@
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="subscribe_scan_descriptor" default="false"/>
<arg name="scan_descriptor_topic" default="/scan_descriptor"/>
<arg name="scan_deskewing" default="false"/>
<arg name="scan_deskewing_slerp" default="false"/>
<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="scan_cloud_filtered" default="$(arg scan_deskewing)"/> <!-- 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 -->
@@ -312,6 +315,17 @@
<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)"/>
<param name="deskewing" type="bool" value="$(arg scan_deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg scan_deskewing_slerp)"/>
</node>
<node if="$(eval not icp_odometry and scan_deskewing and subscribe_scan_cloud)" pkg="rtabmap_ros" type="lidar_deskewing" name="lidar_deskewing" clear_params="$(arg clear_params)" output="$(arg output)">
<param name="wait_for_transform" value="$(arg wait_for_transform)"/>
<param if="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg vo_frame_id)"/>
<param unless="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg odom_frame_id)"/>
<param name="slerp" value="$(arg scan_deskewing_slerp)"/>
<remap from="input_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="$(arg scan_cloud_topic)/deskewed" to="odom_filtered_input_scan"/>
</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)">
@@ -349,6 +363,7 @@
<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="initial_pose" type="string" value="$(arg initial_pose)"/>
<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)"/>
@@ -430,10 +445,10 @@
<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 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 if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap unless="$(eval icp_odometry or scan_cloud_filtered)" 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>
launch/tests/test_ouster.launch
+6 -5
View File
@@ -12,14 +12,15 @@
coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
-->
<!-- Required: -->
<arg name="os1_hostname"/>
<arg name="os1_udp_dest"/>
<arg name="use_sim_time" default="false"/>
<arg name="frame_id" default="os1_sensor"/>
<!-- Required: -->
<arg unless="$(arg use_sim_time)" name="os1_hostname"/>
<arg unless="$(arg use_sim_time)" name="os1_udp_dest"/>
<arg name="frame_id" default="os1_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Ouster -->
launch/tests/test_ouster_gen2.launch
+24 -11
View File
@@ -40,11 +40,14 @@
<arg name="frame_id" default="os_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/>
<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 -->
<arg name="imu_topic" default="/os_cloud_node/imu"/>
<arg name="scan_topic" default="/os_cloud_node/points"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
@@ -56,13 +59,12 @@
<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"/>
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
@@ -70,15 +72,26 @@
<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"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
<arg if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<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"/>
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<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"/>
@@ -116,8 +129,8 @@
<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"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters -->
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
@@ -158,7 +171,7 @@
</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="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/>
<param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="" />
@@ -170,7 +183,7 @@
<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"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
</node>
</group>
launch/tests/test_velodyne.launch
+27 -13
View File
@@ -14,7 +14,9 @@
<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="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
<arg name="organize_cloud" default="$(arg deskewing)"/> <!-- Should be organized if deskewing is enabled -->
<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"/>
@@ -24,7 +26,7 @@
<arg name="queue_size_odom" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="loop_ratio" default="0.2"/>
<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="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
@@ -34,7 +36,7 @@
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.8"/>
<arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry -->
<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 -->
@@ -46,9 +48,18 @@
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
</include>
<!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
<node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf">
<remap from="imu/data" to="$(arg imu_topic)"/>
<!-- IMU orientation estimation and publish tf -->
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="$(arg imu_topic)"/>
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
</node>
<node if="$(arg use_imu)" 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 if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
@@ -58,12 +69,14 @@
<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="deskewing" type="bool" value="$(arg deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg slerp)"/>
<param name="queue_size" type="int" value="$(arg queue_size_odom)"/>
<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"/>
<remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>
<remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
<param if="$(arg use_imu)" name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
@@ -92,8 +105,9 @@
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param unless="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
@@ -105,7 +119,7 @@
<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)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
@@ -127,7 +141,7 @@
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below -->
<param name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
@@ -151,12 +165,12 @@
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic)"/>
<remap if="$(arg deskewing)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg deskewing)" 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>
launch/tests/test_velodyne_t265_deskewing.launch
+184
View File
@@ -0,0 +1,184 @@
<!-- -->
<launch>
<!--
Hand-held 3D lidar mapping example using only a Velodyne PUCK and external odometry (using t265 as example).
Prerequisities: rtabmap should be built with libpointmatcher
We use T265 only for lidar deskewing and icp_odometry guess in this example.
Example:
$ roslaunch rtabmap_ros test_velodyne_t265_deskewing.launch
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
-->
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
<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="odom_frame_id" default="t265_odom_frame"/> <!-- input odometry: here we use T265 odometry, but it could be wheel odometry -->
<arg name="frame_id" default="velodyne"/> <!-- base frame of the robot: for this example, we use velodyne as base frame -->
<arg name="queue_size" default="10"/>
<arg name="queue_size_odom" default="1"/>
<arg name="loop_ratio" default="0.2"/>
<arg name="resolution" default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
<arg name="iterations" default="10"/>
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.6"/>
<!-- For FLOAM Odometry -->
<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 -->
<!-- Static transform between velodyne and T265: TODO: Adjust with real position/orientation!!! -->
<node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="T265_to_velodyne_tf" args="-0.01 0 0.055 0 0 0 t265_link velodyne 100"/>
<!-- Velodyne sensor VLP16 -->
<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="true"/> <!-- should be organized for deskewing -->
</include>
<!-- T265 -->
<group unless="$(arg use_sim_time)" ns="t265">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="device_type" value="t265"/>
<arg name="serial_no" value=""/>
<arg name="tf_prefix" value="t265"/>
<arg name="initial_reset" value="false"/>
<arg name="enable_fisheye1" value="false"/>
<arg name="enable_fisheye2" value="false"/>
<arg name="topic_odom_in" value=""/>
<arg name="calib_odom_file" value=""/>
<arg name="enable_pose" value="true"/>
</include>
</group>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg odom_frame_id)"/>
<param name="slerp" value="$(arg slerp)"/>
<remap from="input_cloud" to="$(arg scan_topic)"/>
</node>
<arg if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_frame_id)"/>
<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_odom)"/>
<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"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<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 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="$(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="$(arg key_frame_thr)"/>
<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="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</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"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<remap from="scan_cloud" to="assembled_cloud"/>
<!-- RTAB-Map's parameters -->
<param 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/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/LaserScanNormalK" type="string" value="20"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="$(arg grid_max_range)"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="$(arg ground_is_obstacle)"/>
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
<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="$(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="$(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="$(arg loop_ratio)"/>
</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="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="$(arg scan_topic_deskewed)"/>
<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="queue_size" type="int" value="$(arg queue_size)" />
</node>
</group>
</launch>