BT interface node works with web-service

rbs_bt_executor/bt_trees/bt_pe_stop.xml

@@ -2,12 +2,13 @@
 <root main_tree_to_execute="Main">
   <BehaviorTree ID="Main">
     <Sequence>
-      <Action ID="RbsBtAction" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
+      <Action ID="RbsBtAction" sid="124" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
     </Sequence>
   </BehaviorTree>
 
   <TreeNodesModel>
     <Action ID="RbsBtAction">
+      <input_port name="sid" />
       <input_port name="do" />
       <input_port name="command" />
       <input_port name="server_name" />

rbs_bt_executor/bt_trees/bt_pe_test.xml

@@ -2,12 +2,13 @@
 <root main_tree_to_execute="Main">
   <BehaviorTree ID="Main">
     <Sequence>
-      <Action ID="RbsBtAction" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
+      <Action ID="RbsBtAction" sid="123" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
     </Sequence>
   </BehaviorTree>
 
   <TreeNodesModel>
     <Action ID="RbsBtAction">
+      <input_port name="sid" />
       <input_port name="do" />
       <input_port name="command" />
       <input_port name="server_name" />

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)])

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<RbsBtActionSrv> {
@@ -11,45 +13,30 @@ public:
   RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
       : BtService<RbsBtActionSrv>(name, config) {
 
-    _action_name = getInput<std::string>("do").value();
+    _action_name = getInput<std::string>(STR_ACTION).value();
     RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
 
-    _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "rbs_interface");
+    _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_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<std::string>("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<RbsBtActionSrv::Request>();
     request->action = _action_name;
-    request->command = getInput<std::string>("command").value();
+    request->sid = getInput<std::string>(STR_SID).value();
-    // RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:populate_request");
+    request->command = getInput<std::string>(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<rbs_utils::AssemblyConfigLoader>("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<std::string>("do"),
+        BT::InputPort<std::string>(STR_SID),
-        BT::InputPort<std::string>("command")
+        BT::InputPort<std::string>(STR_ACTION),
+        BT::InputPort<std::string>(STR_COMMAND)
     });
   }
 
 private:
   std::string _action_name{};
   std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
-  
-  // void set_str_param() {
-  //   RCLCPP_INFO_STREAM(_node->get_logger(),"Set string parameter: <" + _action_name + ">");
-
-  //   std::vector<rclcpp::Parameter> _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"

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()

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": []
+}

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)])

rbs_interface/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rbs_interface</name>
+  <version>0.0.1</version>
+  <description>The interface node</description>
+  <maintainer email="shaniks77s@gmail.com">shalenikol</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>rclcpp</depend>
+  <depend>image_transport</depend>
+  <depend>cv_bridge</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>rbs_skill_interfaces</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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()

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()

rbs_perception/config/PoseEstimation.json

@@ -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": []
-}

rbs_perception/config/peConfigure.yaml

@@ -1,3 +0,0 @@
-object_name: fork
-weights_file: /home/shalenikol/robossembler_ws/fork_e47.pth
-dimensions: [0.137, 0.165, 0.202]

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)
+        super().__init__(NODE_NAME_DEFAULT, **kwargs)
-        self.declare_parameter(PARAM_NAME, rclpy.Parameter.Type.STRING)
+
-        self.declare_parameter(PARAM_SKILL_CONFIG, rclpy.Parameter.Type.STRING)
+        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
+        json_param = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
-        str_param = yaml.load(yaml_param, Loader=yaml.FullLoader)
+        jdata = json.loads(json_param)
-        # !!! вместо PARAM_SKILL_CONFIG можно использовать self.skill_cfg !!! (это одно и то же)
+        dependency = {}
-        # yaml_param = self.get_parameter(PARAM_SKILL_CONFIG).get_parameter_value().string_value
+        for comm in jdata["BTAction"]:
-        # pe_cfg = yaml.load(yaml_param, Loader=yaml.FullLoader)
+            for par in comm["param"]:
-
+                if par["type"] == "weights":
-        # with open("pe.yaml", "w") as f:
+                    dependency = par["dependency"]
-        #     f.write(yaml_param)
+        assert dependency, "no dependency"
-        # print(skill_cfg)
 
         # 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"]
+        obj = dependency["object_name"]
-        dim = str_param["dimensions"]
+        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()

rbs_skill_interfaces/srv/RbsBt.srv

@@ -1,3 +1,4 @@
+string sid
 string action
 string command
 ---