diff --git a/rbs_bt_executor/bt_trees/bt_pe_stop.xml b/rbs_bt_executor/bt_trees/bt_pe_stop.xml index d493571..ffa643e 100644 --- a/rbs_bt_executor/bt_trees/bt_pe_stop.xml +++ b/rbs_bt_executor/bt_trees/bt_pe_stop.xml @@ -2,12 +2,13 @@ - + + diff --git a/rbs_bt_executor/bt_trees/bt_pe_test.xml b/rbs_bt_executor/bt_trees/bt_pe_test.xml index cbed900..9580ebe 100644 --- a/rbs_bt_executor/bt_trees/bt_pe_test.xml +++ b/rbs_bt_executor/bt_trees/bt_pe_test.xml @@ -2,12 +2,13 @@ - + + diff --git a/rbs_bt_executor/launch/rbs_bt_web.launch.py b/rbs_bt_executor/launch/rbs_bt_web.launch.py new file mode 100644 index 0000000..a6d05ba --- /dev/null +++ b/rbs_bt_executor/launch/rbs_bt_web.launch.py @@ -0,0 +1,59 @@ +""" + rbs_bt_web + ROS 2 launch program for Robossembler + + @shalenikol release 0.1 +""" +import os +# import json + +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" + +def launch_setup(context, *args, **kwargs): + # Initialize Arguments + bt_path = LaunchConfiguration("bt_path") + bt_path = bt_path.perform(context) + + rbs_bt = Node( + package = "behavior_tree", + executable = "bt_engine", + parameters = [ + {"bt_file_path": os.path.join(bt_path, FILE_BT)}, + {"plugins": ["rbs_interface"]} + ] + ) + + bt_param = Node( + package="rbs_interface", + executable="bt_param.py", + parameters = [{"bt_path": bt_path}] + ) + # nodes_to_start = [rbs_bt] + # return nodes_to_start + # return [bt_param] + return [ + RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=bt_param, + on_completion=[rbs_bt], + ) + ), + bt_param + ] # the order of elements is strictly required + +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)]) diff --git a/rbs_bt_executor/src/rbsBTAction.cpp b/rbs_bt_executor/src/rbsBTAction.cpp index ed49505..4ecd000 100644 --- a/rbs_bt_executor/src/rbsBTAction.cpp +++ b/rbs_bt_executor/src/rbsBTAction.cpp @@ -1,9 +1,11 @@ -// #include "behavior_tree/BtAction.hpp" #include "behavior_tree/BtService.hpp" -// #include "rbs_skill_interfaces/action/rbs_bt.hpp" #include "rbs_skill_interfaces/srv/rbs_bt.hpp" -// using RbsBtActionSrv = rbs_skill_interfaces::action::RbsBt; +#define STR_ACTION "do" +#define STR_SID "sid" +#define STR_COMMAND "command" +#define NODE_NAME "rbs_interface" + using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt; class RbsBtAction : public BtService { @@ -11,45 +13,30 @@ public: RbsBtAction(const std::string &name, const BT::NodeConfiguration &config) : BtService(name, config) { - _action_name = getInput("do").value(); + _action_name = getInput(STR_ACTION).value(); RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name); - _set_params_client = std::make_shared(_node, "rbs_interface"); + _set_params_client = std::make_shared(_node, NODE_NAME); while (!_set_params_client->wait_for_service(1s)) { if (!rclcpp::ok()) { RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting."); break; } - RCLCPP_WARN(_node->get_logger(), "'rbs_interface' service not available, waiting again..."); + RCLCPP_WARN(_node->get_logger(), NODE_NAME " service not available, waiting again..."); } - // set_str_param(); } - // RbsBtActionSrv::Goal populate_goal() override { - // auto goal = RbsBtActionSrv::Goal(); - // goal.action = _action_name; - // goal.command = getInput("command").value(); - // RCLCPP_INFO_STREAM(_node->get_logger(), "Goal send " + _action_name); - // return goal; - // } - RbsBtActionSrv::Request::SharedPtr populate_request() override { auto request = std::make_shared(); request->action = _action_name; - request->command = getInput("command").value(); - // RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:populate_request"); + request->sid = getInput(STR_SID).value(); + request->command = getInput(STR_COMMAND).value(); return request; } BT::NodeStatus handle_response(const RbsBtActionSrv::Response::SharedPtr response) override { if (response->ok) { - // TODO We need better perfomance for call it in another place for all BT nodes - // - Make it via shared_ptr forward throught blackboard - // auto t = std::make_shared("asp-example2", _node); - // auto p = t->getWorkspaceInspectorTrajectory(); - // setOutput("workspace", p); - // RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:handle_response"); return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::FAILURE; @@ -57,22 +44,15 @@ public: static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("do"), - BT::InputPort("command") + BT::InputPort(STR_SID), + BT::InputPort(STR_ACTION), + BT::InputPort(STR_COMMAND) }); } private: std::string _action_name{}; std::shared_ptr _set_params_client; - - // void set_str_param() { - // RCLCPP_INFO_STREAM(_node->get_logger(),"Set string parameter: <" + _action_name + ">"); - - // std::vector _parameters{rclcpp::Parameter("action_name", _action_name)}; - // _set_params_client->set_parameters(_parameters); - // } -// auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception"); }; #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/rbs_interface/CMakeLists.txt b/rbs_interface/CMakeLists.txt new file mode 100644 index 0000000..2b62a1f --- /dev/null +++ b/rbs_interface/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(rbs_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(rclcpp REQUIRED) +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rbs_skill_interfaces REQUIRED) +find_package(PCL 1.12 REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/rbs_interface.py + scripts/bt_param.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/rbs_interface/config/interface.json b/rbs_interface/config/interface.json new file mode 100644 index 0000000..c3184e6 --- /dev/null +++ b/rbs_interface/config/interface.json @@ -0,0 +1,17 @@ +{ + "SkillPackage": { + "name": "Robossembler", "version": "1.0", "format": "1" + }, + "Module": { + "name": "Interface", "description": "Interface node for the Robossembler skills" + }, + "Launch": { + "package": "rbs_interface", + "executable": "rbs_interface.py", + "param": ["bt_path"] + }, + "ROS2": { + "node_name": "rbs_interface", "comment": "!!! no change !!!" + }, + "Settings": [] +} diff --git a/rbs_interface/launch/interface.launch.py b/rbs_interface/launch/interface.launch.py new file mode 100644 index 0000000..8a2fb44 --- /dev/null +++ b/rbs_interface/launch/interface.launch.py @@ -0,0 +1,69 @@ +""" + 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"]: + 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"] = [{launch["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_interface", + 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)]) diff --git a/rbs_interface/package.xml b/rbs_interface/package.xml new file mode 100644 index 0000000..3591e93 --- /dev/null +++ b/rbs_interface/package.xml @@ -0,0 +1,27 @@ + + + + rbs_interface + 0.0.1 + The interface node + shalenikol + Apache License 2.0 + + ament_cmake + ament_cmake_python + + rclpy + rclcpp + image_transport + cv_bridge + sensor_msgs + std_msgs + rbs_skill_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rbs_interface/rbs_interface/__init__.py b/rbs_interface/rbs_interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rbs_interface/scripts/bt_param.py b/rbs_interface/scripts/bt_param.py new file mode 100755 index 0000000..68b0af8 --- /dev/null +++ b/rbs_interface/scripts/bt_param.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +""" + btParam_node only for passing the path to the interface node + ROS 2 program for Robossembler + + @shalenikol release 0.1 +""" +# import os + +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.parameter import Parameter +from rcl_interfaces.srv import SetParameters + +NODE_NAME = "bt_param" +NODE_INTERFACE = "rbs_interface" +PARAM_BT = "bt_path" + +class btParam(Node): + def __init__(self, node_name): + """Construct the node.""" + super().__init__(node_name) + self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING) + self.cb_group = ReentrantCallbackGroup() + + timer_period = 0.1 # seconds + self._timer = self.create_timer(timer_period, self.timer_callback, self.cb_group) + + def timer_callback(self): + bt_path = self.get_parameter(PARAM_BT).get_parameter_value().string_value + + if not self.set_remote_parameter(NODE_INTERFACE, PARAM_BT, bt_path): + # self.get_logger().info(f"'{NODE_NAME}' set param '{bt_path}'") + # else: + self.get_logger().info("Error 'set_remote_parameter'") + self.destroy_timer(self._timer) + + def set_remote_parameter(self, remote_node_name: str, parameter_name: str, new_parameter_value) -> bool: + self.cli = self.create_client(SetParameters, remote_node_name + "/set_parameters") + while not self.cli.wait_for_service(timeout_sec=1.1): + self.get_logger().info("'" + remote_node_name + "' service not available, waiting again...") + req = SetParameters.Request() + req.parameters = [Parameter(parameter_name, value=new_parameter_value).to_parameter_msg()] + future = self.cli.call_async(req) + + self.executor.spin_until_future_complete(future) + res = future.result() + + return res.results[0].successful + +def main(): + rclpy.init() + executor = rclpy.executors.SingleThreadedExecutor() + # executor = rclpy.executors.MultiThreadedExecutor() + i_node = btParam(NODE_NAME) + executor.add_node(i_node) + try: + executor.spin_once() + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + i_node.destroy_node() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rbs_interface/scripts/rbs_interface.py b/rbs_interface/scripts/rbs_interface.py new file mode 100755 index 0000000..38b29d4 --- /dev/null +++ b/rbs_interface/scripts/rbs_interface.py @@ -0,0 +1,195 @@ +#!/usr/bin/env python3 +""" + rbsInterface_node + ROS 2 program for Robossembler + + @shalenikol release 0.1 +""" +import os +import json +# import yaml + +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import ReentrantCallbackGroup +from ament_index_python.packages import get_package_share_directory + +from rclpy.parameter import Parameter +from rcl_interfaces.srv import SetParameters #, GetParameters +from rcl_interfaces.msg import SetParametersResult #, ParameterEvent +from lifecycle_msgs.srv import ChangeState #, GetState +from lifecycle_msgs.msg import Transition +from rbs_skill_interfaces.srv import RbsBt + +# from rclpy.parameter_client import AsyncParameterClient # only Iron + +# PARAM_NAME = "str_param" +# PARAM_SKILL_CONFIG = "skill_cfg" +PARAM_BT = "bt_path" +NODE_NAME = "rbs_interface" +FILE_SKILLS = "skills.json" +PARAM_SUFFIX = "_cfg" + +# def get_transfer_path_(): +# return os.path.join(get_package_share_directory("rbs_interface"), "config") + +class rbsInterface(Node): + def __init__(self, node_name): + """Construct the node.""" + self.bt_path = "" # path to the current BehaviorTree + self.cfg_data = None # config for current action + super().__init__(node_name) + self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING) + self.cb_group = ReentrantCallbackGroup() + self._service = self.create_service(RbsBt, node_name, self.service_callback, callback_group=self.cb_group) + # 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): + if self.bt_path: + return self.bt_path + return os.path.join(get_package_share_directory("rbs_interface"), "config") + + def _on_set_btpath_param(self, parameter_list): + for parameter in parameter_list: + if parameter.name == PARAM_BT: + self.bt_path = parameter.value + # self.get_logger().info(f'$ {parameter.name}={parameter.value}') + return SetParametersResult(successful=True) + + + # def get_remote_parameter(self, remote_node_name, param_name): + # cli = self.create_client(GetParameters, remote_node_name + '/get_parameters') + # while not cli.wait_for_service(timeout_sec=1.0): + # self.get_logger().info('service not available, waiting again...') + # req = GetParameters.Request() + # req.names = [param_name] + # future = cli.call_async(req) + + # while rclpy.ok(): + # rclpy.spin_once(self) + # if future.done(): + # try: + # res = future.result() + # return getattr(res.values[0], self.type_arr[res.values[0].type]) + # except Exception as e: + # self.get_logger().warn('Service call failed %r' % (e,)) + # break + + def set_remote_parameter(self, remote_node_name: str, parameter_name: str, new_parameter_value) -> bool: + self.cli = self.create_client(SetParameters, remote_node_name + "/set_parameters") + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info("'" + remote_node_name + "' service not available, waiting again...") + req = SetParameters.Request() + req.parameters = [Parameter(parameter_name, value=new_parameter_value).to_parameter_msg()] + future = self.cli.call_async(req) + + self.executor.spin_until_future_complete(future) + res = future.result() + + return res.results[0].successful + + def _deserialize(self, file_path: str, sid: str): + with open(file_path, "r") as f: + # if file_path.split() == ".yaml": + # s = yaml.load(f, Loader=yaml.FullLoader) + # else: # ".json" + data = json.load(f) + for skill in data["skills"]: + if skill["sid"] == sid: + return skill + assert False, f"Error: sid not valid '{sid}'" + # return {"result": f"Error: sid not valid '{sid}'"} + + def _load_config(self, sid: str): + p = os.path.join(self.get_transfer_path(), FILE_SKILLS) # action+".json") + # load config + return self._deserialize(p,sid) + + def run_action(self, command_data: dict) -> bool: + p_list = command_data["param"] + oper_type = command_data["type"] + node_name = self.cfg_data["Launch"]["name"] #["ROS2"]["node_name"] + par_name = node_name + PARAM_SUFFIX + if len(p_list) > 0: + # ext = command_data["format"] # 'yaml' or 'json' + # param_file = os.path.join(self.get_transfer_path(), command_data["name"]+"."+ext) + # with open(param_file, "r") as f: + # data = f.read() + # if not self.set_remote_parameter(node_name, par_name, data): + # return False + # if not self.set_remote_parameter(node_name, par_name, yaml.dump(self.cfg_data)): + data = json.dumps(self.cfg_data) + # self.get_logger().info(f"{data}") + if not self.set_remote_parameter(node_name, par_name, data): + return False + + ret = False # default return value + if oper_type == "run": + self.cli_changestate = self.create_client(ChangeState, f"/{node_name}/change_state") #, callback_group=self.cb_group) + + while not self.cli_changestate.wait_for_service(timeout_sec=1.0): + self.get_logger().info(f"'{node_name}' not available... wait") + + req = ChangeState.Request() + req.transition.id = Transition.TRANSITION_CONFIGURE + future = self.cli_changestate.call_async(req) + self.executor.spin_until_future_complete(future) + res = future.result() + if res: # is not None: + if res.success: + req = ChangeState.Request() + req.transition.id = Transition.TRANSITION_ACTIVATE + future = self.cli_changestate.call_async(req) + self.executor.spin_until_future_complete(future) + res = future.result() + if res: # is not None: + ret = res.success + + elif oper_type == "stop": + self.cli_changestate = self.create_client(ChangeState, f"{node_name}/change_state") #, callback_group=self.cb_group) + while not self.cli_changestate.wait_for_service(timeout_sec=1.0): + self.get_logger().info(f"'{node_name}' not available... wait") + + req = ChangeState.Request() + req.transition.id = Transition.TRANSITION_DEACTIVATE + future = self.cli_changestate.call_async(req) + self.executor.spin_until_future_complete(future) + res = future.result() + if res: # is not None: + if res.success: + req = ChangeState.Request() + req.transition.id = Transition.TRANSITION_CLEANUP + future = self.cli_changestate.call_async(req) + self.executor.spin_until_future_complete(future) + res = future.result() + if res: # is not None: + ret = res.success + return ret + + def service_callback(self, request, response): + self.get_logger().info(f"Incoming request for Action ({request.action}/{request.command})") + self.cfg_data = self._load_config(request.sid) #, request.command) + self.get_logger().info(f'Config: Ok ({self.cfg_data["Module"]["description"]})') + + is_action = False + for comm in self.cfg_data["BTAction"]: + if comm["name"] == request.command: + is_action = self.run_action(comm) + + response.ok = is_action #True + return response + +def main(): + rclpy.init() + # executor = rclpy.executors.SingleThreadedExecutor() + executor = rclpy.executors.MultiThreadedExecutor() + i_node = rbsInterface(NODE_NAME) + executor.add_node(i_node) + try: + executor.spin() + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + i_node.destroy_node() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rbs_perception/config/PoseEstimation.json b/rbs_perception/config/PoseEstimation.json deleted file mode 100644 index 8c530d9..0000000 --- a/rbs_perception/config/PoseEstimation.json +++ /dev/null @@ -1,47 +0,0 @@ -{ - "SkillPackage": { - "name": "Robossembler", "version": "1.0", "format": "1" - }, - "Module": { - "name": "PoseEstimation", "description": "Pose Estimation skill with DOPE" - }, - "Launch": { - "executable": "pe_dope_lc.py" - }, - "ROS2": { - "node_name": "lc_dope" - }, - "BTAction": [ - { - "name": "peConfigure", - "format": "yaml", - "type": "run", - "param":["object_name","weights_file"], - "result":["Pose"] - }, - { - "name": "peStop", - "format": "yaml", - "type": "stop", - "param":[], - "result":[] - } - ], - "Interface": { - "Input": [ - { - "name": "cameraLink", "type": "CAMERA" - }, - { - "name": "object_name", "type": "MODEL" - } - ], - "Output": - [ - { - "name": "pose_estimation_topic", "type": "Pose" - } - ] - }, - "Settings": [] -} diff --git a/rbs_perception/config/peConfigure.yaml b/rbs_perception/config/peConfigure.yaml deleted file mode 100644 index c776cee..0000000 --- a/rbs_perception/config/peConfigure.yaml +++ /dev/null @@ -1,3 +0,0 @@ -object_name: fork -weights_file: /home/shalenikol/robossembler_ws/fork_e47.pth -dimensions: [0.137, 0.165, 0.202] diff --git a/rbs_perception/scripts/pe_dope_lc.py b/rbs_perception/scripts/pe_dope_lc.py index 7a4f579..1e566eb 100755 --- a/rbs_perception/scripts/pe_dope_lc.py +++ b/rbs_perception/scripts/pe_dope_lc.py @@ -27,32 +27,34 @@ from tf2_ros.buffer import Buffer from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images import cv2 # OpenCV library -FILE_SKILL_CONFIG = "PoseEstimation.json" # используется при запуске ноды и при запуске BT с этим навыком FILE_DOPE_CONFIG = "pe_dope_config.yaml" # FILE_TEMP_IMAGE = "image_rgb.png" -PARAM_NAME = "str_param" -PARAM_SKILL_CONFIG = "skill_cfg" CAMERA_LINK_DEFAULT = "outer_rgbd_camera" -# NODE_NAME_DEFAULT = "lc_dope" +NODE_NAME_DEFAULT = "lc_dope" # the name doesn't matter in this node (defined in Launch) +PARAM_SKILL_CFG = "lc_dope_cfg" +# PARAM_SUFFIX = "_cfg" +# node_name = self.cfg_data["Launch"]["name"] +# par_name = node_name + PARAM_SUFFIX + def get_transfer_path_() -> str: return os.path.join(get_package_share_directory("rbs_perception"), "config") class PE_DOPE(Node): """Pose estimation lifecycle node with DOPE.""" - def __init__(self, node_name="", **kwargs): + def __init__(self, **kwargs): """Construct the node.""" - self.skill_cfg = self._load_config() - ros_cfg = self.skill_cfg["ROS2"] - self.nodeName = ros_cfg["node_name"] #node_name - out_par = self.skill_cfg["Interface"]["Output"][0] - self.topicSrv = self.nodeName + "/" + out_par["name"] # for other nodes kwargs["allow_undeclared_parameters"] = True kwargs["automatically_declare_parameters_from_overrides"] = True - super().__init__(self.nodeName, **kwargs) - self.declare_parameter(PARAM_NAME, rclpy.Parameter.Type.STRING) - self.declare_parameter(PARAM_SKILL_CONFIG, rclpy.Parameter.Type.STRING) + super().__init__(NODE_NAME_DEFAULT, **kwargs) + + str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value + self.skill_cfg = json.loads(str_cfg) + self.nodeName = self.skill_cfg["Launch"]["name"] + out_par = self.skill_cfg["Interface"]["Output"][0] + self.topicSrv = self.nodeName + "/" + out_par["name"] + # Used to convert between ROS and OpenCV images self.br = CvBridge() self.dope_cfg = self._load_config_DOPE() @@ -70,6 +72,12 @@ class PE_DOPE(Node): self._pub = None self._image_cnt = 0 self._K = [] + + def _load_config_DOPE(self): + p = os.path.join(get_transfer_path_(), FILE_DOPE_CONFIG) + with open(p, "r") as f: + y = yaml.load(f, Loader=yaml.FullLoader) + return y def _set_camera_topic(self, camera_link: str): """ Service for camera name topics """ @@ -91,18 +99,6 @@ class PE_DOPE(Node): ] # set the indicator for receiving the camera info self._is_camerainfo = True - - def _load_config(self): - p = os.path.join(get_transfer_path_(), FILE_SKILL_CONFIG) - with open(p, "r") as f: - j = json.load(f) - return j - - def _load_config_DOPE(self): - p = os.path.join(get_transfer_path_(), FILE_DOPE_CONFIG) - with open(p, "r") as f: - y = yaml.load(f, Loader=yaml.FullLoader) - return y def on_configure(self, state: State) -> TransitionCallbackReturn: """ @@ -114,15 +110,14 @@ class PE_DOPE(Node): TransitionCallbackReturn.FAILURE transitions to "unconfigured". TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing" """ - yaml_param = self.get_parameter(PARAM_NAME).get_parameter_value().string_value - str_param = yaml.load(yaml_param, Loader=yaml.FullLoader) - # !!! вместо PARAM_SKILL_CONFIG можно использовать self.skill_cfg !!! (это одно и то же) - # yaml_param = self.get_parameter(PARAM_SKILL_CONFIG).get_parameter_value().string_value - # pe_cfg = yaml.load(yaml_param, Loader=yaml.FullLoader) - - # with open("pe.yaml", "w") as f: - # f.write(yaml_param) - # print(skill_cfg) + json_param = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value + jdata = json.loads(json_param) + dependency = {} + for comm in jdata["BTAction"]: + for par in comm["param"]: + if par["type"] == "weights": + dependency = par["dependency"] + assert dependency, "no dependency" # Create the subscribers. self._sub_info = self.create_subscription(CameraInfo, self.topicCameraInfo, self.listener_camera_info, 2) @@ -130,16 +125,16 @@ class PE_DOPE(Node): self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 1) # Load model weights - w = str_param["weights_file"] + w = dependency["weights_file"] if not os.path.isfile(w): self.get_logger().warning(f"No weights found <{w}>") return TransitionCallbackReturn.FAILURE - obj = str_param["object_name"] - dim = str_param["dimensions"] + obj = dependency["object_name"] + dim = dependency["dimensions"] self.dope_node = Dope(self.dope_cfg, w, obj, dim) - self.get_logger().info(f"'{self.nodeName}' configure is success (with '{obj}')") + self.get_logger().info(f"configure is success (with '{obj}')") return TransitionCallbackReturn.SUCCESS def on_activate(self, state: State) -> TransitionCallbackReturn: @@ -400,7 +395,7 @@ def main(): executor = rclpy.executors.SingleThreadedExecutor() # executor = rclpy.executors.MultiThreadedExecutor() - lc_node = PE_DOPE() #NODE_NAME_DEFAULT) + lc_node = PE_DOPE() executor.add_node(lc_node) try: executor.spin() diff --git a/rbs_skill_interfaces/srv/RbsBt.srv b/rbs_skill_interfaces/srv/RbsBt.srv index 7c0e994..4ba134d 100644 --- a/rbs_skill_interfaces/srv/RbsBt.srv +++ b/rbs_skill_interfaces/srv/RbsBt.srv @@ -1,3 +1,4 @@ +string sid string action string command ---