70 lines
1.9 KiB
Python
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)])
|