runtime/rbs_bt_executor/launch/interface.launch.py

70 lines
1.9 KiB
Python

"""
Launching interface node with connecting skills
ROS 2 launch program for Robossembler
@shalenikol release 0.1
"""
import os
import json
from launch_ros.actions import Node
from launch.actions import (DeclareLaunchArgument, OpaqueFunction)
from launch.substitutions import LaunchConfiguration
from launch import LaunchDescription
FILE_SKILLS = "skills.json"
PARAM_SUFFIX = "_cfg"
def get_skill_list_(path: str) -> list:
f = os.path.join(path, FILE_SKILLS)
if not os.path.isfile(f):
return []
with open(f, "r") as fh:
data = json.load(fh)
# str_data = fh.read()
# data = json.loads(str_data)
nn_skills = 0
excluding = {}
skills = []
for skill in data["skills"]:
node_name = skill["Module"]["node_name"]
launch = skill["Launch"]
p = launch["package"]
e = launch["executable"]
excluding[p+e] = None # unique key
if len(excluding) == nn_skills:
continue
nn_skills += 1
# skills.append(Node(package=p, executable=e))
launch["parameters"] = [{node_name+PARAM_SUFFIX: json.dumps(skill)}]
skills.append(Node(**launch))
return skills
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
bt_path = LaunchConfiguration("bt_path")
bt_path = bt_path.perform(context)
skills = get_skill_list_(bt_path)
rbs_interface = Node(
package="rbs_bt_executor",
executable="rbs_interface.py",
# parameters = [{"bt_path": bt_path}] # can be included as default path
)
nodes_to_start = [rbs_interface]
return nodes_to_start + skills
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bt_path",
default_value="''",
description="path to Behavior tree instance"
)
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])