def generate_test_description(executable, ready_fn):
command = [executable]
# Execute python files using same python used to start this test
env = dict(os.environ)
if command[0][-3:] == '.py':
command.insert(0, sys.executable)
env['PYTHONUNBUFFERED'] = '1'
launch_description = LaunchDescription()
test_context = {}
for replacement_name, (replacement_value, cli_argument) in TEST_CASES.items():
random_string = '%d_%s' % (
random.randint(0, 9999), time.strftime('%H_%M_%S', time.gmtime()))
launch_description.add_action(
ExecuteProcess(
cmd=command + [cli_argument.format(**locals())],
name='name_maker_' + replacement_name, env=env
)
)
test_context[replacement_name] = replacement_value.format(**locals())
launch_description.add_action(
OpaqueFunction(function=lambda context: ready_fn())
)
return launch_description, test_context
def generate_launch_description():
# Get the launch directory
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(nav2_bringup_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
headless = LaunchConfiguration('headless')
world = LaunchConfiguration('world')
remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_simulator_cmd = DeclareLaunchArgument(
'headless',
default_value='True',
description='Whether to execute gzclient)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# 'worlds/turtlebot3_worlds/waffle.model'),
default_value=os.path.join(nav2_bringup_dir, 'worlds', 'waffle.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
cwd=[launch_dir],
output='screen')
start_gazebo_client_cmd = ExecuteProcess(condition=IfCondition(
PythonExpression([use_simulator, ' and not ', headless])),
cmd=['gzclient'],
cwd=[launch_dir],
output='screen')
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
remappings=remappings,
arguments=[urdf])
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
ld.add_action(start_robot_state_publisher_cmd)
return ld
def generate_launch_description():
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
nav2_launch_dir = os.path.join(nav2_bringup_dir, 'launch')
# Names and poses of the robots
robots = [
{'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01},
{'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}]
amazon_gazebo_package_dir = get_package_share_directory('amazon_robot_gazebo')
amazon_gazebo_package_launch_dir= os.path.join(amazon_gazebo_package_dir, 'launch')
amazon_description_dir = get_package_share_directory('amazon_robot_description')
this_launch_dir = os.path.dirname(os.path.realpath(__file__))
amazon_bringup_package_dir = get_package_share_directory('amazon_robot_bringup')
amazon_bringup_package_launch_dir= os.path.join(amazon_bringup_package_dir, 'launch')
# Simulation settings
world = LaunchConfiguration('world')
simulator = LaunchConfiguration('simulator')
# On this example all robots are launched with the same settings
map_yaml_file = LaunchConfiguration('map')
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
autostart = LaunchConfiguration('autostart')
rviz_config_file = LaunchConfiguration('rviz_config')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
log_settings = LaunchConfiguration('log_settings', default='true')
declare_world_cmd = DeclareLaunchArgument(
'world',
default_value=os.path.join(amazon_gazebo_package_dir, 'worlds', 'warehouse.world'),
description='Full path to world model file to load')
declare_simulator_cmd = DeclareLaunchArgument(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(this_launch_dir, 'maps', 'map.yaml'),
description='Full path to map file to load')
declare_robot1_params_file_cmd = DeclareLaunchArgument(
'robot1_params_file',
default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_multirobot_params_1.yaml'),
description='Full path to the ROS2 parameters file to use for robot1 launched nodes')
declare_robot2_params_file_cmd = DeclareLaunchArgument(
'robot2_params_file',
default_value=os.path.join(nav2_bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'),
description='Full path to the ROS2 parameters file to use for robot2 launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'default_bt_xml_filename',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'),
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='false',
description='Automatically startup the stacks')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(nav2_bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'),
description='Full path to the RVIZ config file to use.')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz',
default_value='True',
description='Whether to start RVIZ')
# Start Gazebo with plugin providing the robot spawing service
start_gazebo_cmd = ExecuteProcess(
cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', '-s' , 'libgazebo_ros_force_system.so' , world],
output='screen')
# Define commands for spawing the robots into Gazebo
spawn_robots_cmds = []
for robot in robots:
spawn_robots_cmds.append(
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(amazon_bringup_package_dir, 'launch',
'spawn_tb3_launch.py')),
launch_arguments={
'x_pose': TextSubstitution(text=str(robot['x_pose'])),
'y_pose': TextSubstitution(text=str(robot['y_pose'])),
'z_pose': TextSubstitution(text=str(robot['z_pose'])),
'robot_name': robot['name']
}.items()))
# Define commands for launching the navigation instances
nav_instances_cmds = []
for robot in robots:
params_file = LaunchConfiguration(robot['name'] + '_params_file')
group = GroupAction([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(nav2_launch_dir, 'rviz_launch.py')),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': TextSubstitution(text=robot['name']),
'use_namespace': 'True',
'rviz_config': rviz_config_file}.items()),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(this_launch_dir,
'amazon_warehouse_world.py')),
launch_arguments={'namespace': robot['name'],
'use_namespace': 'True',
'map': map_yaml_file,
'use_sim_time': 'True',
'params_file': params_file,
'default_bt_xml_filename': default_bt_xml_filename,
'autostart': autostart,
'use_rviz': 'False',
'use_simulator': 'False',
'headless': 'False',
'use_robot_state_pub': use_robot_state_pub}.items()),
LogInfo(
condition=IfCondition(log_settings),
msg=['Launching ', robot['name']]),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot['name'], ' map yaml: ', map_yaml_file]),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot['name'], ' params yaml: ', params_file]),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot['name'], ' behavior tree xml: ', default_bt_xml_filename]),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot['name'], ' rviz config file: ', rviz_config_file]),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot['name'], ' using robot state pub: ', use_robot_state_pub]),
LogInfo(
condition=IfCondition(log_settings),
msg=[robot['name'], ' autostart: ', autostart])
])
nav_instances_cmds.append(group)
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_robot1_params_file_cmd)
ld.add_action(declare_robot2_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
# Add the actions to start gazebo, robots and simulations
ld.add_action(start_gazebo_cmd)
for spawn_robot_cmd in spawn_robots_cmds:
ld.add_action(spawn_robot_cmd)
for simulation_instance_cmd in nav_instances_cmds:
ld.add_action(simulation_instance_cmd)
return ld
def generate_launch_description():
"""Launch system CPU and system memory metrics collector nodes."""
ld = LaunchDescription()
ld.add_action(
DeclareLaunchArgument(
CPU_NODE_NAME,
default_value=DEFAULT_CPU_NODE_NAME,
description=
'A custom name for the node that collects the total CPU usage'
' on a Linux system'))
ld.add_action(
DeclareLaunchArgument(
MEMORY_NODE_NAME,
default_value=DEFAULT_MEMORY_NODE_NAME,
description=
'A custom name for the node that collects the total memory usage'
' on a Linux system'))
ld.add_action(
DeclareLaunchArgument(
MEASUREMENT_PERIOD,
default_value=DEFAULT_MEASUREMENT_PERIOD_IN_MS,
description=
'The period (in ms) between each subsequent metrics measurement made'
' by the collector nodes'))
ld.add_action(
DeclareLaunchArgument(
PUBLISH_PERIOD,
default_value=DEFAULT_PUBLISH_PERIOD_IN_MS,
description=
'The period (in ms) between each subsequent metrics message published'
' by the collector nodes'))
ld.add_action(
DeclareLaunchArgument(
PUBLISH_TOPIC,
default_value=DEFAULT_PUBLISH_TOPIC,
description=
'The name of the topic to which the collector nodes should publish'
))
node_parameters = [{
MEASUREMENT_PERIOD:
LaunchConfiguration(MEASUREMENT_PERIOD)
}, {
PUBLISH_PERIOD: LaunchConfiguration(PUBLISH_PERIOD)
}]
"""Run system CPU and memory collector nodes using launch."""
ld.add_action(
Node(package='system_metrics_collector',
node_executable='linux_cpu_collector',
node_name=LaunchConfiguration(CPU_NODE_NAME),
parameters=node_parameters,
remappings=[('system_metrics', LaunchConfiguration(PUBLISH_TOPIC))
],
output='screen'))
ld.add_action(
Node(package='system_metrics_collector',
node_executable='linux_memory_collector',
node_name=LaunchConfiguration(MEMORY_NODE_NAME),
parameters=node_parameters,
remappings=[('system_metrics', LaunchConfiguration(PUBLISH_TOPIC))
],
output='screen'))
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
lifecycle_nodes = ['controller_server',
'planner_server',
'recoveries_server',
'bt_navigator',
'waypoint_follower']
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'default_bt_xml_filename': default_bt_xml_filename,
'autostart': autostart,
'map_subscribe_transient_local': map_subscribe_transient_local}
configured_params = RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
DeclareLaunchArgument(
'namespace', default_value='',
description='Top-level namespace'),
DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack'),
DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'my_robot_params.yaml'),
description='Full path to the ROS2 parameters file to use'),
DeclareLaunchArgument(
'default_bt_xml_filename',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'),
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use'),
DeclareLaunchArgument(
'map_subscribe_transient_local', default_value='false',
description='Whether to set the map subscriber QoS to transient local'),
Node(
package='nav2_controller',
executable='controller_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(
package='nav2_recoveries',
executable='recoveries_server',
name='recoveries_server',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(
package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(
package='nav2_waypoint_follower',
executable='waypoint_follower',
name='waypoint_follower',
output='screen',
parameters=[configured_params],
remappings=remappings),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_navigation',
output='screen',
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': lifecycle_nodes}]),
])
def generate_launch_description():
return LaunchDescription([
Node(package='realsense_ros2',
node_executable='rs_t265_node',
node_name='rs_t265',
output='screen'),
Node(
package='realsense_ros2',
node_executable='rs_d435_node',
node_name='rs_d435',
output='screen',
parameters=[
{
"publish_depth": True
},
{
"publish_pointcloud": True
},
{
"is_color": True
},
{
"publish_image_raw_": True
},
{
"fps": 30
} # Can only take values of 6,15,30 or 60
]),
Node(
## Configure the TF of the robot
package='tf2_ros',
node_executable='static_transform_publisher',
output='screen',
arguments=[
'0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 't265_frame',
'base_link'
]),
Node(package='tf2_ros',
node_executable='static_transform_publisher',
output='screen',
arguments=[
'0.0', '0.025', '0.03', '0.0', '0.0', '0.0', 'base_link',
'camera_link_d435'
]),
Node(
package='tf2_ros',
node_executable='static_transform_publisher',
output='screen',
arguments=[
'0.0', '0.025', '0.03', '-1.5708', '0.0', '-1.5708',
'base_link', 'camera_link_d435_pcl'
]
# arguments=['0.0', '0.025', '0.03', '0.0', '0.0', '0.0', 'base_link', 'camera_link_d435_pcl']
),
# Node(
# package='depthimage_to_laserscan',
# node_executable='depthimage_to_laserscan_node',
# node_name='scan',
# output='screen',
# parameters=[{'output_frame':'camera_link_d435'}],
# remappings=[('depth','rs_d435/aligned_depth/image_raw'),
# ('depth_camera_info', 'rs_d435/aligned_depth/camera_info')],
# ),
])
def generate_launch_description():
"""Launch description."""
# Get the directories
rover_config_dir = get_package_share_directory('rover_config')
# ## ROBOT MODEL
# Load XACRO and parse to URDF
xacro_model_name = 'marta.xacro'
xacro_model_path = os.path.join(rover_config_dir, 'urdf', xacro_model_name)
# Parse XACRO file to URDF
urdf_model_path, robot_desc = to_urdf(xacro_model_path)
# Create the launch configuration variables
config_file = LaunchConfiguration('config_file')
robot_description = LaunchConfiguration('robot_description')
urdf_path = LaunchConfiguration('urdf_path')
use_sim_time = LaunchConfiguration('use_sim_time')
use_ptu = LaunchConfiguration('use_ptu')
# Launch declarations
declare_config_file_cmd = DeclareLaunchArgument(
'config_file',
default_value=os.path.join(rover_config_dir, 'config', 'marta.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes.')
declare_urdf_path_cmd = DeclareLaunchArgument(
'urdf_path',
default_value=urdf_model_path,
description='Full path to robot urdf file.')
declare_robot_description_cmd = DeclareLaunchArgument(
'robot_description',
default_value=robot_desc,
description='Full path to robot urdf file.')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='False',
description='Use simulation (Gazebo) clock if true')
declare_use_ptu_cmd = DeclareLaunchArgument(
'use_ptu',
default_value='True',
description='Whether to start the PTU controller')
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'robot_description': robot_description,
'urdf_path': urdf_path}
configured_params = RewrittenYaml(
source_file=config_file,
root_key='',
param_rewrites=param_substitutions,
convert_types=True)
robot_state_pub_cmd = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher_node',
remappings=[('/joint_states', '/joint_states')],
parameters=[configured_params]
)
joy_cmd = Node(
package='joy',
executable='joy_node',
name='joy_node',
remappings=[('joy', 'gamepad')],
parameters=[configured_params],
)
gamepad_parser_cmd = Node(
package='gamepad_parser',
executable='gamepad_parser_node',
name='gamepad_parser_node',
remappings=[('/rover_motion_cmd', '/cmd_vel')],
parameters=[configured_params],
)
locomotion_manager_cmd = Node(
package='locomotion_manager',
executable='locomotion_manager_node',
name='locomotion_manager_node',
parameters=[configured_params],
)
simple_rover_locomotion_cmd = Node(
package='simple_rover_locomotion',
executable='simple_rover_locomotion_node',
name='simple_rover_locomotion_node',
remappings=[('/rover_motion_cmd', '/cmd_vel')],
# Parameters can be passed as dict or path to the .yaml
parameters=[configured_params],
)
stop_mode_cmd = Node(
package='locomotion_mode',
executable='stop_mode_node',
name='stop_mode_node',
remappings=[('/rover_motion_cmd', '/cmd_vel')],
# Parameters can be passed as dict or path to the .yaml
parameters=[configured_params],
)
ptu_control_cmd = Node(
condition=IfCondition(use_ptu),
package='ptu_control',
executable='ptu_control_node',
name='ptu_control_node',
parameters=[configured_params],
)
return LaunchDescription([
# Set env var to print messages colored. The ANSI color codes will appear in a log.
SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'),
# Launch Arguments
declare_config_file_cmd,
declare_urdf_path_cmd,
declare_robot_description_cmd,
declare_use_sim_time_cmd,
declare_use_ptu_cmd,
# Start Nodes
robot_state_pub_cmd,
joy_cmd,
gamepad_parser_cmd,
locomotion_manager_cmd,
simple_rover_locomotion_cmd,
stop_mode_cmd,
ptu_control_cmd
])
def generate_launch_description():
# Get the launch directory
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
pilot_bringup_dir = get_package_share_directory('pilot_urjc_bringup')
launch_dir = os.path.join(nav2_bringup_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
headless = LaunchConfiguration('headless')
world = LaunchConfiguration('world')
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='',
description='Top-level namespace')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_simulator_cmd = DeclareLaunchArgument(
'headless',
default_value='True',
description='Whether to execute gzclient)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# 'worlds/turtlebot3_worlds/waffle.model'),
default_value=os.path.join(pilot_bringup_dir, 'worlds', 'waffle_house.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
cwd=[launch_dir], output='screen')
start_gazebo_client_cmd = ExecuteProcess(
condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])),
cmd=['gzclient'],
cwd=[launch_dir], output='screen')
urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
remappings=remappings,
arguments=[urdf])
pcl2laser_cmd = LifecycleNode(
package='pointcloud_to_laserscan',
executable='pointcloud_to_laserscan_managed',
name='pointcloud_to_laser',
remappings=[('cloud_in', '/intel_realsense_r200_depth/points'),
('scan', '/mros_scan')],
parameters=[{
'target_frame': 'base_scan',
'transform_tolerance': 0.01,
'min_height': 0.0,
'max_height': 1.0,
'angle_min': -1.5708, # -M_PI/2
'angle_max': 1.5708, # M_PI/2
'angle_increment': 0.0087, # M_PI/360.0
'scan_time': 0.3333,
'range_min': 0.45,
'range_max': 4.0,
'use_inf': True,
'inf_epsilon': 1.0
}],
)
emit_event_to_request_that_pcl2laser_configure_transition = EmitEvent(
event=ChangeState(
lifecycle_node_matcher=launch.events.matches_action(pcl2laser_cmd),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
)
)
laser_resender_cmd = LifecycleNode(
name='laser_resender',
package='laser_resender',
executable='laser_resender_node',
output='screen')
battery_contingency_cmd = Node(
name='battery_contingency_sim',
package='mros_contingencies_sim',
executable='battery_contingency_sim_node',
output='screen')
components_file_path = (get_package_share_directory('mros_modes_observer') +
'/params/components.yaml')
modes_observer_node = Node(
package='mros_modes_observer',
executable='modes_observer_node',
parameters=[{'componentsfile': components_file_path}],
output='screen')
emit_event_to_request_that_laser_resender_configure_transition = EmitEvent(
event=ChangeState(
lifecycle_node_matcher=launch.events.matches_action(laser_resender_cmd),
transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
)
)
shm_model_path = (get_package_share_directory('pilot_urjc_bringup') +
'/params/pilot_modes.yaml')
# Start as a normal node is currently not possible.
# Path to SHM file should be passed as a ROS parameter.
mode_manager_node = Node(
package='system_modes',
executable='mode_manager',
parameters=[{'modelfile': shm_model_path}],
output='screen')
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
ld.add_action(start_robot_state_publisher_cmd)
# Add system modes manager
ld.add_action(mode_manager_node)
# Add system modes observer node
ld.add_action(modes_observer_node)
ld.add_action(pcl2laser_cmd)
ld.add_action(laser_resender_cmd)
ld.add_action(battery_contingency_cmd)
ld.add_action(emit_event_to_request_that_pcl2laser_configure_transition)
ld.add_action(emit_event_to_request_that_laser_resender_configure_transition)
return ld
def generate_launch_description():
return LaunchDescription([
Node(package='create_driver',
executable='create_driver',
output='screen'),
])
def generate_launch_description():
# Input parameters declaration
namespace = LaunchConfiguration('namespace')
params_file = LaunchConfiguration('params_file')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
# Variables
lifecycle_nodes = ['map_saver']
# Getting directories and launch-files
bringup_dir = get_package_share_directory('nav2_bringup')
slam_toolbox_dir = get_package_share_directory('slam_toolbox')
slam_launch_file = os.path.join(slam_toolbox_dir, 'launch',
'online_sync_launch.py')
# Create our own temporary YAML files that include substitutions
param_substitutions = {'use_sim_time': use_sim_time}
configured_params = RewrittenYaml(source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params',
'my_robot_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack')
# Nodes launching commands
start_slam_toolbox_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(slam_launch_file),
launch_arguments={'use_sim_time': use_sim_time}.items())
start_map_saver_server_cmd = Node(package='nav2_map_server',
node_executable='map_saver_server',
output='screen',
parameters=[configured_params])
start_lifecycle_manager_cmd = Node(package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager_slam',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'autostart': autostart
}, {
'node_names':
lifecycle_nodes
}])
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_autostart_cmd)
# Running SLAM Toolbox
ld.add_action(start_slam_toolbox_cmd)
# Running Map Saver Server
ld.add_action(start_map_saver_server_cmd)
ld.add_action(start_lifecycle_manager_cmd)
return ld
def _assert_launch_no_errors(self, actions):
ld = LaunchDescription(actions)
ls = LaunchService()
ls.include_launch_description(ld)
assert 0 == ls.run()
def generate_launch_description():
pkgShareDir = get_package_share_directory('yumi_launch')
configDir_L = os.path.join(pkgShareDir, 'config', 'yumi_params_L_sim.yaml')
configDir_R = os.path.join(pkgShareDir, 'config', 'yumi_params_R_sim.yaml')
urdf = os.path.join(get_package_share_directory('yumi_description'),
'urdf', 'yumi.urdf')
assert os.path.exists(urdf)
robot_description_config = load_file('yumi_description', 'urdf/yumi.urdf')
robot_description = {'robot_description': robot_description_config}
rviz_config_dir = os.path.join(
get_package_share_directory('yumi_description'), 'config',
'yumi_moveit2.rviz')
assert os.path.exists(rviz_config_dir)
# Globals
yumi_robot_manager = Node(package='yumi_robot_manager',
node_executable='yumi_robot_manager_node')
#output='screen')
global_joint_state = Node(package='ros2_control_utils',
node_executable='global_joint_state_node')
# Left Arm
abb_egm_hardware_left = Node(
package='abb_egm_hardware',
node_executable='abb_egm_hardware_node',
node_namespace='/l',
arguments=['/l'],
#output='screen',
parameters=[
os.path.join(get_package_share_directory("yumi_launch"), "config",
"yumi_left_controllers.yaml")
])
param_server_left = Node(package='parameter_server',
node_executable='param_server_node',
node_namespace='/l',
arguments=[configDir_L])
sg_control_left = Node(package='sg_control',
node_executable='sg_control_node',
node_namespace='/l')
# Right Arm
abb_egm_hardware_right = Node(
package='abb_egm_hardware',
node_executable='abb_egm_hardware_node',
node_namespace='/r',
arguments=['/r'],
output='screen',
parameters=[
os.path.join(get_package_share_directory("yumi_launch"), "config",
"yumi_right_controllers.yaml")
])
param_server_right = Node(package='parameter_server',
node_executable='param_server_node',
node_namespace='/r',
arguments=[configDir_R])
sg_control_right = Node(package='sg_control',
node_executable='sg_control_node',
node_namespace='/r')
# RViz
rviz_node = Node(package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[robot_description])
# # Publish base link TF
static_tf = Node(package='tf2_ros',
node_executable='static_transform_publisher',
node_name='static_transform_publisher',
arguments=[
'0.0', '0.0', '0.0', '0.0', '0.0', '0.0',
'yumi_base_link', 'yumi_body'
])
return LaunchDescription([
rviz_node, static_tf, yumi_robot_manager, global_joint_state,
abb_egm_hardware_left, param_server_left, sg_control_left,
abb_egm_hardware_right, param_server_right, sg_control_right
])
def generate_launch_description():
xacro_path = os.path.join(get_package_share_directory('dsr_description2'),
'xacro')
# RViz2
rviz_config_file = get_package_share_directory(
'dsr_description2') + "/rviz/default.rviz"
rviz_node = Node(package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file])
# Static TF
static_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
output='log',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'base', 'base_0'])
# robot_state_publisher
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
#output='both',
output='screen',
parameters=[{
'robot_description':
Command([
'xacro', ' ', xacro_path, '/',
LaunchConfiguration('model'), '.urdf.xacro color:=',
LaunchConfiguration('color')
])
}])
# dsr_control2
dsr_control2 = Node(
package='dsr_control2',
executable='dsr_control_node2',
name='dsr_control_node2',
output='screen',
parameters=[
{
"name": LaunchConfiguration('name')
},
{
"rate": 100
},
{
"standby": 5000
},
{
"command": True
},
{
"host": LaunchConfiguration('host')
},
{
"port": LaunchConfiguration('port')
},
{
"mode": LaunchConfiguration('mode')
},
{
"model": LaunchConfiguration('model')
},
{
"gripper": "none"
},
{
"mobile": "none"
},
#parameters_file_path # 파라미터 설정을 동일이름으로 launch 파일과 yaml 파일에서 할 경우 yaml 파일로 셋팅된다.
])
# gazebo2
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'm1013'],
output='screen')
return LaunchDescription(args + [
static_tf,
robot_state_publisher,
gazebo,
spawn_entity,
dsr_control2,
])
def generate_launch_description():
move_robot = Node(package='my_pkg',
node_executable='move_robot_node',
output='screen')
return LaunchDescription([move_robot])
def generate_launch_description():
# Get the launch directory
sm_aws_warehouse_navigation_dir = get_package_share_directory(
"sm_aws_warehouse_navigation")
nav_configuration_dir = get_package_share_directory("nav2_bringup")
namespace = LaunchConfiguration("namespace")
use_sim_time = LaunchConfiguration("use_sim_time")
autostart = LaunchConfiguration("autostart")
params_file = LaunchConfiguration("params_file")
default_nav_to_pose_bt_xml = LaunchConfiguration(
"default_nav_to_pose_bt_xml")
# map_subscribe_transient_local = LaunchConfiguration("map_subscribe_transient_local")
params_file_path = os.path.join(
sm_aws_warehouse_navigation_dir,
"params",
"nav2z_client",
"nav2_params.yaml",
)
# params_file_path = os.path.join(
# nav_configuration_dir,
# "params",
# "nav2_params.yaml",
# )
default_nav_bt_tree_xml_filepath = os.path.join(
sm_aws_warehouse_navigation_dir, "params", "nav2z_client",
"navigation_tree.xml")
# default_nav_bt_tree_xml_filepath = os.path.join(
# get_package_share_directory("nav2_bt_navigator"), "behavior_trees", "navigate_to_pose_w_replanning_and_recovery.xml")
# default_nav_bt_tree_xml_filepath = os.path.join(get_package_share_directory('nav2_bt_navigator'),
# 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
params_file_path = os.path.join(
sm_aws_warehouse_navigation_dir,
"params",
"nav2z_client",
"nav2_params.yaml",
)
# params_file_path = os.path.join(
# nav_configuration_dir,
# "params",
# "nav2_params.yaml",
# )
default_nav_bt_tree_xml_filepath = os.path.join(
sm_aws_warehouse_navigation_dir, "params", "nav2z_client",
"navigation_tree.xml")
# default_nav_bt_tree_xml_filepath = os.path.join(
# get_package_share_directory("nav2_bt_navigator"), "behavior_trees", "navigate_to_pose_w_replanning_and_recovery.xml")
# default_nav_bt_tree_xml_filepath = os.path.join(get_package_share_directory('nav2_bt_navigator'),
# 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
lifecycle_nodes = [
"controller_server", "planner_server", "recoveries_server",
"bt_navigator"
]
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
"use_sim_time": use_sim_time,
# 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
"default_nav_to_pose_bt_xml": default_nav_bt_tree_xml_filepath,
# "autostart": autostart,
# "map_subscribe_transient_local": map_subscribe_transient_local,
}
configured_params = RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True,
)
xtermprefix = (
"xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' "
"-hold -geometry 1000x600 -sl 10000 -e")
print("+********************************")
print(str(param_substitutions))
print(str(default_nav_to_pose_bt_xml))
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"),
DeclareLaunchArgument("namespace",
default_value="",
description="Top-level namespace"),
DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Use simulation (Gazebo) clock if true",
),
DeclareLaunchArgument(
"autostart",
default_value="true",
description="Automatically startup the nav2 stack",
),
DeclareLaunchArgument(
"params_file",
default_value=params_file_path,
description="Full path to the ROS2 parameters file to use",
),
DeclareLaunchArgument(
"default_nav_to_pose_bt_xml",
# default_value=os.path.join(get_package_share_directory('nav2_bt_navigator'),
# 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
default_value=os.path.join(
nav_configuration_dir,
"params",
"nav2z_client",
"navigation_tree.xml",
),
description="Full path to the behavior tree xml file to use",
),
DeclareLaunchArgument(
"map_subscribe_transient_local",
default_value="false",
description=
"Whether to set the map subscriber QoS to transient local",
),
Node(
package="nav2_controller",
executable="controller_server",
output="screen",
prefix=xtermprefix,
parameters=[configured_params],
remappings=remappings,
arguments=["--ros-args", "--log-level", "INFO"],
),
Node(
package="nav2_planner",
executable="planner_server",
name="planner_server",
output="screen",
prefix=xtermprefix,
parameters=[configured_params],
remappings=remappings,
arguments=["--ros-args", "--log-level", "INFO"],
),
Node(
package="nav2_recoveries",
executable="recoveries_server",
name="recoveries_server",
output="screen",
parameters=[configured_params],
remappings=remappings,
arguments=["--ros-args", "--log-level", "INFO"],
),
Node(
package="nav2_bt_navigator",
executable="bt_navigator",
name="bt_navigator",
output="screen",
prefix=xtermprefix,
parameters=[configured_params],
remappings=remappings,
arguments=["--ros-args", "--log-level", "INFO"],
),
Node(
package="nav2_waypoint_follower",
executable="waypoint_follower",
name="waypoint_follower",
output="screen",
parameters=[configured_params],
remappings=remappings,
),
Node(
package="nav2_lifecycle_manager",
executable="lifecycle_manager",
name="lifecycle_manager_navigation",
output="screen",
prefix=xtermprefix,
parameters=[
{
"use_sim_time": use_sim_time
},
{
"autostart": autostart
},
{
"node_names": lifecycle_nodes
},
],
arguments=["--ros-args", "--log-level", "INFO"],
),
])
def generate_launch_description():
# 1 or more drones:
drones = ['drone1', 'drone2', 'drone3', 'drone4']
# Starting locations:
starting_locations = [
# Face the wall of markers in fiducial.world
['-2.5', '1.5', '1', '0'],
['-1.5', '0.5', '1', '0.785'],
['-0.5', '1.5', '1', '0'],
['-1.5', '2.5', '1', '-0.785']
# Face all 4 directions in f2.world
# ['-2.5', '1.5', '1', '0'],
# ['-1.5', '0.5', '1', '1.57'],
# ['-0.5', '1.5', '1', '3.14'],
# ['-1.5', '2.5', '1', '-1.57']
]
tello_gazebo_path = get_package_share_directory('tello_gazebo')
tello_description_path = get_package_share_directory('tello_description')
world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world')
map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml')
# Global entities
entities = [
# Launch Gazebo, loading tello.world
ExecuteProcess(cmd=[
'gazebo',
'--verbose',
'-s', 'libgazebo_ros_init.so', # Publish /clock
'-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node
world_path
], output='screen'),
# Load and publish a known map
Node(package='fiducial_vlam', executable='vmap_main', output='screen',
name='vmap_main', parameters=[{
'use_sim_time': True, # Use /clock if available
'publish_tfs': 1, # Publish marker /tf
'marker_length': 0.1778, # Marker length
'marker_map_load_full_filename': map_path, # Load a pre-built map from disk
'make_not_use_map': 0 # Don't save a map to disk
}]),
# Joystick driver, generates /namespace/joy messages
Node(package='joy', executable='joy_node', output='screen',
name='joy_node', parameters=[{
'use_sim_time': True, # Use /clock if available
}]),
# Flock controller (basically a joystick multiplexer, also starts/stops missions)
Node(package='flock2', executable='flock_base', output='screen',
name='flock_base', parameters=[{
'use_sim_time': True, # Use /clock if available
'drones': drones
}]),
# WIP: planner
Node(package='flock2', executable='planner_node', output='screen',
name='planner_node', parameters=[{
'use_sim_time': True, # Use /clock if available
'drones': drones,
'arena_x': -5.0,
'arena_y': -5.0,
'arena_z': 10.0,
}]),
]
# Per-drone entities
for idx, namespace in enumerate(drones):
suffix = '_' + str(idx + 1)
urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf')
entities.extend([
# Add a drone to the simulation
Node(package='tello_gazebo', executable='inject_entity.py', output='screen',
arguments=[urdf_path]+starting_locations[idx]),
# Publish base_link to camera_link tf
# Node(package='robot_state_publisher', executable='robot_state_publisher', output='screen',
# name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{
# 'use_sim_time': True, # Use /clock if available
# }]),
# Localize this drone against the map
# Future: need odometry for base_link, not camera_link
Node(package='fiducial_vlam', executable='vloc_main', output='screen',
name='vloc_main', namespace=namespace, parameters=[{
'use_sim_time': True, # Use /clock if available
'publish_tfs': 1, # Publish drone and camera /tf
'stamp_msgs_with_current_time': 0, # Use incoming message time, not now()
'base_frame_id': 'base_link' + suffix,
'map_init_pose_z': -0.035,
'camera_frame_id': 'camera_link' + suffix,
'base_odometry_pub_topic': 'base_odom',
'sub_camera_info_best_effort_not_reliable': 1, # Gazebo camera uses 'best effort'
}]),
# Drone controller
Node(package='flock2', executable='drone_base', output='screen',
name='drone_base', namespace=namespace, parameters=[{
'use_sim_time': True, # Use /clock if available
}]),
])
return LaunchDescription(entities)
def test_launch_description_deprecated():
ld = LaunchDescription(deprecated_reason='DEPRECATED MESSAGE')
ld.visit(MockLaunchContext())
assert ld.deprecated is True
assert ld.deprecated_reason == 'DEPRECATED MESSAGE'
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
namespace = LaunchConfiguration('namespace')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
use_remappings = LaunchConfiguration('use_remappings')
map_subscribe_transient_local = LaunchConfiguration(
'map_subscribe_transient_local')
# TODO(orduno) Remove once `PushNodeRemapping` is resolved
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'), ('/tf', 'tf'),
('/tf_static', 'tf_static')]
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'bt_xml_filename': bt_xml_file,
'autostart': autostart,
'map_subscribe_transient_local': map_subscribe_transient_local
}
configured_params = RewrittenYaml(source_file=params_file,
root_key=namespace,
param_rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
DeclareLaunchArgument('namespace',
default_value='',
description='Top-level namespace'),
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack'),
DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params',
'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use'),
DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'),
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use'),
DeclareLaunchArgument(
'use_lifecycle_mgr',
default_value='true',
description='Whether to launch the lifecycle manager'),
DeclareLaunchArgument(
'use_remappings',
default_value='false',
description='Arguments to pass to all nodes launched by the file'),
DeclareLaunchArgument(
'map_subscribe_transient_local',
default_value='false',
description=
'Whether to set the map subscriber QoS to transient local'),
Node(package='nav2_controller',
node_executable='controller_server',
output='screen',
parameters=[configured_params],
use_remappings=IfCondition(use_remappings),
remappings=remappings),
Node(package='nav2_planner',
node_executable='planner_server',
node_name='planner_server',
output='screen',
parameters=[configured_params],
use_remappings=IfCondition(use_remappings),
remappings=remappings),
Node(package='nav2_recoveries',
node_executable='recoveries_server',
node_name='recoveries_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
use_remappings=IfCondition(use_remappings),
remappings=remappings),
Node(package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[configured_params],
use_remappings=IfCondition(use_remappings),
remappings=remappings),
Node(package='nav2_waypoint_follower',
node_executable='waypoint_follower',
node_name='waypoint_follower',
output='screen',
parameters=[configured_params],
use_remappings=IfCondition(use_remappings),
remappings=remappings),
Node(condition=IfCondition(use_lifecycle_mgr),
package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager_navigation',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'autostart': autostart
}, {
'node_names': [
'controller_server', 'planner_server',
'recoveries_server', 'bt_navigator', 'waypoint_follower'
]
}]),
])
def test_launch_description_get_launch_arguments():
"""Test the get_launch_arguments() method of the LaunchDescription class."""
ld = LaunchDescription([])
assert len(ld.get_launch_arguments()) == 0
ld = LaunchDescription([DeclareLaunchArgument('foo')])
la = ld.get_launch_arguments()
assert len(la) == 1
assert la[0]._conditionally_included is False
ld = LaunchDescription(
[DeclareLaunchArgument('foo', condition=IfCondition('True'))])
la = ld.get_launch_arguments()
assert len(la) == 1
assert la[0]._conditionally_included is True
ld = LaunchDescription([
IncludeLaunchDescription(
LaunchDescriptionSource(
LaunchDescription([
DeclareLaunchArgument('foo'),
]))),
])
la = ld.get_launch_arguments()
assert len(la) == 0
this_dir = os.path.dirname(os.path.abspath(__file__))
ld = LaunchDescription([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(this_dir,
'launch_file_with_argument.launch.py'))),
])
la = ld.get_launch_arguments()
assert len(la) == 0
# From issue #144: get_launch_arguments was broken when an entitity had conditional
# sub entities
class EntityWithConditional(LaunchDescriptionEntity):
def describe_conditional_sub_entities(self):
return [('String describing condition',
[DeclareLaunchArgument('foo')])]
ld = LaunchDescription([EntityWithConditional()])
la = ld.get_launch_arguments()
assert len(la) == 1
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
slam = LaunchConfiguration('slam')
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
headless = LaunchConfiguration('headless')
world = LaunchConfiguration('world')
# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
# https://github.com/ros/geometry2/issues/32
# https://github.com/ros/robot_state_publisher/pull/30
# TODO(orduno) Substitute with `PushNodeRemapping`
# https://github.com/ros2/launch_ros/issues/56
remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace', default_value='', description='Top-level namespace')
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_slam_cmd = DeclareLaunchArgument('slam',
default_value='False',
description='Whether run a SLAM')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps',
'turtlebot3_world.yaml'),
description='Full path to map file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'), 'behavior_trees',
'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_simulator_cmd = DeclareLaunchArgument(
'headless',
default_value='False',
description='Whether to execute gzclient)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# 'worlds/turtlebot3_worlds/waffle.model'),
default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
cwd=[launch_dir],
output='screen')
start_gazebo_client_cmd = ExecuteProcess(condition=IfCondition(
PythonExpression([use_simulator, ' and not ', headless])),
cmd=['gzclient'],
cwd=[launch_dir],
output='screen')
urdf = os.path.join(bringup_dir, 'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
namespace=namespace,
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
remappings=remappings,
arguments=[urdf])
rviz_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'rviz_launch.py')),
condition=IfCondition(use_rviz),
launch_arguments={
'namespace': '',
'use_namespace': 'False',
'rviz_config': rviz_config_file
}.items())
bringup_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'bringup_launch.py')),
launch_arguments={
'namespace': namespace,
'use_namespace': use_namespace,
'slam': slam,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': autostart
}.items())
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_slam_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(rviz_cmd)
ld.add_action(bringup_cmd)
return ld
def generate_launch_description():
return LaunchDescription([
Node(package='lkas5', executable='splash_server'),
Node(package='lkas5', executable='default_build_unit'),
])
def generate_launch_description():
"""Launch topic statistics collector nodes."""
ld = LaunchDescription()
ld.add_action(
DeclareLaunchArgument(
COLLECTOR_NODE_NAME,
default_value=DEFAULT_COLLECTOR_NODE_NAME,
description='A name for the node that collects topic statistics'))
ld.add_action(
DeclareLaunchArgument(
MONITORED_TOPIC_NAME,
default_value=DEFAULT_MONITORED_TOPIC_NAME,
description=
'The topic to subscribe to in order to measure message metrics'))
ld.add_action(
DeclareLaunchArgument(
PUBLISH_PERIOD_IN_MS,
default_value=DEFAULT_PUBLISH_PERIOD_IN_MS,
description=
'The period (in ms) between each subsequent metrics message published'
' by the collector node'))
ld.add_action(
DeclareLaunchArgument(
PUBLISH_TOPIC_NAME,
default_value=DEFAULT_PUBLISH_TOPIC,
description=
'The name of the topic to which the collector nodes should publish'
))
node_parameters = [{
MONITORED_TOPIC_NAME:
LaunchConfiguration(MONITORED_TOPIC_NAME)
}, {
PUBLISH_TOPIC_NAME:
LaunchConfiguration(PUBLISH_TOPIC_NAME)
}, {
PUBLISH_PERIOD_IN_MS:
LaunchConfiguration(PUBLISH_PERIOD_IN_MS)
}]
"""Run topic statistics collector node using launch."""
ld.add_action(
Node(package='system_metrics_collector',
node_executable='topic_statistics_node',
name=LaunchConfiguration(COLLECTOR_NODE_NAME),
parameters=node_parameters,
remappings=[('system_metrics',
LaunchConfiguration(PUBLISH_TOPIC_NAME))],
output='screen'))
return ld
def test_include_launch_description_constructors():
"""Test the constructors for IncludeLaunchDescription class."""
IncludeLaunchDescription(LaunchDescriptionSource(LaunchDescription()))
IncludeLaunchDescription(LaunchDescriptionSource(LaunchDescription()),
launch_arguments={'foo': 'FOO'}.items())
def run(self):
"""
Launch the processes under test and run the tests.
:return: A tuple of two unittest.Results - one for tests that ran while processes were
active, and another set for tests that ran after processes were shutdown
"""
test_ld, test_context = self._test_run.normalized_test_description(
ready_fn=lambda: self._processes_launched.set())
# Data that needs to be bound to the tests:
proc_info = ActiveProcInfoHandler()
proc_output = ActiveIoHandler()
full_context = dict(test_context, **self._test_run.param_args)
parsed_launch_arguments = parse_launch_arguments(
self._launch_file_arguments)
test_args = {}
for k, v in parsed_launch_arguments:
test_args[k] = v
self._test_run.bind(
self._test_run.pre_shutdown_tests,
injected_attributes={
'proc_info': proc_info,
'proc_output': proc_output,
'test_args': test_args,
},
injected_args=dict(
full_context,
# Add a few more things to the args dictionary:
**{
'proc_info': proc_info,
'proc_output': proc_output,
'test_args': test_args
}))
self._test_run.bind(
self._test_run.post_shutdown_tests,
injected_attributes={
'proc_info': proc_info._proc_info_handler,
'proc_output': proc_output._io_handler,
'test_args': test_args,
},
injected_args=dict(
full_context,
# Add a few more things to the args dictionary:
**{
'proc_info': proc_info._proc_info_handler,
'proc_output': proc_output._io_handler,
'test_args': test_args
}))
# Wrap the test_ld in another launch description so we can bind command line arguments to
# the test and add our own event handlers for process IO and process exit:
launch_description = LaunchDescription([
*self._test_run_preamble,
launch.actions.IncludeLaunchDescription(
launch.LaunchDescriptionSource(launch_description=test_ld),
launch_arguments=parsed_launch_arguments),
RegisterEventHandler(
OnProcessExit(
on_exit=lambda info, unused: proc_info.append(info))),
RegisterEventHandler(
OnProcessIO(
on_stdout=proc_output.append,
on_stderr=proc_output.append,
)),
])
self._launch_service.include_launch_description(launch_description)
self._test_tr.start() # Run the tests on another thread
self._launch_service.run(
) # This will block until the test thread stops it
if not self._tests_completed.wait(timeout=0):
# LaunchService.run returned before the tests completed. This can be because the user
# did ctrl+c, or because all of the launched nodes died before the tests completed
print('Processes under test stopped before tests completed')
# Give some extra help debugging why processes died early
self._print_process_output_summary(proc_info, proc_output)
# We treat this as a test failure and return some test results indicating such
raise _LaunchDiedException()
inactive_results = unittest.TextTestRunner(
verbosity=2,
resultclass=TestResult).run(self._test_run.post_shutdown_tests)
self._results.append(inactive_results)
return self._results
def test_include_launch_description_launch_arguments():
"""Test the interactions between declared launch arguments and IncludeLaunchDescription."""
# test that arguments are set when given, even if they are not declared
ld1 = LaunchDescription([])
action1 = IncludeLaunchDescription(
LaunchDescriptionSource(ld1),
launch_arguments={'foo': 'FOO'}.items(),
)
assert len(action1.launch_arguments) == 1
lc1 = LaunchContext()
result1 = action1.visit(lc1)
assert len(result1) == 2
assert isinstance(result1[0], SetLaunchConfiguration)
assert perform_substitutions(lc1, result1[0].name) == 'foo'
assert perform_substitutions(lc1, result1[0].value) == 'FOO'
assert result1[1] == ld1
# test that a declared argument that is not provided raises an error
ld2 = LaunchDescription([DeclareLaunchArgument('foo')])
action2 = IncludeLaunchDescription(LaunchDescriptionSource(ld2))
lc2 = LaunchContext()
with pytest.raises(RuntimeError) as excinfo2:
action2.visit(lc2)
assert 'Included launch description missing required argument' in str(
excinfo2.value)
# test that a declared argument that is not provided raises an error, but with other args set
ld2 = LaunchDescription([DeclareLaunchArgument('foo')])
action2 = IncludeLaunchDescription(
LaunchDescriptionSource(ld2),
launch_arguments={'not_foo': 'NOT_FOO'}.items(),
)
lc2 = LaunchContext()
with pytest.raises(RuntimeError) as excinfo2:
action2.visit(lc2)
assert 'Included launch description missing required argument' in str(
excinfo2.value)
assert 'not_foo' in str(excinfo2.value)
# test that a declared argument with a default value that is not provided does not raise
ld2 = LaunchDescription(
[DeclareLaunchArgument('foo', default_value='FOO')])
action2 = IncludeLaunchDescription(LaunchDescriptionSource(ld2))
lc2 = LaunchContext()
action2.visit(lc2)
# Test that default arguments in nested IncludeLaunchDescription actions do not raise
ld1 = LaunchDescription(
[DeclareLaunchArgument('foo', default_value='FOO')])
action1 = IncludeLaunchDescription(LaunchDescriptionSource(ld1), )
ld2 = LaunchDescription([action1, DeclareLaunchArgument('foo2')])
action2 = IncludeLaunchDescription(
LaunchDescriptionSource(ld2),
launch_arguments={'foo2': 'FOO2'}.items(),
)
lc2 = LaunchContext()
action2.visit(lc2)
# Test that provided launch arguments of nested IncludeLaunchDescription actions do not raise
ld1 = LaunchDescription([DeclareLaunchArgument('foo')])
action1 = IncludeLaunchDescription(
LaunchDescriptionSource(ld1),
launch_arguments={'foo': 'FOO'}.items(),
)
ld2 = LaunchDescription([action1, DeclareLaunchArgument('foo2')])
action2 = IncludeLaunchDescription(
LaunchDescriptionSource(ld2),
launch_arguments={'foo2': 'FOO2'}.items(),
)
lc2 = LaunchContext()
action2.visit(lc2)
# Test that arguments can not be passed from the parent launch description
ld1 = LaunchDescription([DeclareLaunchArgument('foo')])
action1 = IncludeLaunchDescription(LaunchDescriptionSource(ld1))
ld2 = LaunchDescription([action1, DeclareLaunchArgument('foo2')])
action2 = IncludeLaunchDescription(
LaunchDescriptionSource(ld2),
launch_arguments={
'foo': 'FOO',
'foo2': 'FOO2'
}.items(),
)
ld3 = LaunchDescription([action2])
ls = LaunchService()
ls.include_launch_description(ld3)
assert 1 == ls.run()
# Test that arguments can be redeclared in the parent launch description
ld1 = LaunchDescription([DeclareLaunchArgument('foo')])
action1 = IncludeLaunchDescription(LaunchDescriptionSource(ld1))
ld2 = LaunchDescription(
[action1,
DeclareLaunchArgument('foo'),
DeclareLaunchArgument('foo2')])
action2 = IncludeLaunchDescription(
LaunchDescriptionSource(ld2),
launch_arguments={
'foo': 'FOO',
'foo2': 'FOO2'
}.items(),
)
lc2 = LaunchContext()
action2.visit(lc2)
def generate_launch_description():
camera_name = 'forward_camera'
orca_bringup_dir = get_package_share_directory('orca_bringup')
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
orca_description_dir = get_package_share_directory('orca_description')
nav2_bringup_launch_dir = os.path.join(nav2_bringup_dir, 'launch')
urdf_file = os.path.join(orca_description_dir, 'urdf', 'hw7.urdf')
nav2_bt_file = os.path.join(orca_bringup_dir, 'behavior_trees',
'orca3_bt.xml')
use_sim_time = LaunchConfiguration('use_sim_time')
orca_params_file = LaunchConfiguration('orca_params_file')
nav2_params_file = LaunchConfiguration('nav2_params_file')
vlam_map_file = LaunchConfiguration('vlam_map')
nav2_map_file = LaunchConfiguration('nav2_map')
# get_package_share_directory('orb_slam2_ros') will fail if orb_slam2_ros isn't installed
orb_voc_file = os.path.join('install', 'orb_slam2_ros', 'share',
'orb_slam2_ros', 'orb_slam2', 'Vocabulary',
'ORBvoc.txt')
# Read the params file and make some global substitutions
configured_orca_params = RewrittenYaml(source_file=orca_params_file,
param_rewrites={
'use_sim_time':
use_sim_time,
'marker_map_load_full_filename':
vlam_map_file,
},
convert_types=True)
configured_nav2_params = RewrittenYaml(
source_file=nav2_params_file,
param_rewrites={
'use_sim_time': use_sim_time,
'yaml_filename': nav2_map_file,
'default_nav_through_poses_bt_xml': 'invalid_file',
'default_nav_to_pose_bt_xml': nav2_bt_file,
},
convert_types=True)
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
DeclareLaunchArgument(
'use_sim_time',
default_value='False', # TODO sim time broken
description='Use simulation (Gazebo) clock (BROKEN BROKEN BROKEN)?'
),
DeclareLaunchArgument('slam',
default_value='orb',
description='Choose SLAM strategy: ' +
', '.join(slams)),
DeclareLaunchArgument('nav',
default_value='True',
description='Launch nav?'),
DeclareLaunchArgument(
'vlam_map',
default_value=
'install/orca_gazebo/share/orca_gazebo/worlds/medium_ring_map.yaml',
description='Full path to Vlam map file'),
DeclareLaunchArgument(
'nav2_map',
default_value=
'install/orca_bringup/share/orca_bringup/worlds/empty_world.yaml',
description='Full path to Nav2 map file'),
DeclareLaunchArgument(
'orca_params_file',
default_value=os.path.join(orca_bringup_dir, 'params',
'orca_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for Orca nodes'),
DeclareLaunchArgument(
'nav2_params_file',
default_value=os.path.join(orca_bringup_dir, 'params',
'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for Nav2 nodes'),
# Publish static /tf
Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
name='robot_state_publisher',
arguments=[urdf_file],
parameters=[configured_orca_params]),
# Barometer filter
Node(package='orca_base',
executable='baro_filter_node',
output='screen',
name='baro_filter_node',
parameters=[configured_orca_params]),
# Publish /joy
# joy::joy_node uses SDL; multiplatform, but burns a lot of CPU
# Use joy_linux::joy_linux_node instead
# https://discourse.libsdl.org/t/patch-for-sdl-waitevent-to-avoid-using-cpu-while-waiting-an-event/28555
Node(package='joy_linux',
executable='joy_linux_node',
output='screen',
name='joy_linux_node',
parameters=[configured_orca_params]),
# Subscribe to /joy and publish /cmd_vel
Node(package='orca_topside',
executable='teleop_node',
output='screen',
name='teleop_node',
parameters=[configured_orca_params],
remappings=[
('slam', 'orb_slam2_stereo_node/status'),
('debug_image', 'orb_slam2_stereo_node/debug_image'),
]),
# Subscribe to /cmd_vel and publish /thrust, /odom and /tf odom->base_link
Node(package='orca_base',
executable='base_controller',
output='screen',
name='base_controller',
parameters=[configured_orca_params],
remappings=[
('barometer', 'filtered_barometer'),
]),
# fiducial_vlam: publish a map of ArUco markers
Node(package='fiducial_vlam',
executable='vmap_main',
output='screen',
name='vmap_main',
parameters=[configured_orca_params],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# fiducial_vlam: find ArUco markers and publish the camera pose
Node(package='fiducial_vlam',
executable='vloc_main',
output='screen',
name='vloc_main',
namespace=camera_name,
parameters=[configured_orca_params],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# fiducial_vlam: subscribe to the camera pose and publish /tf map->odom
Node(package='orca_localize',
executable='fiducial_localizer',
output='screen',
name='fiducial_localizer',
parameters=[configured_orca_params],
remappings=[
('camera_pose', '/' + camera_name + '/camera_pose'),
],
condition=LaunchConfigurationEquals('slam', 'vlam')),
# orb_slam2: build a map of 3d points, localize against the map, and publish the camera pose
Node(package='orb_slam2_ros',
executable='orb_slam2_ros_stereo',
output='screen',
name='orb_slam2_stereo',
parameters=[configured_orca_params, {
'voc_file': orb_voc_file,
}],
remappings=[
('/image_left/image_color_rect', '/stereo/left/image_raw'),
('/image_right/image_color_rect', '/stereo/right/image_raw'),
('/camera/camera_info', '/stereo/left/camera_info'),
],
condition=LaunchConfigurationEquals('slam', 'orb')),
# orb_slam2, but inputs are h264 streams rather than rectified images
Node(
package='orb_slam2_ros',
executable='orb_slam2_ros_h264_stereo',
output='screen',
name=
'orb_slam2_stereo', # Same name so 'orb' and 'orb_h264' can share param files
parameters=[configured_orca_params, {
'voc_file': orb_voc_file,
}],
remappings=[
# No need to remap. Node expects:
# /stereo/left/image_raw/h264
# /stereo/left/camera_info
# /stereo/right/image_raw/h264
# /stereo/right/camera_info
],
condition=LaunchConfigurationEquals('slam', 'orb_h264')),
# orb_slam2: subscribe to the camera pose and publish /tf map->odom
Node(
package='orca_localize',
executable='orb_slam2_localizer',
output='screen',
name='orb_slam2_localizer',
parameters=[configured_orca_params],
remappings=[
# Topic is hard coded in orb_slam2_ros to /orb_slam2_stereo_node/pose
('/camera_pose', '/orb_slam2_stereo_node/pose'),
],
# Launch if slam:=orb or slam:=orb_h264
condition=IfCondition(
PythonExpression(
["'orb' in '",
LaunchConfiguration('slam'), "'"]))),
# Publish a [likely empty] nav2 map
Node(package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[configured_nav2_params],
condition=IfCondition(LaunchConfiguration('nav'))),
# Manage the lifecycle of map_server
Node(package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_map_server',
output='screen',
parameters=[{
'use_sim_time': use_sim_time,
'autostart': True,
'node_names': ['map_server'],
}],
condition=IfCondition(LaunchConfiguration('nav'))),
# Include the rest of Nav2
IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(nav2_bringup_launch_dir, 'navigation_launch.py')),
launch_arguments={
'use_sim_time': use_sim_time,
'autostart': 'True',
'params_file': nav2_params_file,
'map_subscribe_transient_local': 'true',
}.items(),
condition=IfCondition(
LaunchConfiguration('nav'))),
])
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
use_sim_time = LaunchConfiguration('use_sim_time')
autostart = LaunchConfiguration('autostart')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
map_subscribe_transient_local = LaunchConfiguration(
'map_subscribe_transient_local')
# Create our own temporary YAML files that include substitutions
param_substitutions = {
'use_sim_time': use_sim_time,
'bt_xml_filename': bt_xml_file,
'autostart': autostart,
'map_subscribe_transient_local': map_subscribe_transient_local
}
configured_params = RewrittenYaml(source_file=params_file,
rewrites=param_substitutions,
convert_types=True)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'autostart',
default_value='true',
description='Automatically startup the nav2 stack'),
DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params',
'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use'),
DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_prefix('nav2_bt_navigator'), 'behavior_trees',
'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use'),
DeclareLaunchArgument(
'use_lifecycle_mgr',
default_value='true',
description='Whether to launch the lifecycle manager'),
DeclareLaunchArgument(
'map_subscribe_transient_local',
default_value='false',
description=
'Whether to set the map subscriber QoS to transient local'),
Node(package='dwb_controller',
node_executable='dwb_controller',
output='screen',
parameters=[configured_params]),
Node(package='nav2_planner',
node_executable='planner_server',
node_name='planner_server',
output='screen',
parameters=[configured_params]),
Node(package='nav2_recoveries',
node_executable='recoveries_node',
node_name='recoveries',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}]),
Node(package='nav2_bt_navigator',
node_executable='bt_navigator',
node_name='bt_navigator',
output='screen',
parameters=[configured_params]),
Node(condition=IfCondition(use_lifecycle_mgr),
package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager_navigation',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'autostart': autostart
}, {
'node_names':
['dwb_controller', 'planner_server', 'bt_navigator']
}]),
])
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
use_remappings = LaunchConfiguration('use_remappings')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_simulator = LaunchConfiguration('use_simulator')
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
use_rviz = LaunchConfiguration('use_rviz')
headless = LaunchConfiguration('headless')
world = LaunchConfiguration('world')
# TODO(orduno) Remove once `PushNodeRemapping` is resolved
# https://github.com/ros2/launch_ros/issues/56
remappings = [((namespace, '/tf'), '/tf'),
((namespace, '/tf_static'), '/tf_static'),
('/tf', 'tf'),
('/tf_static', 'tf_static')]
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='',
description='Top-level namespace')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
description='Full path to map file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
declare_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'),
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='false',
description='Automatically startup the nav2 stack')
declare_use_remappings_cmd = DeclareLaunchArgument(
'use_remappings', default_value='false',
description='Arguments to pass to all nodes launched by the file')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config_file',
default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_simulator_cmd = DeclareLaunchArgument(
'use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
'use_robot_state_pub',
default_value='True',
description='Whether to start the robot state publisher')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz',
default_value='True',
description='Whether to start RVIZ')
declare_simulator_cmd = DeclareLaunchArgument(
'headless',
default_value='False',
description='Whether to execute gzclient)')
declare_world_cmd = DeclareLaunchArgument(
'world',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'),
# 'worlds/turtlebot3_worlds/waffle.model'),
default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'),
description='Full path to world model file to load')
# Specify the actions
start_gazebo_server_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=['gzserver', '-s', 'libgazebo_ros_init.so', world],
cwd=[launch_dir], output='screen')
start_gazebo_client_cmd = ExecuteProcess(
condition=(IfCondition(use_simulator) and UnlessCondition(headless)),
cmd=['gzclient'],
cwd=[launch_dir], output='screen')
urdf = os.path.join(
get_package_share_directory('turtlebot3_description'), 'urdf', 'turtlebot3_waffle.urdf')
start_robot_state_publisher_cmd = Node(
condition=IfCondition(use_robot_state_pub),
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
use_remappings=IfCondition(use_remappings),
remappings=remappings,
arguments=[urdf])
start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', rviz_config_file],
output='screen',
use_remappings=IfCondition(use_remappings),
remappings=[('/tf', 'tf'),
('/tf_static', 'tf_static'),
('goal_pose', 'goal_pose'),
('/clicked_point', 'clicked_point'),
('/initialpose', 'initialpose')])
exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_bringup_launch.py')),
launch_arguments={'namespace': namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': autostart,
'use_remappings': use_remappings}.items())
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_bt_xml_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_remappings_cmd)
ld.add_action(declare_rviz_config_file_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_use_robot_state_pub_cmd)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any conditioned actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
ld.add_action(start_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
ld.add_action(bringup_cmd)
return ld
def generate_launch_description():
# Set to False to start only Rviz2 when the ZED node is started by other launch files
# For example when you run this script on a remote console to monitor the robot status.
start_zed_node = True
# Camera model
# use:
# - 'zed' for "ZED" camera
# - 'zedm' for "ZED mini" camera
# - 'zed2' for "ZED2" camera
# - 'zed2i' for "ZED2i" camera
camera_model = 'zedm'
# Camera name. Can be different from camera model, used to distinguish camera in multi-camera systems
camera_name = 'zedm'
# Rviz2 Configurations to be loaded by ZED Node
config_rviz2 = os.path.join(
get_package_share_directory('zed_display_rviz2'),
'rviz2',
camera_model + '.rviz'
)
# Set LOG format
os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time} [{name}] [{severity}] {message}'
# Rviz2 node
rviz2_node = Node(
package='rviz2',
namespace=camera_name,
executable='rviz2',
name=camera_name+'_rviz2',
output='screen',
arguments=[["-d"], [config_rviz2]],
)
# Define LaunchDescription variable
ld = LaunchDescription()
if start_zed_node:
# Launch configuration variables (can be changed by CLI command)
svo_path = LaunchConfiguration('svo_path')
# Configuration variables
node_name = 'zed_node' # Zed Node name
publish_urdf = 'true' # Publish static frames from camera URDF
# Robot base frame. Note: overrides the parameter `pos_tracking.base_frame` in `common.yaml`.
base_frame = 'base_link'
# Position X of the camera with respect to the base frame [m].
cam_pos_x = '0.0'
# Position Y of the camera with respect to the base frame [m].
cam_pos_y = '0.0'
# Position Z of the camera with respect to the base frame [m].
cam_pos_z = '0.0'
# Roll orientation of the camera with respect to the base frame [rad].
cam_roll = '0.0'
# Pitch orientation of the camera with respect to the base frame [rad].
cam_pitch = '0.0'
# Yaw orientation of the camera with respect to the base frame [rad].
cam_yaw = '0.0'
# ZED Configurations to be loaded by ZED Node
config_common_path = os.path.join(
get_package_share_directory('zed_wrapper'),
'config',
'common.yaml'
)
if(camera_model != 'zed'):
config_camera_path = os.path.join(
get_package_share_directory('zed_wrapper'),
'config',
camera_model + '.yaml'
)
else:
config_camera_path = ''
# URDF/xacro file to be loaded by the Robot State Publisher node
xacro_path = os.path.join(
get_package_share_directory('zed_wrapper'),
'urdf', 'zed_descr.urdf.xacro'
)
# ZED Wrapper node
zed_wrapper_launch = IncludeLaunchDescription(
launch_description_source=PythonLaunchDescriptionSource([
get_package_share_directory('zed_wrapper'),
'/launch/include/zed_camera.launch.py'
]),
launch_arguments={
'camera_model': camera_model,
'camera_name': camera_name,
'node_name': node_name,
'config_common_path': config_common_path,
'config_camera_path': config_camera_path,
'publish_urdf': publish_urdf,
'xacro_path': xacro_path,
'svo_path': svo_path,
'base_frame': base_frame,
'cam_pos_x': cam_pos_x,
'cam_pos_y': cam_pos_y,
'cam_pos_z': cam_pos_z,
'cam_roll': cam_roll,
'cam_pitch': cam_pitch,
'cam_yaw': cam_yaw
}.items()
)
declare_svo_path_cmd = DeclareLaunchArgument(
'svo_path',
default_value='',
description='Path to an input SVO file. Note: overrides the parameter `general.svo_file` in `common.yaml`.')
# Launch parameters
ld.add_action(declare_svo_path_cmd)
# Add nodes to LaunchDescription
ld.add_action(zed_wrapper_launch)
ld.add_action(rviz2_node)
return ld
def generate_launch_description():
declare_sitl_world_cmd = DeclareLaunchArgument(
'sitl_world',
default_value='yosemite',
description='World [yosemite, mcmillan, ksql, baylands]')
sitl_world = ReplaceWorldString(
sitl_world=launch.substitutions.LaunchConfiguration('sitl_world'))
included_launch = launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
sitl_world))
use_rviz = LaunchConfiguration('use_rviz')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
use_qgroundcontrol = LaunchConfiguration('use_qgroundcontrol')
declare_use_qgroundcontrol_cmd = DeclareLaunchArgument(
'use_qgroundcontrol',
default_value='True',
description='Whether to start QGroundcontrol')
include_nodes = [included_launch]
sitl_launcher_dir = get_package_share_directory('sitl_launcher')
start_rviz_cmd = Node(condition=IfCondition(use_rviz),
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=[
'-d', sitl_launcher_dir + "/rviz/rviz.rviz",
"--ros-args", "-p", "use_sim_time:=True"
],
output='screen')
start_qgroundcontrol_cmd = Node(condition=IfCondition(use_qgroundcontrol),
package='qgroundcontrol',
node_executable='qgroundcontrol-start.sh',
node_name='qgroundcontrol',
output='screen')
ld = LaunchDescription(include_nodes)
ld.add_action(declare_use_rviz_cmd)
ld.add_action(start_rviz_cmd)
ld.add_action(declare_use_qgroundcontrol_cmd)
ld.add_action(start_qgroundcontrol_cmd)
ld.add_action(declare_sitl_world_cmd)
return ld
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
rviz_config_file = LaunchConfiguration('rviz_config')
# Declare the launch arguments
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='navigation',
description=(
'Top-level namespace. The value will be used to replace the '
'<robot_namespace> keyword on the rviz config file.'))
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
# Launch rviz
start_rviz_cmd = Node(condition=UnlessCondition(use_namespace),
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_file],
output='screen')
namespaced_rviz_config_file = ReplaceString(
source_file=rviz_config_file,
replacements={'<robot_namespace>': ('/', namespace)})
start_namespaced_rviz_cmd = Node(
condition=IfCondition(use_namespace),
package='rviz2',
executable='rviz2',
name='rviz2',
namespace=namespace,
arguments=['-d', namespaced_rviz_config_file],
output='screen',
remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static'),
('/goal_pose', 'goal_pose'),
('/clicked_point', 'clicked_point'),
('/initialpose', 'initialpose')])
exit_event_handler = RegisterEventHandler(
condition=UnlessCondition(use_namespace),
event_handler=OnProcessExit(
target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
exit_event_handler_namespaced = RegisterEventHandler(
condition=IfCondition(use_namespace),
event_handler=OnProcessExit(
target_action=start_namespaced_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_rviz_config_file_cmd)
# Add any conditioned actions
ld.add_action(start_rviz_cmd)
ld.add_action(start_namespaced_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler)
ld.add_action(exit_event_handler_namespaced)
return ld
