66 lines
2.3 KiB
Python
66 lines
2.3 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
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
|
|
|
|
def generate_launch_description():
|
|
|
|
declared_arguments = []
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"bt_file",
|
|
default_value="test_tree_env_manager.xml",
|
|
description="BehaviorTree file for run.",
|
|
)
|
|
)
|
|
bt_file = LaunchConfiguration("bt_file")
|
|
|
|
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}
|
|
|
|
btfile_description = PathJoinSubstitution(
|
|
[FindPackageShare("rbs_bt_executor"), "bt_trees", bt_file]
|
|
)
|
|
btfile_param = {"bt_file_path": btfile_description}
|
|
|
|
nodes_to_start = [
|
|
Node(
|
|
package='behavior_tree',
|
|
executable='bt_engine',
|
|
#prefix=['xterm -e gdb -ex run --args'],
|
|
parameters=[
|
|
btfile_param,
|
|
#{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/' + bt_file)},
|
|
{'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",
|
|
"rbs_env_manager_starter"
|
|
]},
|
|
# gripperPoints,
|
|
robot_description_semantic
|
|
]
|
|
)
|
|
]
|
|
|
|
return LaunchDescription(declared_arguments + nodes_to_start)
|