sobits_navigation_stack/sobits_slam/launch/active_slam.launch.py at humble-devel · TeamSOBITS/sobits_navigation_stack · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
import os

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
from launch_ros.actions import Node
from launch.conditions import IfCondition

def generate_launch_description():
    ########## Customizable parameters ##########
    declare_robot_name_cmd = DeclareLaunchArgument(
        'robot_name',
        # default_value="sobit_pro",
        # default_value="sobit_edu",
        # default_value="sobit_mini",
        # default_value="sobit_light",
        default_value="hsr_sim",
        # default_value="hsrb_robot",
        description='choice your used robot name')

    declare_save_map_command_cmd = DeclareLaunchArgument(
        'save_map_command', default_value='true',
        description='command map saver select')

    use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true')
    #############################################

    # Get the launch directory
    navigation_dir = get_package_share_directory('sobits_nav')
    mapping_dir = get_package_share_directory('sobits_slam')

    explore_config = os.path.join(get_package_share_directory("explore_lite"), "config", "params.yaml")

    robot_name = LaunchConfiguration('robot_name')
    save_map_command = LaunchConfiguration('save_map_command')
    robot_base_frame = LaunchConfiguration('robot_base_frame')
    use_sim_time = LaunchConfiguration('use_sim_time')

    nav2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(navigation_dir, "launch", "nav2.launch.py")),
        launch_arguments={
                            "use_sim_time": use_sim_time,
                            'slam': "True",
                            'location_file_path': "",
                            'robot_name': robot_name,
                            'use_rviz': 'False',
                            'use_flex_nav': 'False',
                            'use_keepout_filter': 'False',
                        }.items())

    explore_node_cmd = Node(
        package="explore_lite",
        name="explore_node",
        executable="explore",
        parameters=[explore_config,
                    {
                        "robot_base_frame": robot_base_frame,
                        "return_to_init": "False",
                        "use_sim_time": use_sim_time,
                        "progress_timeout": "90.0"
                    }],
        output="screen",
        remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
    )

    sobits_map_saver = Node(
        package='sobits_slam',
        executable='sobits_map_saver',
        name='sobits_map_saver',
        output='log',
        parameters=[{'use_sim_time': use_sim_time}],
        condition=IfCondition(save_map_command),
    )

    rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', os.path.join(navigation_dir, 'rviz', 'sobits_nav.rviz')]
    )

    # Create the launch description and populate
    ld = LaunchDescription()

    ld.add_action(declare_robot_name_cmd)
    ld.add_action(declare_save_map_command_cmd)
    ld.add_action(use_sim_time_cmd)
    ld.add_action(OpaqueFunction(function=declare_param_value))
    ld.add_action(nav2_cmd)
    ld.add_action(explore_node_cmd)
    ld.add_action(sobits_map_saver)
    ld.add_action(rviz_cmd)

    return ld


def declare_param_value(context, *args, **kwargs):
    robot_name_value = LaunchConfiguration('robot_name').perform(context)
    if   ("pro" in robot_name_value):
        robot_base_frame = robot_name_value + "/lidar_laser" # "/base_footprint"
    elif ("edu" in robot_name_value):
        robot_base_frame = robot_name_value + "/lidar_laser" # "/base_footprint"
    elif ("mini" in robot_name_value):
        robot_base_frame = robot_name_value + "/lidar_laser" # "/base_footprint"
    elif ("light" in robot_name_value):
        robot_base_frame = robot_name_value + "/lidar_laser" # "/base_footprint"
    elif ("hsrb" in robot_name_value):
        robot_base_frame = "base_footprint"
    elif ("hsr" in robot_name_value):
        robot_base_frame = "base_footprint"
    else:
        robot_base_frame = ""  ## CUSTOM FRAME

    return [
        DeclareLaunchArgument('robot_base_frame', default_value=robot_base_frame, description='ROBOT base frame name.'),
    ]