robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

update bt_path param

Browse source
This commit is contained in:
shalenikol 2025-05-16 11:15:29 +03:00
parent c4cb3f2141
commit ac265f74a3
4 changed files with 34 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

15
rbs_bt_executor/launch/interface.launch.py
View file

@ -1,10 +1,13 @@
"""
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
@ -17,15 +20,12 @@ 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)
def get_skill_list(f: str) -> list:
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 = {}
@ -49,16 +49,17 @@ 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_(bt_path)
skills = get_skill_list(skills_cfg_file)
rbs_interface = Node(
package="rbs_bt_executor",
executable="rbs_interface.py",
parameters = [{"bt_path": bt_path},{"mode": mode},{"use_sim_time": True}]
# parameters = [{"bt_path": bt_path}] # can be included as default path
parameters = [{"bt_path": skills_cfg_file},{"mode": mode},{"use_sim_time": True}]
)
nodes_to_start = [rbs_interface]
return nodes_to_start + skills

19
rbs_bt_executor/launch/rbs_bt_web.launch.py
View file

@ -1,25 +1,36 @@
"""
rbs_bt_web
ROS 2 launch program for Robossembler
```bash
ros2 launch rbs_bt_executor rbs_bt_web.launch.py bt_path:=</path/to/bt.xml>
@shalenikol release 0.1
@shalenikol release 0.2 BT v.4
@shalenikol release 0.3 bt_path = os.path.dirname(bt_path)
@shalenikol release 0.4 bt_path : either name of BT-file or path to default BT-file
"""
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
from launch.substitutions import LaunchConfiguration
from launch.event_handlers import OnExecutionComplete
FILE_BT = "bt.xml"
FILE_SKILLS = "skills.json"
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
bt_path = LaunchConfiguration("bt_path")
bt_path = bt_path.perform(context)
if bt_path[-4:] == ".xml":
import os
bt_path = os.path.dirname(bt_path)
bt_file = bt_path
skills_file = os.path.join(os.path.dirname(bt_path), FILE_SKILLS)
else:
bt_file = os.path.join(bt_path, FILE_BT)
skills_file = os.path.join(bt_path, FILE_SKILLS)
# rbs_bt = Node(
# package = "rbs_bt_executor",
@ -35,14 +46,14 @@ def launch_setup(context, *args, **kwargs):
bt_exec = Node(
package="rbs_bt_executor",
executable="bt_exec",
arguments=[bt_path]
arguments=[bt_file]
# prefix=['gdbserver localhost:3000'],
)
bt_param = Node(
package="rbs_bt_executor",
executable="bt_param.py",
parameters=[{"bt_path": bt_path}]
parameters=[{"bt_path": skills_file}]
)
return [
RegisterEventHandler(

12
rbs_bt_executor/scripts/rbs_interface.py
View file

@ -8,6 +8,7 @@
@shalenikol release 0.3 synchronize
@shalenikol release 0.4 mode:="benchmark"
@shalenikol release 0.5 bt_path # included as default path (in __init__)
@shalenikol release 0.6
"""
import os
import json
@ -40,12 +41,14 @@ SERVER_NAME = "rbs_interface_a"
FILE_SKILLS = "skills.json"
PARAM_SUFFIX = "_cfg"
KEY_BTPARAM = "BTAction"
KEY_SIDPATH = "@path@" # prefix for filepath
class rbsInterface(Node):
def __init__(self, node_name):
"""Construct the node."""
# self.bt_path = "" # path to the current BehaviorTree
# self._mode = "" # run mode of the interface node
self.file_skills = FILE_SKILLS
self.cfg_data = None # config for current action
super().__init__(node_name)
self.declare_parameter(BT_PARAM, rclpy.Parameter.Type.STRING)
@ -62,7 +65,7 @@ class rbsInterface(Node):
# self.client = AsyncParameterClient(self.client_node, 'test_parameter_client_target') # only Iron
self.add_on_set_parameters_callback(self._on_set_btpath_param)
def get_transfer_path(self):
def get_skillcfg_file(self):
if self.bt_path:
return self.bt_path
return os.path.join(get_package_share_directory("rbs_bt_executor"), "config")
@ -119,9 +122,12 @@ class rbsInterface(Node):
assert False, f"Error: sid not valid '{sid}'"
def _load_config(self, sid: str):
p = os.path.join(self.get_transfer_path(), FILE_SKILLS) # action+".json")
if sid.startswith(KEY_SIDPATH):
p = sid[len(KEY_SIDPATH):]
else:
p = self.get_skillcfg_file() #os.path.join(self.get_transfer_path(), self.file_skills)
# load config
return self._deserialize(p,sid)
return self._deserialize(p, sid)
def run_action(self, command_data: dict) -> bool:
p_list = command_data["param"]

4
rbs_bt_executor/src/BTExec.cpp
View file

@ -14,7 +14,7 @@
#define NODE_NAME "rbs_interface"
#define SERVICE_NAME "rbs_interface_s"
#define SERVER_NAME "rbs_interface_a"
#define FILE_BT "bt.xml"
// #define FILE_BT "bt.xml"
template<typename T> std::string to_string(const T &t)
{
@ -161,7 +161,7 @@ private:
int main(int argc, char** argv)
{
// filename Behavior Tree
std::string bt = std::string(argv[1]) + "/" FILE_BT;
std::string bt = std::string(argv[1]); // + "/" FILE_BT
std::ifstream fh(bt, std::ios::in);
// reading xml
std::string xml{std::istreambuf_iterator<char>(fh), std::istreambuf_iterator<char>()};