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

Added lidar_deskewing node and nodelet. Added "deskewing" option for icp_odometry. Updated velodyne and ouster examples with deskewing option. Added velodyne+T265 deskewing example.

Browse Source
This commit is contained in:
matlabbe
2022-10-16 21:36:11 -07:00
parent cf50e93195
commit 44bbaa2cef
15 changed files with 931 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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>