Added new demo turtlebot3_sim_rgbd_fake_scan_demo.launch.py

rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py

@@ -0,0 +1,143 @@
+# Example:
+#
+#   Bringup turtlebot3:
+#     $ export TURTLEBOT3_MODEL=waffle
+#     $ export LDS_MODEL=LDS-01
+#     $ ros2 launch turtlebot3_bringup robot.launch.py
+#
+#   SLAM:
+#     $ ros2 launch rtabmap_demos turtlebot3_rgbd_fake_scan.launch.py
+#
+#   Navigation (install nav2_bringup package):
+#     $ ros2 launch nav2_bringup navigation_launch.py
+#     $ ros2 launch nav2_bringup rviz_launch.py
+#
+#   Teleop:
+#     $ ros2 run turtlebot3_teleop teleop_keyboard
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+
+
+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_cloud':True,
+          'use_action_for_goal':True,
+          'scan_cloud_is_2d': True,
+          # RTAB-Map's parameters should be strings:
+          'Reg/Strategy':'1',
+          'Reg/Force3DoF':'true',
+          'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
+    }
+
+    remappings=[
+          ('rgb/image', '/camera/image_raw'),
+          ('rgb/camera_info', '/camera/camera_info'),
+          ('depth/image', '/camera/depth/image_raw'),
+          ('scan_cloud', 'assembled_cloud')]
+
+    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
+        Node(
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
+            remappings=remappings),
+
+        # Convert middle row of depth pixels to a fake laser scan
+        Node(
+            package='depthimage_to_laserscan', executable='depthimage_to_laserscan_node', output='screen',
+            parameters=[{
+                'use_sim_time':use_sim_time,
+                'range_max': 5.0
+            }],
+            remappings=[
+                ('depth', '/camera/depth/image_raw'),
+                ('depth_camera_info', '/camera/camera_info'),
+                ('scan', '/camera/scan')
+            ]),
+        
+        # Just to convert the fake laser scan to PointCloud2
+        Node(
+            package='rtabmap_util', executable='lidar_deskewing', output='screen',
+            parameters=[{'use_sim_time':use_sim_time,
+                         'fixed_frame_id': 'camera_link'}], # use camera frame
+            remappings=[
+                ('input_scan', '/camera/scan')
+            ]),
+        
+        # Assemble the fake laser scans using a circular buffer, then feed that cloud to rtabmap
+        Node(
+            package='rtabmap_util', executable='point_cloud_assembler', output='screen',
+            parameters=[{'use_sim_time':use_sim_time,
+                         'max_clouds': 20,
+                         'voxel_size': 0.05,
+                         'wait_for_transform': 1.0,
+                         'linear_update': 0.3,
+                         'angular_update': 0.5,
+                         'circular_buffer': True,
+                         'frame_id': 'base_link'}],
+            remappings=[
+                ('assembled_cloud', 'assembled_cloud'),
+                ('cloud', '/camera/scan/deskewed')
+            ]),
+
+        # 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),
+
+        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/camera_info'),
+                        ('cloud', '/camera/cloud')]),
+        Node(
+            package='rtabmap_util', executable='obstacles_detection', output='screen',
+            parameters=[parameters],
+            remappings=[('cloud', '/camera/cloud'),
+                        ('obstacles', '/camera/obstacles'),
+                        ('ground', '/camera/ground')]),
+    ])

rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py

@@ -0,0 +1,117 @@
+# Requirements:
+#   Install Turtlebot3 packages
+#   Modify turtlebot3_waffle SDF:
+#     1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
+#     2) Add
+#          <joint name="camera_rgb_optical_joint" type="fixed">
+#            <parent>camera_rgb_frame</parent>
+#            <child>camera_rgb_optical_frame</child>
+#            <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
+#            <axis>
+#              <xyz>0 0 1</xyz>
+#            </axis>
+#          </joint> 
+#     3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
+#     4) Add <link name="camera_rgb_frame"/>
+#     5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
+#     6) Change image width/height from 1920x1080 to 640x480
+# Example:
+#   $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py
+#
+#   Teleop:
+#     $ ros2 run turtlebot3_teleop teleop_keyboard
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+import os
+
+def launch_setup(context, *args, **kwargs):
+    if not 'TURTLEBOT3_MODEL' in os.environ:
+        os.environ['TURTLEBOT3_MODEL'] = 'waffle'
+
+    # Directories
+    pkg_turtlebot3_gazebo = get_package_share_directory(
+        'turtlebot3_gazebo')
+    pkg_nav2_bringup = get_package_share_directory(
+        'nav2_bringup')
+    pkg_rtabmap_demos = get_package_share_directory(
+        'rtabmap_demos')
+
+    world = LaunchConfiguration('world').perform(context)
+    
+    nav2_params_file = PathJoinSubstitution(
+        [FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
+    )
+
+    # Paths
+    gazebo_launch = PathJoinSubstitution(
+        [pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py'])
+    nav2_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
+    rviz_launch = PathJoinSubstitution(
+        [pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
+    rtabmap_launch = PathJoinSubstitution(
+        [pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py'])
+
+    # Includes
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([gazebo_launch]),
+        launch_arguments=[
+            ('x_pose', LaunchConfiguration('x_pose')),
+            ('y_pose', LaunchConfiguration('y_pose'))
+        ]
+    )
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([nav2_launch]),
+        launch_arguments=[
+            ('use_sim_time', 'true'),
+            ('params_file', nav2_params_file)
+        ]
+    )
+    rviz = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([rviz_launch])
+    )
+    rtabmap = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([rtabmap_launch]),
+        launch_arguments=[
+            ('localization', LaunchConfiguration('localization')),
+            ('use_sim_time', 'true')
+        ]
+    )
+    return [
+        # Nodes to launch
+        nav2,
+        rviz,
+        rtabmap,
+        gazebo
+    ]
+
+def generate_launch_description():
+    return LaunchDescription([
+        
+        # Launch arguments
+        DeclareLaunchArgument(
+            'localization', default_value='false',
+            description='Launch in localization mode.'),
+        
+        DeclareLaunchArgument(
+            'world', default_value='house',
+            choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
+            description='Turtlebot3 gazebo world.'),
+        
+        DeclareLaunchArgument(
+            'x_pose', default_value='-2.0',
+            description='Initial position of the robot in the simulator.'),
+        
+        DeclareLaunchArgument(
+            'y_pose', default_value='0.5',
+            description='Initial position of the robot in the simulator.'),
+
+        OpaqueFunction(function=launch_setup)
+    ])

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -130,7 +130,7 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 
 	if(maxClouds_==0 && assemblingTime_ ==0.0)
 	{
-		RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
+		RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_clouds or assembling_time parameters should be set!");
 		exit(-1);
 	}