|
| 1 | +# Copyright (C) 2023 Open Source Robotics Foundation |
| 2 | +# Copyright (C) 2024 Open Navigation LLC |
| 3 | +# |
| 4 | +# Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | +# you may not use this file except in compliance with the License. |
| 6 | +# You may obtain a copy of the License at |
| 7 | +# |
| 8 | +# http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | +# |
| 10 | +# Unless required by applicable law or agreed to in writing, software |
| 11 | +# distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | +# See the License for the specific language governing permissions and |
| 14 | +# limitations under the License. |
| 15 | + |
| 16 | +import os |
| 17 | +from pathlib import Path |
| 18 | + |
| 19 | + |
| 20 | +from ament_index_python.packages import get_package_share_directory |
| 21 | + |
| 22 | +from launch import LaunchDescription |
| 23 | +from launch.actions import AppendEnvironmentVariable |
| 24 | +from launch.actions import DeclareLaunchArgument |
| 25 | +from launch.substitutions import LaunchConfiguration |
| 26 | +from launch.substitutions.command import Command |
| 27 | +from launch.substitutions.find_executable import FindExecutable |
| 28 | + |
| 29 | +from launch_ros.actions import Node |
| 30 | + |
| 31 | + |
| 32 | +def generate_launch_description(): |
| 33 | + bringup_dir = get_package_share_directory('nav2_minimal_tb3_sim') |
| 34 | + |
| 35 | + namespace = LaunchConfiguration('namespace') |
| 36 | + robot_name = LaunchConfiguration('robot_name') |
| 37 | + robot_sdf = LaunchConfiguration('robot_sdf') |
| 38 | + pose = { |
| 39 | + 'x': LaunchConfiguration('x_pose', default='-2.00'), |
| 40 | + 'y': LaunchConfiguration('y_pose', default='-0.50'), |
| 41 | + 'z': LaunchConfiguration('z_pose', default='0.01'), |
| 42 | + 'R': LaunchConfiguration('roll', default='0.00'), |
| 43 | + 'P': LaunchConfiguration('pitch', default='0.00'), |
| 44 | + 'Y': LaunchConfiguration('yaw', default='0.00'), |
| 45 | + } |
| 46 | + |
| 47 | + # Declare the launch arguments |
| 48 | + declare_namespace_cmd = DeclareLaunchArgument( |
| 49 | + 'namespace', default_value='', description='Top-level namespace' |
| 50 | + ) |
| 51 | + |
| 52 | + declare_robot_name_cmd = DeclareLaunchArgument( |
| 53 | + 'robot_name', |
| 54 | + default_value='turtlebot3_waffle_gps', |
| 55 | + description='name of the robot', |
| 56 | + ) |
| 57 | + |
| 58 | + declare_robot_sdf_cmd = DeclareLaunchArgument( |
| 59 | + 'robot_sdf', |
| 60 | + default_value=os.path.join(bringup_dir, 'urdf', 'gz_waffle_gps.sdf.xacro'), |
| 61 | + description='Full path to robot sdf file to spawn the robot in gazebo', |
| 62 | + ) |
| 63 | + |
| 64 | + bridge = Node( |
| 65 | + package='ros_gz_bridge', |
| 66 | + executable='parameter_bridge', |
| 67 | + namespace=namespace, |
| 68 | + parameters=[ |
| 69 | + { |
| 70 | + 'config_file': os.path.join( |
| 71 | + bringup_dir, 'configs', 'turtlebot3_waffle_gps_bridge.yaml' |
| 72 | + ), |
| 73 | + 'expand_gz_topic_names': True, |
| 74 | + 'use_sim_time': True, |
| 75 | + } |
| 76 | + ], |
| 77 | + output='screen', |
| 78 | + ) |
| 79 | + |
| 80 | + spawn_model = Node( |
| 81 | + package='ros_gz_sim', |
| 82 | + executable='create', |
| 83 | + output='screen', |
| 84 | + namespace=namespace, |
| 85 | + arguments=[ |
| 86 | + '-name', |
| 87 | + robot_name, |
| 88 | + '-string', |
| 89 | + Command( |
| 90 | + [ |
| 91 | + FindExecutable(name='xacro'), |
| 92 | + ' ', |
| 93 | + 'namespace:=', |
| 94 | + LaunchConfiguration('namespace'), |
| 95 | + ' ', |
| 96 | + robot_sdf, |
| 97 | + ] |
| 98 | + ), |
| 99 | + '-x', |
| 100 | + pose['x'], |
| 101 | + '-y', |
| 102 | + pose['y'], |
| 103 | + '-z', |
| 104 | + pose['z'], |
| 105 | + '-R', |
| 106 | + pose['R'], |
| 107 | + '-P', |
| 108 | + pose['P'], |
| 109 | + '-Y', |
| 110 | + pose['Y'], |
| 111 | + ], |
| 112 | + ) |
| 113 | + |
| 114 | + set_env_vars_resources = AppendEnvironmentVariable( |
| 115 | + 'GZ_SIM_RESOURCE_PATH', os.path.join(bringup_dir, 'models') |
| 116 | + ) |
| 117 | + set_env_vars_resources2 = AppendEnvironmentVariable( |
| 118 | + 'GZ_SIM_RESOURCE_PATH', str(Path(os.path.join(bringup_dir)).parent.resolve()) |
| 119 | + ) |
| 120 | + |
| 121 | + # Create the launch description and populate |
| 122 | + ld = LaunchDescription() |
| 123 | + ld.add_action(declare_namespace_cmd) |
| 124 | + ld.add_action(declare_robot_name_cmd) |
| 125 | + ld.add_action(declare_robot_sdf_cmd) |
| 126 | + |
| 127 | + ld.add_action(set_env_vars_resources) |
| 128 | + ld.add_action(set_env_vars_resources2) |
| 129 | + |
| 130 | + ld.add_action(bridge) |
| 131 | + ld.add_action(spawn_model) |
| 132 | + return ld |
0 commit comments