From 5d3b567fad7cf1d45759699bf8118b1af0bb8b44 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 29 Jun 2024 18:42:20 -0700 Subject: [PATCH] Added turtlebot4 demo launch files --- .../launch/turtlebot4_ignition_demo.launch.py | 73 +++++++++++ .../launch/turtlebot4_slam.launch.py | 113 ++++++++++++++++++ 2 files changed, 186 insertions(+) create mode 100644 rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py create mode 100644 rtabmap_demos/launch/turtlebot4_slam.launch.py diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py new file mode 100644 index 00000000..c083d349 --- /dev/null +++ b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py @@ -0,0 +1,73 @@ +# Example: +# 1) Launch simulator (turtlebot4, nav2 and rtabmap): +# $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py +# +# 2) Click on "Play" button on bottom-right of gazebo. +# +# 3) Click on double points ".." button on top right next to power button to undock. +# +# 4) Teleop the robot: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# +# 5) Send goals with RVIZ's "Nav2 Goal" button in action bar. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +ARGUMENTS = [ + DeclareLaunchArgument('rviz', default_value='true', + choices=['true', 'false'], description='Start rviz.'), + DeclareLaunchArgument('rtabmap_viz', default_value='true', + choices=['true', 'false'], description='Start rtabmap_viz.'), + DeclareLaunchArgument('localization', default_value='false', + choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'), + DeclareLaunchArgument('nav2', default_value='true', + choices=['true', 'false'], description='Start nav2.'), + DeclareLaunchArgument('world', default_value='warehouse', + description='Ignition World'), +] + +def generate_launch_description(): + # Directories + pkg_turtlebot4_ignition_bringup = get_package_share_directory( + 'turtlebot4_ignition_bringup') + pkg_rtabmap_demos = get_package_share_directory( + 'rtabmap_demos') + + # Paths + ignition_launch = PathJoinSubstitution( + [pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_ignition.launch.py']) + rtabmap_launch = PathJoinSubstitution( + [pkg_rtabmap_demos, 'launch', 'turtlebot4_slam.launch.py']) + + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ignition_launch]), + launch_arguments=[ + ('world', LaunchConfiguration('world')), + ('slam', 'false'), + ('localization', 'false'), + ('nav2', LaunchConfiguration('nav2')), + ('rviz', LaunchConfiguration('rviz')) + ] + ) + + rtabmap = IncludeLaunchDescription( + PythonLaunchDescriptionSource([rtabmap_launch]), + launch_arguments=[ + ('rtabmap_viz', LaunchConfiguration('rtabmap_viz')), + ('localization', LaunchConfiguration('localization')), + ('qos', '2'), + ('use_sim_time', 'true') + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(rtabmap) # put it first so that localization arg is not overwritten by the same used by ignition + ld.add_action(ignition) + return ld \ No newline at end of file diff --git a/rtabmap_demos/launch/turtlebot4_slam.launch.py b/rtabmap_demos/launch/turtlebot4_slam.launch.py new file mode 100644 index 00000000..58dac4aa --- /dev/null +++ b/rtabmap_demos/launch/turtlebot4_slam.launch.py @@ -0,0 +1,113 @@ +# Example with gazebo: +# 1) Launch simulator (turtlebot4 and nav2): +# $ ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py slam:=false nav2:=true rviz:=true +# +# 2) Launch SLAM: +# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true qos:=2 +# OR +# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" qos:=2 use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true +# +# 3) Click on "Play" button on bottom-right of gazebo. +# +# 4) Click on double points ".." button on top right next to power button to undock. +# +# 5) Teleop the robot: +# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard +# +# 6) Send goals with RVIZ's "Nav2 Goal" button in action bar: + +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') + qos = LaunchConfiguration('qos') + localization = LaunchConfiguration('localization') + + icp_parameters={ + 'odom_frame_id':'icp_odom', + 'guess_frame_id':'odom', + 'qos':qos + } + + rtabmap_parameters={ + 'subscribe_rgbd':True, + 'subscribe_scan':True, + 'use_action_for_goal':True, + 'qos_scan':qos, + 'qos_image':qos, + 'qos_imu':qos, + # RTAB-Map's parameters should be strings: + 'Mem/NotLinkedNodesKept':'false' + } + + # Shared parameters between different nodes + shared_parameters={ + 'frame_id':'base_link', + 'use_sim_time':use_sim_time, + # RTAB-Map's parameters should be strings: + 'Reg/Strategy':'1', + 'Reg/Force3DoF':'true', + 'Mem/NotLinkedNodesKept':'false' + } + + remappings=[ + ('odom', 'icp_odom'), + ('rgb/image', '/oakd/rgb/preview/image_raw'), + ('rgb/camera_info', '/oakd/rgb/preview/camera_info'), + ('depth/image', '/oakd/rgb/preview/depth')] + + return LaunchDescription([ + + # Launch arguments + DeclareLaunchArgument( + 'use_sim_time', default_value='false', choices=['true', 'false'], + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'qos', default_value='0', + description='QoS used for input sensor topics'), + + DeclareLaunchArgument( + 'localization', default_value='false', choices=['true', 'false'], + description='Launch rtabmap in localization mode (a map should have been already created).'), + + # Nodes to launch + Node( + package='rtabmap_sync', executable='rgbd_sync', output='screen', + parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}], + remappings=remappings), + + Node( + package='rtabmap_odom', executable='icp_odometry', output='screen', + parameters=[icp_parameters, shared_parameters], + remappings=remappings, + arguments=["--ros-args", "--log-level", 'icp_odometry:=warn']), + + # SLAM Mode: + Node( + condition=UnlessCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings, + arguments=['-d']), + + # Localization mode: + Node( + condition=IfCondition(localization), + package='rtabmap_slam', executable='rtabmap', output='screen', + parameters=[rtabmap_parameters, shared_parameters, + {'Mem/IncrementalMemory':'False', + 'Mem/InitWMWithAllNodes':'True'}], + remappings=remappings), + + Node( + package='rtabmap_viz', executable='rtabmap_viz', output='screen', + parameters=[rtabmap_parameters, shared_parameters], + remappings=remappings), + ])