From bc8123089e3f28aadadc61bcae050df25ff20039 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 4 Jul 2025 16:59:24 -0700 Subject: [PATCH] Added new demo turtlebot3_sim_rgbd_fake_scan_demo.launch.py --- .../turtlebot3_rgbd_fake_scan.launch.py | 143 ++++++++++++++++++ ...rtlebot3_sim_rgbd_fake_scan_demo.launch.py | 117 ++++++++++++++ .../src/nodelets/point_cloud_assembler.cpp | 2 +- 3 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py create mode 100644 rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py new file mode 100644 index 00000000..1ccacd65 --- /dev/null +++ b/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')]), + ]) diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py new file mode 100644 index 00000000..314ad3eb --- /dev/null +++ b/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 +# +# camera_rgb_frame +# camera_rgb_optical_frame +# 0 0 0 -1.57079632679 0 -1.57079632679 +# +# 0 0 1 +# +# +# 3) Rename to +# 4) Add +# 5) Change to +# 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) + ]) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 2edf5509..99b64ccf 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/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); }