def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
gazebo_ros2_control_demos_path = os.path.join(
get_package_share_directory('gazebo_ros2_control_demos'))
xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf',
'test_cart_velocity.xacro.urdf')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
params = {'robot_description': doc.toxml()}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params])
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'cartpole'],
output='screen')
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_state_broadcaster'
],
output='screen')
load_joint_trajectory_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'velocity_controller'
],
output='screen')
load_imu_sensor_broadcaster = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', 'imu_sensor_broadcaster'],
output='screen')
return LaunchDescription([
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_trajectory_controller,
on_exit=[load_imu_sensor_broadcaster],
)),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def generate_launch_description():
keystroke_node = Node(package='keystroke',
node_executable='keystroke_listen',
output='screen',
node_name='keystroke_listen',
parameters=[{
'exit_on_esc': True
}],
arguments=['__log_level:=warn'])
twist_node = Node(package='keystroke',
node_executable='keystroke_arrows_to_twist',
output='screen',
node_name='arrows_to_twist',
parameters=[{
'publish_period': 0.1,
'linear_scale': 0.1,
'angular_scale': 0.2
}])
return LaunchDescription([
keystroke_node, twist_node,
RegisterEventHandler(event_handler=OnProcessExit(
target_action=keystroke_node,
on_exit=[EmitEvent(event=Shutdown())],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=twist_node,
on_exit=[EmitEvent(event=Shutdown())],
))
])
def generate_launch_description():
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name='xacro')]),
" ",
PathJoinSubstitution([
FindPackageShare('gazebo_ros2_control_demos'), 'urdf',
'test_gripper_mimic_joint.xacro.urdf'
]),
])
robot_description = {"robot_description": robot_description_content}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description])
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[FindPackageShare("gazebo_ros"), "/launch",
"/gazebo.launch.py"]), )
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'gripper'],
output='screen')
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_state_broadcaster'
],
output='screen')
load_gripper_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'gripper_controller'
],
output='screen')
return LaunchDescription([
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_gripper_controller],
)),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def generate_launch_description():
teleop_config = Path(get_package_share_directory('openrover_demo'), 'config', 'teleop.yaml')
assert teleop_config.is_file()
nodes = [
Node(
package='keystroke', node_executable='keystroke_listen', output='screen',
arguments=['__log_level:=warn'],
parameters=[teleop_config]
),
Node(
package='keystroke', node_executable='keystroke_arrows_to_twist', output='screen',
arguments=['__log_level:=info'],
parameters=[teleop_config]
)]
events = [RegisterEventHandler(
event_handler=OnProcessExit(
target_action=n,
on_exit=[EmitEvent(event=Shutdown())]
)
) for n in nodes]
return LaunchDescription([
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
*nodes,
*events
])
def add_fixture_action(
self,
launch_description,
action,
exit_allowed=[0],
):
"""
Add action used as testing fixture.
If a process action and it exits, a shutdown event is emitted.
"""
launch_description.add_action(action)
if isinstance(action, ExecuteProcess):
def on_fixture_process_exit(event, context):
process_name = event.action.process_details['name']
allowed_to_exit = exit_allowed
if isinstance(exit_allowed, list):
allowed_to_exit = event.returncode in exit_allowed
if not context.is_shutdown and not allowed_to_exit:
rc = event.returncode if event.returncode else 1
self.__processes_rc[process_name] = rc
return EmitEvent(event=Shutdown(
reason='{} fixture process died!'.format(
process_name)))
launch_description.add_action(
RegisterEventHandler(
OnProcessExit(target_action=action,
on_exit=on_fixture_process_exit)))
return action
def add_test_action(self, launch_description, action):
"""
Add action used for testing.
If either all test actions exited with a return code of zero or any
test action exited with a non-zero return code a shutdown event is
emitted.
"""
assert isinstance(action, ExecuteProcess), \
'The passed action must be a ExecuteProcess action'
self.__test_processes.append(action)
def on_test_process_exit(event, context):
nonlocal action
nonlocal self
self.__test_returncodes[action] = event.returncode
if len(self.__test_returncodes) == len(self.__test_processes):
shutdown_event = Shutdown(
reason='all test processes finished')
return EmitEvent(event=shutdown_event)
launch_description.add_action(
RegisterEventHandler(OnProcessExit(
target_action=action, on_exit=on_test_process_exit,
))
)
def generate_launch_description():
localization_benchmark_supervisor_node = Node(
package='localization_performance_modelling',
node_executable='localization_benchmark_supervisor',
node_name='localization_benchmark_supervisor',
output='screen',
emulate_tty=True,
parameters=[LaunchConfiguration('configuration')],
)
return LaunchDescription([
# Set env var to print messages to stdout immediately
SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
DeclareLaunchArgument('configuration',
description='Configuration yaml file path'),
DeclareLaunchArgument('use_sim_time',
description='Use simulation/bag clock if true'),
localization_benchmark_supervisor_node,
Node(package='nav2_lifecycle_manager',
node_executable='lifecycle_manager',
node_name='lifecycle_manager',
output='screen',
parameters=[{
'use_sim_time': LaunchConfiguration('use_sim_time')
}, {
'node_names': [
'map_server', 'amcl', 'controller_server',
'planner_server', 'recoveries_server', 'bt_navigator',
'waypoint_follower'
]
}]),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=localization_benchmark_supervisor_node,
on_exit=EmitEvent(event=Shutdown(reason='supervisor_finished')))),
])
def generate_launch_description():
return launch.LaunchDescription([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[get_package_share_directory(robot), "/launch/interfaces.py"]),
launch_arguments={
"namespace":
robot,
"params_file":
os.path.join(get_package_share_directory(robot), "param",
f"{robot}.yml"),
}.items(),
) for robot in robots
] + [
GroupAction([
Node(
package="strategix",
executable="strategix",
output="screen",
),
])
] + [
RegisterEventHandler(event_handler=OnProcessExit(on_exit=[
EmitEvent(event=Shutdown()),
]))
])
def generate_launch_description():
return launch.LaunchDescription(
[
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[get_package_share_directory(robot), "/launch/nav-core.py"]
),
launch_arguments={
"namespace": robot,
"params_file": os.path.join(
get_package_share_directory(robot), "param", f"{robot}.yml"
),
}.items(),
)
for robot in robots
]
+ [
RegisterEventHandler(
event_handler=OnProcessExit(
on_exit=[
EmitEvent(event=Shutdown()),
]
)
)
]
)
def generate_launch_description():
return launch.LaunchDescription(
[
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[get_package_share_directory(robot), "/launch/interfaces.py"]
),
launch_arguments={
"namespace": robot,
"params_file": os.path.join(
get_package_share_directory(robot), "param", f"{robot}.yml"
),
}.items(),
)
for robot in robots
]
+ [
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
get_package_share_directory("assurancetourix"),
"/launch/launch.py",
]
),
launch_arguments={
"namespace": "assurancetourix",
"params_file": os.path.join(
get_package_share_directory("assurancetourix"),
"param",
"assurancetourix.yml",
),
}.items(),
)
]
+ [
GroupAction(
[
Node(
package="strategix",
executable="strategix",
output="screen",
),
WebotsLauncher(
mode="realtime",
world="tools/simulation/worlds/cdr2022.wbt",
),
]
)
]
+ [
RegisterEventHandler(
event_handler=OnProcessExit(
on_exit=[
EmitEvent(event=Shutdown()),
]
)
)
]
)
def __init__(
self,
target_action,
log_file,
timeout=None,
**kwargs
) -> None:
"""
Construct a SystemMetricCollector action.
Parameters
----------
target_action : ExecuteProcess
ExecuteProcess (or Node) instance to collect metrics on
log_file : str or LaunchSubstitutionsType
Path to where the collected metrics should be written to
timeout : int or LaunchSubstitutionsType
Maximum time to run the metrics collector if the target process does
not exit
"""
# These Node/ExecuteProcess arguments are invalid in this context
# because we implicitly set them right here.
assert 'arguments' not in kwargs
assert 'package' not in kwargs
assert 'executable' not in kwargs
assert 'executable' not in kwargs
self.__pid_var_name = '__PROCESS_ID_%d' % id(self)
kwargs['package'] = 'buildfarm_perf_tests'
kwargs['executable'] = 'system_metric_collector'
kwargs['arguments'] = [
'--log', log_file,
'--process_pid', LocalSubstitution(self.__pid_var_name)]
if timeout is not None:
kwargs['arguments'] += [
'--timeout',
str(timeout) if isinstance(timeout, int) else timeout
]
super().__init__(**kwargs)
self.__target_start_handler = OnProcessStart(
target_action=target_action, on_start=self.__on_target_start)
self.__target_exit_handler = OnProcessExit(
target_action=target_action, on_exit=EmitEvent(
event=ShutdownProcess(
process_matcher=matches_action(self))))
def generate_rviz_launch_description(namespace="robot"):
# Get the launch directory
titan_dir = get_package_share_directory("titan")
rviz_config_file = os.path.join(titan_dir, "rviz", "namespaced_view.rviz")
use_namespace = "true"
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,
# TODO: FIX onload MUTEX ERROR
# 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_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()
# Add any conditioned actions
ld.add_action(start_namespaced_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler_namespaced)
return ld
def add_test_action(self, launch_description, action):
"""
Add action used for testing.
If either all test actions have completed or a process action has
exited with a non-zero return code, a shutdown event is emitted.
"""
test_name = 'test_{}'.format(id(action))
if isinstance(action, ExecuteProcess):
def on_test_process_exit(event, context):
if event.returncode != 0:
process_name = event.action.process_details['name']
self.__processes_rc[process_name] = event.returncode
return self._fail(
test_name,
reason='{} test failed!'.format(process_name))
return self._succeed(test_name)
launch_description.add_action(
RegisterEventHandler(
OnProcessExit(target_action=action,
on_exit=on_test_process_exit)))
else:
def on_test_completion(event, context):
future = event.action.get_asyncio_future()
if future is not None:
if future.cancelled():
return self._drop(test_name)
exc = future.exception()
if exc is not None:
return self._fail(test_name, str(exc))
return self._succeed(test_name)
launch_description.add_action(
RegisterEventHandler(
OnExecutionComplete(target_action=action,
on_completion=on_test_completion)))
launch_description.add_action(action)
self._arm(test_name)
return action
def execute(self, context):
"""
Execute the action.
Delegated to :meth:`launch.actions.ExecuteProcess.execute`.
"""
actions = super().execute(context)
if not self.__timeout:
return actions
self.__timer = TimerAction(period=self.__timeout,
actions=[
OpaqueFunction(
function=self._shutdown_process,
kwargs={'send_sigint': True})
])
on_process_exit_event = OnProcessExit(on_exit=self.__on_process_exit,
target_action=self)
context.register_event_handler(on_process_exit_event)
if not actions:
return [self.__timer]
return actions.append(self.__timer)
def execute(self, context: LaunchContext):
continue_after_fail = self.__continue_after_fail
if not isinstance(continue_after_fail, bool):
continue_after_fail = perform_substitutions(
context, normalize_to_list_of_substitutions(
continue_after_fail)).lower()
if continue_after_fail in ['true', 'on', '1']:
continue_after_fail = True
elif continue_after_fail in ['false', 'off', '1']:
continue_after_fail = False
else:
raise ValueError(
'continue_after_fail should be a boolean, got {}'.format(
continue_after_fail))
on_first_action_exited = OnProcessExit(
target_action=self.__actions[0],
on_exit=lambda event, context:
([InOrderGroup(self.__actions[1:])]
if event.exitcode == 0 or continue_after_fail else []))
return [
self.__actions[0],
RegisterEventHandler(on_first_action_exited)
]
def generate_launch_description():
"""
"""
pkg_share = launch_ros.substitutions.FindPackageShare(
package='tareeqav').find('tareeqav')
arg_joy_dev = launch.actions.DeclareLaunchArgument(
'joy_dev', default_value='/dev/input/js0')
arg_config_filepath = launch.actions.DeclareLaunchArgument(
'config_filepath',
default_value=[os.path.join(pkg_share, 'config/ps4.config.yaml')])
# TO-D0: fix hard-coded string
sim_world = "/ros2/models/gazebo_models_worlds_collection/worlds/small_city.world"
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("tareeqav"), "description",
"tareeqav_simulation.urdf.xacro"
]),
])
robot_description = {"robot_description": robot_description_content}
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
delay_spawn_entity_for_gazebo_launch = ExecuteProcess(cmd=['sleep', '10'],
output='screen')
start_gazebo = ExecuteProcess(
cmd=[
'gazebo',
'--verbose',
'-s',
'libgazebo_ros_init.so',
'-s',
'libgazebo_ros_factory.so',
# '-s', 'libgazebo_ros_force_system.so',
sim_world
],
output='screen')
# start_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', 'tareeqav'],
output='screen')
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_state_broadcaster'
],
output='screen')
load_joint_trajectory_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'tareeqav_base_controller'
],
output='screen')
# joystick teleop nodes and config
joy_dev = launch.substitutions.LaunchConfiguration('joy_dev')
config_filepath = launch.substitutions.LaunchConfiguration(
'config_filepath')
joy_node = launch_ros.actions.Node(
package='joy',
executable='joy_node',
name='joy_node',
parameters=[{
'dev': joy_dev,
'deadzone': 0.3,
'autorepeat_rate': 20.0,
}],
remappings=[
("/cmd_vel", "/tareeqav_base_controller/cmd_vel_unstamped"),
],
)
teleop_node = launch_ros.actions.Node(
package='teleop_twist_joy',
executable='teleop_node',
name='teleop_twist_joy_node',
parameters=[config_filepath],
remappings=[
("/cmd_vel", "/tareeqav_base_controller/cmd_vel_unstamped"),
],
)
return LaunchDescription([
arg_joy_dev, arg_config_filepath,
RegisterEventHandler(event_handler=OnProcessExit(
target_action=delay_spawn_entity_for_gazebo_launch,
on_exit=[spawn_entity],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller],
)), start_gazebo, node_robot_state_publisher,
delay_spawn_entity_for_gazebo_launch, joy_node, teleop_node
# spawn_entity,
])
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('amazon_robot_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', 'amazon_robot_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
def add_output_test(
self,
launch_description,
action,
output_test,
test_suffix='output',
output_filter=None,
side_effect=None,
):
"""
Test an action process' output against a given test.
:param launch_description: test launch description that owns the given action.
:param action: launch action to test whose output is to be tested.
:param output_test: test tuple as returned by launch_testing.output.create_* functions.
:param test_suffix: an optional test suffix to disambiguate multiple test instances,
defaults to 'output'.
:param output_filter: an optional function to filter out i.e. ignore output lines for
the test.
:param side_effect: an optional side effect of a passing test, currently only 'shutdown'
is supported.
"""
assert isinstance(action, ExecuteProcess)
test_name = 'test_{}_{}'.format(id(action), test_suffix)
out, collate_output, match_output, match_patterns = output_test
if not output_filter:
output_filter = (lambda x: x)
assert any(match_patterns)
def on_process_exit(event, context):
nonlocal match_patterns
if any(match_patterns):
# Finish test instead of failing to prevent process exit
# and process output event handlers from racing.
return self._finish(test_name)
launch_description.add_action(
RegisterEventHandler(
OnProcessExit(target_action=action, on_exit=on_process_exit)))
def on_shutdown(event, context):
nonlocal match_patterns
if any(match_patterns):
process_name = action.process_details['name']
reason = 'not all {} output matched!'.format(process_name)
self._fail(test_name, reason)
self._succeed(test_name, side_effect)
launch_description.add_action(
RegisterEventHandler(OnShutdown(on_shutdown=on_shutdown)))
def on_process_stdout(event):
nonlocal out
nonlocal match_patterns
out = collate_output(out, output_filter(event.text))
match_patterns = [
pattern for pattern in match_patterns
if not match_output(out, pattern)
]
if not any(match_patterns):
return self._succeed(test_name, side_effect)
return None
launch_description.add_action(
RegisterEventHandler(
OnProcessIO(target_action=action,
on_stdout=on_process_stdout)))
self._arm(test_name)
return action
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)
# TODO pete: this can be simplified as a call to the dict ctor:
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([
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 generate_launch_description():
pkg_share = launch_ros.substitutions.FindPackageShare(package='tareeqav').find('tareeqav')
# arg_joy_dev = launch.actions.DeclareLaunchArgument('joy_dev', default_value='/dev/input/js0')
# arg_config_filepath = launch.actions.DeclareLaunchArgument(
# 'config_filepath',
# default_value=[os.path.join(pkg_share, 'config/ps4.config.yaml')]
# )
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare("tareeqav"), "models", "tareeqav_system.urdf.xacro"]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("tareeqav"),
"config",
"diff_drive_controller.yaml",
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("tareeqav"), "config", "tareeqav.rviz"]
)
control_node = Node(
# namespace="tareeqav",
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
remappings=[
("/tareeqav_base_controller/cmd_vel_unstamped", "/cmd_vel"),
],
)
robot_state_pub_node = Node(
# namespace="tareeqav",
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
# namespace="tareeqav",
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
# namespace="tareeqav",
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
robot_controller_spawner = Node(
# namespace="tareeqav",
package="controller_manager",
executable="spawner",
arguments=["tareeqav_base_controller", "-c", "/controller_manager"],
)
# Delay rviz start after `joint_state_broadcaster`
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
)
)
# Delay start of robot_controller after `joint_state_broadcaster`
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
)
)
# joystick teleop nodes and config
# joy_dev = launch.substitutions.LaunchConfiguration('joy_dev')
# config_filepath = launch.substitutions.LaunchConfiguration('config_filepath')
# joy_node = launch_ros.actions.Node(
# package='joy',
# executable='joy_node',
# name='joy_node',
# parameters=[{
# 'dev': joy_dev,
# 'deadzone': 0.3,
# 'autorepeat_rate': 20.0,}],
# )
# teleop_node = launch_ros.actions.Node(
# package='teleop_twist_joy',
# executable='teleop_node',
# name='teleop_twist_joy_node',
# parameters=[config_filepath],
# )
nodes = [
# arg_joy_dev,
# arg_config_filepath,
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
# joy_node,
# teleop_node
]
return LaunchDescription(nodes)
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 = _normalize_ld(self._gen_launch_description_fn)(
lambda: self._processes_launched.set())
# Data to squirrel away for post-shutdown tests
self.proc_info = ActiveProcInfoHandler()
self.proc_output = ActiveIoHandler()
self.test_context = test_context
parsed_launch_arguments = parse_launch_arguments(
self._launch_file_arguments)
self.test_args = {}
for k, v in parsed_launch_arguments:
self.test_args[k] = v
# 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([
launch.actions.IncludeLaunchDescription(
launch.LaunchDescriptionSource(launch_description=test_ld),
launch_arguments=parsed_launch_arguments),
RegisterEventHandler(
OnProcessExit(
on_exit=lambda info, unused: self.proc_info.append(info))),
RegisterEventHandler(
OnProcessIO(
on_stdout=self.proc_output.append,
on_stderr=self.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")
self._print_process_output_summary(
) # <-- Helpful to debug why processes died early
# We treat this as a test failure and return some test results indicating such
return FailResult(), FailResult()
# Now, run the post-shutdown tests
inactive_suite = PostShutdownTestLoader(
injected_attributes={
"proc_info": self.proc_info,
"proc_output": self.proc_output._io_handler,
"test_args": self.test_args,
},
injected_args=dict(
self.test_context,
# Add a few more things to the args dictionary:
**{
"proc_info": self.proc_info,
"proc_output": self.proc_output._io_handler,
"test_args": self.test_args
})).loadTestsFromModule(self._test_module)
inactive_results = unittest.TextTestRunner(
verbosity=2, resultclass=TestResult).run(inactive_suite)
return self._results, inactive_results
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')
simulator = LaunchConfiguration('simulator')
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'), ('/scan', 'scan'),
('/tf', 'tf'), ('/tf_static', 'tf_static'),
('/cmd_vel', 'cmd_vel'), ('/map', 'map')]
# 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(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
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_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=[simulator, '-s', 'libgazebo_ros_init.so', world],
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_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():
# Get the launch directory
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
marathon_dir = get_package_share_directory('marathon_ros2_bringup')
marathon_launch_dir = os.path.join(marathon_dir, 'launch')
# Create the launch configuration variables
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')
cmd_vel_topic = LaunchConfiguration('cmd_vel_topic')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_rviz = LaunchConfiguration('use_rviz')
# 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_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(marathon_dir, 'maps/aulario', 'map.yaml'),
description='Full path to map file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(marathon_dir, 'params',
'nav2_marathon_teb_tiago_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launched nodes')
#declare_params_file_cmd = DeclareLaunchArgument(
# 'params_file',
# default_value=os.path.join(nav2_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('marathon_ros2_bringup'),
'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(nav2_bringup_dir, 'rviz',
'nav2_default_view.rviz'),
description='Full path to the RVIZ config file to use')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_cmd_vel_topic_cmd = DeclareLaunchArgument(
'cmd_vel_topic',
default_value='nav_vel',
description='Command velocity topic')
start_rviz_cmd = Node(condition=IfCondition(use_rviz),
package='rviz2',
node_executable='rviz2',
arguments=['-d', rviz_config_file],
output='log',
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(marathon_launch_dir, 'nav2_bringup_launch.py')),
launch_arguments={
'namespace': namespace,
'use_namespace': use_namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'autostart': autostart,
'cmd_vel_topic': cmd_vel_topic
}.items())
marathon_cmd = IncludeLaunchDescription(PythonLaunchDescriptionSource(
os.path.join(marathon_launch_dir, 'nav2_marathon_launch.py')),
launch_arguments={
'total_distance_sum': '0.25',
'next_wp': '0'
}.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_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_rviz_cmd)
ld.add_action(declare_cmd_vel_topic_cmd)
# Add any conditioned actions
ld.add_action(start_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler)
ld.add_action(marathon_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd)
return ld
def generate_launch_description():
"""
Generate launch description from list of nodes to launch.
:return:
LaunchDescription object
"""
drivetrain_description_package_path = get_package_share_path('uwrt_mars_rover_drivetrain_description')
model_path = drivetrain_description_package_path / 'urdf' / 'drivetrain.urdf.xacro'
rviz_config_path = drivetrain_description_package_path / 'rviz' / 'urdf.rviz'
controllers_config_path = get_package_share_path(
'uwrt_mars_rover_drivetrain_hw') / 'config' / 'drivetrain_controllers.yaml'
robot_description_content = ParameterValue(Command(['ros2 run xacro xacro ', str(model_path)]), value_type=str)
robot_description = {'robot_description': robot_description_content}
# Nodes
nodes = []
nodes += [Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[robot_description, controllers_config_path],
output={
'stdout': 'screen',
'stderr': 'screen',
},
)] # TODO: use custom control node w/ RT scheduling(port from ros1 uwrt_mars_rover branch)
nodes += [Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[robot_description],
)]
nodes += [joint_state_broadcaster_spawner := Node(
package='controller_manager',
executable='spawner',
arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
)]
# Delay rviz2 start after joint_state_broadcaster_spawner finishes
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', str(rviz_config_path)],
)
nodes += [RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
)
)]
# Delay start of drivetrain_controller_spawner after joint_state_broadcaster_spawner
drivetrain_controller_spawner = Node(
package='controller_manager',
executable='spawner',
arguments=['differential_drivetrain_controller', '-c', '/controller_manager'],
)
nodes += [RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[drivetrain_controller_spawner],
)
)]
return LaunchDescription(nodes)
def generate_launch_description():
# Get the launch directory
marta_launch_dir = os.path.join(
get_package_share_directory('marta_launch'), 'launch')
rover_config_dir = get_package_share_directory('rover_config')
# 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')
default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
autostart = LaunchConfiguration('autostart')
# Launch configuration variables specific to simulation
rviz_config_file = LaunchConfiguration('rviz_config_file')
use_rviz = LaunchConfiguration('use_rviz')
use_simulator = LaunchConfiguration('use_simulator')
use_gazebo_gui = LaunchConfiguration('use_gazebo_gui')
world_path = LaunchConfiguration('world_path')
# 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(rover_config_dir, 'maps',
'tb3_world_big.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(rover_config_dir, 'config',
'nav2_params.yaml'),
description=
'Full path to the ROS2 parameters file to use for all launchde 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_distance.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(rover_config_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_gazebo_gui_cmd = DeclareLaunchArgument(
'use_gazebo_gui',
default_value='True',
description='Whether to execute gzclient)')
declare_use_rviz_cmd = DeclareLaunchArgument(
'use_rviz', default_value='True', description='Whether to start RVIZ')
declare_world_path_cmd = DeclareLaunchArgument(
'world_path',
# TODO(orduno) Switch back once ROS argument passing has been fixed upstream
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91
# TURTLEBOT EXAMPLE
# default_value=os.path.join(rover_config_dir, 'worlds', 'tb3_world.model'),
# EMPTY WORLD
default_value=os.path.join(rover_config_dir, 'worlds', 'empty.world'),
description='Full path to world model file to load')
# Specify the actions
locomotion_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(marta_launch_dir, 'locomotion.launch.py')))
simulation_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(marta_launch_dir, 'simulation.launch.py')))
# Start Nav2 Stack
bringup_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(marta_launch_dir, 'nav2_bringup_launch.py')),
# 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,
'default_bt_xml_filename': default_bt_xml_filename,
'autostart': autostart
}.items())
start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
# Use this option to see all output from rviz:
# output='screen',
# Use this option to supress messages of missing tfs,
# at startup of rviz and gazebo:
output={'stdout': 'log'},
arguments=['-d', rviz_config_file],
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'))))
# Create the launch description and populate
return LaunchDescription([
# Set env var to print messages colored. The ANSI color codes will appear in a log.
SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'),
# Declare the launch options
declare_namespace_cmd,
declare_use_sim_time_cmd,
declare_map_yaml_cmd,
declare_params_file_cmd,
declare_bt_xml_cmd,
declare_autostart_cmd,
declare_rviz_config_file_cmd,
declare_use_simulator_cmd,
declare_use_rviz_cmd,
declare_use_gazebo_gui_cmd,
declare_world_path_cmd,
# Add any conditioned actions
start_rviz_cmd,
# Add other nodes and processes we need
exit_event_handler,
# Add the actions to launch all of the navigation nodes
locomotion_cmd,
simulation_cmd,
bringup_cmd
])
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
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')
simulator = LaunchConfiguration('simulator')
world = LaunchConfiguration('world')
# Declare the launch arguments
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_prefix('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_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_simulator_cmd = DeclareLaunchArgument(
'simulator',
default_value='gazebo',
description='The simulator to use (gazebo or gzserver)')
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_cmd = ExecuteProcess(
condition=IfCondition(use_simulator),
cmd=[simulator, '-s', 'libgazebo_ros_init.so', world],
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(
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}],
arguments=[urdf])
# TODO(orduno) RVIZ crashing if launched as a node: https://github.com/ros2/rviz/issues/442
# Launching as node works after applying the change described on the github issue.
# start_rviz_cmd = Node(
# package='rviz2',
# node_executable='rviz2',
# node_name='rviz2',
# arguments=['-d', rviz_config_file],
# output='screen')
start_rviz_cmd = ExecuteProcess(cmd=[
os.path.join(get_package_prefix('rviz2'), 'lib/rviz2/rviz2'),
['-d', rviz_config_file]
],
cwd=[launch_dir],
output='screen')
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={
'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_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_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any actions to launch in simulation (conditioned on 'use_simulator')
ld.add_action(start_gazebo_cmd)
# Add other nodes and processes we need
ld.add_action(start_rviz_cmd)
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():
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("diffbot_description"), "urdf",
"diffbot.urdf.xacro"
]),
])
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution([
FindPackageShare("ros2_control_demo_bringup"),
"config",
"diffbot_controllers.yaml",
])
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("diffbot_description"), "config", "diffbot.rviz"])
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
remappings=[
("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster", "--controller-manager",
"/controller_manager"
],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["diffbot_base_controller", "-c", "/controller_manager"],
)
# Delay rviz start after `joint_state_broadcaster`
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
))
# Delay start of robot_controller after `joint_state_broadcaster`
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
))
nodes = [
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
]
return LaunchDescription(nodes)
def generate_launch_description():
# Get the launch directory
social_bringup_dir = get_package_share_directory('social_nav2_bringup')
nav_bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(nav_bringup_dir, 'launch')
# Create the launch configuration variables
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')
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')
# 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_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value=os.path.join(social_bringup_dir, 'maps',
'turtlebot3_house.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_use_remappings_cmd = DeclareLaunchArgument(
'use_remappings',
default_value='false',
description='Arguments to pass to all nodes launched by the file')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(social_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('social_nav2_bringup'),
'behavior_trees', 'follow_point.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(social_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(social_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', '-s',
'libgazebo_ros_factory.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(social_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])
start_rviz_cmd = Node(
condition=IfCondition(use_rviz),
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_file],
output='screen',
)
exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(
reason='rviz exited'))))
agent_spawner_cmd = Node(package='pedsim_gazebo_plugin',
executable='spawn_pedsim_agents.py',
name='spawn_pedsim_agents',
output='screen')
# 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_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(agent_spawner_cmd)
ld.add_action(start_rviz_cmd)
ld.add_action(exit_event_handler)
# Add the actions to launch all of the navigation nodes
ld.add_action(start_robot_state_publisher_cmd)
return ld
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(get_package_share_directory('gazebo_ros'), 'launch'),
'/gazebo.launch.py'
]), )
# NOTE: Load the processed xacro file directly
description_location = os.path.join(
get_package_share_directory('racecar_description'))
xacro_file = os.path.join(description_location, 'urdf', 'racecar.xacro')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
robot_description_value = doc.toxml()
params = {"robot_description": robot_description_value}
node_robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params])
spawn_entity = Node(package='gazebo_ros',
executable='spawn_entity.py',
arguments=[
'-topic', 'robot_description', '-entity',
'racecar', '-x', '0', '-y', '0', '-z', '0.5'
],
output='screen')
# NOTE: THIS IS DEPRECATED VERSION OF LOADING
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_start_controller', 'joint_state_broadcaster'
],
output='screen')
# NOTE: THIS IS DEPRECATED VERSION OF LOADING CONTROLLER
controllers = [
'left_rear_wheel_velocity_controller',
'right_rear_wheel_velocity_controller',
'left_front_wheel_velocity_controller',
'right_front_wheel_velocity_controller',
'left_steering_hinge_position_controller',
'right_steering_hinge_position_controller'
]
load_controllers = [
ExecuteProcess(cmd=['ros2', 'control', 'load_start_controller', state],
output='screen') for state in controllers
]
racecar_controller = os.path.join(
get_package_share_directory("racecar_control"),
"config",
"racecar_control.yaml",
)
return LaunchDescription([
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=load_controllers,
)),
gazebo,
node_robot_state_publisher,
spawn_entity,
])
def generate_launch_description():
# Setting arguments, currently fake robot works without using arguments so default is always used
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"start_rviz",
default_value="true",
description="start RViz automatically with the launch file",
))
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
))
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="true",
description=
"Start robot with fake hardware mirroring command to its states.",
))
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="true",
description=
"Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
))
declared_arguments.append(
DeclareLaunchArgument("slowdown",
default_value="3.0",
description="Slowdown factor of the RRbot."))
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("inmoov_description"),
"robots",
"inmoov.urdf.xacro",
]),
" prefix:=",
LaunchConfiguration("prefix"),
" use_fake_hardware:=",
LaunchConfiguration("use_fake_hardware"),
" fake_sensor_commands:=",
LaunchConfiguration("fake_sensor_commands"),
" slowdown:=",
LaunchConfiguration("slowdown"),
])
robot_description = {"robot_description": robot_description_content}
rrbot_controllers = PathJoinSubstitution([
FindPackageShare("robot"),
"controllers",
"head.yaml",
])
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description],
)
controller_manager_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, rrbot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
spawn_jsb_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster"],
output="screen",
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("inmoov_description"), "config", "inmoov.rviz"])
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
condition=IfCondition(LaunchConfiguration("start_rviz")),
)
# Not currently used/needed, but could be useful later when needing delayed start
delayed_rviz_node = RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_jsb_controller,
on_exit=[rviz_node],
))
head_fake_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["head_controller", "-c", "/controller_manager"],
)
eyes_fake_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["eyes_controller", "-c", "/controller_manager"],
)
jaw_fake_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["jaw_controller", "-c", "/controller_manager"],
)
nodes = [
controller_manager_node, node_robot_state_publisher,
spawn_jsb_controller, rviz_node, head_fake_controller_spawner,
eyes_fake_controller_spawner, jaw_fake_controller_spawner
]
return LaunchDescription(declared_arguments + nodes)
