def test_if_condition():
"""Test UnlessCondition class."""
class MockLaunchContext:
def perform_substitution(self, substitution):
return substitution.perform(self)
lc = MockLaunchContext()
test_cases = [
('true', True),
('True', True),
('TRUE', True),
('1', True),
('false', False),
('False', False),
('FALSE', False),
('0', False),
]
for string, expected in test_cases:
assert UnlessCondition([TextSubstitution(text=string)]).evaluate(lc) is not expected
with pytest.raises(InvalidConditionExpressionError):
UnlessCondition([TextSubstitution(text='')]).evaluate(lc)
with pytest.raises(InvalidConditionExpressionError):
UnlessCondition([TextSubstitution(text='typo')]).evaluate(lc)
def generate_launch_description():
return LaunchDescription([
# Arguments first
DeclareLaunchArgument('offline', default_value='false'),
# Drivers
IncludeLaunchDescription(
PythonLaunchDescriptionSource([launch_file('drivers.launch.py')]),
condition=UnlessCondition(LaunchConfiguration('offline')),
),
# TODO: Process GPS sentences into Fix
#<include file="$(find robomagellan)/launch/compute/gps.launch.xml" />
# TODO: Process IMU into usable values
#<include file="$(find robomagellan)/launch/compute/imu.launch.xml" />
# TODO: Local frame localization (base_link to odom)
#<include file="$(find robomagellan)/launch/compute/ekf_local.launch.xml" />
# TODO: Global frame localization (map to odom)
#<include file="$(find robomagellan)/launch/compute/ekf_global.launch.xml" />
# TODO: Setup global localization
# TODO: add navigation, yeah, that is totally one line
])
def group(self, ns=None, if_arg=None, unless_arg=None):
'''
Group the next nodes / entities into
- another namespace
- a if / unless condition depending on some argument
'''
# save current entity index
prev_index = self.index
self.entities.append([])
self.index = len(self.entities) - 1
if ns is not None:
self.entities[-1].append(PushRosNamespace(ns))
try:
yield self
finally:
new_entities = self.entities[self.index]
# restore state
self.index = prev_index
condition = None
# get condition
if if_arg is not None:
condition = IfCondition(self.arg(if_arg))
elif unless_arg is not None:
condition = UnlessCondition(self.arg(unless_arg))
# add new entities as sub-group
self.entity(GroupAction(new_entities, condition=condition))
def generate_launch_description():
ld = LaunchDescription()
launchturtle = LaunchConfiguration('launchturtle')
#declare this argument in the LaunchDescription class
declareturtle = DeclareLaunchArgument(
'launchturtle',
default_value='false',
description=
'true would launch turtlesim. false would launch the turtle teleop key'
)
run_teleop = Node(condition=UnlessCondition(launchturtle),
package='turtlesim',
executable='turtle_teleop_key',
name='turtle_teleop_key',
output='screen')
run_turtlesim = Node(condition=IfCondition(launchturtle),
package='turtlesim',
executable='turtlesim_node',
name='turtlesim_node',
output='screen')
ld.add_action(declareturtle)
ld.add_action(run_teleop)
ld.add_action(run_turtlesim)
return ld
def generate_launch_description():
# Webots
webots = WebotsLauncher(
#world=os.path.join(get_package_share_directory('robotis_mini_description'), 'worlds', 'ros_example.wbt')
world=os.path.join('./worlds', 'empty.wbt'))
# Controller node
# synchronization = launch.substitutions.LaunchConfiguration('synchronization', default=False)
# controller = ControllerLauncher(
# package='head_node',
# executable='head_publisher',
# parameters=[{'synchronization': synchronization}],
# output='screen'
#)
# urdf for state publisher
robot_description_filename = 'robotis_mini.urdf.xacro'
print("robot_description_filename : {}".format(robot_description_filename))
xacro = os.path.join(
get_package_share_directory('robotis_mini_description'), 'urdf',
robot_description_filename)
print("xacro : {}".format(xacro))
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
xacro_path = LaunchConfiguration('xacro_path', default='{}'.format(xacro))
print("xacro_path : {}".format(xacro_path))
gui = LaunchConfiguration('gui', default='true')
return launch.LaunchDescription([
webots,
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
Node(package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{
'use_sim_time':
use_sim_time,
'robot_description':
Command(['xacro', ' ', xacro_path])
}]),
Node(condition=UnlessCondition(gui),
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
output='screen'),
Node(condition=IfCondition(gui),
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui'),
launch.actions.
RegisterEventHandler(event_handler=launch.event_handlers.OnProcessExit(
target_action=webots,
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
)),
])
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
default_params_file = os.path.join(
get_package_share_directory("slam_toolbox"), 'config',
'mapper_params_online_async.yaml')
declare_use_sim_time_argument = DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation/Gazebo clock')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=default_params_file,
description=
'Full path to the ROS2 parameters file to use for the slam_toolbox node'
)
# If the provided param file doesn't have slam_toolbox params, we must pass the
# default_params_file instead. This could happen due to automatic propagation of
# LaunchArguments. See:
# https://github.com/ros-planning/navigation2/pull/2243#issuecomment-800479866
has_node_params = HasNodeParams(source_file=params_file,
node_name='slam_toolbox')
actual_params_file = PythonExpression([
'"', params_file, '" if ', has_node_params, ' else "',
default_params_file, '"'
])
log_param_change = LogInfo(msg=[
'provided params_file ', params_file,
' does not contain slam_toolbox parameters. Using default: ',
default_params_file
],
condition=UnlessCondition(has_node_params))
start_async_slam_toolbox_node = Node(
parameters=[actual_params_file, {
'use_sim_time': use_sim_time
}],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen')
ld = LaunchDescription()
ld.add_action(declare_use_sim_time_argument)
ld.add_action(declare_params_file_cmd)
ld.add_action(log_param_change)
ld.add_action(start_async_slam_toolbox_node)
return ld
def generate_launch_description():
hexapod_path = get_package_share_path('hexapod_desc')
default_model_path = hexapod_path / 'urdf/hexapod.urdf'
default_rviz_config_path = hexapod_path / 'rviz/urdf.rviz'
gui_arg = DeclareLaunchArgument(
name='gui',
default_value='true',
choices=['true', 'false'],
description='Flag to enable joint_state_publisher_gui')
model_arg = DeclareLaunchArgument(
name='model',
default_value=str(default_model_path),
description='Absolute path to robot urdf file')
rviz_arg = DeclareLaunchArgument(
name='rvizconfig',
default_value=str(default_rviz_config_path),
description='Absolute path to rviz config file')
robot_description = ParameterValue(Command(
['xacro ', LaunchConfiguration('model')]),
value_type=str)
robot_state_publisher_node = Node(package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{
'robot_description':
robot_description
}])
# Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
joint_state_publisher_node = Node(package='joint_state_publisher',
executable='joint_state_publisher',
condition=UnlessCondition(
LaunchConfiguration('gui')))
joint_state_publisher_gui_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
condition=IfCondition(LaunchConfiguration('gui')))
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
return LaunchDescription([
gui_arg, model_arg, rviz_arg, joint_state_publisher_node,
joint_state_publisher_gui_node, robot_state_publisher_node, rviz_node
])
def joint_state_publisher(self, use_gui=True, **node_args):
'''
Adds a joint_state_publisher / joint_state_publisher_gui with passed arguments as parameters
Assumes some robot_description topic is published inside the namespace
'''
if type(use_gui) == bool:
use_gui = str(use_gui)
pkg_exec = 'joint_state_publisher'
self.node(pkg_exec,
pkg_exec,
parameters=node_args,
condition=UnlessCondition(use_gui))
pkg_exec = 'joint_state_publisher_gui'
self.node(pkg_exec,
pkg_exec,
parameters=node_args,
condition=IfCondition(use_gui))
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():
gazebo_package_prefix = get_package_share_directory("gazebo_ros")
package_prefix = get_package_share_directory("halodi-controller-gazebo")
model, plugin, media = GazeboRosPaths.get_paths()
return LaunchDescription([
DeclareLaunchArgument(
"world",
default_value=os.path.join(package_prefix, "worlds",
"eve_flat_ground.world"),
description="Set world to launch",
),
DeclareLaunchArgument(
"verbose",
default_value="true",
description="Enable verbosity",
),
# Note that the Halodi Controller publishes /clock and therefore conflicts with gazebo_ros_init
# Also, resetting the world state is not currently supported by the controller.
# Therefore, we disable the gazebo_ros_init plugin
DeclareLaunchArgument(
"init",
default_value="false",
description='Set "false" not to load "libgazebo_ros_init.so"',
),
DeclareLaunchArgument(
"factory",
default_value="true",
description='Set "false" not to load "libgazebo_ros_factory.so"',
),
DeclareLaunchArgument(
"trajectory-api",
default_value="true",
description=
'Set "false" to disable trajectory api and use realtime api',
),
DeclareLaunchArgument(
"headless",
default_value="false",
description='Set "true" to disable the visual UI',
),
SetEnvironmentVariable("HALODI_TRAJECTORY_API",
LaunchConfiguration("trajectory-api")),
DeclareLaunchArgument(
"variable-server",
default_value="false",
description=
'Set "true" to enable the variable server and use SCSVisualizer to tune variables',
),
SetEnvironmentVariable("SCS_VARIABLE_SERVER",
LaunchConfiguration("variable-server")),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(gazebo_package_prefix, "launch",
"gzserver.launch.py"))),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(gazebo_package_prefix, "launch",
"gzclient.launch.py")),
condition=UnlessCondition(LaunchConfiguration("headless")),
),
])
def generate_launch_description():
# Command-line arguments
tutorial_arg = DeclareLaunchArgument('rviz_tutorial',
default_value='False',
description='Tutorial flag')
# planning_context
robot_description_config = load_file('moveit_resources_panda_description',
'urdf/panda.urdf')
robot_description = {'robot_description': robot_description_config}
robot_description_semantic_config = load_file(
'moveit_resources_panda_moveit_config', 'config/panda.srdf')
robot_description_semantic = {
'robot_description_semantic': robot_description_semantic_config
}
kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config',
'config/kinematics.yaml')
robot_description_kinematics = {
'robot_description_kinematics': kinematics_yaml
}
# Planning Functionality
ompl_planning_pipeline_config = {
'move_group': {
'planning_plugin': 'ompl_interface/OMPLPlanner',
'request_adapters':
"""default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
'start_state_max_bounds_error': 0.1
}
}
ompl_planning_yaml = load_yaml('moveit_resources_panda_moveit_config',
'config/ompl_planning.yaml')
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
# Trajectory Execution Functionality
controllers_yaml = load_yaml('run_move_group', 'config/controllers.yaml')
moveit_controllers = {
'moveit_simple_controller_manager':
controllers_yaml,
'moveit_controller_manager':
'moveit_simple_controller_manager/MoveItSimpleControllerManager'
}
trajectory_execution = {
'moveit_manage_controllers': True,
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
'trajectory_execution.allowed_goal_duration_margin': 0.5,
'trajectory_execution.allowed_start_tolerance': 0.01
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True
}
# Start the actual move_group node/action server
run_move_group_node = Node(package='moveit_ros_move_group',
executable='move_group',
output='screen',
parameters=[
robot_description,
robot_description_semantic, kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution, moveit_controllers,
planning_scene_monitor_parameters
])
# RViz
tutorial_mode = LaunchConfiguration('rviz_tutorial')
rviz_base = os.path.join(get_package_share_directory('moveit2_tutorials'),
'launch')
rviz_full_config = os.path.join(rviz_base, 'panda_moveit_config_demo.rviz')
rviz_empty_config = os.path.join(rviz_base,
'panda_moveit_config_demo_empty.rviz')
rviz_node_tutorial = Node(package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_empty_config],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml
],
condition=IfCondition(tutorial_mode))
rviz_node = Node(package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_full_config],
parameters=[
robot_description, robot_description_semantic,
ompl_planning_pipeline_config, kinematics_yaml
],
condition=UnlessCondition(tutorial_mode))
# 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', 'world',
'panda_link0'
])
# Publish TF
robot_state_publisher = Node(package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='both',
parameters=[robot_description])
# Fake joint driver
fake_joint_driver_node = Node(
package='fake_joint_driver',
executable='fake_joint_driver_node',
parameters=[{
'controller_name': 'panda_arm_controller'
},
os.path.join(get_package_share_directory("run_move_group"),
"config", "panda_controllers.yaml"),
os.path.join(get_package_share_directory("run_move_group"),
"config", "start_positions.yaml"),
robot_description])
# Warehouse mongodb server
mongodb_server_node = Node(
package='warehouse_ros_mongo',
executable='mongo_wrapper_ros.py',
parameters=[{
'warehouse_port': 33829
}, {
'warehouse_host': 'localhost'
}, {
'warehouse_plugin':
'warehouse_ros_mongo::MongoDatabaseConnection'
}],
output='screen')
return LaunchDescription([
tutorial_arg, rviz_node, rviz_node_tutorial, static_tf,
robot_state_publisher, run_move_group_node, fake_joint_driver_node,
mongodb_server_node
])
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', 'nav2_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_map_saver_server_cmd = Node(package='nav2_map_server',
executable='map_saver_server',
output='screen',
parameters=[configured_params])
start_lifecycle_manager_cmd = Node(package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_slam',
output='screen',
parameters=[{
'use_sim_time': use_sim_time
}, {
'autostart': autostart
}, {
'node_names':
lifecycle_nodes
}])
# If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file'
# LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load
# the default file
has_slam_toolbox_params = HasNodeParams(source_file=params_file,
node_name='slam_toolbox')
start_slam_toolbox_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(slam_launch_file),
launch_arguments={'use_sim_time': use_sim_time}.items(),
condition=UnlessCondition(has_slam_toolbox_params))
start_slam_toolbox_cmd_with_params = IncludeLaunchDescription(
PythonLaunchDescriptionSource(slam_launch_file),
launch_arguments={
'use_sim_time': use_sim_time,
'slam_params_file': params_file
}.items(),
condition=IfCondition(has_slam_toolbox_params))
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 Map Saver Server
ld.add_action(start_map_saver_server_cmd)
ld.add_action(start_lifecycle_manager_cmd)
# Running SLAM Toolbox (Only one of them will be run)
ld.add_action(start_slam_toolbox_cmd)
ld.add_action(start_slam_toolbox_cmd_with_params)
return ld
def generate_launch_description():
"""
Generate launch description from launch arguments and list of nodes to launch.
:return:
LaunchDescription object
"""
package_path = get_package_share_path(
'uwrt_mars_rover_drivetrain_description')
default_model_path = package_path / 'urdf' / 'drivetrain.urdf.xacro'
default_rviz_config_path = package_path / 'rviz' / 'urdf.rviz'
robot_description_content = ParameterValue(Command(
['ros2 run xacro xacro ',
LaunchConfiguration('model')]),
value_type=str)
# Declared Arguments
declared_arguments = []
declared_arguments += [
DeclareLaunchArgument(
name='gui',
default_value='true',
choices=['true', 'false'],
description='Flag to enable joint_state_publisher_gui')
]
declared_arguments += [
DeclareLaunchArgument(name='model',
default_value=str(default_model_path),
description='Absolute path to robot urdf file')
]
declared_arguments += [
DeclareLaunchArgument(name='rvizconfig',
default_value=str(default_rviz_config_path),
description='Absolute path to rviz config file')
]
# Nodes
nodes = []
nodes += [
Node(package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{
'robot_description': robot_description_content
}])
]
# Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
nodes += [
Node(package='joint_state_publisher',
executable='joint_state_publisher',
condition=UnlessCondition(LaunchConfiguration('gui')))
]
nodes += [
Node(package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
condition=IfCondition(LaunchConfiguration('gui')))
]
nodes += [
Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
]
return LaunchDescription(declared_arguments + nodes)
def generate_launch_description():
def add_launch_arg(name: str, default_value=None):
return DeclareLaunchArgument(name, default_value=default_value)
set_container_executable = SetLaunchConfiguration(
"container_executable",
"component_container",
condition=UnlessCondition(LaunchConfiguration("use_multithread")),
)
set_container_mt_executable = SetLaunchConfiguration(
"container_executable",
"component_container_mt",
condition=IfCondition(LaunchConfiguration("use_multithread")),
)
composable_nodes = [
ComposableNode(
package="pointcloud_to_laserscan",
plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode",
name="pointcloud_to_laserscan_node",
remappings=[
("~/input/pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
("~/output/laserscan", LaunchConfiguration("output/laserscan")),
("~/output/pointcloud", LaunchConfiguration("output/pointcloud")),
("~/output/ray", LaunchConfiguration("output/ray")),
("~/output/stixel", LaunchConfiguration("output/stixel")),
],
parameters=[
{
"target_frame": "base_link", # Leave disabled to output scan in pointcloud frame
"transform_tolerance": 0.01,
"min_height": 0.0,
"max_height": 2.0,
"angle_min": -3.141592, # -M_PI
"angle_max": 3.141592, # M_PI
"angle_increment": 0.00436332222, # 0.25*M_PI/180.0
"scan_time": 0.0,
"range_min": 1.0,
"range_max": 200.0,
"use_inf": True,
"inf_epsilon": 1.0,
# Concurrency level, affects number of pointclouds queued for processing
# and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
"concurrency_level": 1,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
),
ComposableNode(
package="laserscan_to_occupancy_grid_map",
plugin="occupancy_grid_map::OccupancyGridMapNode",
name="occupancy_grid_map_node",
remappings=[
("~/input/laserscan", LaunchConfiguration("output/laserscan")),
("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")),
("~/output/occupancy_grid_map", LaunchConfiguration("output")),
],
parameters=[
{
"map_resolution": 0.5,
"use_height_filter": True,
"input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"),
"input_obstacle_and_raw_pointcloud": LaunchConfiguration(
"input_obstacle_and_raw_pointcloud"
),
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
),
]
occupancy_grid_map_container = ComposableNodeContainer(
condition=LaunchConfigurationEquals("container", ""),
name="occupancy_grid_map_container",
namespace="",
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=composable_nodes,
output="screen",
)
load_composable_nodes = LoadComposableNodes(
condition=LaunchConfigurationNotEquals("container", ""),
composable_node_descriptions=composable_nodes,
target_container=LaunchConfiguration("container"),
)
return LaunchDescription(
[
add_launch_arg("container", ""),
add_launch_arg("use_multithread", "false"),
add_launch_arg("use_intra_process", "false"),
add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"),
add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"),
add_launch_arg("output", "occupancy_grid"),
add_launch_arg("output/laserscan", "virtual_scan/laserscan"),
add_launch_arg("output/pointcloud", "virtual_scan/pointcloud"),
add_launch_arg("output/ray", "virtual_scan/ray"),
add_launch_arg("output/stixel", "virtual_scan/stixel"),
add_launch_arg("input_obstacle_pointcloud", "false"),
add_launch_arg("input_obstacle_and_raw_pointcloud", "true"),
set_container_executable,
set_container_mt_executable,
occupancy_grid_map_container,
load_composable_nodes,
]
)
def generate_launch_description():
enable_dummy = LaunchConfiguration('enable_dummy', default=False)
enable_dummy_arg = DeclareLaunchArgument(
'enable_dummy',
default_value=enable_dummy,
description="if true, enable dummy mini-v.")
view_model = LaunchConfiguration('view_model', default=False)
view_model_arg = DeclareLaunchArgument(
'view_model',
default_value=view_model,
description="if true, launch with given rviz configuration.")
rviz = Node(package='rviz2',
executable='rviz2',
name='rviz2',
output={
'stderr': 'log',
'stdout': 'log',
},
condition=IfCondition(view_model),
arguments=[
'-d',
os.path.join(
get_package_share_directory("miniv_description"),
"config", "miniv.rviz")
])
controller_config = os.path.join(
get_package_share_directory("miniv_description"), "config",
"controllers.yaml")
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
condition=UnlessCondition(enable_dummy),
parameters=[generate_robot_description(False)])
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[generate_robot_description(False), controller_config],
output={
"stdout": "screen",
"stderr": "screen",
},
condition=UnlessCondition(enable_dummy))
robot_state_publisher_dummy = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
condition=IfCondition(enable_dummy),
parameters=[generate_robot_description(True)])
control_node_dummy = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[generate_robot_description(True), controller_config],
output={
"stdout": "screen",
"stderr": "screen",
},
condition=IfCondition(enable_dummy))
return launch.LaunchDescription([
robot_state_publisher, robot_state_publisher_dummy, view_model_arg,
enable_dummy_arg, rviz, control_node, control_node_dummy,
ExecuteProcess(
cmd=[
"ros2", "control", "load_start_controller",
"joint_state_controller"
],
output="screen",
shell=True,
),
ExecuteProcess(
cmd=[
"ros2", "control", "load_start_controller",
"usv_joy_controller"
],
output="screen",
shell=True,
)
])
def generate_launch_description():
# Command-line arguments
tutorial_arg = DeclareLaunchArgument("rviz_tutorial",
default_value="False",
description="Tutorial flag")
# planning_context
robot_description_config = xacro.process_file(
os.path.join(
get_package_share_directory(
"moveit_resources_panda_moveit_config"),
"config",
"panda.urdf.xacro",
))
robot_description = {"robot_description": robot_description_config.toxml()}
robot_description_semantic_config = load_file(
"moveit_resources_panda_moveit_config", "config/panda.srdf")
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": kinematics_yaml
}
# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters":
"""default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("moveit_resources_panda_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config",
"config/moveit_controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager":
moveit_simple_controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# Start the actual move_group node/action server
run_move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# RViz
tutorial_mode = LaunchConfiguration("rviz_tutorial")
rviz_base = os.path.join(get_package_share_directory("moveit2_tutorials"),
"launch")
rviz_full_config = os.path.join(rviz_base, "panda_moveit_config_demo.rviz")
rviz_empty_config = os.path.join(rviz_base,
"panda_moveit_config_demo_empty.rviz")
rviz_node_tutorial = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_empty_config],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
],
condition=IfCondition(tutorial_mode),
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_full_config],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
],
condition=UnlessCondition(tutorial_mode),
)
# 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", "world", "panda_link0"
],
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"ros2_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output={
"stdout": "screen",
"stderr": "screen",
},
)
# Load controllers
load_controllers = []
for controller in [
"panda_arm_controller",
"panda_hand_controller",
"joint_state_broadcaster",
]:
load_controllers += [
ExecuteProcess(
cmd=[
"ros2 run controller_manager spawner {}".format(controller)
],
shell=True,
output="screen",
)
]
return LaunchDescription([
tutorial_arg,
rviz_node,
rviz_node_tutorial,
static_tf,
robot_state_publisher,
run_move_group_node,
ros2_control_node,
] + load_controllers)
def generate_launch_description():
ld = LaunchDescription()
world_arg = DeclareLaunchArgument(
'world',
default_value='empty.world',
description='Gazebo world file'
)
headless_arg = DeclareLaunchArgument(
'headless',
default_value='false',
description="Set to 'true' to run gazebo headless"
)
# We need to add the models and worlds directories to env so gazebo can find them
triton_gazebo_dir = get_package_share_directory('triton_gazebo')
gmp = 'GAZEBO_MODEL_PATH'
add_model_path = SetEnvironmentVariable(
name=gmp,
value=[
EnvironmentVariable(gmp),
os.pathsep + os.path.join(triton_gazebo_dir, 'gazebo', 'models')
]
)
grp = 'GAZEBO_RESOURCE_PATH'
add_resource_path = SetEnvironmentVariable(
name=grp,
value=[
EnvironmentVariable(grp),
os.pathsep + os.path.join(triton_gazebo_dir, 'gazebo')
]
)
gazebo_server = IncludeLaunchDescription(
launch_description_source=PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('gazebo_ros'),
'launch', 'gzserver.launch.py')
),
launch_arguments={
'world': ['worlds/', LaunchConfiguration('world')],
'verbose': 'true'
}.items()
)
gazebo_client = IncludeLaunchDescription(
launch_description_source=PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('gazebo_ros'),
'launch', 'gzclient.launch.py')
),
condition=UnlessCondition(LaunchConfiguration('headless')),
)
ld.add_action(world_arg)
ld.add_action(headless_arg)
ld.add_action(add_model_path)
ld.add_action(add_resource_path)
ld.add_action(gazebo_server)
ld.add_action(gazebo_client)
return ld
def generate_launch_description():
relay_components = []
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="route_relay",
namespace="awapi",
parameters=[{
"input_topic": LaunchConfiguration("input_route"),
"output_topic": LaunchConfiguration("get_route"),
"type": "autoware_planning_msgs/msg/Route",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="predict_object_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_object"),
"output_topic":
LaunchConfiguration("get_predicted_object"),
"type":
"autoware_perception_msgs/msg/DynamicObjectArray",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="nearest_traffic_light_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_nearest_traffic_light_state"),
"output_topic":
LaunchConfiguration("get_nearest_traffic_light_status"),
"type":
"autoware_perception_msgs/msg/LookingTrafficLightState",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="ready_module_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_path_change_ready"),
"output_topic":
LaunchConfiguration("get_path_change_ready"),
"type":
"autoware_planning_msgs/msg/PathChangeModule",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="force_available_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_path_change_force_available"),
"output_topic":
LaunchConfiguration("get_path_change_force_available"),
"type":
"autoware_planning_msgs/msg/PathChangeModuleArray",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="running_modules_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_path_change_running"),
"output_topic":
LaunchConfiguration("get_path_change_running"),
"type":
"autoware_planning_msgs/msg/PathChangeModuleArray",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="autoware_engage_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_engage"),
"output_topic":
LaunchConfiguration("output_autoware_engage"),
"type":
"autoware_vehicle_msgs/msg/Engage",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="vehicle_engage_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_engage"),
"output_topic":
LaunchConfiguration("output_vehicle_engage"),
"type":
"autoware_vehicle_msgs/msg/Engage",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="put_route_relay",
namespace="awapi",
parameters=[{
"input_topic": LaunchConfiguration("set_route"),
"output_topic": LaunchConfiguration("output_route"),
"type": "autoware_planning_msgs/msg/Route",
"durability": "transient_local",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="put_goal_relay",
namespace="awapi",
parameters=[{
"input_topic": LaunchConfiguration("set_goal"),
"output_topic": LaunchConfiguration("output_goal"),
"type": "geometry_msgs/msg/PoseStamped",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="lane_change_approval_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_lane_change_approval"),
"output_topic":
LaunchConfiguration("output_lane_change_approval"),
"type":
"autoware_planning_msgs/msg/LaneChangeCommand",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="force_lane_change_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_force_lane_change"),
"output_topic":
LaunchConfiguration("output_force_lane_change"),
"type":
"autoware_planning_msgs/msg/LaneChangeCommand",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="external_approval_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_path_change_approval"),
"output_topic":
LaunchConfiguration("output_path_change_approval"),
"type":
"autoware_planning_msgs/msg/Approval",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="force_approval_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_path_change_force"),
"output_topic":
LaunchConfiguration("output_path_change_force"),
"type":
"autoware_planning_msgs/msg/PathChangeModule",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="obstacle_avoid_approval_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_obstacle_avoid_approval"),
"output_topic":
LaunchConfiguration("output_obstacle_avoid_approval"),
"type":
"autoware_planning_msgs/msg/EnableAvoidance",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="traffic_light_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_traffic_light_state"),
"output_topic":
LaunchConfiguration("get_traffic_light_status"),
"type":
"autoware_perception_msgs/msg/TrafficLightStateArray",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="overwrite_traffic_light_state_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_overwrite_traffic_light_state"),
"output_topic":
LaunchConfiguration("output_overwrite_traffic_light_state"),
"type":
"autoware_perception_msgs/msg/TrafficLightStateArray",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="speed_exceeded_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("input_stop_speed_exceeded"),
"output_topic":
LaunchConfiguration("get_stop_speed_exceeded"),
"type":
"autoware_planning_msgs/msg/StopSpeedExceeded",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="crosswalk_status_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_crosswalk_status"),
"output_topic":
LaunchConfiguration("input_external_crosswalk_status"),
"type":
"autoware_api_msgs/msg/CrosswalkStatus",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="intersection_status_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_intersection_status"),
"output_topic":
LaunchConfiguration("input_external_intersection_status"),
"type":
"autoware_api_msgs/msg/IntersectionStatus",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="expand_stop_range_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_expand_stop_range"),
"output_topic":
LaunchConfiguration("input_expand_stop_range"),
"type":
"autoware_planning_msgs/msg/ExpandStopRange",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
relay_components.append(
ComposableNode(
package="topic_tools",
plugin="topic_tools::RelayNode",
name="pose_initialization_request_relay",
namespace="awapi",
parameters=[{
"input_topic":
LaunchConfiguration("set_pose_initialization_request"),
"output_topic":
LaunchConfiguration("input_pose_initialization_request"),
"type":
"autoware_localization_msgs/msg/PoseInitializationRequest",
}],
extra_arguments=[{
"use_intra_process_comms":
LaunchConfiguration("use_intra_process")
}],
))
container = ComposableNodeContainer(
name="awapi_relay_container",
namespace="awapi",
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=relay_components,
output="screen",
)
set_container_executable = SetLaunchConfiguration(
"container_executable",
"component_container",
condition=UnlessCondition(LaunchConfiguration("use_multithread")),
)
set_container_mt_executable = SetLaunchConfiguration(
"container_executable",
"component_container_mt",
condition=IfCondition(LaunchConfiguration("use_multithread")),
)
return launch.LaunchDescription(
[set_container_executable, set_container_mt_executable] + [container])
def generate_launch_description():
enable_dummy = LaunchConfiguration('enable_dummy', default=True)
enable_dummy_arg = DeclareLaunchArgument(
'enable_dummy',
default_value=enable_dummy,
description="if true, enable dummy wam-v.")
controller_config = os.path.join(
get_package_share_directory('wamv_description'), 'config',
'controllers.yaml')
doc = xacro.process_file(xacro_path)
robot_desc = doc.toprettyxml(indent=' ')
f = open(urdf_path, 'w')
f.write(robot_desc)
f.close()
rsp = launch_ros.actions.Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
arguments=[urdf_path],
condition=UnlessCondition(enable_dummy),
parameters=[{
'robot_description': robot_desc
}])
control_node = launch_ros.actions.Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[{
'robot_description': robot_desc
}, controller_config],
output={
'stdout': 'screen',
'stderr': 'screen',
},
condition=UnlessCondition(enable_dummy))
rsp_dummy = launch_ros.actions.Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
condition=IfCondition(enable_dummy),
arguments=[urdf_path],
parameters=[{
'robot_description':
robot_desc
}])
control_node_dummy = launch_ros.actions.Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[{
'robot_description': robot_desc
}, controller_config],
output={
'stdout': 'screen',
'stderr': 'screen',
},
condition=IfCondition(enable_dummy))
return launch.LaunchDescription([
rsp,
rsp_dummy,
enable_dummy_arg,
control_node,
control_node_dummy,
ExecuteProcess(
cmd=[
'ros2', 'control', 'load_controller', 'joint_state_broadcaster'
],
output='screen',
shell=True,
),
ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', 'usv_joy_controller'],
output='screen',
shell=True,
),
])
def generate_launch_description():
cmd = [
'gzserver',
# Pass through arguments to gzserver
LaunchConfiguration('world'), ' ',
_boolean_command('version'), ' ',
_boolean_command('verbose'), ' ',
_boolean_command('lockstep'), ' ',
_boolean_command('help'), ' ',
_boolean_command('pause'), ' ',
_arg_command('physics'), ' ', LaunchConfiguration('physics'), ' ',
_arg_command('play'), ' ', LaunchConfiguration('play'), ' ',
_boolean_command('record'), ' ',
_arg_command('record_encoding'), ' ', LaunchConfiguration('record_encoding'), ' ',
_arg_command('record_path'), ' ', LaunchConfiguration('record_path'), ' ',
_arg_command('record_period'), ' ', LaunchConfiguration('record_period'), ' ',
_arg_command('record_filter'), ' ', LaunchConfiguration('record_filter'), ' ',
_arg_command('seed'), ' ', LaunchConfiguration('seed'), ' ',
_arg_command('iters'), ' ', LaunchConfiguration('iters'), ' ',
_boolean_command('minimal_comms'),
_plugin_command('init'), ' ',
_plugin_command('factory'), ' ',
_plugin_command('force_system'), ' ',
# Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941)
# _plugin_command('force_system'), ' ',
_arg_command('profile'), ' ', LaunchConfiguration('profile'),
LaunchConfiguration('extra_gazebo_args'),
]
model, plugin, media = GazeboRosPaths.get_paths()
if 'GAZEBO_MODEL_PATH' in environ:
model += pathsep+environ['GAZEBO_MODEL_PATH']
if 'GAZEBO_PLUGIN_PATH' in environ:
plugin += pathsep+environ['GAZEBO_PLUGIN_PATH']
if 'GAZEBO_RESOURCE_PATH' in environ:
media += pathsep+environ['GAZEBO_RESOURCE_PATH']
env = {
'GAZEBO_MODEL_PATH': model,
'GAZEBO_PLUGIN_PATH': plugin,
'GAZEBO_RESOURCE_PATH': media
}
prefix = PythonExpression([
'"gdb -ex run --args" if "true" == "',
LaunchConfiguration('gdb'),
'" else "valgrind" if "true" == "',
LaunchConfiguration('valgrind'),
'" else ""'
])
return LaunchDescription([
DeclareLaunchArgument(
'world', default_value='',
description='Specify world file name'
),
DeclareLaunchArgument(
'version', default_value='false',
description='Set "true" to output version information.'
),
DeclareLaunchArgument(
'verbose', default_value='false',
description='Set "true" to increase messages written to terminal.'
),
DeclareLaunchArgument(
'lockstep', default_value='false',
description='Set "true" to respect update rates'
),
DeclareLaunchArgument(
'help', default_value='false',
description='Set "true" to produce gzserver help message.'
),
DeclareLaunchArgument(
'pause', default_value='false',
description='Set "true" to start the server in a paused state.'
),
DeclareLaunchArgument(
'physics', default_value='',
description='Specify a physics engine (ode|bullet|dart|simbody).'
),
DeclareLaunchArgument(
'play', default_value='',
description='Play the specified log file.'
),
DeclareLaunchArgument(
'record', default_value='false',
description='Set "true" to record state data.'
),
DeclareLaunchArgument(
'record_encoding', default_value='',
description='Specify compression encoding format for log data (zlib|bz2|txt).'
),
DeclareLaunchArgument(
'record_path', default_value='',
description='Absolute path in which to store state data.'
),
DeclareLaunchArgument(
'record_period', default_value='',
description='Specify recording period (seconds).'
),
DeclareLaunchArgument(
'record_filter', default_value='',
description='Specify recording filter (supports wildcard and regular expression).'
),
DeclareLaunchArgument(
'seed', default_value='', description='Start with a given a random number seed.'
),
DeclareLaunchArgument(
'iters', default_value='', description='Specify number of iterations to simulate.'
),
DeclareLaunchArgument(
'minimal_comms', default_value='false',
description='Set "true" to reduce TCP/IP traffic output.'
),
DeclareLaunchArgument(
'profile', default_value='',
description='Specify physics preset profile name from the options in the world file.'
),
DeclareLaunchArgument(
'extra_gazebo_args', default_value='',
description='Extra arguments to be passed to Gazebo'
),
# Specific to gazebo_ros
DeclareLaunchArgument(
'gdb', default_value='false',
description='Set "true" to run gzserver with gdb'
),
DeclareLaunchArgument(
'valgrind', default_value='false',
description='Set "true" to run gzserver with valgrind'
),
DeclareLaunchArgument(
'init', default_value='true',
description='Set "false" not to load "libgazebo_ros_init.so"'
),
DeclareLaunchArgument(
'factory', default_value='true',
description='Set "false" not to load "libgazebo_ros_factory.so"'
),
DeclareLaunchArgument(
'force_system', default_value='true',
description='Set "false" not to load "libgazebo_ros_force_system.so"'
),
DeclareLaunchArgument(
'server_required', default_value='false',
description='Set "true" to shut down launch script when server is terminated'
),
# Execute node with on_exit=Shutdown if server_required is specified.
# See ros-simulation/gazebo_ros_pkgs#1086. Simplification of logic
# would be possible pending ros2/launch#290.
ExecuteProcess(
cmd=cmd,
output='screen',
additional_env=env,
shell=True,
prefix=prefix,
on_exit=Shutdown(),
condition=IfCondition(LaunchConfiguration('server_required')),
),
# Execute node with default on_exit if the node is not required
ExecuteProcess(
cmd=cmd,
output='screen',
additional_env=env,
shell=True,
prefix=prefix,
condition=UnlessCondition(LaunchConfiguration('server_required')),
),
])
def generate_launch_description():
"""Launch controller_testing_node and pure purusit controller."""
controller_testing_pkg_prefix = get_package_share_directory("controller_testing")
controller_testing_param_file = os.path.join(
controller_testing_pkg_prefix, "param/defaults.param.yaml"
)
rviz_cfg_path = os.path.join(controller_testing_pkg_prefix, 'config/pure_pursuit_cotrols.rviz')
pure_pursuit_pkg_prefix = get_package_share_directory("pure_pursuit_nodes")
pure_pursuit_param_file = os.path.join(
pure_pursuit_pkg_prefix, "param/pure_pursuit.param.yaml"
)
urdf_pkg_prefix = get_package_share_directory('lexus_rx_450h_description')
urdf_path = os.path.join(urdf_pkg_prefix, 'urdf/lexus_rx_450h.urdf')
# Arguments
controller_testing_param = DeclareLaunchArgument(
"controller_testing_param_file",
default_value=controller_testing_param_file,
description="Path to config file for Controller Testing",
)
pure_pursuit_controller_param = DeclareLaunchArgument(
"pure_pursuit_param_file",
default_value=pure_pursuit_param_file,
description="Path to config file to Pure Pursuit Controller",
)
with_rviz_param = DeclareLaunchArgument(
'with_rviz',
default_value='True',
description='Launch RVIZ2 in addition to other nodes'
)
real_time_sim_param = DeclareLaunchArgument(
'real_time_sim',
default_value='False',
description='Launch RVIZ2 in addition to other nodes'
)
# Nodes
controller_testing = Node(
package="controller_testing",
node_executable="controller_testing_main.py",
node_namespace="control",
node_name="controller_testing_node",
output="screen",
parameters=[LaunchConfiguration("controller_testing_param_file"), {
'real_time_sim': LaunchConfiguration('real_time_sim')
}],
remappings=[
("vehicle_state", "/vehicle/vehicle_kinematic_state"),
("planned_trajectory", "/planning/trajectory"),
("control_command", "/vehicle/control_command"),
("control_diagnostic", "/control/control_diagnostic"),
],
on_exit=Shutdown(),
condition=UnlessCondition(LaunchConfiguration('with_rviz'))
)
controller_testing_delayed = Node(
package="controller_testing",
node_executable="controller_testing_main.py",
node_namespace="control",
node_name="controller_testing_node",
output="screen",
parameters=[LaunchConfiguration("controller_testing_param_file"), {
'real_time_sim': LaunchConfiguration('real_time_sim')
}],
remappings=[
("vehicle_state", "/vehicle/vehicle_kinematic_state"),
("planned_trajectory", "/planning/trajectory"),
("control_command", "/vehicle/control_command"),
("control_diagnostic", "/control/control_diagnostic"),
],
on_exit=Shutdown(),
# delay added to allow rviz to be ready, better to start rviz separately, beforehand
prefix="bash -c 'sleep 2.0; $0 $@'",
condition=IfCondition(LaunchConfiguration('with_rviz'))
)
# pure pursuit controller
pure_pursuit_controller = Node(
package="pure_pursuit_nodes",
node_executable="pure_pursuit_node_exe",
# node_namespace="control",
node_name="pure_pursuit_node",
output="screen",
parameters=[LaunchConfiguration("pure_pursuit_param_file"), {}],
remappings=[
("current_pose", "/vehicle/vehicle_kinematic_state"),
("trajectory", "/planning/trajectory"),
("ctrl_cmd", "/vehicle/control_command"),
("ctrl_diag", "/control/control_diagnostic"),
],
)
rviz2 = Node(
package='rviz2',
node_executable='rviz2',
node_name='rviz2',
arguments=['-d', str(rviz_cfg_path)],
condition=IfCondition(LaunchConfiguration('with_rviz'))
)
urdf_publisher = Node(
package='robot_state_publisher',
node_executable='robot_state_publisher',
node_name='robot_state_publisher',
arguments=[str(urdf_path)]
)
return LaunchDescription(
[
real_time_sim_param,
controller_testing_param,
pure_pursuit_controller_param,
with_rviz_param,
rviz2,
urdf_publisher,
pure_pursuit_controller,
controller_testing, # if not with_rviz
controller_testing_delayed # with_rviz
]
)
def launch_setup(context, *args, **kwargs):
# https://github.com/ros2/launch_ros/issues/156
def load_composable_node_param(param_path):
with open(LaunchConfiguration(param_path).perform(context), "r") as f:
return yaml.safe_load(f)["/**"]["ros__parameters"]
ns = "euclidean_cluster"
pkg = "euclidean_cluster"
# set voxel grid filter as a component
voxel_grid_filter_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
name=AnonName("voxel_grid_filter"),
remappings=[
("input", LaunchConfiguration("input_pointcloud")),
("output", "voxel_grid_filtered/pointcloud"),
],
parameters=[load_composable_node_param("voxel_grid_param_path")],
)
# set compare map filter as a component
compare_map_filter_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::VoxelBasedCompareMapFilterComponent",
name=AnonName("compare_map_filter"),
remappings=[
("input", "voxel_grid_filtered/pointcloud"),
("map", LaunchConfiguration("input_map")),
("output", "compare_map_filtered/pointcloud"),
],
)
use_map_euclidean_cluster_component = ComposableNode(
package=pkg,
plugin="euclidean_cluster::EuclideanClusterNode",
name=AnonName("euclidean_cluster"),
remappings=[
("input", "compare_map_filtered/pointcloud"),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("euclidean_param_path")],
)
disuse_map_euclidean_cluster_component = ComposableNode(
package=pkg,
plugin="euclidean_cluster::EuclideanClusterNode",
name=AnonName("euclidean_cluster"),
remappings=[
("input", "voxel_grid_filtered/pointcloud"),
("output", LaunchConfiguration("output_clusters")),
],
parameters=[load_composable_node_param("euclidean_param_path")],
)
container = ComposableNodeContainer(
name="euclidean_cluster_container",
namespace=ns,
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[voxel_grid_filter_component],
output="screen",
)
use_map_loader = LoadComposableNodes(
composable_node_descriptions=[
compare_map_filter_component,
use_map_euclidean_cluster_component,
],
target_container=container,
condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
)
disuse_map_loader = LoadComposableNodes(
composable_node_descriptions=[disuse_map_euclidean_cluster_component],
target_container=container,
condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
)
return [container, use_map_loader, disuse_map_loader]
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
robot_ip = LaunchConfiguration("robot_ip")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
runtime_config_package = LaunchConfiguration("runtime_config_package")
controllers_file = LaunchConfiguration("controllers_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
activate_joint_controller = LaunchConfiguration("activate_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
headless_mode = LaunchConfiguration("headless_mode")
launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
use_tool_communication = LaunchConfiguration("use_tool_communication")
tool_parity = LaunchConfiguration("tool_parity")
tool_baud_rate = LaunchConfiguration("tool_baud_rate")
tool_stop_bits = LaunchConfiguration("tool_stop_bits")
tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars")
tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars")
tool_device_name = LaunchConfiguration("tool_device_name")
tool_tcp_port = LaunchConfiguration("tool_tcp_port")
tool_voltage = LaunchConfiguration("tool_voltage")
joint_limit_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
)
kinematics_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"]
)
physical_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"]
)
visual_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"]
)
script_filename = PathJoinSubstitution(
[FindPackageShare("ur_robot_driver"), "resources", "ros_control.urscript"]
)
input_recipe_filename = PathJoinSubstitution(
[FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
)
output_recipe_filename = PathJoinSubstitution(
[FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
" ",
"robot_ip:=",
robot_ip,
" ",
"joint_limit_params:=",
joint_limit_params,
" ",
"kinematics_params:=",
kinematics_params,
" ",
"physical_params:=",
physical_params,
" ",
"visual_params:=",
visual_params,
" ",
"safety_limits:=",
safety_limits,
" ",
"safety_pos_margin:=",
safety_pos_margin,
" ",
"safety_k_position:=",
safety_k_position,
" ",
"name:=",
ur_type,
" ",
"script_filename:=",
script_filename,
" ",
"input_recipe_filename:=",
input_recipe_filename,
" ",
"output_recipe_filename:=",
output_recipe_filename,
" ",
"prefix:=",
prefix,
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"fake_sensor_commands:=",
fake_sensor_commands,
" ",
"headless_mode:=",
headless_mode,
" ",
"use_tool_communication:=",
use_tool_communication,
" ",
"tool_parity:=",
tool_parity,
" ",
"tool_baud_rate:=",
tool_baud_rate,
" ",
"tool_stop_bits:=",
tool_stop_bits,
" ",
"tool_rx_idle_chars:=",
tool_rx_idle_chars,
" ",
"tool_tx_idle_chars:=",
tool_tx_idle_chars,
" ",
"tool_device_name:=",
tool_device_name,
" ",
"tool_tcp_port:=",
tool_tcp_port,
" ",
"tool_voltage:=",
tool_voltage,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
initial_joint_controllers = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "view_robot.rviz"]
)
# define update rate
update_rate_config_file = PathJoinSubstitution(
[
FindPackageShare(runtime_config_package),
"config",
ur_type.perform(context) + "_update_rate.yaml",
]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, update_rate_config_file, initial_joint_controllers],
output="screen",
condition=IfCondition(use_fake_hardware),
)
ur_control_node = Node(
package="ur_robot_driver",
executable="ur_ros2_control_node",
parameters=[robot_description, update_rate_config_file, initial_joint_controllers],
output="screen",
condition=UnlessCondition(use_fake_hardware),
)
dashboard_client_node = Node(
package="ur_robot_driver",
condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware),
executable="dashboard_client",
name="dashboard_client",
output="screen",
emulate_tty=True,
parameters=[{"robot_ip": robot_ip}],
)
tool_communication_node = Node(
package="ur_robot_driver",
condition=IfCondition(use_tool_communication),
executable="tool_communication.py",
name="ur_tool_comm",
output="screen",
parameters=[
{
"robot_ip": robot_ip,
"tcp_port": tool_tcp_port,
"device_name": tool_device_name,
}
],
)
controller_stopper_node = Node(
package="ur_robot_driver",
executable="controller_stopper_node",
name="controller_stopper",
output="screen",
emulate_tty=True,
condition=UnlessCondition(use_fake_hardware),
parameters=[
{"headless_mode": headless_mode},
{"joint_controller_active": activate_joint_controller},
{
"consistent_controllers": [
"io_and_status_controller",
"force_torque_sensor_broadcaster",
"joint_state_broadcaster",
"speed_scaling_state_broadcaster",
]
},
],
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
condition=IfCondition(launch_rviz),
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"],
)
io_and_status_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["io_and_status_controller", "-c", "/controller_manager"],
)
speed_scaling_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"speed_scaling_state_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
force_torque_sensor_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"force_torque_sensor_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
forward_position_controller_spawner_stopped = Node(
package="controller_manager",
executable="spawner",
arguments=["forward_position_controller", "-c", "/controller_manager", "--stopped"],
)
# There may be other controllers of the joints, but this is the initially-started one
initial_joint_controller_spawner_started = Node(
package="controller_manager",
executable="spawner",
arguments=[initial_joint_controller, "-c", "/controller_manager"],
condition=IfCondition(activate_joint_controller),
)
initial_joint_controller_spawner_stopped = Node(
package="controller_manager",
executable="spawner",
arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"],
condition=UnlessCondition(activate_joint_controller),
)
nodes_to_start = [
control_node,
ur_control_node,
dashboard_client_node,
tool_communication_node,
controller_stopper_node,
robot_state_publisher_node,
rviz_node,
joint_state_broadcaster_spawner,
io_and_status_controller_spawner,
speed_scaling_state_broadcaster_spawner,
force_torque_sensor_broadcaster_spawner,
forward_position_controller_spawner_stopped,
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
]
return nodes_to_start
def generate_launch_description():
# Use a simpler urdf file: no forward camera, no barometer, no thrust, no drag
# Does contain a motion plugin, so the AUV will be pushed around in a repeating pattern
orca_description_dir = get_package_share_directory('orca_description')
urdf_file = os.path.join(orca_description_dir, 'urdf', 'slam_test.urdf')
# No fiducial markers
orca_gazebo_dir = get_package_share_directory('orca_gazebo')
world_file = os.path.join(orca_gazebo_dir, 'worlds', 'empty.world')
# ORB features vocabulary file
# This works well in simulation, but I'm sure how it will do in a marine environment
orb_slam_dir = get_package_share_directory('orb_slam2_ros')
orb_voc_file = os.path.join(orb_slam_dir, 'orb_slam2', 'Vocabulary', 'ORBvoc.txt')
# Orb-slam2 params
orca_bringup_dir = get_package_share_directory('orca_bringup')
slam_params_file = os.path.join(orca_bringup_dir, 'params', 'slam_test_params.yaml')
# Rviz config
rviz_cfg_file = os.path.join(orca_bringup_dir, 'cfg', 'slam_test_launch.rviz')
left_components = [
ComposableNode(
package='image_proc',
plugin='image_proc::RectifyNode',
name='left_rectify_node',
remappings=[
('image', '/stereo/left/image_raw'),
('camera_info', '/stereo/left/camera_info'),
('image_rect', '/stereo/left/image_rect')
],
),
]
right_components = [
ComposableNode(
package='image_proc',
plugin='image_proc::RectifyNode',
name='right_rectify_node',
remappings=[
('image', '/stereo/right/image_raw'),
('camera_info', '/stereo/right/camera_info'),
('image_rect', '/stereo/right/image_rect')
],
),
]
return LaunchDescription([
DeclareLaunchArgument(
'gzclient',
default_value='False',
description='Launch Gazebo UI?',
),
DeclareLaunchArgument(
'rviz',
default_value='True',
description='Launch rviz?',
),
DeclareLaunchArgument(
'rectify',
default_value='False',
description='Rectify nodes?',
),
# Launch gzserver
ExecuteProcess(
cmd=['gzserver',
'-s', 'libgazebo_ros_init.so', # Publish /clock
'-s', 'libgazebo_ros_factory.so', # Injection endpoint
world_file],
output='screen',
),
# Launch gzclient
ExecuteProcess(
cmd=['gzclient'],
output='screen',
condition=IfCondition(LaunchConfiguration('gzclient')),
),
# Launch rviz
ExecuteProcess(
cmd=['rviz2', '-d', rviz_cfg_file],
output='screen',
condition=IfCondition(LaunchConfiguration('rviz')),
),
# Replacement for base_controller: odom->base_link is static
ExecuteProcess(
cmd=['/opt/ros/foxy/lib/tf2_ros/static_transform_publisher',
'0', '0', '0', '0', '0', '0', 'odom', 'base_link',
'--ros-args', '-p', 'use_sim_time:=true'],
output='screen',
),
# Inject the urdf file
Node(
package='sim_fiducial',
executable='inject_entity.py',
output='screen',
arguments=[urdf_file, '0', '0', '0', '0', '0', '0'],
parameters=[{'use_sim_time': True}],
),
# Publish static transforms from the urdf
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
name='robot_state_publisher',
arguments=[urdf_file],
parameters=[{'use_sim_time': True}],
),
ComposableNodeContainer(
name='left_container',
package='rclcpp_components',
executable='component_container',
namespace='',
composable_node_descriptions=left_components,
output='screen',
condition=IfCondition(LaunchConfiguration('rectify')),
),
ComposableNodeContainer(
name='right_container',
package='rclcpp_components',
executable='component_container',
namespace='',
composable_node_descriptions=right_components,
output='screen',
condition=IfCondition(LaunchConfiguration('rectify')),
),
# Run orb_slam2_ros_stereo (sans rectification)
Node(
package='orb_slam2_ros',
executable='orb_slam2_ros_stereo',
output='screen',
name='orb_slam2_stereo',
parameters=[slam_params_file, {
'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=UnlessCondition(LaunchConfiguration('rectify')),
),
# Run orb_slam2_ros_stereo (mit rectification)
Node(
package='orb_slam2_ros',
executable='orb_slam2_ros_stereo',
output='screen',
name='orb_slam2_stereo',
parameters=[slam_params_file, {
'voc_file': orb_voc_file,
}],
remappings=[
('/image_left/image_color_rect', '/stereo/left/image_rect'),
('/image_right/image_color_rect', '/stereo/right/image_rect'),
('/camera/camera_info', '/stereo/left/camera_info'),
],
condition=IfCondition(LaunchConfiguration('rectify')),
),
# Run orb_slam2_localizer, a shim that publishes tf map->odom
Node(
package='orca_localize',
executable='orb_slam2_localizer',
output='screen',
name='orb_slam2_localizer',
parameters=[slam_params_file],
remappings=[
('/camera_pose', '/orb_slam2_stereo_node/pose'),
]),
])
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 generate_launch_description():
cmd = [[
'gzclient',
_boolean_command('version'),
' ',
_boolean_command('verbose'),
' ',
_boolean_command('help'),
' ',
LaunchConfiguration('extra_gazebo_args'),
]]
model, plugin, media = GazeboRosPaths.get_paths()
if 'GAZEBO_MODEL_PATH' in environ:
model += pathsep + environ['GAZEBO_MODEL_PATH']
if 'GAZEBO_PLUGIN_PATH' in environ:
plugin += pathsep + environ['GAZEBO_PLUGIN_PATH']
if 'GAZEBO_RESOURCE_PATH' in environ:
media += pathsep + environ['GAZEBO_RESOURCE_PATH']
env = {
'GAZEBO_MODEL_PATH': model,
'GAZEBO_PLUGIN_PATH': plugin,
'GAZEBO_RESOURCE_PATH': media,
}
prefix = PythonExpression([
'"gdb -ex run --args" if "true" == "',
LaunchConfiguration('gdb'),
'"else "valgrind" if "true" == "',
LaunchConfiguration('valgrind'),
'"else ""',
])
return LaunchDescription([
DeclareLaunchArgument(
'version',
default_value='false',
description='Set "true" to output version information'),
DeclareLaunchArgument(
'verbose',
default_value='false',
description='Set "true" to increase messages written to terminal'),
DeclareLaunchArgument(
'help',
default_value='false',
description='Set "true" to produce gzclient help message'),
DeclareLaunchArgument(
'extra_gazebo_args',
default_value='',
description='Extra arguments to be passed to Gazebo'),
# Specific to gazebo_ros
DeclareLaunchArgument(
'gdb',
default_value='false',
description='Set "true" to run gzserver with gdb'),
DeclareLaunchArgument(
'valgrind',
default_value='false',
description='Set "true" to run gzserver with valgrind'),
DeclareLaunchArgument(
'gui_required',
default_value='false',
description=
'Set "true" to shut down launch script when GUI is terminated'),
# Execute node with on_exit=Shutdown if gui_required is specified.
# See ros-simulation/gazebo_ros_pkgs#1086. Simplification of logic
# would be possible pending ros2/launch#290.
ExecuteProcess(
cmd=cmd,
output='screen',
additional_env=env,
shell=True,
prefix=prefix,
on_exit=Shutdown(),
condition=IfCondition(LaunchConfiguration('gui_required')),
),
# Execute node with default on_exit if the node is not required
ExecuteProcess(
cmd=cmd,
output='screen',
additional_env=env,
shell=True,
prefix=prefix,
condition=UnlessCondition(LaunchConfiguration('gui_required')),
),
])