mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
ROS2 various QOL updates (preparing new binary release) (#1225)
* Updating examples and README * updated main readme * odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning. * update readme * Added husky demo * converted demo_robot_mapping.launch to ros2 * Converted stereo_outdoor demo from ros1 to ros2 * Converted multi-session demo to ros2 * Converted demo_find_object launch to ros2 * renamed files * uniformized default qos, increased sync_queue_size default to 10 * Added vlp16 + imu example * Added husky 3D lidar demos * moved demos in subdir * Added vlp16+zed example * rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo. * forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo * pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces * Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them. * Added isaac sim demo * Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty. * Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo. * rtabmap_demos: added README with examples * Added TOC * bump version to 0.21.9
This commit is contained in:
matlabbe
@@ -0,0 +1,220 @@
|
||||
# Description:
|
||||
# In this example, we keep only minimal data to do LiDAR SLAM.
|
||||
#
|
||||
# Example:
|
||||
# Launch your lidar sensor:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# If an IMU is used, make sure TF between lidar/base frame and imu is
|
||||
# already calibrated. In this example, we assume the imu topic has
|
||||
# already the orientation estimated, it not, you can use
|
||||
# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
|
||||
# and set imu_topic to output topic of the filter.
|
||||
#
|
||||
# If a camera is used, make sure TF between lidar/base frame and camera is
|
||||
# already calibrated. To provide image data to this example, you should use
|
||||
# rtabmap_sync's rgbd_sync or stereo_sync node.
|
||||
#
|
||||
# Launch the example by adjusting the lidar topic and base frame:
|
||||
# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
frame_id = LaunchConfiguration('frame_id')
|
||||
|
||||
imu_topic = LaunchConfiguration('imu_topic')
|
||||
imu_used = imu_topic.perform(context) != ''
|
||||
|
||||
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
|
||||
rgbd_image_used = rgbd_image_topic.perform(context) != ''
|
||||
|
||||
voxel_size = LaunchConfiguration('voxel_size')
|
||||
voxel_size_value = float(voxel_size.perform(context))
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
lidar_topic = LaunchConfiguration('lidar_topic')
|
||||
lidar_topic_value = lidar_topic.perform(context)
|
||||
lidar_topic_deskewed = lidar_topic_value + "/deskewed"
|
||||
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'true' or localization == 'True'
|
||||
|
||||
fixed_frame_from_imu = False
|
||||
fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context)
|
||||
if not fixed_frame_id and imu_used:
|
||||
fixed_frame_from_imu = True
|
||||
fixed_frame_id = frame_id.perform(context) + "_stabilized"
|
||||
|
||||
if not fixed_frame_id:
|
||||
lidar_topic_deskewed = lidar_topic
|
||||
|
||||
# Rule of thumb:
|
||||
max_correspondence_distance = voxel_size_value * 10.0
|
||||
|
||||
shared_parameters = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'frame_id': frame_id,
|
||||
'qos': LaunchConfiguration('qos'),
|
||||
'approx_sync': rgbd_image_used,
|
||||
'wait_for_transform': 0.2,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': str(voxel_size_value),
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
}
|
||||
|
||||
icp_odometry_parameters = {
|
||||
'expected_update_rate': 15.0,
|
||||
'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': fixed_frame_id,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false',
|
||||
'Icp/CorrespondenceRatio': '0.01'
|
||||
}
|
||||
if imu_used:
|
||||
icp_odometry_parameters['wait_imu_to_init'] = True
|
||||
|
||||
rtabmap_parameters = {
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_odom_info': True,
|
||||
'subscribe_scan_cloud': True,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/CorrespondenceRatio': '0.2'
|
||||
}
|
||||
|
||||
arguments = []
|
||||
if localization:
|
||||
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
remappings = [('odom', 'icp_odom')]
|
||||
if imu_used:
|
||||
remappings.append(('imu', LaunchConfiguration('imu_topic')))
|
||||
else:
|
||||
remappings.append(('imu', 'imu_not_used'))
|
||||
if rgbd_image_used:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
|
||||
nodes = [
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[shared_parameters, icp_odometry_parameters],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)],
|
||||
arguments=arguments),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
|
||||
]
|
||||
|
||||
if fixed_frame_from_imu:
|
||||
# Create a stabilized base frame based on imu for lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'base_frame_id': frame_id,
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', imu_topic)]))
|
||||
|
||||
if fixed_frame_id:
|
||||
# Lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2}],
|
||||
remappings=[
|
||||
('input_cloud', lidar_topic)
|
||||
])
|
||||
)
|
||||
|
||||
return nodes
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulated clock.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing', default_value='true',
|
||||
description='Enable lidar deskewing.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id', default_value='velodyne',
|
||||
description='Base frame of the robot.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'fixed_frame_id', default_value='',
|
||||
description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar_topic', default_value='/velodyne_points',
|
||||
description='Name of the lidar PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'imu_topic', default_value='',
|
||||
description='IMU topic (ignored if empty).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_image_topic', default_value='',
|
||||
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user