def test_this_launch_file_path():
path = ['asd', 'bsd', 'cds']
sub = PathJoinSubstitution(path)
assert sub.perform(None) == os.path.join(*path)
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
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")
launch_servo = LaunchConfiguration("launch_servo")
headless_mode = LaunchConfiguration("headless_mode")
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,
" ",
"headless_mode:=",
headless_mode,
" ",
]
)
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}
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")
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,
}
# 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,
],
)
# 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,
robot_description_planning,
],
)
# 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"],
)
# 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={
"stdout": "screen",
"stderr": "screen",
},
)
nodes_to_start = [move_group_node, mongodb_server_node, rviz_node, static_tf, servo_node]
return nodes_to_start