Cartesian Move_to_pose Skill for BT Interface Node

rbs_interface/scripts/rbs_interface.py

@@ -4,44 +4,54 @@
   ROS 2 program for Robossembler
 
   @shalenikol release 0.1
+  @shalenikol release 0.2 BT v.4
 """
 import os
 import json
-# import yaml
 
 import rclpy
 from rclpy.node import Node
+# from rclpy.action import ActionClient
+from rclpy.action import ActionServer
 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 lifecycle_msgs.srv import ChangeState, GetState
+from lifecycle_msgs.msg import Transition, State
 from rbs_skill_interfaces.srv import RbsBt
+from rbs_skill_interfaces.action import RbsBt as RbsBtAction
+# from btcpp_ros2_interfaces.action import ExecuteTree
 
 # from rclpy.parameter_client import AsyncParameterClient # only Iron
 
-# PARAM_NAME   = "str_param"
-# PARAM_SKILL_CONFIG = "skill_cfg"
-PARAM_BT     = "bt_path"
+# BT_SERVER    = "bt_execution"
+CONDITION_SRV_NAME = "/condition"
+BT_PARAM     = "bt_path"
 NODE_NAME    = "rbs_interface"
+SERVICE_NAME = "rbs_interface_s"
+SERVER_NAME  = "rbs_interface_a"
 FILE_SKILLS  = "skills.json"
 PARAM_SUFFIX = "_cfg"
-
-# def get_transfer_path_():
-#     return os.path.join(get_package_share_directory("rbs_interface"), "config")
+KEY_BTPARAM  = "BTAction" # TODO "bt_param"
 
 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
+        self._timer = None   # for BT v.4
         super().__init__(node_name)
-        self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING)
+        self.declare_parameter(BT_PARAM, rclpy.Parameter.Type.STRING)
         self.cb_group = ReentrantCallbackGroup()
-        self._service = self.create_service(RbsBt, node_name, self.service_callback, callback_group=self.cb_group)
+        # for Condition
+        self._service = self.create_service(RbsBt, SERVICE_NAME, self.service_callback, callback_group=self.cb_group)
+        # for Action
+        self._action = ActionServer(self, RbsBtAction, SERVER_NAME, self.action_callback, callback_group=self.cb_group)
+        # self.bt_client = ActionClient(self, ExecuteTree, BT_SERVER, 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)
 
@@ -52,12 +62,11 @@ class rbsInterface(Node):
 
     def _on_set_btpath_param(self, parameter_list):
         for parameter in parameter_list:
-            if parameter.name == PARAM_BT:
+            if parameter.name == BT_PARAM:
                 self.bt_path = parameter.value
-            # self.get_logger().info(f'$ {parameter.name}={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):
@@ -85,8 +94,8 @@ class rbsInterface(Node):
         future = self.cli.call_async(req)
 
         self.executor.spin_until_future_complete(future)
-        res = future.result()
-        
+        res = future.result()        
+        # self.get_logger().info(f"{res}")
         return res.results[0].successful
 
     def _deserialize(self, file_path: str, sid: str):
@@ -109,18 +118,11 @@ class rbsInterface(Node):
     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"]
+        node_name = self.cfg_data["Module"]["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
 
@@ -165,19 +167,53 @@ class rbsInterface(Node):
                     res = future.result()
                     if res: # is not None:
                         ret = res.success
+
+        elif oper_type == "action":
+            ret = True
+
         return ret
 
+    def run_condition(self, command_data: dict) -> bool:
+        node_name = self.cfg_data["Module"]["node_name"]
+
+        self.cli = self.create_client(RbsBt, node_name + CONDITION_SRV_NAME)
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("'" + node_name + "' service not available, waiting again...")
+        req = RbsBt.Request()
+        req.command = command_data["name"]
+        future = self.cli.call_async(req)
+
+        self.executor.spin_until_future_complete(future)
+        res = future.result()        
+        return res.ok
+
+    def _interface(self, sid: str, action: str, command: str, isCondition: bool) -> bool:
+        self.get_logger().info(f"Incoming request ({action}/{command})")
+        self.cfg_data = self._load_config(sid)
+        typeBTnode = "Condition" if isCondition else "Action"
+        self.get_logger().info(f'{typeBTnode}: Ok ({self.cfg_data["Module"]["description"]})')
+
+        is_success = False
+        for comm in self.cfg_data[KEY_BTPARAM]:
+            if comm["name"] == command:
+                is_success = self.run_condition(comm) if isCondition else self.run_action(comm)
+
+        return is_success
+
+    def action_callback(self, goal_h):
+        goal = goal_h.request
+        is_success = self._interface(goal.sid, goal.action, goal.command, isCondition=False)
+        res = RbsBtAction.Result()
+        res.ok = is_success
+
+        if is_success:
+            goal_h.succeed()
+        else:
+            goal_h.abort()
+        return res
+
     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
+        response.ok = self._interface(request.sid, request.action, request.command, isCondition=True)
         return response
 
 def main():