feat(rtabmap_ros): Add demo configurations, launch files, and navigation parameters for agvpro

2025-09-28 19:18:59 +08:00
parent 049d3995ba
commit cf17bb899b
10 changed files with 3690 additions and 0 deletions
@@ -0,0 +1,109 @@
+# Requirements:
+#   Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+#     $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#     $ ros2 launch orbbec_camera gemini2.launch.py
+#     $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
+#     $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.actions import SetParameter
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    localization = LaunchConfiguration('localization')
+
+    parameters={
+          'frame_id':'base_footprint',
+          'odom_frame_id':'odom',
+          'odom_tf_linear_variance':0.001,
+          'odom_tf_angular_variance':0.001,
+          'subscribe_rgbd':True,
+          'subscribe_scan':True,
+          'approx_sync':True,
+          'sync_queue_size': 10,
+          # RTAB-Map's internal parameters should be strings
+          'RGBD/NeighborLinkRefining': 'true',    # Do odometry correction with consecutive laser scans
+          'RGBD/ProximityBySpace':     'true',    # Local loop closure detection (using estimated position) with locations in WM
+          'RGBD/ProximityByTime':      'false',   # Local loop closure detection with locations in STM
+          'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together.
+          'Reg/Strategy':              '1',       # 0=Visual, 1=ICP, 2=Visual+ICP
+          'Vis/MinInliers':            '12',      # 3D visual words minimum inliers to accept loop closure
+          'RGBD/OptimizeFromGraphEnd': 'false',   # Optimize graph from initial node so /map -> /odom transform will be generated
+          'RGBD/OptimizeMaxError':     '4',       # Reject any loop closure causing large errors (>3x link's covariance) in the map
+          'Reg/Force3DoF':             'true',    # 2D SLAM
+          'Grid/FromDepth':            'false',   # Create 2D occupancy grid from laser scan
+          'Mem/STMSize':               '30',      # increased to 30 to avoid adding too many loop closures on just seen locations
+          'RGBD/LocalRadius':          '5',       # limit length of proximity detections
+          'Icp/CorrespondenceRatio':   '0.2',     # minimum scan overlap to accept loop closure
+          'Icp/PM':                    'false',
+          'Icp/PointToPlane':          'false',
+          'Icp/MaxCorrespondenceDistance': '0.15',
+          'Icp/VoxelSize':             '0.05'
+    }
+    
+    remappings=[
+         ('rgb/image',       '/camera/color/image_raw'),
+         ('depth/image',     '/camera/depth/image_raw'),
+         ('rgb/camera_info', '/camera/color/camera_info'),
+         ('scan',            '/scan')]
+    
+    config_rviz = os.path.join(
+        get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
+    )
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument('rtabmap_viz',  default_value='false',  description='Launch RTAB-Map UI (optional).'),
+        DeclareLaunchArgument('rviz',         default_value='true', description='Launch RVIZ (optional).'),
+        DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
+        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz,  description='Configuration path of rviz2.'),
+
+        SetParameter(name='use_sim_time', value=False),
+
+        # Nodes to launch
+        Node(
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=[parameters,
+              {
+            #    'rgb_image_transport':'compressed',
+            #    'depth_image_transport':'compressedDepth',
+               'approx_sync_max_interval': 0.1}],
+            remappings=remappings),
+        
+        # SLAM mode:
+        Node(
+            condition=UnlessCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters],
+            remappings=remappings,
+            arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
+            
+        # Localization mode:
+        Node(
+            condition=IfCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters,
+              {'Mem/IncrementalMemory':'False',
+               'Mem/InitWMWithAllNodes':'True'}],
+            remappings=remappings),
+
+        # Visualization:
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
+            parameters=[parameters],
+            remappings=remappings),
+        Node(
+            package='rviz2', executable='rviz2', name="rviz2", output='screen',
+            condition=IfCondition(LaunchConfiguration("rviz")),
+            arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+    ])
@@ -0,0 +1,132 @@
+# Requirements:
+#   Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+#   Bringup agvpro:
+#     $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#
+#   Bringup orbbec gemini2 camera:
+#     $ ros2 launch orbbec_camera gemini2.launch.py
+#
+#   SLAM:
+#   $ ros2 launch rtabmap_demos agvpro_rgbd_demo.launch.py
+#
+#   Teleop:
+#     $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    localization = LaunchConfiguration('localization')
+
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_depth':True,
+          'use_action_for_goal':True,
+          'Reg/Force3DoF':'true',
+          'Grid/RayTracing':'true', # Fill empty space
+          'Grid/3D':'false', # Use 2D occupancy
+          'Grid/RangeMax':'3',
+          'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
+          'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+          'Grid/MaxObstacleHeight':'0.4',  # All points over 1 meter are ignored
+          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+    }
+
+    remappings=[
+          ('rgb/image',       '/camera/color/image_raw'),
+          ('rgb/camera_info', '/camera/color/camera_info'),
+          ('depth/image',     '/camera/depth/image_raw')]
+    
+    # Directories
+    pkg_nav2_bringup = get_package_share_directory(
+        'nav2_bringup')
+
+    nav2_params_file = PathJoinSubstitution(
+        [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_rgbd_nav2_params.yaml']
+    )
+
+    # Paths
+    nav2_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+    rviz_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+
+        # Nodes to launch
+        
+        # Navigation2
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([nav2_launch]),
+            launch_arguments=[
+                ('use_sim_time', 'false'),
+                ('params_file', nav2_params_file)
+            ]
+        ),
+
+        # RViz
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([rviz_launch])
+        ),
+
+        # SLAM mode:
+        Node(
+            condition=UnlessCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters],
+            remappings=remappings,
+            arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
+            
+        # Localization mode:
+        Node(
+            condition=IfCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters,
+              {'Mem/IncrementalMemory':'False',
+               'Mem/InitWMWithAllNodes':'True'}],
+            remappings=remappings),
+
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=[parameters],
+            remappings=remappings),
+        
+        # Obstacle detection with the camera for nav2 local costmap.
+        # First, we need to convert depth image to a point cloud.
+        # Second, we segment the floor from the obstacles.
+        Node(
+            package='rtabmap_util', executable='point_cloud_xyz', output='screen',
+            parameters=[{'decimation': 2,
+                         'max_depth': 3.0,
+                         'voxel_size': 0.02}],
+            remappings=[('depth/image', '/camera/depth/image_raw'),
+                        ('depth/camera_info', '/camera/depth/camera_info'),
+                        ('cloud', '/camera/depth_registered/points')]),
+        Node(
+            package='rtabmap_util', executable='obstacles_detection', output='screen',
+            parameters=[parameters],
+            remappings=[('cloud', '/camera/depth_registered/points'),
+                        ('obstacles', '/camera/obstacles'),
+                        ('ground', '/camera/ground')]),
+    ])
@@ -0,0 +1,161 @@
+# Requirements:
+#   Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+#   Bringup agvpro:
+#     $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#
+#   Bringup orbbec gemini2 camera:
+#     $ ros2 launch orbbec_camera gemini2.launch.py
+#
+#   SLAM:
+#   $ ros2 launch rtabmap_demos agvpro_rgbd_scan.launch.py
+#
+#   Teleop:
+#     $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+import os
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    localization = LaunchConfiguration('localization')
+
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_rgbd':True,
+          'subscribe_scan':True,
+          'use_action_for_goal':True,
+          'approx_sync':True,
+          'sync_queue_size': 10,
+          # RTAB-Map's parameters should be strings:
+          'Reg/Strategy':'1',
+          'Reg/Force3DoF':'true',
+          'RGBD/NeighborLinkRefining':'True',
+          'Grid/RayTracing':'true', # Fill empty space
+          'Grid/3D':'false', # Use 2D occupancy
+          'Grid/RangeMax':'3',
+          'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
+          'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map
+          'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
+          'Grid/MaxObstacleHeight':'0.4',  # All points over 1 meter are ignored
+          'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
+          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+    }
+
+    remappings=[
+         ('rgb/image',       '/camera/color/image_raw'),
+         ('depth/image',     '/camera/depth/image_raw'),
+         ('rgb/camera_info', '/camera/color/camera_info'),
+         ('scan',            '/scan')]
+
+    # Directories
+    pkg_nav2_bringup = get_package_share_directory(
+        'nav2_bringup')
+
+    nav2_params_file = PathJoinSubstitution(
+        [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_rgbd_scan_nav2_params.yaml']
+    )
+
+    # Paths
+    nav2_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+
+        DeclareLaunchArgument(
+            'rtabmap_viz', default_value='false',
+            description='Launch RTAB-Map UI (optional).'),
+        
+        DeclareLaunchArgument(
+            'rviz', default_value='true',
+            description='Launch RVIZ (optional).'), 
+        
+        DeclareLaunchArgument(
+            'rviz_cfg', default_value=os.path.join(
+                get_package_share_directory('rtabmap_demos'), 'config', 'rtabmap_rgbd_scan.rviz'),
+            description='Configuration path of rviz2.'),
+
+        # Nodes to launch
+        # Navigation2
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([nav2_launch]),
+            launch_arguments=[
+                ('use_sim_time', 'false'),
+                ('params_file', nav2_params_file)
+            ]
+        ),
+
+        Node(
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=[parameters,
+              {
+                'use_sim_time':use_sim_time,
+                'approx_sync_max_interval': 0.1}],
+            remappings=remappings),
+
+        # SLAM Mode:
+        Node(
+            condition=UnlessCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters],
+            remappings=remappings,
+            arguments=['-d']),
+            
+        # Localization mode:
+        Node(
+            condition=IfCondition(localization),
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters,
+              {'Mem/IncrementalMemory':'False',
+               'Mem/InitWMWithAllNodes':'True'}],
+            remappings=remappings),
+
+        # Visualization:
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
+            parameters=[parameters],
+            remappings=remappings),
+        Node(
+            package='rviz2', executable='rviz2', name="rviz2", output='screen',
+            condition=IfCondition(LaunchConfiguration("rviz")),
+            arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
+        
+        # Obstacle detection with the camera for nav2 local costmap.
+        # First, we need to convert depth image to a point cloud.
+        # Second, we segment the floor from the obstacles.
+        Node(
+            package='rtabmap_util', executable='point_cloud_xyz', output='screen',
+            parameters=[{'decimation': 2,
+                         'max_depth': 3.0,
+                         'voxel_size': 0.02}],
+            remappings=[('depth/image', '/camera/depth/image_raw'),
+                        ('depth/camera_info', '/camera/depth/camera_info'),
+                        ('cloud', '/camera/depth_registered/points')]),
+        Node(
+            package='rtabmap_util', executable='obstacles_detection', output='screen',
+            parameters=[parameters],
+            remappings=[('cloud', '/camera/depth_registered/points'),
+                        ('obstacles', '/camera/obstacles'),
+                        ('ground', '/camera/ground')]),
+    ])
@@ -0,0 +1,133 @@
+# Requirements:
+#   Compile agv_pro_base and agv_pro_bringup packages
+#
+# Example:
+#   Bringup agvpro:
+#     $ ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
+#
+#   SLAM:
+#   $ ros2 launch rtabmap_demos agvpro_scan_demo.launch.py
+#
+#   Teleop:
+#     $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction,IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+def launch_setup(context, *args, **kwargs):
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    localization = LaunchConfiguration('localization').perform(context)
+    localization = localization == 'True' or localization == 'true'
+    icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
+    icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
+    
+    parameters={
+          'frame_id':'base_footprint',
+          'use_sim_time':use_sim_time,
+          'subscribe_depth':False,
+          'subscribe_rgb':False,
+          'subscribe_scan':True,
+          'approx_sync':True,
+          'use_action_for_goal':True,
+          'Reg/Strategy':'1',
+          'Reg/Force3DoF':'true',
+          'RGBD/NeighborLinkRefining':'True',
+          'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
+          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+    }
+    arguments = []
+    if localization:
+        parameters['Mem/IncrementalMemory'] = 'False'
+        parameters['Mem/InitWMWithAllNodes'] = 'True'
+    else:
+        arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
+               
+    remappings=[
+          ('scan', '/scan')]
+    if icp_odometry:
+        remappings.append(('odom', 'icp_odom'))
+        # modified nav2 params to use icp_odom instead odom frame
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('rtabmap_demos'), 'params', 'agvpro_scan_nav2_params.yaml']
+        )
+    else:
+        # original nav2 params
+        nav2_params_file = PathJoinSubstitution(
+            [FindPackageShare('agv_pro_navigation2'), 'param', 'agvpro.yaml']
+        )
+
+    # Directories
+    pkg_nav2_bringup = get_package_share_directory(
+        'nav2_bringup')
+
+    # Paths
+    nav2_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+    rviz_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
+    
+    return [
+        # Nodes to launch
+
+        # Navigation2
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([nav2_launch]),
+            launch_arguments=[
+                ('use_sim_time', 'false'),
+                ('params_file', nav2_params_file)
+            ]
+        ),
+
+        # RViz
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([rviz_launch])
+        ),
+
+        # ICP odometry (optional)
+        Node(
+            condition=IfCondition(LaunchConfiguration('icp_odometry')),
+            package='rtabmap_odom', executable='icp_odometry', output='screen',
+            parameters=[parameters, 
+                        {'odom_frame_id':'icp_odom',
+                         'guess_frame_id':'odom'}],
+            remappings=remappings),
+        
+        # SLAM:
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[parameters],
+            remappings=remappings,
+            arguments=arguments),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=[parameters],
+            remappings=remappings),
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        # Launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'icp_odometry', default_value='false',
+            description='Launch ICP odometry on top of wheel odometry.'),
+
+        OpaqueFunction(function=launch_setup)
+    ])