feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,101 @@
|
||||
# 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.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_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/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
# Nodes to 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/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')]),
|
||||
])
|
||||
@@ -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')]),
|
||||
])
|
||||
@@ -0,0 +1,112 @@
|
||||
# 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_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':True,
|
||||
'use_action_for_goal':True,
|
||||
# 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/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw')]
|
||||
|
||||
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),
|
||||
|
||||
# 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')]),
|
||||
])
|
||||
@@ -0,0 +1,100 @@
|
||||
# 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_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, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
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'))
|
||||
|
||||
return [
|
||||
# Nodes to 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='true',
|
||||
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)
|
||||
])
|
||||
@@ -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_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.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)
|
||||
])
|
||||
+117
@@ -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)
|
||||
])
|
||||
@@ -0,0 +1,119 @@
|
||||
# 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
|
||||
# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans
|
||||
# hitting the robot itself
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_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_scan_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_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)
|
||||
])
|
||||
@@ -0,0 +1,161 @@
|
||||
# Requirements:
|
||||
# Install Turtlebot3 packages
|
||||
# Modify turtlebot3_waffle SDF:
|
||||
# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
|
||||
# 2) We can increase min scan range from 0.12 to 0.2 to avoid having scans
|
||||
# hitting the robot itself
|
||||
#
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_sim_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_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
world_name = LaunchConfiguration('world').perform(context)
|
||||
|
||||
icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
|
||||
icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
|
||||
if icp_odometry:
|
||||
# modified nav2 params to use icp_odom instead odom frame
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml']
|
||||
)
|
||||
else:
|
||||
# original nav2 params
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml']
|
||||
)
|
||||
|
||||
# Paths
|
||||
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_scan.launch.py'])
|
||||
|
||||
# To use ICP odometry, we should increase clock rate of gazebo, we copied content of
|
||||
# turtlebot3_gazebo/launch/turtlebot3_world.launch here
|
||||
launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
|
||||
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
|
||||
|
||||
world = os.path.join(
|
||||
get_package_share_directory('turtlebot3_gazebo'),
|
||||
'worlds',
|
||||
f'turtlebot3_{world_name}.world'
|
||||
)
|
||||
|
||||
import tempfile
|
||||
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file:
|
||||
clock_override_file.write("---\n"+
|
||||
"gazebo:\n"+
|
||||
" ros__parameters:\n"+
|
||||
" publish_rate: 100.0")
|
||||
|
||||
gzserver_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'world': world,
|
||||
'params_file': clock_override_file.name}.items()
|
||||
)
|
||||
|
||||
gzclient_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
|
||||
)
|
||||
)
|
||||
|
||||
robot_state_publisher_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
|
||||
),
|
||||
launch_arguments={'use_sim_time': 'true'}.items()
|
||||
)
|
||||
|
||||
spawn_turtlebot_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'x_pose': LaunchConfiguration('x_pose'),
|
||||
'y_pose': LaunchConfiguration('y_pose')
|
||||
}.items()
|
||||
)
|
||||
|
||||
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,
|
||||
gzserver_cmd,
|
||||
gzclient_cmd,
|
||||
robot_state_publisher_cmd,
|
||||
spawn_turtlebot_cmd
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='world',
|
||||
choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
|
||||
description='Turtlebot3 gazebo world.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'icp_odometry', default_value='false',
|
||||
description='Launch ICP odometry on top of wheel odometry.'),
|
||||
|
||||
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)
|
||||
])
|
||||
Reference in New Issue
Block a user