def generate_launch_description():
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("diffbot_description"), "urdf",
"diffbot.urdf.xacro"
]),
])
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution([
FindPackageShare("ros2_control_demo_bringup"),
"config",
"diffbot_controllers.yaml",
])
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("diffbot_description"), "config", "diffbot.rviz"])
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
remappings=[
("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster", "--controller-manager",
"/controller_manager"
],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["diffbot_base_controller", "-c", "/controller_manager"],
)
# Delay rviz start after `joint_state_broadcaster`
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
))
# Delay start of robot_controller after `joint_state_broadcaster`
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
))
nodes = [
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
]
return LaunchDescription(nodes)
def generate_launch_description():
# Setting arguments, currently fake robot works without using arguments so default is always used
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"start_rviz",
default_value="true",
description="start RViz automatically with the launch file",
))
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
))
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="true",
description=
"Start robot with fake hardware mirroring command to its states.",
))
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="true",
description=
"Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
))
declared_arguments.append(
DeclareLaunchArgument("slowdown",
default_value="3.0",
description="Slowdown factor of the RRbot."))
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("inmoov_description"),
"robots",
"inmoov.urdf.xacro",
]),
" prefix:=",
LaunchConfiguration("prefix"),
" use_fake_hardware:=",
LaunchConfiguration("use_fake_hardware"),
" fake_sensor_commands:=",
LaunchConfiguration("fake_sensor_commands"),
" slowdown:=",
LaunchConfiguration("slowdown"),
])
robot_description = {"robot_description": robot_description_content}
rrbot_controllers = PathJoinSubstitution([
FindPackageShare("robot"),
"controllers",
"head.yaml",
])
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description],
)
controller_manager_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, rrbot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
spawn_jsb_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster"],
output="screen",
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("inmoov_description"), "config", "inmoov.rviz"])
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
condition=IfCondition(LaunchConfiguration("start_rviz")),
)
# Not currently used/needed, but could be useful later when needing delayed start
delayed_rviz_node = RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_jsb_controller,
on_exit=[rviz_node],
))
head_fake_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["head_controller", "-c", "/controller_manager"],
)
eyes_fake_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["eyes_controller", "-c", "/controller_manager"],
)
jaw_fake_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["jaw_controller", "-c", "/controller_manager"],
)
nodes = [
controller_manager_node, node_robot_state_publisher,
spawn_jsb_controller, rviz_node, head_fake_controller_spawner,
eyes_fake_controller_spawner, jaw_fake_controller_spawner
]
return LaunchDescription(declared_arguments + nodes)
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="rrbot_description",
description="Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
# Initialize Arguments
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
" ",
"prefix:=",
prefix,
]
)
robot_description = {"robot_description": robot_description_content}
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "rrbot.rviz"]
)
joint_state_publisher_node = Node(
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
return LaunchDescription(
declared_arguments.append(
[
joint_state_publisher_node,
robot_state_publisher_node,
rviz_node,
]
)
)
def generate_launch_description():
declared_arguments = []
# UR specific arguments
declared_arguments.append(
DeclareLaunchArgument("ur_type", description="Type/series of used UR robot.")
)
# TODO(anyone): enable this when added into ROS2-foxy
# choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e']))
declared_arguments.append(
DeclareLaunchArgument(
"robot_ip", description="IP address by which the robot can be reached."
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
default_value="true",
description="Enables the safety limits controller if true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_pos_margin",
default_value="0.15",
description="The margin to lower and upper limits in the safety controller.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_k_position",
default_value="20",
description="k-position factor in the safety controller.",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"runtime_config_package",
default_value="ur_bringup",
description='Package with the controller\'s configuration in "config" folder. \
Usually the argument is not set, it enables use of a custom setup.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="ur_controllers.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ur_description",
description="Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="ur.urdf.xacro",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description="Start robot with fake hardware mirroring command to its states.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="false",
description="Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)
# 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")
robot_controller = LaunchConfiguration("robot_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
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,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[FindPackageShare(runtime_config_package), "config", controllers_file]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "view_robot.rviz"]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
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.py",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
io_and_status_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["io_and_status_controller", "-c", "/controller_manager"],
)
speed_scaling_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=[
"speed_scaling_state_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
force_torque_sensor_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=[
"force_torque_sensor_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=[robot_controller, "-c", "/controller_manager"],
)
nodes_to_start = [
control_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,
robot_controller_spawner,
]
return LaunchDescription(declared_arguments + nodes_to_start)
def generate_launch_description():
declared_arguments = []
# UR specific arguments
declared_arguments.append(
DeclareLaunchArgument("ur_type",
default_value="ur3",
description="Type/series of used UR robot."))
declared_arguments.append(
DeclareLaunchArgument(
"robot_ip",
default_value="192.168.178.71",
description="IP address by which the robot can be reached."))
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
default_value="true",
description="Enables the safety limits controller if true.",
))
declared_arguments.append(
DeclareLaunchArgument(
"safety_pos_margin",
default_value="0.15",
description=
"The margin to lower and upper limits in the safety controller.",
))
declared_arguments.append(
DeclareLaunchArgument(
"safety_k_position",
default_value="20",
description="k-position factor in the safety controller.",
))
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"runtime_config_package",
default_value="ur_bringup",
description=
'Package with the controller\'s configuration in "config" folder. \
Usually the argument is not set, it enables use of a custom setup.',
))
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ur_description",
description=
"Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
))
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="suii_description.urdf.xacro",
description="URDF/XACRO description file with the robot.",
))
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='ur3/',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
))
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description=
"Start robot with fake hardware mirroring command to its states.",
))
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="false",
description=
"Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
))
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
))
declared_arguments.append(
DeclareLaunchArgument("launch_rviz",
default_value="true",
description="Launch RViz?"))
# Initialize Arguments
ur_type = "ur3"
robot_ip = LaunchConfiguration("robot_ip")
safety_limits = "true"
safety_pos_margin = "0.15"
safety_k_position = "20"
# General arguments
description_file = "suii_description.urdf.xacro"
prefix = "ur3/"
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = "false"
joint_limit_params = PathJoinSubstitution(
[FindPackageShare("suii_manipulation"), "config", "joint_limits.yaml"])
kinematics_params = PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "config",
"default_kinematics.yaml"
])
physical_params = PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "config",
"physical_parameters.yaml"
])
visual_params = PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "config",
"visual_parameters.yaml"
])
script_filename = PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "resources",
"ros_control.urscript"
])
input_recipe_filename = PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "resources",
"rtde_input_recipe.txt"
])
output_recipe_filename = PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "resources",
"rtde_output_recipe.txt"
])
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]), " ",
PathJoinSubstitution([
FindPackageShare("suii_manipulation"), "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, " "
])
robot_description = {"robot_description": robot_description_content}
pkg_share = FindPackageShare(
package='suii_manipulation').find('suii_manipulation')
rviz_config_path = os.path.join(pkg_share, 'rviz/display.rviz')
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[robot_description])
joint_state_publisher_node = launch_ros.actions.Node(
package='suii_manipulation',
executable='joint_state_publisher',
name='joint_state_publisher')
rviz_node = launch_ros.actions.Node(package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_path])
nodes_to_start = [
robot_state_publisher_node, joint_state_publisher_node, rviz_node
]
return LaunchDescription(declared_arguments + nodes_to_start)
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"start_rviz",
default_value="false",
description="start RViz automatically with the launch file",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description="Start robot with fake hardware mirroring command to its states.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="false",
description="Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"slowdown", default_value="3.0", description="Slowdown factor of the RRbot."
)
)
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("rrbot_description"),
"urdf",
"rrbot_system_position_only.urdf.xacro",
]
),
" prefix:=",
LaunchConfiguration("prefix"),
" use_fake_hardware:=",
LaunchConfiguration("use_fake_hardware"),
" fake_sensor_commands:=",
LaunchConfiguration("fake_sensor_commands"),
" slowdown:=",
LaunchConfiguration("slowdown"),
]
)
robot_description = {"robot_description": robot_description_content}
rrbot_controllers = PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_bringup"),
"config",
"rrbot_controllers.yaml",
]
)
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description],
)
controller_manager_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, rrbot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
spawn_jsb_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster"],
output="screen",
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rrbot_description"), "config", "rrbot.rviz"]
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
condition=IfCondition(LaunchConfiguration("start_rviz")),
)
nodes = [
controller_manager_node,
node_robot_state_publisher,
spawn_jsb_controller,
rviz_node,
]
return LaunchDescription(declared_arguments + nodes)
def generate_launch_description():
"""
"""
pkg_share = launch_ros.substitutions.FindPackageShare(
package='tareeqav').find('tareeqav')
arg_joy_dev = launch.actions.DeclareLaunchArgument(
'joy_dev', default_value='/dev/input/js0')
arg_config_filepath = launch.actions.DeclareLaunchArgument(
'config_filepath',
default_value=[os.path.join(pkg_share, 'config/ps4.config.yaml')])
# TO-D0: fix hard-coded string
sim_world = "/ros2/models/gazebo_models_worlds_collection/worlds/small_city.world"
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("tareeqav"), "description",
"tareeqav_simulation.urdf.xacro"
]),
])
robot_description = {"robot_description": robot_description_content}
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
delay_spawn_entity_for_gazebo_launch = ExecuteProcess(cmd=['sleep', '10'],
output='screen')
start_gazebo = ExecuteProcess(
cmd=[
'gazebo',
'--verbose',
'-s',
'libgazebo_ros_init.so',
'-s',
'libgazebo_ros_factory.so',
# '-s', 'libgazebo_ros_force_system.so',
sim_world
],
output='screen')
# start_gazebo = IncludeLaunchDescription(
# PythonLaunchDescriptionSource([os.path.join(
# get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
# )
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'tareeqav'],
output='screen')
load_joint_state_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_state_broadcaster'
],
output='screen')
load_joint_trajectory_controller = ExecuteProcess(cmd=[
'ros2', 'control', 'load_controller', '--set-state', 'start',
'tareeqav_base_controller'
],
output='screen')
# joystick teleop nodes and config
joy_dev = launch.substitutions.LaunchConfiguration('joy_dev')
config_filepath = launch.substitutions.LaunchConfiguration(
'config_filepath')
joy_node = launch_ros.actions.Node(
package='joy',
executable='joy_node',
name='joy_node',
parameters=[{
'dev': joy_dev,
'deadzone': 0.3,
'autorepeat_rate': 20.0,
}],
remappings=[
("/cmd_vel", "/tareeqav_base_controller/cmd_vel_unstamped"),
],
)
teleop_node = launch_ros.actions.Node(
package='teleop_twist_joy',
executable='teleop_node',
name='teleop_twist_joy_node',
parameters=[config_filepath],
remappings=[
("/cmd_vel", "/tareeqav_base_controller/cmd_vel_unstamped"),
],
)
return LaunchDescription([
arg_joy_dev, arg_config_filepath,
RegisterEventHandler(event_handler=OnProcessExit(
target_action=delay_spawn_entity_for_gazebo_launch,
on_exit=[spawn_entity],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)),
RegisterEventHandler(event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller],
)), start_gazebo, node_robot_state_publisher,
delay_spawn_entity_for_gazebo_launch, joy_node, teleop_node
# spawn_entity,
])
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
prefix = LaunchConfiguration("prefix")
use_sim_time = LaunchConfiguration("use_sim_time")
launch_rviz = LaunchConfiguration("launch_rviz")
launch_servo = LaunchConfiguration("launch_servo")
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"
])
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]),
" ",
"robot_ip:=xxx.yyy.zzz.www",
" ",
"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",
" ",
"ur_type:=",
ur_type,
" ",
"script_filename:=ros_control.urscript",
" ",
"input_recipe_filename:=rtde_input_recipe.txt",
" ",
"output_recipe_filename:=rtde_output_recipe.txt",
" ",
"prefix:=",
prefix,
" ",
])
robot_description = {"robot_description": robot_description_content}
# MoveIt Configuration
robot_description_semantic_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare(moveit_config_package), "srdf", moveit_config_file
]),
" ",
"name:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"prefix:=",
prefix,
" ",
])
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"])
# robot_description_planning = {
# "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
# }
# Planning Configuration
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("ur_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
# the scaled_joint_trajectory_controller does not work on fake hardware
if use_fake_hardware:
controllers_yaml["scaled_joint_trajectory_controller"][
"default"] = False
controllers_yaml["joint_trajectory_controller"]["default"] = True
moveit_controllers = {
"moveit_simple_controller_manager":
controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": False,
"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,
}
warehouse_ros_config = {
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
"warehouse_host": warehouse_sqlite_path,
}
# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
# robot_description_planning,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{
"use_sim_time": use_sim_time
},
warehouse_ros_config,
],
)
# rviz with moveit configuration
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"])
rviz_node = Node(
package="rviz2",
condition=IfCondition(launch_rviz),
executable="rviz2",
name="rviz2_moveit",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
robot_description_kinematics,
# robot_description_planning,
warehouse_ros_config,
],
)
# Servo node for realtime control
servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml")
servo_params = {"moveit_servo": servo_yaml}
servo_node = Node(
package="moveit_servo",
condition=IfCondition(launch_servo),
executable="servo_node_main",
parameters=[
servo_params,
robot_description,
robot_description_semantic,
],
output="screen",
)
nodes_to_start = [move_group_node, rviz_node, servo_node]
return nodes_to_start
def generate_launch_description():
arg_show_rviz = DeclareLaunchArgument(
"start_rviz",
default_value="false",
description="start RViz automatically with the launch file",
)
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare("diffbot_description"), "urdf", "diffbot_system.urdf.xacro"]
),
]
)
robot_description = {"robot_description": robot_description_content}
diffbot_diff_drive_controller = PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_bringup"),
"config",
"diffbot_diff_drive_controller.yaml",
]
)
node_robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[robot_description],
)
controller_manager_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, diffbot_diff_drive_controller],
output={
"stdout": "screen",
"stderr": "screen",
},
)
spawn_dd_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["diffbot_base_controller"],
output="screen",
)
spawn_jsb_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster"],
output="screen",
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("diffbot_description"), "config", "diffbot.rviz"]
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
condition=IfCondition(LaunchConfiguration("start_rviz")),
)
return LaunchDescription(
[
arg_show_rviz,
node_robot_state_publisher,
controller_manager_node,
spawn_dd_controller,
spawn_jsb_controller,
rviz_node,
]
)
def generate_launch_description():
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("rrbot_description"),
"urdf",
"rrbot.urdf.xacro",
]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("ros2_control_demo_bringup"),
"config",
"rrbot_controllers.yaml",
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rrbot_description"), "config", "rrbot.rviz"]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["forward_position_controller", "-c", "/controller_manager"],
)
nodes = [
control_node,
robot_state_pub_node,
rviz_node,
joint_state_broadcaster_spawner,
robot_controller_spawner,
]
return LaunchDescription(nodes)
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
description_package = LaunchConfiguration("description_package")
description_filepath = LaunchConfiguration("description_filepath")
name = LaunchConfiguration("name")
prefix = LaunchConfiguration("prefix")
gripper = LaunchConfiguration("gripper")
collision_arm = LaunchConfiguration("collision_arm")
collision_gripper = LaunchConfiguration("collision_gripper")
safety_limits = LaunchConfiguration("safety_limits")
safety_position_margin = LaunchConfiguration("safety_position_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
safety_k_velocity = LaunchConfiguration("safety_k_velocity")
ros2_control = LaunchConfiguration("ros2_control")
ros2_control_plugin = LaunchConfiguration("ros2_control_plugin")
ros2_control_command_interface = LaunchConfiguration(
"ros2_control_command_interface")
gazebo_preserve_fixed_joint = LaunchConfiguration(
"gazebo_preserve_fixed_joint")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
log_level = LaunchConfiguration("log_level")
# Extract URDF from description file
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), description_filepath]),
" ",
"name:=",
name,
" ",
"prefix:=",
prefix,
" ",
"gripper:=",
gripper,
" ",
"collision_arm:=",
collision_arm,
" ",
"collision_gripper:=",
collision_gripper,
" ",
"safety_limits:=",
safety_limits,
" ",
"safety_position_margin:=",
safety_position_margin,
" ",
"safety_k_position:=",
safety_k_position,
" ",
"safety_k_velocity:=",
safety_k_velocity,
" ",
"ros2_control:=",
ros2_control,
" ",
"ros2_control_plugin:=",
ros2_control_plugin,
" ",
"ros2_control_command_interface:=",
ros2_control_command_interface,
" ",
"gazebo_preserve_fixed_joint:=",
gazebo_preserve_fixed_joint,
])
robot_description = {"robot_description": robot_description_content}
# List of nodes to be launched
nodes = [
# robot_state_publisher
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[robot_description, {
"use_sim_time": use_sim_time
}],
),
# rviz2
Node(
package="rviz2",
executable="rviz2",
output="log",
arguments=[
"--display-config",
rviz_config,
"--ros-args",
"--log-level",
log_level,
],
parameters=[{
"use_sim_time": use_sim_time
}],
),
# joint_state_publisher_gui
Node(
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{
"use_sim_time": use_sim_time
}],
),
]
return LaunchDescription(declared_arguments + nodes)
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bringup_package",
default_value="ar3_bringup",
description=
'Package with the controller\'s configuration in "config" folder. \
Usually the argument is not set, it enables use of a custom setup.',
))
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="ar3_controllers.yaml",
description="YAML file with the controllers configuration.",
))
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ar3_description",
description=
"Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.",
))
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="ar3.urdf.xacro",
description="URDF/XACRO description file with the robot.",
))
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
))
declared_arguments.append(
DeclareLaunchArgument(
"use_sim",
default_value="false",
description="Start robot in Gazebo simulation.",
))
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description=
"Start robot with fake hardware mirroring command to its states.",
))
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="false",
description=
"Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
))
declared_arguments.append(
DeclareLaunchArgument("slowdown",
default_value="3.0",
description="Slowdown factor of the AR3."))
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
#default_value="forward_position_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
))
declared_arguments.append(
DeclareLaunchArgument(
"start_rviz",
default_value="true",
description="Start RViz2 automatically with this launch file.",
))
declared_arguments.append(
DeclareLaunchArgument(
"serial_device",
default_value="/dev/ttyACM0",
description="Serial device name.",
))
declared_arguments.append(
DeclareLaunchArgument(
"serial_baudrate",
default_value="115200",
description="Serial device baudrate.",
))
declared_arguments.append(
DeclareLaunchArgument(
"firmware_version",
default_value="0.0.1",
description="Serial firmware version.",
))
# Initialize Arguments
bringup_package = LaunchConfiguration("bringup_package")
controllers_file = LaunchConfiguration("controllers_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
use_sim = LaunchConfiguration("use_sim")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
slowdown = LaunchConfiguration("slowdown")
robot_controller = LaunchConfiguration("robot_controller")
start_rviz = LaunchConfiguration("start_rviz")
serial_device = LaunchConfiguration("serial_device")
serial_baudrate = LaunchConfiguration("serial_baudrate")
firmware_version = LaunchConfiguration("firmware_version")
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]),
" ",
"prefix:=",
prefix,
" ",
"use_sim:=",
use_sim,
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"fake_sensor_commands:=",
fake_sensor_commands,
" ",
"slowdown:=",
slowdown,
" ",
"serial_device:=",
serial_device,
" ",
"serial_baudrate:=",
serial_baudrate,
" ",
"firmware_version:=",
firmware_version,
" ",
])
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution([
FindPackageShare(bringup_package),
"config",
controllers_file,
])
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "config", "ar3.rviz"])
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
condition=IfCondition(start_rviz),
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster", "--controller-manager",
"/controller_manager"
],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[robot_controller, "-c", "/controller_manager"],
)
# Delay rviz start after `joint_state_broadcaster`
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
))
# Delay start of robot_controller after `joint_state_broadcaster`
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
))
nodes = [
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
]
return LaunchDescription(declared_arguments + nodes)
def generate_launch_description():
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
description_package = LaunchConfiguration("description_package")
description_filepath = LaunchConfiguration("description_filepath")
moveit_config_package = "panda_moveit_config"
name = LaunchConfiguration("name")
prefix = LaunchConfiguration("prefix")
gripper = LaunchConfiguration("gripper")
collision_arm = LaunchConfiguration("collision_arm")
collision_gripper = LaunchConfiguration("collision_gripper")
safety_limits = LaunchConfiguration("safety_limits")
safety_position_margin = LaunchConfiguration("safety_position_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
safety_k_velocity = LaunchConfiguration("safety_k_velocity")
ros2_control = LaunchConfiguration("ros2_control")
ros2_control_plugin = LaunchConfiguration("ros2_control_plugin")
ros2_control_command_interface = LaunchConfiguration(
"ros2_control_command_interface")
gazebo_preserve_fixed_joint = LaunchConfiguration(
"gazebo_preserve_fixed_joint")
enable_servo = LaunchConfiguration("enable_servo")
enable_rviz = LaunchConfiguration("enable_rviz")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
log_level = LaunchConfiguration("log_level")
# URDF
_robot_description_xml = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), description_filepath]),
" ",
"name:=",
name,
" ",
"prefix:=",
prefix,
" ",
"gripper:=",
gripper,
" ",
"collision_arm:=",
collision_arm,
" ",
"collision_gripper:=",
collision_gripper,
" ",
"safety_limits:=",
safety_limits,
" ",
"safety_position_margin:=",
safety_position_margin,
" ",
"safety_k_position:=",
safety_k_position,
" ",
"safety_k_velocity:=",
safety_k_velocity,
" ",
"ros2_control:=",
ros2_control,
" ",
"ros2_control_plugin:=",
ros2_control_plugin,
" ",
"ros2_control_command_interface:=",
ros2_control_command_interface,
" ",
"gazebo_preserve_fixed_joint:=",
gazebo_preserve_fixed_joint,
])
robot_description = {"robot_description": _robot_description_xml}
# SRDF
_robot_description_semantic_xml = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare(moveit_config_package),
"srdf",
"panda.srdf.xacro",
]),
" ",
"name:=",
name,
" ",
"prefix:=",
prefix,
])
robot_description_semantic = {
"robot_description_semantic": _robot_description_semantic_xml
}
# Kinematics
kinematics = load_yaml(moveit_config_package,
path.join("config", "kinematics.yaml"))
# Joint limits
joint_limits = {
"robot_description_planning":
load_yaml(moveit_config_package,
path.join("config", "joint_limits.yaml"))
}
# Servo
servo_params = {
"moveit_servo":
load_yaml(moveit_config_package, path.join("config", "servo.yaml"))
}
servo_params["moveit_servo"].update({"use_gazebo": use_sim_time})
# Planning pipeline
planning_pipeline = {
"planning_pipelines": ["ompl"],
"default_planning_pipeline": "ompl",
"ompl": {
"planning_plugin": "ompl_interface/OMPLPlanner",
# TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
"request_adapters":
"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",
# TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
"start_state_max_bounds_error": 0.31416,
},
}
_ompl_yaml = load_yaml(moveit_config_package,
path.join("config", "ompl_planning.yaml"))
planning_pipeline["ompl"].update(_ompl_yaml)
# Planning scene
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
# MoveIt controller manager
moveit_controller_manager_yaml = load_yaml(
moveit_config_package,
path.join("config", "moveit_controller_manager.yaml"))
moveit_controller_manager = {
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
"moveit_simple_controller_manager": moveit_controller_manager_yaml,
}
# Trajectory execution
trajectory_execution = {
"allow_trajectory_execution": True,
"moveit_manage_controllers": False,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
# Controller parameters
declared_arguments.append(
DeclareLaunchArgument(
"__controller_parameters_basename",
default_value=[
"controllers_", ros2_control_command_interface, ".yaml"
],
))
controller_parameters = PathJoinSubstitution([
FindPackageShare(moveit_config_package),
"config",
LaunchConfiguration("__controller_parameters_basename"),
])
# List of nodes to be launched
nodes = [
# robot_state_publisher
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
{
"publish_frequency": 50.0,
"frame_prefix": "",
"use_sim_time": use_sim_time,
},
],
),
# ros2_control_node (only for fake controller)
Node(
package="controller_manager",
executable="ros2_control_node",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
controller_parameters,
{
"use_sim_time": use_sim_time
},
],
condition=(IfCondition(
PythonExpression([
"'",
ros2_control_plugin,
"'",
" == ",
"'fake'",
]))),
),
# move_group (with execution)
Node(
package="moveit_ros_move_group",
executable="move_group",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
robot_description_semantic,
kinematics,
joint_limits,
planning_pipeline,
trajectory_execution,
planning_scene_monitor_parameters,
moveit_controller_manager,
{
"use_sim_time": use_sim_time
},
],
),
# move_servo
Node(
package="moveit_servo",
executable="servo_node_main",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
robot_description_semantic,
kinematics,
joint_limits,
planning_pipeline,
trajectory_execution,
planning_scene_monitor_parameters,
servo_params,
{
"use_sim_time": use_sim_time
},
],
condition=IfCondition(enable_servo),
),
# rviz2
Node(
package="rviz2",
executable="rviz2",
output="log",
arguments=[
"--display-config",
rviz_config,
"--ros-args",
"--log-level",
log_level,
],
parameters=[
robot_description,
robot_description_semantic,
kinematics,
planning_pipeline,
joint_limits,
{
"use_sim_time": use_sim_time
},
],
condition=IfCondition(enable_rviz),
),
]
# Add nodes for loading controllers
for controller in moveit_controller_manager_yaml["controller_names"] + [
"joint_state_broadcaster"
]:
nodes.append(
# controller_manager_spawner
Node(
package="controller_manager",
executable="spawner",
output="log",
arguments=[controller, "--ros-args", "--log-level", log_level],
parameters=[{
"use_sim_time": use_sim_time
}],
), )
return LaunchDescription(declared_arguments + nodes)
def generate_launch_description():
"""
"""
pkg_share = launch_ros.substitutions.FindPackageShare(
package='tareeqav').find('tareeqav')
arg_joy_dev = launch.actions.DeclareLaunchArgument(
'joy_dev', default_value='/dev/input/js0')
arg_config_filepath = launch.actions.DeclareLaunchArgument(
'config_filepath',
default_value=[os.path.join(pkg_share, 'config/ps4.config.yaml')])
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare("tareeqav"), "description",
"tareeqav_system.urdf.xacro"
]),
])
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution([
FindPackageShare("tareeqav"),
"config",
"diff_drive_controller.yaml",
])
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("tareeqav"), "config", "tareeqav.rviz"])
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
# remappings=[
# ("/tareeqav_base_controller/cmd_vel_unstamped", "/cmd_vel"),
# ],
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster", "--controller-manager",
"/controller_manager"
],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["tareeqav_base_controller", "-c", "/controller_manager"],
)
# Delay rviz start after `joint_state_broadcaster`
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[rviz_node],
))
# Delay start of robot_controller after `joint_state_broadcaster`
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
))
# joystick teleop nodes and config
joy_dev = launch.substitutions.LaunchConfiguration('joy_dev')
config_filepath = launch.substitutions.LaunchConfiguration(
'config_filepath')
joy_node = launch_ros.actions.Node(
package='joy',
executable='joy_node',
name='joy_node',
parameters=[{
'dev': joy_dev,
'deadzone': 0.3,
'autorepeat_rate': 20.0,
}],
)
teleop_node = launch_ros.actions.Node(
package='teleop_twist_joy',
executable='teleop_node',
name='teleop_twist_joy_node',
parameters=[config_filepath],
)
nodes = [
arg_joy_dev, arg_config_filepath, control_node, robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_rviz_after_joint_state_broadcaster_spawner,
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
joy_node, teleop_node
]
return LaunchDescription(nodes)
def generate_launch_description():
declared_launch_args = []
declared_launch_args.append(
DeclareLaunchArgument(
'controller_config',
default_value=TextSubstitution(text='acker_diff_controller.yaml'),
description='The controller configuration you want to use.'))
declared_launch_args.append(
DeclareLaunchArgument(
'descritption_file',
default_value=TextSubstitution(text='pilsbot.urdf.xacro'),
description='URDF/XACRO description file with the robot.'))
declared_launch_args.append(
DeclareLaunchArgument(
'description_package',
default_value=TextSubstitution(text='pilsbot_description'),
description=
' Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.'))
declared_launch_args.append(
DeclareLaunchArgument('world',
default_value=TextSubstitution(text='')))
declared_launch_args.append(
DeclareLaunchArgument('verbose',
default_value=TextSubstitution(text='false'),
description='Set Gazebo output to verbose.'))
# intialise args
controller_config = LaunchConfiguration('controller_config')
description_file = LaunchConfiguration('descritption_file')
description_package = LaunchConfiguration('description_package')
world = LaunchConfiguration('world')
verbose = LaunchConfiguration('verbose')
use_sim_time = LaunchConfiguration('use_sim_time', default=True)
gazebo = IncludeLaunchDescription(PythonLaunchDescriptionSource(
[FindPackageShare("gazebo_ros"), '/launch/gazebo.launch.py']),
launch_arguments={
'world': world,
'verbose': verbose
}.items())
# Get URDF via xacro
robot_description_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare(description_package),
"urdf",
description_file,
]),
" ",
"controller_config:=",
controller_config,
" ",
"use_fake_hardware:=true",
])
params = {
'robot_description': robot_description_content,
'use_sim_time': use_sim_time
}
robot_description = Node(package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[params])
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', '/robot_description', '-entity', 'pilsbot'],
output='screen')
spawn_dd_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["pilsbot_velocity_controller"],
output="screen",
)
spawn_jsb_controller = Node(
package="controller_manager",
executable="spawner.py",
arguments=["pilsbot_joint_publisher"],
output="screen",
)
nodes = [
gazebo,
robot_description,
spawn_entity,
spawn_dd_controller,
spawn_jsb_controller,
]
return LaunchDescription(declared_launch_args + nodes)
def generate_launch_description() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
description_package = LaunchConfiguration("description_package")
description_filepath = LaunchConfiguration("description_filepath")
world = LaunchConfiguration("world")
model = LaunchConfiguration("model")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# URDF
_robot_description_xml = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), description_filepath]),
" ",
"name:=",
model,
])
robot_description = {"robot_description": _robot_description_xml}
# List of included launch descriptions
launch_descriptions = [
# Launch Ignition Gazebo
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("ros_ign_gazebo"),
"launch",
"ign_gazebo.launch.py",
])),
launch_arguments=[("ign_args", [world, " -v ", ign_verbosity])],
),
]
# List of nodes to be launched
nodes = [
# robot_state_publisher
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
{
"publish_frequency": 50.0,
"frame_prefix": "",
"use_sim_time": use_sim_time,
},
],
),
# ros_ign_gazebo_create
Node(
package="ros_ign_gazebo",
executable="create",
output="log",
arguments=["-file", model, "--ros-args", "--log-level", log_level],
parameters=[{
"use_sim_time": use_sim_time
}],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"runtime_config_package",
default_value="rrbot_bringup",
description='Package with the controller\'s configuration in "config" folder. \
Usually the argument is not set, it enables use of a custom setup.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rrbot_controllers.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="rrbot_description",
description="Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="rrbot_robot.urdf.xacro",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description="Start robot with fake hardware mirroring command to its states.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="false",
description="Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"slowdown", default_value="3.0", description="Slowdown factor of the RRbot."
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_controller",
default_value="forward_position_controller",
description="Robot controller to start.",
)
)
# Initialize 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")
robot_controller = LaunchConfiguration("robot_controller")
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "urdf", description_file]
),
" ",
"prefix:=",
prefix,
" ",
"use_fake_hardware:=",
use_fake_hardware,
" ",
"fake_sensor_commands:=",
fake_sensor_commands,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare(runtime_config_package),
"config",
controllers_file,
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "rrbot.rviz"]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers],
output={
"stdout": "screen",
"stderr": "screen",
},
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner.py",
arguments=[robot_controller, "-c", "/controller_manager"],
)
nodes_to_run = [
control_node,
robot_state_pub_node,
rviz_node,
joint_state_broadcaster_spawner,
robot_controller_spawner,
]
return LaunchDescription(declared_arguments + nodes_to_run)
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() -> LaunchDescription:
# Declare all launch arguments
declared_arguments = generate_declared_arguments()
# Get substitution for all arguments
description_package = LaunchConfiguration("description_package")
description_filepath = LaunchConfiguration("description_filepath")
moveit_config_package = "panda_moveit_config"
robot_type = LaunchConfiguration("robot_type")
rviz_config = LaunchConfiguration("rviz_config")
use_sim_time = LaunchConfiguration("use_sim_time")
ign_verbosity = LaunchConfiguration("ign_verbosity")
log_level = LaunchConfiguration("log_level")
# URDF
_robot_description_xml = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), description_filepath]),
" ",
"name:=",
robot_type,
])
robot_description = {"robot_description": _robot_description_xml}
# SRDF
_robot_description_semantic_xml = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare(moveit_config_package),
"srdf",
"panda.srdf.xacro",
]),
" ",
"name:=",
robot_type,
])
robot_description_semantic = {
"robot_description_semantic": _robot_description_semantic_xml
}
# List of included launch descriptions
launch_descriptions = [
# Launch world with robot (configured for this example)
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("ign_moveit2_examples"),
"launch",
"default.launch.py",
])),
launch_arguments=[
("world_type", "follow_target"),
("robot_type", robot_type),
("rviz_config", rviz_config),
("use_sim_time", use_sim_time),
("ign_verbosity", ign_verbosity),
("log_level", log_level),
],
),
]
# List of nodes to be launched
nodes = [
# Run the example node (C++)
Node(
package="ign_moveit2_examples",
executable="ex_follow_target",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
robot_description_semantic,
{
"use_sim_time": use_sim_time
},
],
),
]
return LaunchDescription(declared_arguments + launch_descriptions + nodes)
def generate_launch_description():
declared_arguments = []
# UR specific arguments
declared_arguments.append(
DeclareLaunchArgument("ur_type",
description="Type/series of used UR robot."))
# TODO(anyone): enable this when added into ROS2-foxy
# choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e']))
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
default_value="true",
description="Enables the safety limits controller if true.",
))
declared_arguments.append(
DeclareLaunchArgument(
"safety_pos_margin",
default_value="0.15",
description=
"The margin to lower and upper limits in the safety controller.",
))
declared_arguments.append(
DeclareLaunchArgument(
"safety_k_position",
default_value="20",
description="k-position factor in the safety controller.",
))
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ur_description",
description=
"Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
))
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="ur.urdf.xacro",
description="URDF/XACRO description file with the robot.",
))
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
))
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
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]),
" ",
"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,
])
robot_description = {"robot_description": robot_description_content}
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "view_robot.rviz"])
joint_state_publisher_node = Node(
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
)
nodes_to_start = [
joint_state_publisher_node,
robot_state_publisher_node,
rviz_node,
]
return LaunchDescription(declared_arguments + nodes_to_start)
def generate_launch_description():
declared_arguments = []
# UR specific arguments
declared_arguments.append(
DeclareLaunchArgument("ur_type",
description="Type/series of used UR robot."))
# TODO(anyone): enable this when added into ROS2-foxy
# choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e']))
declared_arguments.append(
DeclareLaunchArgument(
"robot_ip",
description="IP address by which the robot can be reached."))
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
default_value="true",
description="Enables the safety limits controller if true.",
))
declared_arguments.append(
DeclareLaunchArgument(
"safety_pos_margin",
default_value="0.15",
description=
"The margin to lower and upper limits in the safety controller.",
))
declared_arguments.append(
DeclareLaunchArgument(
"safety_k_position",
default_value="20",
description="k-position factor in the safety controller.",
))
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ur_description",
description=
"Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
))
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="ur.urdf.xacro",
description="URDF/XACRO description file with the robot.",
))
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="ur_moveit_config",
description=
"MoveIt config package with robot SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
))
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="ur.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
))
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
))
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description=
"Start robot with fake hardware mirroring command to its states.",
))
declared_arguments.append(
DeclareLaunchArgument(
"fake_sensor_commands",
default_value="false",
description=
"Enable fake command interfaces for sensors used for simple simulations. \
Used only if 'use_fake_hardware' parameter is true.",
))
declared_arguments.append(
DeclareLaunchArgument("launch_rviz",
default_value="true",
description="Launch RViz?"))
# 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
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
prefix = LaunchConfiguration("prefix")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
launch_rviz = LaunchConfiguration("launch_rviz")
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:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"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,
" ",
])
robot_description = {"robot_description": robot_description_content}
# MoveIt Configuration
robot_description_semantic_content = Command([
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([
FindPackageShare(moveit_config_package), "srdf", moveit_config_file
]),
" ",
"name:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"prefix:=",
prefix,
" ",
])
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
kinematics_yaml = load_yaml("ur_moveit_config", "config/kinematics.yaml")
robot_description_kinematics = {
"robot_description_kinematics": kinematics_yaml
}
# Planning Configuration
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("ur_moveit_config",
"config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager":
controllers_yaml,
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": False,
"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,
"planning_scene_monitor_options": {
"name": "planning_scene_monitor",
"robot_description": "robot_description",
"joint_state_topic": "/joint_states",
"attached_collision_object_topic":
"/move_group/planning_scene_monitor",
"publish_planning_scene_topic":
"/move_group/publish_planning_scene",
"monitored_planning_scene_topic":
"/move_group/monitored_planning_scene",
"wait_for_initial_state_timeout": 10.0,
},
}
# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
],
)
# 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",
)
# rviz with moveit configuration
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"])
rviz_node = Node(
package="rviz2",
condition=IfCondition(launch_rviz),
executable="rviz2",
name="rviz2_moveit",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
robot_description_kinematics,
],
)
# 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", "base_link"
],
)
nodes_to_start = [
move_group_node,
mongodb_server_node,
rviz_node,
static_tf,
]
return LaunchDescription(declared_arguments + nodes_to_start)
