89 lines
2.9 KiB
Python
89 lines
2.9 KiB
Python
from launch import LaunchDescription
|
|
from launch_ros.actions import Node
|
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
|
from launch_ros.substitutions import FindPackageShare
|
|
from ament_index_python.packages import get_package_share_directory
|
|
import os
|
|
import yaml
|
|
import math
|
|
|
|
|
|
def construct_angle_radians(loader, node):
|
|
"""Utility function to construct radian values from yaml."""
|
|
value = loader.construct_scalar(node)
|
|
try:
|
|
return float(value)
|
|
except SyntaxError:
|
|
raise Exception("invalid expression: %s" % value)
|
|
|
|
def construct_angle_degrees(loader, node):
|
|
"""Utility function for converting degrees into radians from yaml."""
|
|
return math.radians(construct_angle_radians(loader, node))
|
|
|
|
def load_yaml(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
|
|
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
|
|
except Exception:
|
|
raise Exception("yaml support not available; install python-yaml")
|
|
|
|
try:
|
|
with open(absolute_file_path) as file:
|
|
return yaml.safe_load(file)
|
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
bt_config = os.path.join(
|
|
get_package_share_directory('rbs_bt_executor'),
|
|
'config',
|
|
'params.yaml'
|
|
)
|
|
points = load_yaml(
|
|
package_name="rbs_bt_executor",
|
|
file_path="config/points.yaml"
|
|
)
|
|
|
|
gripperPoints = load_yaml(
|
|
package_name="rbs_bt_executor",
|
|
file_path="config/gripperPositions.yaml")
|
|
|
|
robot_description_semantic_content = Command(
|
|
[
|
|
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
|
" ",
|
|
PathJoinSubstitution(
|
|
[FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
|
|
),
|
|
" ",
|
|
"name:=",
|
|
"ur",
|
|
" ",
|
|
"prefix:=",
|
|
"",
|
|
" ",
|
|
]
|
|
)
|
|
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
|
|
|
|
|
|
|
|
return LaunchDescription([
|
|
Node(
|
|
package='behavior_tree',
|
|
namespace='',
|
|
executable='bt_engine',
|
|
#prefix=['xterm -e gdb -ex run --args'],
|
|
parameters=[
|
|
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
|
|
{'plugins':['rbs_skill_move_topose_bt_action_client', "rbs_skill_get_pick_place_pose_service_client", "rbs_skill_gripper_move_bt_action_client", "rbs_skill_move_joint_state", "rbs_add_planning_scene_object"]},
|
|
gripperPoints,
|
|
robot_description_semantic
|
|
]
|
|
)
|
|
])
|