diff --git a/rbs_bt_executor/bt_trees/bt_pe_test.xml b/rbs_bt_executor/bt_trees/bt_pe_test.xml
new file mode 100644
index 0000000..cbed900
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/bt_pe_test.xml
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rbs_bt_executor/src/rbsBTAction.cpp b/rbs_bt_executor/src/rbsBTAction.cpp
new file mode 100644
index 0000000..ed49505
--- /dev/null
+++ b/rbs_bt_executor/src/rbsBTAction.cpp
@@ -0,0 +1,82 @@
+// #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;
+using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
+
+class RbsBtAction : public BtService {
+public:
+ RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
+ : BtService(name, config) {
+
+ _action_name = getInput("do").value();
+ RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
+
+ _set_params_client = std::make_shared(_node, "rbs_interface");
+
+ 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...");
+ }
+ // 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");
+
+ 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;
+ }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({
+ BT::InputPort("do"),
+ BT::InputPort("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"
+
+BT_REGISTER_NODES(factory) {
+ factory.registerNodeType("RbsBtAction");
+}
diff --git a/rbs_perception/launch/perception.launch.py b/rbs_perception/launch/perception.launch.py
index 7f25bfe..4f26045 100644
--- a/rbs_perception/launch/perception.launch.py
+++ b/rbs_perception/launch/perception.launch.py
@@ -4,6 +4,10 @@ from launch import LaunchDescription
def generate_launch_description():
declared_arguments = []
+ rbs_interface = Node(
+ package="rbs_perception",
+ executable="rbs_interface.py",
+ )
pose_estimation = Node(
package="rbs_perception",
executable="pose_estimation_lifecycle.py",
@@ -14,6 +18,7 @@ def generate_launch_description():
)
nodes_to_start = [
+ rbs_interface,
pose_estimation,
object_detection,
]
diff --git a/rbs_perception/scripts/rbs_interface.py b/rbs_perception/scripts/rbs_interface.py
new file mode 100755
index 0000000..21e04d7
--- /dev/null
+++ b/rbs_perception/scripts/rbs_interface.py
@@ -0,0 +1,222 @@
+#!/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.action import ActionServer, CancelResponse, GoalResponse
+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 Parameter, ParameterValue
+# rcl_interfaces.msg.ParameterValue
+from lifecycle_msgs.srv import ChangeState, GetState
+from lifecycle_msgs.msg import Transition
+# from lifecycle_msgs.msg import State
+from rbs_skill_interfaces.srv import RbsBt
+# from ros2node.api import get_node_names as get_all_node_names
+
+PARAM_NAME = "str_param"
+PARAM_SKILL_CONFIG = "skill_cfg"
+
+def get_transfer_path_():
+ return os.path.join(get_package_share_directory("rbs_perception"), "config")
+
+class rbsInterface(Node):
+ def __init__(self, node_name):
+ """Construct the node."""
+ self._i_exe = 0 # run index
+ self._timer = None # defer run after callback
+ # self._cli_changestate = None # client for lifecycle node
+ self.cfg_data = None # config for current action
+ super().__init__(node_name)
+ # self.declare_parameter(PARAM_NAME, rclpy.Parameter.Type.STRING)
+ self.cb_group = ReentrantCallbackGroup()
+ self._service = self.create_service(RbsBt, "rbs_interface", self.service_callback, callback_group=self.cb_group)
+
+ # 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):
+ 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()
+ # self.get_logger().info(f"result : {type(res)}/{res}")
+
+ return res.results[0].successful
+
+ # while rclpy.ok():
+ # rclpy.spin_once(self)
+ # if future.done():
+ # try:
+ # res = future.result()
+ # return res.results[0].successful
+ # except Exception as e:
+ # self.get_logger().warn("Service call failed %r" % (e,))
+ # break
+
+ def _deserialize(self, file_path: str):
+ with open(file_path, "r") as f:
+ if file_path.split() == ".yaml":
+ s = yaml.load(f, Loader=yaml.FullLoader)
+ else: # ".json"
+ s = json.load(f)
+ return s
+
+ def _load_config(self, action: str): #, command: str):
+ p = os.path.join(get_transfer_path_(), action+".json")
+ # load config
+ return self._deserialize(p)
+
+ def run_action(self, command_data: dict) -> bool:
+ p_list = command_data["param"]
+ oper_type = command_data["type"]
+ node_name = self.cfg_data["ROS2"]["node_name"]
+ if len(p_list) > 0:
+ ext = command_data["format"] # 'yaml' or 'json'
+ param_file = os.path.join(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, PARAM_NAME, data):
+ return False
+ if not self.set_remote_parameter(node_name, PARAM_SKILL_CONFIG, yaml.dump(self.cfg_data)):
+ return False
+
+ ret = False # default return value
+ if oper_type == "run":
+ # if not self._cli_changestate:
+ # self.cb_group = ReentrantCallbackGroup()
+ self.cli_changestate = self.create_client(ChangeState, f"{node_name}/change_state") #, callback_group=self.cb_group)
+ # self._cli_changestate = self.create_client(GetState, f"{node_name}/get_state")
+
+ 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()
+ # self._req = GetState.Request()
+ req.transition.id = Transition.TRANSITION_CONFIGURE
+ # self._i_exe = 1 # call_async(self._req)
+ # self._timer = self.create_timer(0.0001, self.timer_callback)
+ 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
+ # self.cli_changestate.destroy()
+ # else:
+ # return False # state = future.exception()
+ # self.get_logger().info(f"state : {type(state)}/{state}")
+
+ 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
+ # self.cli_changestate.destroy()
+ return ret
+
+ def timer_callback(self):
+ if self._i_exe == 1:
+ self._i_exe = 2
+ # self.get_logger().info(f"{self._oper_type} timer_callback 1")
+ # self._future = self._cli_changestate.call_async(self._req)
+ # self.get_logger().info(f"self._future : {type(self._future)}/{self._future}")
+
+ elif self._i_exe == 2:
+ # self.get_logger().info(f"{self._oper_type} timer_callback 2")
+ responce = True #self._future.result()
+ if responce:
+ # self.get_logger().info(f"responce : {responce}")
+ self._i_exe == 0
+ self._timer.cancel()
+ else:
+ self._timer.cancel()
+
+ # def changestate_callback(self):
+ # self.get_logger().info(f"changestate_callback is")
+
+ 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.action) #, 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)
+
+ # if not os.path.isfile(request.object.mesh_path):
+ # response.call_status = False
+ # response.error_msg = f"{request.object.mesh_path}: no such file"
+
+ response.ok = is_action #True
+ return response
+
+def main():
+ rclpy.init()
+ # i_node = rbsInterface("rbs_interface")
+ # rclpy.spin(i_node)
+ # rclpy.shutdown()
+
+ # executor = rclpy.executors.SingleThreadedExecutor()
+ executor = rclpy.executors.MultiThreadedExecutor()
+ i_node = rbsInterface("rbs_interface")
+ 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_skill_interfaces/CMakeLists.txt b/rbs_skill_interfaces/CMakeLists.txt
index c6ab6fb..7eb04a8 100644
--- a/rbs_skill_interfaces/CMakeLists.txt
+++ b/rbs_skill_interfaces/CMakeLists.txt
@@ -31,6 +31,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/GetPickPlacePoses.srv"
"srv/AddPlanningSceneObject.srv"
"srv/PlanningSceneObjectState.srv"
+ "srv/RbsBt.srv"
DEPENDENCIES std_msgs geometry_msgs moveit_msgs shape_msgs
)
diff --git a/rbs_skill_interfaces/srv/RbsBt.srv b/rbs_skill_interfaces/srv/RbsBt.srv
new file mode 100644
index 0000000..7c0e994
--- /dev/null
+++ b/rbs_skill_interfaces/srv/RbsBt.srv
@@ -0,0 +1,4 @@
+string action
+string command
+---
+bool ok
\ No newline at end of file