feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,87 @@
|
||||
|
||||
# Requires installed https://github.com/chvmp/champ/tree/ros2
|
||||
#
|
||||
# Example:
|
||||
# 1) Launch simulator (gazebo, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos champ_sim_vslam.launch.py
|
||||
#
|
||||
# Note that the first time we launch gazebo, it may take a
|
||||
# while to download all assets. You may need to restart the
|
||||
# launch to make sure all nodes are started after the sim is ready.
|
||||
#
|
||||
# 2) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 launch champ_teleop teleop.launch.py
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, 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):
|
||||
|
||||
sim_launch_path = PathJoinSubstitution(
|
||||
[FindPackageShare('champ_config'), 'launch', 'gazebo.launch.py']
|
||||
)
|
||||
|
||||
gz_pkg_share = FindPackageShare(package="champ_gazebo").find("champ_gazebo")
|
||||
|
||||
champ_vslam = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'launch', 'champ', 'champ_vslam.launch.py']
|
||||
)
|
||||
|
||||
rviz = LaunchConfiguration('rviz').perform(context)
|
||||
world = LaunchConfiguration('world').perform(context)
|
||||
|
||||
return [
|
||||
TimerAction(
|
||||
actions = [
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(champ_vslam),
|
||||
launch_arguments={
|
||||
'use_sim_time': 'true',
|
||||
'rviz': rviz,
|
||||
'rtabmap_viz': LaunchConfiguration('rtabmap_viz'),
|
||||
'localization': LaunchConfiguration('localization'),
|
||||
}.items()
|
||||
)], period = 5.0), # Wait 5 sec to make sure simulator is ready
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(sim_launch_path),
|
||||
launch_arguments={'rviz': 'false',
|
||||
'world': os.path.join(gz_pkg_share, f"worlds/{world}.world")}.items()
|
||||
),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rviz',
|
||||
default_value='true',
|
||||
description='Run rviz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rtabmap_viz',
|
||||
default_value='true',
|
||||
description='Run rtabmap_viz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='playground',
|
||||
choices=['outdoor', 'playground'],
|
||||
description='Champ gazebo world.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,179 @@
|
||||
|
||||
# Similar to gazebo example on https://github.com/chvmp/champ/tree/ros2, we can do:
|
||||
#
|
||||
# Run the Gazebo environment:
|
||||
# $ ros2 launch champ_config gazebo.launch.py
|
||||
#
|
||||
# Run Nav2's navigation and rtabmap:
|
||||
# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true
|
||||
#
|
||||
# When a map is already created using command above, we can re-launch in localization-only mode with:
|
||||
# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true localization:=true
|
||||
#
|
||||
|
||||
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.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
navigation_launch_path = PathJoinSubstitution(
|
||||
[FindPackageShare('nav2_bringup'), 'launch', 'navigation_launch.py']
|
||||
)
|
||||
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'champ_nav2_params.yaml']
|
||||
)
|
||||
|
||||
rviz_config_path = PathJoinSubstitution(
|
||||
[FindPackageShare('champ_navigation'), 'rviz', 'navigation.rviz']
|
||||
)
|
||||
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
|
||||
# With the simulator, the imu is not published fast enough
|
||||
# and have a huge delay, disabling imu usage from VO
|
||||
use_imu = use_sim_time.perform(context) in ["false", "False"]
|
||||
|
||||
vslam_params ={
|
||||
'frame_id':'base_link',
|
||||
'guess_frame_id':'odom',
|
||||
'approx_sync': False,
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
'use_action_for_goal':True,
|
||||
'wait_imu_to_init': use_imu,
|
||||
'wait_for_transform': 0.5,
|
||||
# RTAB-Map's parameters should be strings
|
||||
'Grid/DepthDecimation': '1',
|
||||
'Grid/RangeMax': '2',
|
||||
'GridGlobal/MinSize': '20',
|
||||
'Grid/MinClusterSize': '20',
|
||||
'Grid/MaxObstacleHeight': '2',
|
||||
'Odom/ResetCountdown': '2', # sim is very flaky
|
||||
'Kp/RoiRatios': '0.0 0.0 0.0 0.4' # ignore ground for loop closure detection (sim uses a very repetitive texture)
|
||||
}
|
||||
|
||||
vslam_remappings=[('imu', 'imu/data/filtered'),
|
||||
('odom', 'vo')]
|
||||
|
||||
return [
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(navigation_launch_path),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_params_file
|
||||
}.items()
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', rviz_config_path],
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
parameters=[{'use_sim_time': use_sim_time}]
|
||||
),
|
||||
|
||||
# compute imu orientation
|
||||
Node(
|
||||
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
|
||||
parameters=[{
|
||||
'use_mag':False,
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[
|
||||
('imu/data_raw', 'imu/data'),
|
||||
('imu/data', 'imu/data/filtered')
|
||||
]),
|
||||
|
||||
# VSLAM nodes:
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[vslam_params],
|
||||
remappings=[('rgb/image', '/camera/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=[vslam_params, {'odom_frame_id': 'vo'}],
|
||||
remappings=vslam_remappings,
|
||||
arguments=["--ros-args", "--log-level", 'info']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[vslam_params],
|
||||
remappings=vslam_remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[vslam_params,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=vslam_remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
||||
parameters=[vslam_params],
|
||||
remappings=vslam_remappings),
|
||||
|
||||
# Compute ground/obstacle clouds for nav2 voxel layers
|
||||
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/cloud')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='obstacles_detection', output='screen',
|
||||
parameters=[vslam_params],
|
||||
remappings=[('cloud', '/camera/cloud'),
|
||||
('obstacles', '/camera/obstacles'),
|
||||
('ground', '/camera/ground')]),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
name='use_sim_time',
|
||||
default_value='false',
|
||||
description='Enable use_sime_time to true'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rviz',
|
||||
default_value='false',
|
||||
description='Run rviz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rtabmap_viz',
|
||||
default_value='false',
|
||||
description='Run rtabmap_viz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,129 @@
|
||||
# Requirements:
|
||||
# find_object_2d package installed
|
||||
# Download rosbag:
|
||||
# * demo_find_object.db3: https://drive.google.com/file/d/1web54yQkxeGFr2UwOjKeoajGGDm0fZXT/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos find_object_demo.launch.py
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play demo_find_object.db3 --clock
|
||||
#
|
||||
|
||||
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
|
||||
'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP
|
||||
'Reg/Force3DoF': 'true', # 2D SLAM
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/data_throttled_image'),
|
||||
('depth/image', '/camera/data_throttled_image_depth'),
|
||||
('rgb/camera_info', '/camera/data_throttled_camera_info'),
|
||||
('scan', '/base_scan')]
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
|
||||
)
|
||||
|
||||
config_find_object = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'find_object.ini'
|
||||
)
|
||||
|
||||
data_find_object = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'data', 'books'
|
||||
)
|
||||
|
||||
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=True),
|
||||
|
||||
# Nodes to launch
|
||||
|
||||
# Uncompress images for find_object
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_rgb', output='screen',
|
||||
arguments=['compressed', 'raw'],
|
||||
remappings=[('in/compressed', '/camera/data_throttled_image/compressed'),
|
||||
('out', '/camera/data_throttled_image')]),
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_depth', output='screen',
|
||||
arguments=['compressedDepth', 'raw'],
|
||||
remappings=[('in/compressedDepth', '/camera/data_throttled_image_depth/compressedDepth'),
|
||||
('out', '/camera/data_throttled_image_depth')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[parameters,
|
||||
{'approx_sync_max_interval': 0.02}],
|
||||
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")]]),
|
||||
|
||||
# Find-Object
|
||||
Node(
|
||||
package='find_object_2d', executable='find_object_2d', output='screen',
|
||||
parameters=[{'gui': True,
|
||||
'subscribe_depth': True,
|
||||
'settings_path': config_find_object,
|
||||
'objects_path': data_find_object}],
|
||||
remappings=[('rgb/image_rect_color', '/camera/data_throttled_image'),
|
||||
('depth_registered/image_raw', '/camera/data_throttled_image_depth'),
|
||||
('depth_registered/camera_info', '/camera/data_throttled_camera_info')]),
|
||||
])
|
||||
@@ -0,0 +1,104 @@
|
||||
#
|
||||
# Requirements:
|
||||
# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
|
||||
# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml
|
||||
# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
|
||||
# <horizontal_fov>1.047</horizontal_fov>
|
||||
# <image>
|
||||
# <width>320</width>
|
||||
# <height>240</height>
|
||||
# </image>
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py robot_ns:=a200_0000
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 3) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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
|
||||
|
||||
import os
|
||||
|
||||
ARGUMENTS = [
|
||||
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('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
DeclareLaunchArgument('robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_clearpath_gz = get_package_share_directory(
|
||||
'clearpath_gz')
|
||||
pkg_clearpath_viz = get_package_share_directory(
|
||||
'clearpath_viz')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
pkg_clearpath_nav2_demos = get_package_share_directory(
|
||||
'clearpath_nav2_demos')
|
||||
|
||||
# Paths
|
||||
sim_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
|
||||
viz_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam2d.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
|
||||
|
||||
sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([sim_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
]
|
||||
)
|
||||
|
||||
viz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([viz_launch]),
|
||||
launch_arguments=[
|
||||
('namespace', LaunchConfiguration('robot_ns')),
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
('robot_ns', LaunchConfiguration('robot_ns'))
|
||||
]
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('setup_path', os.path.expanduser('~')+'/clearpath/'),
|
||||
('use_sim_time', 'true'),
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap)
|
||||
ld.add_action(sim)
|
||||
ld.add_action(viz)
|
||||
ld.add_action(nav2)
|
||||
return ld
|
||||
@@ -0,0 +1,104 @@
|
||||
#
|
||||
# Requirements:
|
||||
# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
|
||||
# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml
|
||||
# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
|
||||
# <horizontal_fov>1.047</horizontal_fov>
|
||||
# <image>
|
||||
# <width>320</width>
|
||||
# <height>240</height>
|
||||
# </image>
|
||||
# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743):
|
||||
# - <render_engine>ogre2</render_engine>
|
||||
# + <render_engine>ogre</render_engine>
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py robot_ns:=a200_0000
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 3) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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
|
||||
|
||||
import os
|
||||
|
||||
ARGUMENTS = [
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rtabmap_viz.'),
|
||||
DeclareLaunchArgument('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
DeclareLaunchArgument('robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_clearpath_gz = get_package_share_directory(
|
||||
'clearpath_gz')
|
||||
pkg_clearpath_viz = get_package_share_directory(
|
||||
'clearpath_viz')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
pkg_clearpath_nav2_demos = get_package_share_directory(
|
||||
'clearpath_nav2_demos')
|
||||
|
||||
# Paths
|
||||
sim_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
|
||||
viz_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d_assemble.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
|
||||
|
||||
sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([sim_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
]
|
||||
)
|
||||
|
||||
viz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([viz_launch]),
|
||||
launch_arguments=[
|
||||
('namespace', LaunchConfiguration('robot_ns')),
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('use_sim_time', 'true'),
|
||||
('robot_ns', LaunchConfiguration('robot_ns'))
|
||||
]
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('setup_path', os.path.expanduser('~')+'/clearpath/'),
|
||||
('use_sim_time', 'true'),
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap)
|
||||
ld.add_action(sim)
|
||||
ld.add_action(viz)
|
||||
ld.add_action(nav2)
|
||||
return ld
|
||||
@@ -0,0 +1,110 @@
|
||||
#
|
||||
# Requirements:
|
||||
# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
|
||||
# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml
|
||||
# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
|
||||
# <horizontal_fov>1.047</horizontal_fov>
|
||||
# <image>
|
||||
# <width>320</width>
|
||||
# <height>240</height>
|
||||
# </image>
|
||||
# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743):
|
||||
# - <render_engine>ogre2</render_engine>
|
||||
# + <render_engine>ogre</render_engine>
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py robot_ns:=a200_0000
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 3) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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
|
||||
|
||||
import os
|
||||
|
||||
ARGUMENTS = [
|
||||
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('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
DeclareLaunchArgument('robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace'),
|
||||
DeclareLaunchArgument('use_camera', default_value='true',
|
||||
description='Use camera for global loop closure / re-localization.'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_clearpath_gz = get_package_share_directory(
|
||||
'clearpath_gz')
|
||||
pkg_clearpath_viz = get_package_share_directory(
|
||||
'clearpath_viz')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
pkg_clearpath_nav2_demos = get_package_share_directory(
|
||||
'clearpath_nav2_demos')
|
||||
|
||||
# Paths
|
||||
sim_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
|
||||
viz_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
|
||||
|
||||
sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([sim_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
]
|
||||
)
|
||||
|
||||
viz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([viz_launch]),
|
||||
launch_arguments=[
|
||||
('namespace', LaunchConfiguration('robot_ns')),
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
('use_camera', LaunchConfiguration('use_camera')),
|
||||
('robot_ns', LaunchConfiguration('robot_ns'))
|
||||
]
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('setup_path', os.path.expanduser('~')+'/clearpath/'),
|
||||
('use_sim_time', 'true'),
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap)
|
||||
ld.add_action(sim)
|
||||
ld.add_action(viz)
|
||||
ld.add_action(nav2)
|
||||
return ld
|
||||
@@ -0,0 +1,128 @@
|
||||
#
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky):
|
||||
# $ ros2 launch clearpath_gz simulation.launch.py
|
||||
# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 2) Launch rviz:
|
||||
# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
|
||||
#
|
||||
# 3) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos husky_slam2d.launch.py use_sim_time:=true
|
||||
#
|
||||
# 4) Launch nav2"
|
||||
# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
|
||||
#
|
||||
# 4) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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')
|
||||
robot_ns = LaunchConfiguration('robot_ns')
|
||||
|
||||
icp_odom_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Grid/RangeMin':'0.7', # ignore laser scan points on the robot itself
|
||||
'RGBD/OptimizeMaxError':'2',
|
||||
}
|
||||
|
||||
# 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', # we are moving on a 2D flat floor
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/PointToPlaneMinComplexity':'0.04', # to be more robust to long corridors with low geometry
|
||||
'Icp/MaxTranslation': '1'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('odom', 'icp_odom'),
|
||||
('scan', 'sensors/lidar2d_0/scan'),
|
||||
('rgb/image', 'sensors/camera_0/color/image'),
|
||||
('rgb/camera_info', 'sensors/camera_0/color/camera_info'),
|
||||
('depth/image', 'sensors/camera_0/depth/image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[icp_odom_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=["--ros-args", "--log-level", 'warn']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
])
|
||||
@@ -0,0 +1,146 @@
|
||||
#
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky):
|
||||
# $ ros2 launch clearpath_gz simulation.launch.py
|
||||
# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 2) Launch rviz:
|
||||
# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
|
||||
#
|
||||
# 3) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos husky_slam3d.launch.py use_sim_time:=true
|
||||
#
|
||||
# 4) Launch nav2"
|
||||
# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
|
||||
#
|
||||
# 4) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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')
|
||||
robot_ns = LaunchConfiguration('robot_ns')
|
||||
use_camera = LaunchConfiguration('use_camera')
|
||||
|
||||
icp_odom_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom',
|
||||
'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size
|
||||
'OdomF2M/ScanMaxSize': '10000'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgbd': use_camera,
|
||||
'subscribe_scan_cloud':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures
|
||||
}
|
||||
|
||||
# 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', # we are moving on a 2D flat floor
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/VoxelSize': '0.3',
|
||||
'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size
|
||||
'Icp/PointToPlaneGroundNormalsUp': '0.9',
|
||||
'Icp/RangeMin': '0.5',
|
||||
'Icp/MaxTranslation': '1'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('odom', 'icp_odom'),
|
||||
('scan_cloud', 'sensors/lidar3d_0/points'),
|
||||
('rgb/image', 'sensors/camera_0/color/image'),
|
||||
('rgb/camera_info', 'sensors/camera_0/color/camera_info'),
|
||||
('depth/image', 'sensors/camera_0/depth/image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'use_camera', default_value='true',
|
||||
description='Use camera for global loop closure / re-localization.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
condition=IfCondition(use_camera),
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[icp_odom_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=["--ros-args", "--log-level", 'warn']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
])
|
||||
@@ -0,0 +1,134 @@
|
||||
#
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky):
|
||||
# $ ros2 launch clearpath_gz simulation.launch.py
|
||||
# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 2) Launch rviz:
|
||||
# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
|
||||
#
|
||||
# 3) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos husky_slam3d_assemble.launch.py use_sim_time:=true
|
||||
#
|
||||
# 4) Launch nav2"
|
||||
# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
|
||||
#
|
||||
# 4) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
robot_ns = LaunchConfiguration('robot_ns')
|
||||
|
||||
icp_odom_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom',
|
||||
'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size
|
||||
'OdomF2M/ScanMaxSize': '10000'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_scan_cloud':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
'topic_queue_size': 30,
|
||||
'sync_queue_size': 30,
|
||||
'approx_sync': True,
|
||||
'qos': 1,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures
|
||||
'Rtabmap/DetectionRate': '0' # Rate is limited by the assembling time below (1 Hz)
|
||||
}
|
||||
|
||||
# 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', # we are moving on a 2D flat floor
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/VoxelSize': '0.3',
|
||||
'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size
|
||||
'Icp/PointToPlaneGroundNormalsUp': '0.9',
|
||||
'Icp/RangeMin': '0.5',
|
||||
'Icp/MaxTranslation': '2'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('odom', 'icp_odom'),
|
||||
('rgb/image', 'sensors/camera_0/color/image'),
|
||||
('rgb/camera_info', 'sensors/camera_0/color/camera_info'),
|
||||
('depth/image', 'sensors/camera_0/depth/image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[icp_odom_parameters, shared_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')],
|
||||
arguments=["--ros-args", "--log-level", 'warn']),
|
||||
|
||||
#Assemble scans
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'assembling_time': 1.0, 'range_min': 0.5, 'fixed_frame_id': "", 'use_sim_time':use_sim_time, 'sync_queue_size': 30, 'topic_queue_size':30}],
|
||||
remappings=remappings + [('cloud', 'sensors/lidar3d_0/points')]),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'assembled_cloud')],
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]),
|
||||
])
|
||||
@@ -0,0 +1,280 @@
|
||||
#
|
||||
# Requirements:
|
||||
# * Isaac simulator
|
||||
# * isaac_ros_image_proc
|
||||
# * isaac_ros_stereo_image_proc
|
||||
# * nav2_bringup
|
||||
# * isaac_ros_visual_slam (optional, for vo:=isaac)
|
||||
#
|
||||
# 1. Launch Isaac Simulator
|
||||
#
|
||||
# 2. Open Isaac Examples -> ROS2 -> Navigation -> Carter Navigation (or iw.hub Navigation, for more visual features)
|
||||
#
|
||||
# 3. Enable front stereo right camera:
|
||||
# In the Stage tab, open World->Nova_Carter_ROS->front_hawk->right_camera_render_product,
|
||||
# then under Property->Isaac Create Render Product Node->Inputs, check "Enabled". To make
|
||||
# simulation faster, set height=600 and width=960. Do the same for the front stereo left camera.
|
||||
#
|
||||
# 4. Make sure that after you click on Play button in the simulator, you can see these topics:
|
||||
# $ ros2 topic list
|
||||
# /front_stereo_camera/left/camera_info
|
||||
# /front_stereo_camera/left/image_raw
|
||||
# /front_stereo_camera/left/image_raw/nitros_bridge
|
||||
# /front_stereo_camera/right/camera_info
|
||||
# /front_stereo_camera/right/image_raw
|
||||
# /front_stereo_camera/right/image_raw/nitros_bridge
|
||||
# /front_stereo_imu/imu
|
||||
#
|
||||
# 5. Launch the example:
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py
|
||||
#
|
||||
# 6. You should be able to send goals in RVIZ to move the robot, or use:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
#
|
||||
# === Advanced ===
|
||||
# With this launch file, we can also experiment with visual odometry with/without disparity computed on GPU.
|
||||
#
|
||||
# A. Use RTAB-Map's Visual Odometry:
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=true
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=false
|
||||
#
|
||||
# B. Use Isaac Visual Odometry:
|
||||
# We should disable wheel odometry TF publishing in the simulator to make it work. To
|
||||
# do so, in the Stage tab, open World->Nova_Carter_ROS->transform_tree_odometry->ros2_publish_raw_transform_tree,
|
||||
# then under Property->ROS2Publish Raw Transform Tree Node->Inputs, change topicName from "tf" to "tf_odom_ignored".
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=true
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=false
|
||||
#
|
||||
#
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import ComposableNodeContainer
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Directories
|
||||
pkg_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
# Paths
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||||
nav2_vo_params = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'params', 'isaac_vslam_nav2_params.yaml'])
|
||||
nav2_params = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'params', 'isaac_nav2_params.yaml'])
|
||||
rviz_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'isaac', 'isaac_vslam.launch.py'])
|
||||
|
||||
vo = LaunchConfiguration('vo').perform(context)
|
||||
image_width = int(LaunchConfiguration('image_width').perform(context))
|
||||
image_height = int(LaunchConfiguration('image_height').perform(context))
|
||||
|
||||
left_resize_node = ComposableNode(
|
||||
name='left_resize_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::ResizeNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image', 'left/image_raw'),
|
||||
('camera_info', 'left/camera_info'),
|
||||
('resize/image', 'left/image_resize'),
|
||||
('resize/camera_info', 'left/camera_info_resize')
|
||||
]
|
||||
)
|
||||
|
||||
right_resize_node = ComposableNode(
|
||||
name='right_resize_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::ResizeNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image', 'right/image_raw'),
|
||||
('camera_info', 'right/camera_info'),
|
||||
('resize/image', 'right/image_resize'),
|
||||
('resize/camera_info', 'right/camera_info_resize')
|
||||
]
|
||||
)
|
||||
|
||||
left_rectify_node = ComposableNode(
|
||||
name='left_rectify_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::RectifyNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image_raw', 'left/image_resize'),
|
||||
('camera_info', 'left/camera_info_resize'),
|
||||
('image_rect', 'left/image_rect'),
|
||||
('camera_info_rect', 'left/camera_info_rect')
|
||||
]
|
||||
)
|
||||
|
||||
right_rectify_node = ComposableNode(
|
||||
name='right_rectify_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::RectifyNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image_raw', 'right/image_resize'),
|
||||
('camera_info', 'right/camera_info_resize'),
|
||||
('image_rect', 'right/image_rect'),
|
||||
('camera_info_rect', 'right/camera_info_rect')
|
||||
]
|
||||
)
|
||||
|
||||
disparity_node = ComposableNode(
|
||||
name='disparity_node',
|
||||
package='isaac_ros_stereo_image_proc',
|
||||
plugin='nvidia::isaac_ros::stereo_image_proc::DisparityNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'backends': 'CUDA',
|
||||
'max_disparity': 64.0
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('left/camera_info', 'left/camera_info_rect'),
|
||||
('right/camera_info', 'right/camera_info_rect'),
|
||||
],
|
||||
)
|
||||
|
||||
disparity_to_depth_node = ComposableNode(
|
||||
name='disparity_to_depth_node',
|
||||
package='isaac_ros_stereo_image_proc',
|
||||
plugin='nvidia::isaac_ros::stereo_image_proc::DisparityToDepthNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
}],
|
||||
namespace="front_stereo_camera"
|
||||
)
|
||||
|
||||
stereo_img_proc_container = ComposableNodeContainer(
|
||||
name='stereo_img_proc_container',
|
||||
package='rclcpp_components',
|
||||
namespace="front_stereo_camera",
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=[
|
||||
left_resize_node,
|
||||
right_resize_node,
|
||||
left_rectify_node,
|
||||
right_rectify_node,
|
||||
disparity_node,
|
||||
disparity_to_depth_node
|
||||
],
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', 'info',
|
||||
'--log-level', 'color_format_convert:=info',
|
||||
'--log-level', 'NitrosImage:=info',
|
||||
'--log-level', 'NitrosNode:=info'
|
||||
],
|
||||
)
|
||||
|
||||
nav2_args = [('use_sim_time', 'true')]
|
||||
if vo == 'rtabmap':
|
||||
# We need to change the base odom frame to vo
|
||||
nav2_args.append(('params_file', nav2_vo_params))
|
||||
else:
|
||||
# Use custom version with higher velocities
|
||||
nav2_args.append(('params_file', nav2_params))
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=nav2_args
|
||||
)
|
||||
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rviz_launch])
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
('stereo_camera_namespace', 'front_stereo_camera'),
|
||||
('enable_vo', str(vo == 'rtabmap')),
|
||||
('stereo', LaunchConfiguration('stereo'))
|
||||
]
|
||||
)
|
||||
|
||||
# Add actions
|
||||
actions = [rtabmap, nav2, rviz, stereo_img_proc_container]
|
||||
|
||||
if vo == 'isaac':
|
||||
isaac_visual_slam_node = ComposableNode(
|
||||
name='visual_slam_node',
|
||||
package='isaac_ros_visual_slam',
|
||||
plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
|
||||
remappings=[('visual_slam/image_0', 'front_stereo_camera/left/image_rect'),
|
||||
('visual_slam/camera_info_0', 'front_stereo_camera/left/camera_info_rect'),
|
||||
('visual_slam/image_1', 'front_stereo_camera/right/image_rect'),
|
||||
('visual_slam/camera_info_1', 'front_stereo_camera/right/camera_info_rect')],
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'enable_image_denoising': True,
|
||||
'enable_planar_mode': True,
|
||||
'rectified_images': True,
|
||||
'publish_map_to_odom_tf': False,
|
||||
'odom_frame': 'odom',
|
||||
'enable_slam_visualization': True,
|
||||
'enable_observations_view': True,
|
||||
'enable_landmarks_view': True}]
|
||||
)
|
||||
|
||||
isaac_vslam_container = ComposableNodeContainer(
|
||||
name='isaac_visual_slam_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container',
|
||||
composable_node_descriptions=[isaac_visual_slam_node],
|
||||
output='screen',
|
||||
)
|
||||
actions.append(isaac_vslam_container)
|
||||
|
||||
return actions
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
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('vo', default_value='none',
|
||||
choices=['none', 'rtabmap', 'isaac'], description='Enable visual odometry using one of the approach. None means only wheel odometry is used. If you set this to "isaac", make sure to disable odom -> base_link if it exists, because isaac will publish on same TF!'),
|
||||
DeclareLaunchArgument('stereo', default_value='true',
|
||||
choices=['true', 'false'], description='Use stereo images as input instead of left+depth images.'),
|
||||
DeclareLaunchArgument('image_width', default_value='960',
|
||||
description='Resize input images.'),
|
||||
DeclareLaunchArgument('image_height', default_value='600',
|
||||
description='Resize input images.'),
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,139 @@
|
||||
|
||||
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')
|
||||
localization_value = localization.perform(context)
|
||||
localization_value = localization_value == 'True' or localization_value == 'true'
|
||||
enable_vo = LaunchConfiguration('enable_vo')
|
||||
enable_vo_value = enable_vo.perform(context)
|
||||
enable_vo_value = enable_vo_value == 'True' or enable_vo_value == 'true'
|
||||
stereo = LaunchConfiguration('stereo')
|
||||
stereo_value = stereo.perform(context)
|
||||
stereo_value = stereo_value == 'True' or stereo_value == 'true'
|
||||
rtabmap_viz = LaunchConfiguration('rtabmap_viz')
|
||||
stereo_ns = LaunchConfiguration('stereo_camera_namespace').perform(context)
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_link',
|
||||
'use_sim_time': use_sim_time,
|
||||
'subscribe_rgbd': True,
|
||||
'subscribe_odom': enable_vo,
|
||||
'subscribe_odom_info': enable_vo,
|
||||
'approx_sync': False,
|
||||
'use_action_for_goal':True,
|
||||
'Reg/Force3DoF':'true',
|
||||
'Vis/MinDepth': '0.2',
|
||||
'GFTT/MinDistance': '5',
|
||||
'GFTT/QualityLevel': '0.00001',
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.15', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'0.5', # All points over 0.5 meter are ignored
|
||||
'Grid/RangeMin':'0.2', # Ignore invalid points close to camera
|
||||
'Grid/NoiseFilteringMinNeighbors':'8', # Default stereo is quite noisy, enable noise filter
|
||||
'Grid/NoiseFilteringRadius':'0.1', # Default stereo is quite noisy, enable noise filter
|
||||
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
|
||||
}
|
||||
if enable_vo_value:
|
||||
parameters['guess_frame_id'] = 'odom'
|
||||
else:
|
||||
parameters['odom_frame_id'] = 'odom'
|
||||
|
||||
arguments = []
|
||||
if localization_value:
|
||||
parameters['Mem/IncrementalMemory'] = 'True'
|
||||
parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
remappings=[('rgbd_image', '/'+stereo_ns+'/rgbd_image'),
|
||||
('map', '/map')]
|
||||
vo_node_prefix = 'rgbd'
|
||||
if stereo_value:
|
||||
vo_node_prefix = 'stereo'
|
||||
|
||||
return [
|
||||
# Sync image data together
|
||||
Node(
|
||||
condition=UnlessCondition(stereo),
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=stereo_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=[
|
||||
('rgb/image', 'left/image_rect'),
|
||||
('rgb/camera_info', 'left/camera_info_rect'),
|
||||
('depth/image', 'depth')]),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(stereo),
|
||||
package='rtabmap_sync', executable='stereo_sync', output='screen',
|
||||
namespace=stereo_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=[
|
||||
('left/image_rect', 'left/image_rect'),
|
||||
('left/camera_info', 'left/camera_info_rect'),
|
||||
('right/image_rect', 'right/image_rect'),
|
||||
('right/camera_info', 'right/camera_info_rect')]),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(enable_vo),
|
||||
package='rtabmap_odom', executable=vo_node_prefix+'_odometry', output='screen',
|
||||
namespace='rtabmap',
|
||||
parameters=[parameters, {'odom_frame_id': 'vo'}],
|
||||
remappings=remappings),
|
||||
|
||||
# VSLAM:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace='rtabmap',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=arguments),
|
||||
|
||||
# Visualization:
|
||||
Node(
|
||||
condition=IfCondition(rtabmap_viz),
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace='rtabmap',
|
||||
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(
|
||||
'enable_vo', default_value='false',
|
||||
description='Enable RTAB-Map\'s visual odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rtabmap_viz', default_value='true',
|
||||
description='Launch rtabmap_viz for visualization.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'stereo', default_value='false',
|
||||
description='Use stereo images as input instead of left+depth images.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'stereo_camera_namespace', default_value='front_stereo_camera',
|
||||
description='Namespace of the stereo camera.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,115 @@
|
||||
# Requirements:
|
||||
# Download one or more rosbags:
|
||||
# * map1.db3: https://drive.google.com/file/d/1XajzWm0u1Tk7m7x63ybcKVMXj80r5P6r/view?usp=drive_link
|
||||
# * map2.db3: https://drive.google.com/file/d/1_FxEalE2O-DQKq2tRpLIpDn5Mbvu0jZc/view?usp=drive_link
|
||||
# * map3.db3: https://drive.google.com/file/d/1dJzMOoRPA28gQZUIWCeAa08Qn4wG9oRw/view?usp=drive_link
|
||||
# * map4.db3: https://drive.google.com/file/d/19Y6yye0ndIIwdhEWMwTdoiSy9WlKS44c/view?usp=drive_link
|
||||
# * map5.db3: https://drive.google.com/file/d/1zCx4Q4SftPplQtW1xeG-W3OkbTxwd5GD/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ rm ~/.ros/rtabmap.db
|
||||
# $ ros2 launch rtabmap_demos multisession_mapping_demo.launch.py
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play map1.db3 --clock
|
||||
# when done, you can play the next bag(s):
|
||||
# $ ros2 bag play map2.db3 --clock
|
||||
# $ ros2 bag play map3.db3 --clock
|
||||
# $ ros2 bag play map4.db3 --clock
|
||||
# $ ros2 bag play map5.db3 --clock
|
||||
#
|
||||
# Refer to this paper for more info: https://arxiv.org/abs/2407.15305
|
||||
#
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition
|
||||
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():
|
||||
|
||||
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': 'false',
|
||||
'RGBD/ProximityBySpace': 'false', # Referred paper did only global loop closure detection
|
||||
'RGBD/OptimizeFromGraphEnd': 'true',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/Iterations': '30',
|
||||
'Icp/VoxelSize': '0',
|
||||
'Vis/MinInliers': '12',
|
||||
'Vis/MaxDepth': '0',
|
||||
'RGBD/AngularUpdate': '0.01',
|
||||
'RGBD/LinearUpdate': '0.01',
|
||||
'Rtabmap/TimeThr': '700',
|
||||
'Mem/RehearsalSimilarity': '0.30', # Referred paper used 0.45 with SURF, here with SIFT, we will use 0.3
|
||||
'Kp/TfIdfLikelihoodUsed': 'false',
|
||||
'Bayes/FullPredictionUpdate': 'true',
|
||||
'Kp/DetectorStrategy': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries
|
||||
'Vis/FeatureType': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries
|
||||
'Kp/MaxFeatures': '400',
|
||||
'Reg/Force3DoF': 'true',
|
||||
'RGBD/OptimizeMaxError': '10',
|
||||
'Optimizer/Strategy': '2', # Referred paper used TORO (0), latest version recommends GTSAM (2)
|
||||
'Optimizer/Iterations': '100',
|
||||
'Kp/IncrementalFlann': 'false', # Referred paper didn't use incremental FLANN
|
||||
'Icp/MaxTranslation': '0.5',
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/data_throttled_image'),
|
||||
('depth/image', '/data_throttled_image_depth'),
|
||||
('rgb/camera_info', '/data_throttled_camera_info'),
|
||||
('scan', '/base_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='true', description='Launch RTAB-Map UI (optional).'),
|
||||
DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'),
|
||||
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
||||
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
|
||||
# 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.02}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM node:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
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,113 @@
|
||||
# Requirements:
|
||||
# Download rosbag:
|
||||
# * demo_mapping.db3: https://drive.google.com/file/d/1v9qJ2U7GlYhqBJr7OQHWbDSCfgiVaLWb/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play demo_mapping.db3 --clock
|
||||
#
|
||||
|
||||
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,155 @@
|
||||
# Requirements:
|
||||
# Download one or both rosbags:
|
||||
# * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link
|
||||
# * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py rviz:=true rtabmap_viz:=true
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play stereo_outdoorA.db3 --clock
|
||||
# when done, you can play the secon bag:
|
||||
# $ ros2 bag play stereo_outdoorB.db3 --clock
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
|
||||
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, SetParameter, SetRemap
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
pkg_stereo_image_proc = get_package_share_directory(
|
||||
'stereo_image_proc')
|
||||
|
||||
# Paths
|
||||
stereo_image_proc_launch = PathJoinSubstitution(
|
||||
[pkg_stereo_image_proc, 'launch', 'stereo_image_proc.launch.py'])
|
||||
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'subscribe_rgbd':True,
|
||||
'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs
|
||||
'map_negative_poses_ignored':True,
|
||||
'subscribe_odom_info': True,
|
||||
# RTAB-Map's internal parameters should be strings
|
||||
'OdomF2M/MaxSize': '1000',
|
||||
'GFTT/MinDistance': '10',
|
||||
'GFTT/QualityLevel': '0.00001',
|
||||
#'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d
|
||||
#'Vis/FeatureType': '6' # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgbd_image', '/stereo_camera/rgbd_image'),
|
||||
('odom', '/vo')]
|
||||
|
||||
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=True),
|
||||
|
||||
# Nodes to launch
|
||||
|
||||
# Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_left', output='screen',
|
||||
namespace='stereo_camera',
|
||||
arguments=['compressed', 'raw'],
|
||||
remappings=[('in/compressed', 'left/image_raw_throttle/compressed'),
|
||||
('out', 'left/image_raw')]),
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_right', output='screen',
|
||||
namespace='stereo_camera',
|
||||
arguments=['compressed', 'raw'],
|
||||
remappings=[('in/compressed', 'right/image_raw_throttle/compressed'),
|
||||
('out', 'right/image_raw')]),
|
||||
|
||||
# Run the ROS package stereo_image_proc for image rectification
|
||||
GroupAction(
|
||||
actions=[
|
||||
|
||||
SetRemap(src='camera_info',dst='camera_info_throttle'),
|
||||
SetRemap(src='camera_info',dst='camera_info_throttle'),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([stereo_image_proc_launch]),
|
||||
launch_arguments=[
|
||||
('left_namespace', 'stereo_camera/left'),
|
||||
('right_namespace', 'stereo_camera/right'),
|
||||
('disparity_range', '128'),
|
||||
]
|
||||
),
|
||||
]
|
||||
),
|
||||
|
||||
# Synchronize stereo data together in a single topic
|
||||
# Issue: stereo_img_proc doesn't produce color and
|
||||
# grayscale images exactly the same (there is a small
|
||||
# vertical shift with color), we should use grayscale for
|
||||
# left and right images to get similar results than on ros1 noetic.
|
||||
Node(
|
||||
package='rtabmap_sync', executable='stereo_sync', output='screen',
|
||||
namespace='stereo_camera',
|
||||
remappings=[
|
||||
('left/image_rect', 'left/image_rect'),
|
||||
('right/image_rect', 'right/image_rect'),
|
||||
('left/camera_info', 'left/camera_info_throttle'),
|
||||
('right/camera_info', 'right/camera_info_throttle')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='stereo_odometry', output='screen',
|
||||
parameters=[parameters],
|
||||
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,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)
|
||||
])
|
||||
@@ -0,0 +1,79 @@
|
||||
#
|
||||
# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434,
|
||||
# otherwise, the lidar and camera point cloud won't be aligned correctly.
|
||||
#
|
||||
# Example:
|
||||
# 1) Launch simulator (turtlebot4, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 3) Click on double points ".." button on top-right next to power button to undock.
|
||||
#
|
||||
# 4) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2):
|
||||
# $ ros2 launch explore_lite explore.launch.py
|
||||
#
|
||||
|
||||
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', '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')),
|
||||
('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
|
||||
@@ -0,0 +1,119 @@
|
||||
#
|
||||
# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434,
|
||||
# otherwise, the lidar and camera point cloud won't be aligned correctly.
|
||||
#
|
||||
# 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
|
||||
# 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" 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-left of gazebo.
|
||||
#
|
||||
# 4) Click on double points ".." button on top-right next to power button to undock.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2):
|
||||
# $ ros2 launch explore_lite explore.launch.py
|
||||
#
|
||||
|
||||
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')
|
||||
rtabmap_viz = LaunchConfiguration('rtabmap_viz')
|
||||
|
||||
icp_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
# 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',
|
||||
'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry
|
||||
}
|
||||
|
||||
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(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rtabmap_viz', default_value='true', choices=['true', 'false'],
|
||||
description='Launch rtabmap_viz for visualization.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
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(
|
||||
condition=IfCondition(rtabmap_viz),
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
])
|
||||
Reference in New Issue
Block a user