mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 01:07:49 +08:00
88 lines
3.0 KiB
Python
88 lines
3.0 KiB
Python
|
|
||
|
|
# 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)
|
||
|
|
])
|