2023-02-03 07:04:12 +00:00
|
|
|
from launch import LaunchDescription
|
|
|
|
from launch_ros.actions import Node
|
2023-03-31 20:24:56 +00:00
|
|
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
|
|
|
from launch_ros.substitutions import FindPackageShare
|
2023-02-03 07:04:12 +00:00
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
import os
|
2023-03-31 20:24:56 +00:00
|
|
|
import yaml
|
|
|
|
import math
|
2023-11-30 13:47:33 +03:00
|
|
|
from rbs_launch_utils.launch_common import load_yaml
|
2023-02-03 07:04:12 +00:00
|
|
|
|
|
|
|
|
2023-11-16 13:15:27 +03:00
|
|
|
def generate_launch_description():
|
2023-11-14 13:04:11 +03:00
|
|
|
|
2023-11-30 13:47:33 +03:00
|
|
|
# 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"
|
|
|
|
# )
|
2023-11-16 13:15:27 +03:00
|
|
|
|
|
|
|
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}
|
|
|
|
|
2023-03-31 20:24:56 +00:00
|
|
|
|
2023-11-14 13:04:11 +03:00
|
|
|
|
2023-02-03 07:04:12 +00:00
|
|
|
return LaunchDescription([
|
|
|
|
Node(
|
|
|
|
package='behavior_tree',
|
|
|
|
executable='bt_engine',
|
2023-11-16 13:15:27 +03:00
|
|
|
#prefix=['xterm -e gdb -ex run --args'],
|
2023-02-03 07:04:12 +00:00
|
|
|
parameters=[
|
2023-12-14 12:00:35 +03:00
|
|
|
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_gripper.xml')},
|
2023-11-16 13:15:27 +03:00
|
|
|
{'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",
|
|
|
|
"rbs_pose_estimation"
|
|
|
|
]},
|
2023-11-30 13:47:33 +03:00
|
|
|
# gripperPoints,
|
2023-11-16 13:15:27 +03:00
|
|
|
robot_description_semantic
|
2023-02-03 07:04:12 +00:00
|
|
|
]
|
2023-03-31 20:24:56 +00:00
|
|
|
)
|
2023-02-03 07:04:12 +00:00
|
|
|
])
|