108 lines
No EOL
3.3 KiB
Python
108 lines
No EOL
3.3 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, SetEnvironmentVariable
|
|
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():
|
|
|
|
# planning_context
|
|
robot_description_config = load_file(
|
|
"rasms_description", "urdf/rasms_description.urdf.xacro"
|
|
)
|
|
robot_description = {"robot_description": robot_description_config}
|
|
|
|
robot_description_semantic_config = load_file(
|
|
"rasms_moveit_config", "srdf/rasms_description.srdf"
|
|
)
|
|
robot_description_semantic = {
|
|
"robot_description_semantic": robot_description_semantic_config
|
|
}
|
|
|
|
kinematics_yaml = load_yaml(
|
|
"rasms_moveit_config", "config/kinematics.yml"
|
|
)
|
|
|
|
pkg_dir = get_package_share_directory('rasms_manipulator')
|
|
namespace = LaunchConfiguration('namespace')
|
|
|
|
declare_namespace_cmd = DeclareLaunchArgument(
|
|
'namespace',
|
|
default_value='',
|
|
description='Namespace')
|
|
|
|
# stdout_linebuf_envvar = SetEnvironmentVariable(
|
|
# 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
|
|
|
|
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 + '/pddl/domain.pddl',
|
|
'namespace': namespace
|
|
}.items())
|
|
|
|
move_1 = Node(
|
|
package='plansys2_bt_actions',
|
|
executable='bt_action_node',
|
|
name='move',
|
|
namespace=namespace,
|
|
output='screen',
|
|
parameters=[
|
|
pkg_dir + '/config/params.yaml',
|
|
robot_description,
|
|
robot_description_semantic,
|
|
kinematics_yaml,
|
|
{
|
|
'action_name': 'move',
|
|
'bt_xml_file': pkg_dir + '/behavior_trees_xml/move.xml'
|
|
}
|
|
])
|
|
transport_1_cmd = Node(
|
|
package='rasms_manipulator',
|
|
executable='rasms_action_node',
|
|
name='transport_1',
|
|
namespace=namespace,
|
|
output='screen',
|
|
parameters=[]) # Create the launch description and populate
|
|
|
|
ld = LaunchDescription()
|
|
|
|
# ld.add_action(stdout_linebuf_envvar)
|
|
ld.add_action(declare_namespace_cmd)
|
|
|
|
# Declare the launch options
|
|
ld.add_action(plansys2_cmd)
|
|
ld.add_action(move_1)
|
|
# ld.add_action(transport_1_cmd)
|
|
|
|
return ld |