def launch_setup(context, *args, **kwargs):
use_sim_time_cfg = LaunchConfiguration('use_sim_time')
urdf_path_cfg = LaunchConfiguration('urdf_path')
urdf_path = urdf_path_cfg.perform(context)
print('\033[92m' + "robot_state_publisher: Use urdf dir: " + urdf_path +
'\033[0m')
with open(urdf_path, 'r') as infp:
robot_desc = infp.read()
robot_state_publisher_node = Node(package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{
'use_sim_time':
use_sim_time_cfg,
'robot_description':
robot_desc
}])
return [
robot_state_publisher_node,
]
def launch_setup(context, *args, **kwargs):
# Launch Arguments
use_sim_time = LaunchConfiguration('use_sim_time', default=False)
config_rviz2 = LaunchConfiguration(
'config_rviz2',
default=os.path.join(
get_package_share_directory('ur5_rg2_moveit2_config'), 'launch',
'rviz.rviz'))
log_level = LaunchConfiguration('log_level', default='error')
# URDF
robot_urdf_config = load_file('ur5_rg2_ign', 'urdf/ur5_rg2.urdf')
robot_description = {'robot_description': robot_urdf_config}
# SRDF
robot_srdf = load_file('ur5_rg2_moveit2_config', 'srdf/ur5_rg2.srdf')
robot_description_semantic = {'robot_description_semantic': robot_srdf}
# Kinematics
kinematics = load_yaml('ur5_rg2_moveit2_config', 'config/kinematics.yaml')
# Joint limits
joint_limits_yaml = load_yaml('ur5_rg2_moveit2_config',
'config/joint_limits.yaml')
joint_limits = {'robot_description_planning': joint_limits_yaml}
# Planning
ompl_yaml = load_yaml('ur5_rg2_moveit2_config',
'config/ompl_planning.yaml')
planning = {
'move_group': {
'planning_plugin': 'ompl_interface/OMPLPlanner',
'request_adapters':
'default_planner_request_adapters/AddRuckigTrajectorySmoothing default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames 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
}
}
# Trajectory Execution
trajectory_execution = {
'allow_trajectory_execution': False,
'moveit_manage_controllers': False
}
# Planning Scene
planning_scene_monitor_parameters = {
'publish_planning_scene': True,
'publish_geometry_updates': True,
'publish_state_updates': True,
'publish_transforms_updates': True
}
launch_description = [
# Launch Arguments
DeclareLaunchArgument('use_sim_time',
default_value=use_sim_time,
description="If true, use simulated clock"),
DeclareLaunchArgument(
'config_rviz2',
default_value=config_rviz2,
description=
"Path to config for RViz2. If empty, RViz2 will be disabled"),
DeclareLaunchArgument(
'log_level',
default_value=log_level,
description="Log level of all nodes launched by this script"),
# Start move_group action server
Node(package='moveit_ros_move_group',
executable='move_group',
name='move_group',
output='screen',
arguments=['--ros-args', '--log-level', log_level],
parameters=[
robot_description, robot_description_semantic, kinematics,
joint_limits, planning, ompl_yaml, trajectory_execution,
planning_scene_monitor_parameters, {
'use_sim_time': use_sim_time
}
]),
]
# Add RViz2 (if enabled)
if config_rviz2.perform(context):
launch_description.append(
Node(package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=[
'--display-config', config_rviz2, '--ros-args',
'--log-level', log_level
],
parameters=[
robot_description, robot_description_semantic, kinematics,
planning, {
'use_sim_time': use_sim_time
}
]))
return launch_description
def launch_setup(context, *args, **kwargs):
# fmt: off
architecture_type = LaunchConfiguration("architecture_type",
default="awf/universe")
autoware_launch_file = LaunchConfiguration(
"autoware_launch_file",
default=default_autoware_launch_file_of(
architecture_type.perform(context)))
autoware_launch_package = LaunchConfiguration(
"autoware_launch_package",
default=default_autoware_launch_package_of(
architecture_type.perform(context)))
global_frame_rate = LaunchConfiguration("global_frame_rate", default=30.0)
global_real_time_factor = LaunchConfiguration("global_real_time_factor",
default=1.0)
global_timeout = LaunchConfiguration("global_timeout", default=180)
initialize_duration = LaunchConfiguration("initialize_duration",
default=30)
launch_autoware = LaunchConfiguration("launch_autoware", default=True)
launch_rviz = LaunchConfiguration("launch_rviz", default=False)
output_directory = LaunchConfiguration("output_directory",
default=Path("/tmp"))
port = LaunchConfiguration("port", default=8080)
record = LaunchConfiguration("record", default=True)
scenario = LaunchConfiguration("scenario", default=Path("/dev/null"))
sensor_model = LaunchConfiguration("sensor_model", default="")
sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8)
vehicle_model = LaunchConfiguration("vehicle_model", default="")
workflow = LaunchConfiguration("workflow", default=Path("/dev/null"))
# fmt: on
print(f"architecture_type := {architecture_type.perform(context)}")
print(
f"autoware_launch_file := {autoware_launch_file.perform(context)}")
print(
f"autoware_launch_package := {autoware_launch_package.perform(context)}"
)
print(f"global_frame_rate := {global_frame_rate.perform(context)}")
print(
f"global_real_time_factor := {global_real_time_factor.perform(context)}"
)
print(f"global_timeout := {global_timeout.perform(context)}")
print(f"initialize_duration := {initialize_duration.perform(context)}")
print(f"launch_autoware := {launch_autoware.perform(context)}")
print(f"launch_rviz := {launch_rviz.perform(context)}")
print(f"output_directory := {output_directory.perform(context)}")
print(f"port := {port.perform(context)}")
print(f"record := {record.perform(context)}")
print(f"scenario := {scenario.perform(context)}")
print(f"sensor_model := {sensor_model.perform(context)}")
print(f"sigterm_timeout := {sigterm_timeout.perform(context)}")
print(f"vehicle_model := {vehicle_model.perform(context)}")
print(f"workflow := {workflow.perform(context)}")
def make_parameters():
parameters = [
{
"architecture_type": architecture_type
},
{
"autoware_launch_file": autoware_launch_file
},
{
"autoware_launch_package": autoware_launch_package
},
{
"initialize_duration": initialize_duration
},
{
"launch_autoware": launch_autoware
},
{
"port": port
},
{
"record": record
},
{
"sensor_model": sensor_model
},
{
"vehicle_model": vehicle_model
},
]
def description():
return get_package_share_directory(
vehicle_model.perform(context) + "_description")
if vehicle_model.perform(context):
parameters.append(description() +
"/config/vehicle_info.param.yaml")
parameters.append(description() +
"/config/simulator_model.param.yaml")
return parameters
return [
# fmt: off
DeclareLaunchArgument("architecture_type",
default_value=architecture_type),
DeclareLaunchArgument("autoware_launch_file",
default_value=autoware_launch_file),
DeclareLaunchArgument("autoware_launch_package",
default_value=autoware_launch_package),
DeclareLaunchArgument("global_frame_rate",
default_value=global_frame_rate),
DeclareLaunchArgument("global_real_time_factor",
default_value=global_real_time_factor),
DeclareLaunchArgument("global_timeout", default_value=global_timeout),
DeclareLaunchArgument("launch_autoware",
default_value=launch_autoware),
DeclareLaunchArgument("launch_rviz", default_value=launch_rviz),
DeclareLaunchArgument("output_directory",
default_value=output_directory),
DeclareLaunchArgument("scenario", default_value=scenario),
DeclareLaunchArgument("sensor_model", default_value=sensor_model),
DeclareLaunchArgument("sigterm_timeout",
default_value=sigterm_timeout),
DeclareLaunchArgument("vehicle_model", default_value=vehicle_model),
DeclareLaunchArgument("workflow", default_value=workflow),
# fmt: on
Node(
package="scenario_test_runner",
executable="scenario_test_runner",
namespace="simulation",
name="scenario_test_runner",
output="screen",
on_exit=Shutdown(),
arguments=[
# fmt: off
"--global-frame-rate",
global_frame_rate,
"--global-real-time-factor",
global_real_time_factor,
"--global-timeout",
global_timeout,
"--output-directory",
output_directory,
"--scenario",
scenario,
"--workflow",
workflow,
# fmt: on
],
),
Node(
package="simple_sensor_simulator",
executable="simple_sensor_simulator_node",
namespace="simulation",
name="simple_sensor_simulator",
output="screen",
parameters=[{
"port": port
}],
),
LifecycleNode(
package="openscenario_interpreter",
executable="openscenario_interpreter_node",
namespace="simulation",
name="openscenario_interpreter",
output="screen",
parameters=make_parameters(),
# on_exit=Shutdown(),
),
Node(
package="openscenario_visualization",
executable="openscenario_visualization_node",
namespace="simulation",
name="openscenario_visualizer",
output="screen",
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output={
"stderr": "log",
"stdout": "log"
},
condition=IfCondition(launch_rviz),
arguments=[
"-d",
str(
# Path(get_package_share_directory("scenario_test_runner"))
Path(get_package_share_directory("traffic_simulator")) /
"config/scenario_simulator_v2.rviz"),
],
),
]
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
