81 lines
2.5 KiB
Python
81 lines
2.5 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')
|
||
|
|
namespace = LaunchConfiguration('namespace')
|
||
|
|
|
||
|
|
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',
|
||
|
|
robot_description,
|
||
|
|
robot_description_semantic,
|
||
|
|
kinematics_yaml,
|
||
|
|
{
|
||
|
|
'action_name': 'assemble',
|
||
|
|
# 'publisher_port': 1666,
|
||
|
|
# 'server_port': 1667,
|
||
|
|
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
|
||
|
|
}
|
||
|
|
])
|
||
|
|
"""
|
||
|
|
ld = LaunchDescription()
|
||
|
|
ld.add_action(declare_namespace_cmd)
|
||
|
|
|
||
|
|
# Declare the launch options
|
||
|
|
ld.add_action(plansys2_cmd)
|
||
|
|
return ld
|