83 lines
2.5 KiB
Python
83 lines
2.5 KiB
Python
"""
|
|
Launching interface node with connecting skills
|
|
ROS 2 launch program for Robossembler
|
|
```bash
|
|
ros2 launch rbs_bt_executor bt_path:=</path/to/skills_json> mode:=benchmark
|
|
|
|
@shalenikol release 0.1
|
|
@shalenikol release 0.2 mode
|
|
@shalenikol release 0.3 {"bt_path": bt_path} # included as default path
|
|
@shalenikol release 0.4 bt_path : either name of skills config file or path to default skills config file
|
|
"""
|
|
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(f: str) -> list:
|
|
if not os.path.isfile(f):
|
|
return []
|
|
|
|
with open(f, "r") as fh:
|
|
data = json.load(fh)
|
|
|
|
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_cfg_file = bt_path if bt_path.endswith(".json") else os.path.join(bt_path, FILE_SKILLS)
|
|
|
|
mode = LaunchConfiguration("mode")
|
|
mode = mode.perform(context)
|
|
|
|
skills = get_skill_list(skills_cfg_file)
|
|
|
|
rbs_interface = Node(
|
|
package="rbs_bt_executor",
|
|
executable="rbs_interface.py",
|
|
parameters = [{"bt_path": skills_cfg_file},{"mode": mode},{"use_sim_time": True}]
|
|
)
|
|
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"
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"mode",
|
|
default_value="",
|
|
description="Run mode of the interface node"
|
|
)
|
|
)
|
|
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
|