90 lines
2.8 KiB
Python
90 lines
2.8 KiB
Python
import os
|
|
import yaml
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def load_file(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
with open(absolute_file_path, "r") as file:
|
|
return file.read()
|
|
except EnvironmentError:
|
|
# parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
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:
|
|
with open(absolute_file_path, "r") as file:
|
|
return yaml.safe_load(file)
|
|
except EnvironmentError:
|
|
# parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
pkg_dir = get_package_share_directory('rbs_task_planner')
|
|
bt_exec_dir = get_package_share_directory('rbs_bt_executor')
|
|
namespace = LaunchConfiguration('namespace')
|
|
assemble_dir = os.path.join(
|
|
get_package_share_directory("rbs_task_planner"), "example", "sdf_models"
|
|
)
|
|
|
|
declare_namespace_cmd = DeclareLaunchArgument(
|
|
name="namespace",
|
|
default_value='',
|
|
description='Namespace')
|
|
|
|
plansys2_cmd = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(os.path.join(
|
|
get_package_share_directory('plansys2_bringup'),
|
|
'launch',
|
|
'plansys2_bringup_launch_monolithic.py')),
|
|
launch_arguments={
|
|
'model_file': pkg_dir + '/domain/atomic_domain.pddl',
|
|
'namespace': namespace
|
|
}.items())
|
|
assemble = Node(
|
|
package='plansys2_bt_actions',
|
|
executable='bt_action_node',
|
|
name='assemble',
|
|
namespace=namespace,
|
|
output='screen',
|
|
parameters=[
|
|
pkg_dir + '/config/params.yaml',
|
|
{
|
|
'action_name': 'assemble',
|
|
'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml',
|
|
}
|
|
])
|
|
|
|
assemble_state = Node(
|
|
package="rbs_skill_servers",
|
|
executable="assemble_state_service_server",
|
|
output="screen",
|
|
parameters=[
|
|
{'assemble_prefix': 'ASSEMBLE_'},
|
|
{'assemble_dir': assemble_dir}
|
|
]
|
|
)
|
|
ld = LaunchDescription()
|
|
# Args
|
|
ld.add_action(declare_namespace_cmd)
|
|
# Nodes
|
|
ld.add_action(plansys2_cmd)
|
|
ld.add_action(assemble)
|
|
ld.add_action(assemble_state)
|
|
|
|
return ld
|