Cartesian Move_to_pose Skill for BT Interface Node

rbs_bt_executor/CMakeLists.txt

@@ -88,8 +88,8 @@ list(APPEND plugin_libs rbs_get_workspace)
 add_executable(rbs_bt_executor src/TreeRunner.cpp)
 ament_target_dependencies(rbs_bt_executor ${dependencies})
 
-# add_library(rbs_interface SHARED src/rbsBTAction.cpp)
+add_library(rbs_act SHARED src/rbsBTAction.cpp)
-# list(APPEND plugin_libs rbs_interface)
+list(APPEND plugin_libs rbs_act)
 
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})

rbs_bt_executor/launch/rbs_bt_web.launch.py

@@ -3,6 +3,7 @@
   ROS 2 launch program for Robossembler
 
   @shalenikol release 0.1
+  @shalenikol release 0.2 BT v.4
 """
 import os
 # import json
@@ -13,35 +14,50 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventH
 from launch.substitutions import LaunchConfiguration
 from launch.event_handlers import OnExecutionComplete
 
-FILE_BT = "bt.xml"
+# 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"]}
+    #     ]
+    # )
     rbs_bt = Node(
-        package = "behavior_tree",
+        package = "rbs_bt_executor",
-        executable = "bt_engine",
+        executable = "rbs_bt_executor",
         parameters = [
-            {"bt_file_path": os.path.join(bt_path, FILE_BT)},
+            {
-            {"plugins": ["rbs_interface"]}
+                "plugins": ["rbs_bt_executor/bt_plugins"]
+                # "behavior_trees": [bt_path]
+            }
         ]
     )
 
+    bt_exec = Node(
+        package="rbs_interface",
+        executable="bt_exec",
+        arguments=[bt_path]
+        # prefix=['gdbserver localhost:3000'],
+    )
+
     bt_param = Node(
         package="rbs_interface",
         executable="bt_param.py",
-        parameters = [{"bt_path": bt_path}]
+        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],
+                    on_completion=[bt_exec],
+                    # on_completion=[rbs_bt, bt_exec],
                 )
             ),
             bt_param

rbs_bt_executor/src/rbsBTAction.cpp

@@ -34,6 +34,7 @@ public:
   }
 
   bool setRequest(Request::SharedPtr &request) override {
+    request->action = m_action_name; 
     getInput(STR_SID, request->sid);
     getInput(STR_COMMAND, request->command);
     return true;
@@ -59,4 +60,5 @@ private:
   std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
 };
 
-CreateRosNodePlugin(RbsBtAction, "RbsAction")
+// !!! теперь устаревшая версия !!!
+CreateRosNodePlugin(RbsBtAction, "RbsBtAction")

rbs_interface/CMakeLists.txt

@@ -19,9 +19,11 @@ 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_ros REQUIRED)
-find_package(tf2_msgs REQUIRED)
+# find_package(tf2_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
+find_package(btcpp_ros2_interfaces REQUIRED)
 
 # Install Python modules
 ament_python_install_package(${PROJECT_NAME})
@@ -33,6 +35,21 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+set(THIS_PACKAGE_DEPS
+    rbs_skill_interfaces
+    behaviortree_ros2
+    # std_msgs
+    # std_srvs
+    btcpp_ros2_interfaces )
+
+add_executable(bt_exec src/BTExec.cpp)
+ament_target_dependencies(bt_exec ${THIS_PACKAGE_DEPS})
+
+install(TARGETS
+  bt_exec
+  DESTINATION lib/${PROJECT_NAME}
+  )
+
 install(
   DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}

rbs_interface/launch/interface.launch.py

@@ -29,6 +29,7 @@ def get_skill_list_(path: str) -> list:
     excluding = {}
     skills = []
     for skill in data["skills"]:
+        node_name = skill["Module"]["node_name"]
         launch = skill["Launch"]
         p = launch["package"]
         e = launch["executable"]
@@ -37,7 +38,7 @@ def get_skill_list_(path: str) -> list:
             continue
         nn_skills += 1
         # skills.append(Node(package=p, executable=e))
-        launch["parameters"] = [{launch["name"]+PARAM_SUFFIX: json.dumps(skill)}]
+        launch["parameters"] = [{node_name+PARAM_SUFFIX: json.dumps(skill)}]
         skills.append(Node(**launch))
 
     return skills

rbs_interface/package.xml

@@ -17,7 +17,8 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>rbs_skill_interfaces</depend>
-
+  <depend>behaviortree_ros2</depend>
+  
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

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.srv import ChangeState, GetState
-from lifecycle_msgs.msg import Transition
+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"
+# BT_SERVER    = "bt_execution"
-# PARAM_SKILL_CONFIG = "skill_cfg"
+CONDITION_SRV_NAME = "/condition"
-PARAM_BT     = "bt_path"
+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"
-
+KEY_BTPARAM  = "BTAction" # TODO "bt_param"
-# 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
+        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})")
+        response.ok = self._interface(request.sid, request.action, request.command, isCondition=True)
-        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():

rbs_interface/src/BTExec.cpp

@@ -0,0 +1,201 @@
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+#include <string>
+#include <fstream>
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
+#include "rbs_skill_interfaces/action/rbs_bt.hpp"
+
+#define STR_ACTION   "do"
+#define STR_SID      "sid"
+#define STR_COMMAND  "command"
+#define NODE_NAME    "rbs_interface"
+#define SERVICE_NAME "rbs_interface_s"
+#define SERVER_NAME  "rbs_interface_a"
+#define FILE_BT      "bt.xml"
+
+template<typename T> std::string to_string(const T &t)
+{
+    std::stringstream ss;
+    ss << t;
+    return ss.str();
+}
+
+using namespace BT;
+using namespace std::chrono_literals;
+using RbsConditionSrv = rbs_skill_interfaces::srv::RbsBt;
+
+using RbsActionSrv = rbs_skill_interfaces::action::RbsBt;
+
+class RbsAction : public RosActionNode<RbsActionSrv> {
+public:
+  RbsAction(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosActionNode<RbsActionSrv>(name, conf, params) {
+
+    m_action = getInput<std::string>(STR_ACTION).value();
+    m_command = getInput<std::string>(STR_COMMAND).value();
+    RCLCPP_INFO_STREAM(logger(), "Start node RbsAction: " + m_action + "/" + m_command);
+
+    m_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
+
+    while (!m_client->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
+        break;
+      }
+      RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
+    }
+
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({
+      BT::InputPort<std::string>(STR_SID),
+      BT::InputPort<std::string>(STR_ACTION),
+      BT::InputPort<std::string>(STR_COMMAND)
+      });
+  }
+
+  bool setGoal(RosActionNode::Goal& goal) override {
+    getInput(STR_ACTION, goal.action);
+    getInput(STR_COMMAND, goal.command);
+    getInput(STR_SID, goal.sid);
+    m_action = goal.action;
+    m_command = goal.command;
+
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "setGoal: " + to_string(m_i) + ") " + goal.action + "/" + goal.command + " goal.sid = " + to_string(goal.sid));
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult& wr) override {
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "onResultReceived: " + to_string(m_i) + ") " + m_action + "/" + m_command + " wr.result->ok - " + to_string(wr.result->ok));
+
+    if (!wr.result->ok) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  int m_i{0};
+  std::string m_action{};
+  std::string m_command{};
+  std::shared_ptr<rclcpp::AsyncParametersClient> m_client;
+};
+
+// **********************************************************
+class RbsCondition : public RosServiceNode<RbsConditionSrv> {
+public:
+  RbsCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
+      : RosServiceNode<RbsConditionSrv>(name, conf, params) {
+
+    m_action_name = getInput<std::string>(STR_ACTION).value();
+    m_command = getInput<std::string>(STR_COMMAND).value();
+    RCLCPP_INFO_STREAM(logger(), "Start node RbsCondition: " + m_action_name + "/" + m_command);
+
+    m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
+
+    while (!m_params_client->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
+        break;
+      }
+      RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
+    }
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    request->action = m_action_name; 
+    request->command = m_command;
+    getInput(STR_SID, request->sid);
+
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "setRequest: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " request->sid = " + to_string(request->sid));
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    m_i++;
+    RCLCPP_INFO_STREAM(logger(), "onResponseReceived: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " response->ok - " + to_string(response->ok));
+
+    if (!response->ok) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts({
+        InputPort<std::string>(STR_SID),
+        InputPort<std::string>(STR_ACTION),
+        InputPort<std::string>(STR_COMMAND)
+    });
+  }
+
+private:
+  int m_i{0};
+  std::string m_action_name{};
+  std::string m_command{};
+  std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
+};
+
+// class RbsCondition : public RosTopicSubNode {
+// }
+// Simple function that return a NodeStatus
+// BT::NodeStatus TestIf()
+// {
+//   std::string m_if = "if"; //getInput<std::string>("if").value();
+//   // std::cout << "[ Test: OK ]" << std::endl;
+  
+//   std::stringstream ss;
+//   ss << "[ Test: OK ] " << m_if;
+//   std::string str = ss.str();
+//   std::cout << str << std::endl;
+//   return BT::NodeStatus::SUCCESS;
+// }
+
+int main(int argc, char** argv)
+{
+    // filename Behavior Tree
+    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>()}; 
+
+  rclcpp::init(argc, argv);
+
+  // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), xml.c_str());
+  auto nh = std::make_shared<rclcpp::Node>("bt_exec"); //"_action");
+  // auto nh2 = std::make_shared<rclcpp::Node>("bt_exe_condition");
+
+  RosNodeParams params; //,params2;
+  params.nh = nh;
+  params.default_port_value = SERVICE_NAME;
+  params.server_timeout = 3s;
+
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<RbsCondition>("Is", params);
+
+  // params.nh = nh;
+  params.default_port_value = SERVER_NAME;
+  // params.server_timeout = 3s;
+  factory.registerNodeType<RbsAction>("RbsAction", params);
+  // factory.registerSimpleCondition("Is", [&](TreeNode&) { return TestIf(); });
+  // factory.registerRosAction("Is", params);
+
+  auto tree = factory.createTreeFromText(xml);
+
+  // while(rclcpp::ok())
+  if(rclcpp::ok())
+  {
+    tree.tickWhileRunning();
+  }
+
+  rclcpp::shutdown();
+
+  return 0;
+}

rbs_skill_interfaces/CMakeLists.txt

@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "action/MoveitSendPose.action"
     "action/MoveitSendJointStates.action"
     "action/GripperCommand.action"
+    "action/RbsBt.action"
     "msg/ObjectInfo.msg"
     "msg/PropertyValuePair.msg"
     "msg/ActionFeedbackStatusConstants.msg"

rbs_skill_interfaces/action/RbsBt.action

@@ -0,0 +1,11 @@
+string sid
+string action
+string command
+---
+#result definition
+bool ok
+string status  #Use the constants of ActionResultStatusConstants in the status field
+---
+#feedback
+bool ok
+string status  #Use the constants of ActionFeedbackStatusConstants in the status field

rbs_skill_servers/launch/skills.launch.py

@@ -67,15 +67,15 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
-    cartesian_move_to_pose_action_server = Node(
+    # cartesian_move_to_pose_action_server = Node(
-        package="rbs_skill_servers",
+    #     package="rbs_skill_servers",
-        executable="move_to_pose.py",
+    #     executable="move_to_pose.py",
-        namespace=namespace,
+    #     namespace=namespace,
-        parameters=[
+    #     parameters=[
-            {"use_sim_time": use_sim_time},
+    #         {"use_sim_time": use_sim_time},
-            {"robot_name": namespace}
+    #         {"robot_name": namespace}
-            ]
+    #         ]
-    )
+    # )
     
     move_joint_state_action_server = Node(
         package="rbs_skill_servers",
@@ -101,7 +101,7 @@ def launch_setup(context, *args, **kwargs):
         gripper_control_node,
         move_cartesian_path_action_server,
         move_joint_state_action_server,
-        cartesian_move_to_pose_action_server,
+        # cartesian_move_to_pose_action_server,
         # grasp_pose_loader
     ]
     return nodes_to_start

rbss_movetopose/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.8)
+project(rbss_movetopose)
+
+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(std_msgs REQUIRED)
+find_package(rbs_skill_interfaces REQUIRED)
+find_package(PCL 1.12 REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
+
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/mtp_cartesian.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch
+  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()

rbss_movetopose/launch/movetopose.launch.py

@@ -0,0 +1,17 @@
+# from launch_ros.actions import Node
+# from launch import LaunchDescription
+
+def generate_launch_description():
+    print("The skill must be launched via interface node")
+    return []
+    # declared_arguments = []
+    
+    # rbss_od = Node(
+    #     package="rbss_movetopose",
+    #     executable="movetopose.py",
+    # )
+
+    # nodes_to_start = [
+    #     rbss_od,
+    # ]
+    # return LaunchDescription(declared_arguments + nodes_to_start)

rbss_movetopose/package.xml

@@ -0,0 +1,24 @@
+<?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>rbss_movetopose</name>
+  <version>0.0.1</version>
+  <description>The Move to pose skill</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>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>

rbss_movetopose/rbs_package.json

@@ -0,0 +1,30 @@
+{
+    "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
+    "Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers" },
+    "Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py", "name": "mtp_cartesian" },
+    "BTAction": [
+        {
+            "name": "move",
+            "type": "action",
+            "param": [
+              {
+                "type": "move_to_pose",
+                "dependency": { "robot_name": "arm0",
+                                "pose": { "position": {"x":0.0, "y":0.0, "z":0.0}, "orientation": {"x":0.0, "y":0.0, "z":0.0, "w": 1.0} } }
+              }
+            ]
+        }
+    ],
+    "Interface": {
+        "Input": [
+          { "name": "robotName", "type": "ROBOT" },
+          { "name": "pose", "type": "POSE" }
+        ],
+        "Output": []
+      },
+    "Settings": [
+        { "name": "server_name", "value": "cartesian_move_to_pose" },
+        { "name": "end_effector_velocity", "value": 1.0 }, 
+        { "name": "end_effector_acceleration", "value": 1.0 }
+    ]
+}

rbss_movetopose/scripts/mtp_cartesian.py

@@ -0,0 +1,264 @@
+#!/usr/bin/env python3
+"""
+  Move_to_pose_cartesian_node_via_interface_node
+  ROS 2 program for Move to Pose skill
+
+  @shalenikol release 0.1
+"""
+import json
+import math
+import numpy as np
+from scipy.spatial.transform import Slerp, Rotation as R
+
+import rclpy
+from rclpy.action import ActionClient, ActionServer
+from rclpy.node import Node
+from rclpy.callback_groups import ReentrantCallbackGroup
+
+from rcl_interfaces.msg import SetParametersResult
+from geometry_msgs.msg import Pose, PoseStamped
+
+from rbs_skill_interfaces.action import MoveitSendPose
+
+NODE_NAME_DEFAULT = "mtp_cartesian"   # the name doesn't matter in this node (defined in Launch)
+PARAM_SKILL_CFG = "mtp_cartesian_cfg"
+SERVER_NAME = "cartesian_movetopose"
+TOPIC_CURRENT_POSE = "/cartesian_motion_controller/current_pose"
+TOPIC_TARGET_FRAME = "/cartesian_motion_controller/target_frame"
+SERVER_PAR1_BASE_LINK = "base_link"
+SERVER_PAR2_ROBOT_NAME = "robot_name"
+
+class MoveToPoseCartesianSkill(Node):
+    """ <Move to pose> skill node """
+    def _Settings(self):
+        # Initialization service settings
+        for prop in self.skill_cfg["Settings"]:
+            nam = prop["name"]
+            val = prop["value"]
+            if nam == "server_name":
+                self.server_name = val
+            elif nam == "end_effector_velocity":
+                self.end_effector_velocity = val
+            elif nam == "end_effector_acceleration":
+                self.end_effector_acceleration = val
+
+    def __init__(self, **kwargs):
+        self.end_effector_velocity = 1.0
+        self.end_effector_acceleration = 1.0
+        # for other nodes
+        kwargs["allow_undeclared_parameters"] = True
+        kwargs["automatically_declare_parameters_from_overrides"] = True
+        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["Module"]["node_name"] #["Launch"]["name"]
+
+        self.server_name = SERVER_NAME #"cartesian_move_to_pose"
+        self._Settings()
+
+        self._action_client = ActionClient(self, MoveitSendPose, self.server_name) # "/"+ robot_name + 
+
+        self._cnt = 0
+        self.add_on_set_parameters_callback(self._on_set_param)
+
+    def _on_set_param(self, parameter_list):
+        self._cnt += 1
+        for parameter in parameter_list:
+            # self.get_logger().info(f'{self._cnt}$ {parameter.name}={parameter.value}')
+            if parameter.name == PARAM_SKILL_CFG:
+                self.skill_cfg = json.loads(parameter.value)
+
+                self._Settings()
+                dependency = {}
+                for comm in self.skill_cfg["BTAction"]:
+                    for par in comm["param"]:
+                        if par["type"] == "move_to_pose":
+                            dependency = par["dependency"]
+                assert dependency, "no dependency"
+
+                self.send_goal(dependency)
+                self.get_logger().info(f"{self._cnt}) dependency = {dependency}")
+                break
+        return SetParametersResult(successful=True)
+
+    def send_goal(self, dep):
+        goal_msg = MoveitSendPose.Goal()
+        pose = dep["pose"]
+        # goal_msg.target_pose
+        goal_msg.target_pose.position.x = pose["position"]["x"]
+        goal_msg.target_pose.position.y = pose["position"]["y"]
+        goal_msg.target_pose.position.z = pose["position"]["z"]
+
+        goal_msg.target_pose.orientation.x = pose["orientation"]["x"]
+        goal_msg.target_pose.orientation.y = pose["orientation"]["y"]
+        goal_msg.target_pose.orientation.z = pose["orientation"]["z"]
+        goal_msg.target_pose.orientation.w = pose["orientation"]["w"]
+
+        goal_msg.robot_name = dep["robot_name"]
+        goal_msg.end_effector_velocity = self.end_effector_velocity
+        goal_msg.end_effector_acceleration = self.end_effector_acceleration
+
+        self._action_client.wait_for_server()
+
+        return self._action_client.send_goal_async(goal_msg)
+
+class CartesianMoveToPose(Node):
+    """ <Move to pose> action server """
+    def __init__(self, server_name:str = SERVER_NAME):
+        super().__init__("cartesian_server")
+
+        self.declare_parameter(SERVER_PAR1_BASE_LINK, "base_link")
+        self.declare_parameter(SERVER_PAR2_ROBOT_NAME, "")
+
+        self._callback_group = ReentrantCallbackGroup()
+        self._action_server = ActionServer(
+            self,
+            MoveitSendPose,
+            server_name,
+            self.execute_callback, callback_group=self._callback_group)
+
+        self.base_link: str = self.get_parameter(SERVER_PAR1_BASE_LINK).get_parameter_value().string_value
+        # for multirobot setup where each robot name is a namespace
+        self.robot_name: str = self.get_parameter(SERVER_PAR2_ROBOT_NAME).get_parameter_value().string_value
+        self.robot_name = self.robot_name.lstrip('/').rstrip('/')
+        self.robot_name = f"/{self.robot_name}" if self.robot_name else ""
+        self._pub = self.create_publisher(PoseStamped,
+                                          self.robot_name + TOPIC_TARGET_FRAME, 1,
+                                          callback_group=self._callback_group)
+        self.current_pose = None
+        self.goal_tolerance = 0.05
+        self.max_speed = 0.1
+        self.max_acceleration = 0.05
+
+    def on_pose_callback(self, msg: PoseStamped):
+        if isinstance(msg, PoseStamped):
+            self.current_pose = msg
+
+    def execute_callback(self, goal_handle):
+        self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
+        target_pose = goal_handle.request.target_pose
+        start_pose = self.current_pose.pose if self.current_pose else None
+
+        if start_pose is None:
+            self.get_logger().error("Current pose is not available")
+            goal_handle.abort()
+            return MoveitSendPose.Result()
+
+        trajectory = self.generate_trajectory(start_pose, target_pose)
+        for point in trajectory:
+            tp = PoseStamped()
+            tp.pose = point
+            tp.header.stamp = self.get_clock().now().to_msg()
+            tp.header.frame_id = self.base_link
+            self._pub.publish(tp)
+            rclpy.spin_once(self, timeout_sec=0.1)
+
+        goal_handle.succeed()
+
+        result = MoveitSendPose.Result()
+        result.success = True
+        return result
+
+    def generate_trajectory(self, start_pose, target_pose):
+        start_position = np.array([
+            start_pose.position.x,
+            start_pose.position.y,
+            start_pose.position.z
+        ])
+        target_position = np.array([
+            target_pose.position.x,
+            target_pose.position.y,
+            target_pose.position.z
+        ])
+
+        start_orientation = R.from_quat([
+            start_pose.orientation.x,
+            start_pose.orientation.y,
+            start_pose.orientation.z,
+            start_pose.orientation.w
+        ])
+        target_orientation = R.from_quat([
+            target_pose.orientation.x,
+            target_pose.orientation.y,
+            target_pose.orientation.z,
+            target_pose.orientation.w
+        ])
+
+        distance = np.linalg.norm(target_position - start_position)
+        max_speed = self.max_speed
+        max_acceleration = self.max_acceleration
+
+        t_acc = max_speed / max_acceleration
+        d_acc = 0.5 * max_acceleration * t_acc**2
+
+        if distance < 2 * d_acc:
+            t_acc = math.sqrt(distance / max_acceleration)
+            t_flat = 0
+        else:
+            t_flat = (distance - 2 * d_acc) / max_speed
+
+        total_time = 2 * t_acc + t_flat
+        num_points = int(total_time * 10)
+        trajectory = []
+
+        times = np.linspace(0, total_time, num_points + 1)
+        key_rots = R.from_quat([start_orientation.as_quat(), target_orientation.as_quat()])
+        slerp = Slerp([0, total_time], key_rots)
+
+        for t in times:
+            if t < t_acc:
+                fraction = 0.5 * max_acceleration * t**2 / distance
+            elif t < t_acc + t_flat:
+                fraction = (d_acc + max_speed * (t - t_acc)) / distance
+            else:
+                t_decel = t - t_acc - t_flat
+                fraction = (d_acc + max_speed * t_flat + 0.5 * max_acceleration * t_decel**2) / distance
+
+            intermediate_position = start_position + fraction * (target_position - start_position)
+            intermediate_orientation = slerp([t])[0]
+
+            intermediate_pose = Pose()
+            intermediate_pose.position.x = intermediate_position[0]
+            intermediate_pose.position.y = intermediate_position[1]
+            intermediate_pose.position.z = intermediate_position[2]
+            intermediate_orientation_quat = intermediate_orientation.as_quat()
+            intermediate_pose.orientation.x = intermediate_orientation_quat[0]
+            intermediate_pose.orientation.y = intermediate_orientation_quat[1]
+            intermediate_pose.orientation.z = intermediate_orientation_quat[2]
+            intermediate_pose.orientation.w = intermediate_orientation_quat[3]
+            trajectory.append(intermediate_pose)
+
+        return trajectory
+
+class PoseSubscriber(Node):
+    def __init__(self, parent: CartesianMoveToPose, robot_name: str):
+        super().__init__("pose_subscriber")
+        self.parent = parent
+        self._sub = self.create_subscription(PoseStamped,
+                                             robot_name + TOPIC_CURRENT_POSE,
+                                             self.parent.on_pose_callback, 1,
+                                             callback_group=self.parent._callback_group)
+        self.get_logger().info("PoseSubscriber node initialized")
+
+def main():
+    rclpy.init()
+
+    node = MoveToPoseCartesianSkill()
+    cartesian_mtp_node = CartesianMoveToPose(node.server_name)
+    pose_subscriber = PoseSubscriber(parent=cartesian_mtp_node, 
+                                     robot_name=cartesian_mtp_node.robot_name)
+
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(cartesian_mtp_node)
+    executor.add_node(pose_subscriber)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
+        pose_subscriber.destroy_node()
+        cartesian_mtp_node.destroy_node()
+        node.destroy_node()
+
+if __name__ == '__main__':
+    main()

rbss_objectdetection/scripts/od_yolo_lc.py

@@ -20,12 +20,14 @@ from rclpy.timer import Timer
 from ament_index_python.packages import get_package_share_directory
 from sensor_msgs.msg import Image
 from rbs_skill_interfaces.msg import BoundBox
+from rbs_skill_interfaces.srv import RbsBt
 
 from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
 
+CONDITION_SRV_NAME = "/condition"
 FILE_DOPE_CONFIG = "od_yolo_config.yaml"
 CAMERA_LINK_DEFAULT = "rgbd_camera"
-NODE_NAME_DEFAULT = "lc_yolo"   # the name doesn't matter in this node (defined in Launch)
+NODE_NAME_DEFAULT = "lc_yolo"
 PARAM_SKILL_CFG = "lc_yolo_cfg"
 
 class ObjectDetection(Node):
@@ -60,7 +62,7 @@ class ObjectDetection(Node):
         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"]
+        self.nodeName = NODE_NAME_DEFAULT # self.skill_cfg["Module"]["node_name"] #["Launch"]["name"]
 
         self.camera_link = "" 
         self.topicImage = ""
@@ -77,6 +79,17 @@ class ObjectDetection(Node):
         self.image_det = []
         self.bbox_res = None
 
+        self._srv_condition = self.create_service(RbsBt, NODE_NAME_DEFAULT + CONDITION_SRV_NAME, self.condition_callback)
+        # , callback_group=self.cb_group)
+
+    def condition_callback(self, request, response):
+        # is_success = self._interface(request.sid, request.action, request.command)
+        _is = False
+        if request.command == "isDetectionRun":
+            _is = True
+        response.ok = _is
+        return response
+
     def _set_camera_topic(self, camera_link: str):
         """ Service for camera name topics """
         self.camera_link = camera_link